2024-11-06T02:49:20Z* 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-06T02:49:20Z* LcmTrn::init() - using configuration file /home/tethysadmin/Downloads/trncfgs/lcm-trn-lrauv.cfg 2024-11-06T02:49:20Z* ahrs config: AHRS_M2, platform_orientation, platform_pitch_angle, platform_roll_angle 2024-11-06T02:49:20Z* dvl config: RDI_Pathfinder, Beam1Range, Beam2Range, Beam3Range, Beam4Range, BottomVelocityFlag 2024-11-06T02:49:20Z* nav config: DeadReckonUsingMultipleVelocitySources, latitude, longitude 2024-11-06T02:49:20Z* depth config: Depth_Keller, depth, sea_water_pressure 2024-11-06T02:49:20Z* LCM timeout=0.20 sec 2024-11-06T02:49:20Z* TRN settings: 2024-11-06T02:49:20Z* period=4.00 sec 2024-11-06T02:49:20Z* coherence=0.90 sec 2024-11-06T02:49:20Z* map = PortTiles cfg = LRAUV_specs.cfg part= particles.cfg logdir= lrauv 2024-11-06T02:49:20Z* maptype = 2 filtertype = 2 weighting = 4 2024-11-06T02:49:20Z* lowgrade_filter = 0 allow reinit = 1 2024-11-06T02:49:20Z* cmd_estimate = estimate cmd_reinit = reinit 2024-11-06T02:49:20Z* LcmTrn::initTrn() version v1.0.3b0-518-g924cc45 - configuration file /home/tethysadmin/Downloads/trncfgs/lcm-trn-lrauv.cfg 2024-11-06T02:49:20Z* LcmTrn::initTrn() - map: /home/tethysadmin/Downloads/trnmaps/PortTiles 2024-11-06T02:49:20Z* LcmTrn::initTrn() - cfg: /home/tethysadmin/Downloads/trncfgs/LRAUV_specs.cfg 2024-11-06T02:49:20Z* LcmTrn::initTrn() - part: /home/tethysadmin/Downloads/trncfgs/particles.cfg 2024-11-06T02:49:20Z* TerrainMapOctree::tileDataName = /home/tethysadmin/Downloads/trnmaps/PortTiles/tiles.csv 2024-11-06T02:49:20Z* TerrainMapOctree::numTiles_ = 9. 2024-11-06T02:49:20Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile1.bo 4061250.0 594250.0 . 2024-11-06T02:49:20Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile2.bo 4061250.0 594750.0 . 2024-11-06T02:49:20Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile3.bo 4061250.0 595250.0 . 2024-11-06T02:49:20Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile4.bo 4061750.0 594250.0 . 2024-11-06T02:49:20Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile5.bo 4061750.0 594750.0 . 2024-11-06T02:49:20Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile6.bo 4061750.0 595250.0 . 2024-11-06T02:49:20Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile7.bo 4062250.0 594250.0 . 2024-11-06T02:49:20Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile8.bo 4062250.0 594750.0 . 2024-11-06T02:49:20Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile9.bo 4062250.0 595250.0 . 2024-11-06T02:49:20Z* TerrainMapOctree::loaded 9 useable tiles from /home/tethysadmin/Downloads/trnmaps/PortTiles/tiles.csv. 2024-11-06T02:49:20Z* TerrainMapOctree::Octree tile load /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile1.bo took 0.479040 seconds. 2024-11-06T02:49:20Z* session directory is /home/tethysadmin/Downloads/trnlogs/lrauv-TRN.13/ 2024-11-06T02:49:20Z* TRN log directory is /home/tethysadmin/Downloads/trnlogs/lrauv-TRN.13/ 2024-11-06T02:49:20Z* symlink /home/tethysadmin/Downloads/trnlogs/latestTRN to lrauv-TRN.13 OK 2024-11-06T02:49:20Z* parseSensorSpecs parsing sensor of type 1. 2024-11-06T02:49:20Z* Sensor 0 is of type 1. 2024-11-06T02:49:20Z- File created[/home/tethysadmin/Downloads/trnlogs/latestTRN/trn.log.0000] returned[0x5579ab5da0] 2024-11-06T02:49:20Z- TNavConfig::setMapFile: value is now /home/tethysadmin/Downloads/trnmaps/PortTiles 2024-11-06T02:49:20Z- TNavConfig::setVehicleSpecsFile: value is now /home/tethysadmin/Downloads/trncfgs/LRAUV_specs.cfg 2024-11-06T02:49:20Z- TNavConfig::setParticlesFile: value is now /home/tethysadmin/Downloads/trncfgs/particles.cfg 2024-11-06T02:49:20Z- TNavConfig::setLogDir: value is now /home/tethysadmin/Downloads/trnlogs/lrauv-TRN.13/ 2024-11-06T02:49:20Z- createFilter: Loading vehicle config file... /home/tethysadmin/Downloads/trncfgs/LRAUV_specs.cfg 2024-11-06T02:49:20Z- parseSensorSpecs parsing sensor of type 1. 2024-11-06T02:49:20Z- Sensor 0 is of type 1. 2024-11-06T02:49:20Z- Random noise generator initialized with 1730861360 2024-11-06T02:49:20Z- Vehicle name: LRAUV Number of sensors: 1 2024-11-06T02:49:20Z- Sensor #1: 2024-11-06T02:49:20Z- Sensor name: rdiDVL 2024-11-06T02:49:20Z- Sensor type: 1 2024-11-06T02:49:20Z- Number of beams per measurement: 4 2024-11-06T02:49:20Z- Sensor #1 to vehicle transformation information: 2024-11-06T02:49:20Z- Rotation angles (phi, theta, psi): (0.000000 ,0.000000, 0.000000) 2024-11-06T02:49:20Z- Translation vector [dx, dy, dz]: (0.000000 ,0.000000, 0.000000) 2024-11-06T02:49:20Z- 2024-11-06T02:49:20Z- setDistribToSave(0) 2024-11-06T02:49:20Z- setDistribToSave set to 0 2024-11-06T02:49:20Z- TerrainNav::TNavFilter initialized with type 2 2024-11-06T02:49:20Z- TerrainNav::initVariables finished. 2024-11-06T02:49:20Z- TerrainNav::Constructor finished. 2024-11-06T02:49:20Z- TerrainNav::modified weighting set to 4 2024-11-06T02:49:20Z- LcmTrn::initLcm() - configuration file /home/tethysadmin/Downloads/trncfgs/lcm-trn-lrauv.cfg 2024-11-06T02:49:20Z- LcmTrn::run() v1.0.3b0-518-g924cc45 using bursty timeouts of 50 and 150 msec 2024-11-06T02:49:21Z- AHRS_M2 msg: 1730861361.00 epoch sec; seqNo:5 2024-11-06T02:49:21Z- AHRS_M2 msg: 0.14 phi; -0.14 theta; 5.26 psi 2024-11-06T02:49:21Z- handleDepth()- Depth_Keller msg: 1730861361.01 epoch sec; seqNo:6; depth 3.42 2024-11-06T02:49:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861361.03 epoch sec; seqNo:6; 4061422.85 north; 594643.45 east; 36.69 lat; -121.94 lon; 3.42 depth; utmZone: 10 2024-11-06T02:49:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861361.03 2024-11-06T02:49:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:21Z- Waiting for fresh data: AHRS(1730861361.00), Dvl(-1.00), Nav(1730861361.03), Depth(1730861361.01) 2024-11-06T02:49:21Z- AHRS_M2 msg: 1730861361.41 epoch sec; seqNo:6 2024-11-06T02:49:21Z- AHRS_M2 msg: 0.15 phi; -0.16 theta; 5.26 psi 2024-11-06T02:49:21Z- handleDepth()- Depth_Keller msg: 1730861361.42 epoch sec; seqNo:7; depth 3.47 2024-11-06T02:49:21Z- handleDVL() - DVL msg epochMillisec: 1730861361.45 2024-11-06T02:49:21Z- handleDvl() - RDI_Pathfinder msg: 1730861361.45 sec; seqNo:2; #beams:4 2024-11-06T02:49:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.19 , 78.90 , 78.90 , 95.62 2024-11-06T02:49:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.02 , 0.43 2024-11-06T02:49:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861361.46 epoch sec; seqNo:7; 4061422.83 north; 594642.23 east; 36.69 lat; -121.94 lon; 3.47 depth; utmZone: 10 2024-11-06T02:49:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861361.03 2024-11-06T02:49:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:21Z- waiting for 1730861365.03, time is 1730861361.46 2024-11-06T02:49:21Z- AHRS_M2 msg: 1730861361.81 epoch sec; seqNo:7 2024-11-06T02:49:21Z- AHRS_M2 msg: 0.15 phi; -0.17 theta; 5.26 psi 2024-11-06T02:49:21Z- handleDepth()- Depth_Keller msg: 1730861361.82 epoch sec; seqNo:8; depth 3.50 2024-11-06T02:49:21Z- handleDVL() - DVL msg epochMillisec: 1730861361.83 2024-11-06T02:49:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:49:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:49:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:49:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:49:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:49:21Z- handleDvl() - RDI_Pathfinder msg: 1730861361.83 sec; seqNo:3; #beams:0 2024-11-06T02:49:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:49:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.02 , 0.43 2024-11-06T02:49:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861361.84 epoch sec; seqNo:8; 4061422.83 north; 594642.23 east; 36.69 lat; -121.94 lon; 3.50 depth; utmZone: 10 2024-11-06T02:49:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861361.03 2024-11-06T02:49:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:21Z- waiting for 1730861365.03, time is 1730861361.84 2024-11-06T02:49:22Z- handleDepth()- Depth_Keller msg: 1730861362.24 epoch sec; seqNo:9; depth 3.53 2024-11-06T02:49:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861362.25 epoch sec; seqNo:9; 4061422.83 north; 594642.23 east; 36.69 lat; -121.94 lon; 3.53 depth; utmZone: 10 2024-11-06T02:49:22Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:49:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861361.03 2024-11-06T02:49:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:22Z- waiting for 1730861365.03, time is 1730861362.25 2024-11-06T02:49:22Z- AHRS_M2 msg: 1730861362.62 epoch sec; seqNo:8 2024-11-06T02:49:22Z- AHRS_M2 msg: 0.16 phi; -0.18 theta; 5.28 psi 2024-11-06T02:49:22Z- handleDepth()- Depth_Keller msg: 1730861362.62 epoch sec; seqNo:10; depth 3.55 2024-11-06T02:49:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861362.64 epoch sec; seqNo:10; 4061423.21 north; 594642.22 east; 36.69 lat; -121.94 lon; 3.55 depth; utmZone: 10 2024-11-06T02:49:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861361.03 2024-11-06T02:49:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:22Z- waiting for 1730861365.03, time is 1730861362.64 2024-11-06T02:49:23Z- AHRS_M2 msg: 1730861363.03 epoch sec; seqNo:9 2024-11-06T02:49:23Z- AHRS_M2 msg: 0.17 phi; -0.17 theta; 5.29 psi 2024-11-06T02:49:23Z- handleDepth()- Depth_Keller msg: 1730861363.03 epoch sec; seqNo:11; depth 3.56 2024-11-06T02:49:23Z- handleDVL() - DVL msg epochMillisec: 1730861363.03 2024-11-06T02:49:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:49:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:49:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:49:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:49:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:49:23Z- handleDvl() - RDI_Pathfinder msg: 1730861363.03 sec; seqNo:4; #beams:0 2024-11-06T02:49:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:49:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.02 , 0.43 2024-11-06T02:49:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861363.05 epoch sec; seqNo:11; 4061423.21 north; 594642.22 east; 36.69 lat; -121.94 lon; 3.56 depth; utmZone: 10 2024-11-06T02:49:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861361.03 2024-11-06T02:49:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:23Z- waiting for 1730861365.03, time is 1730861363.05 2024-11-06T02:49:23Z- AHRS_M2 msg: 1730861363.43 epoch sec; seqNo:10 2024-11-06T02:49:23Z- AHRS_M2 msg: 0.17 phi; -0.16 theta; 5.30 psi 2024-11-06T02:49:23Z- handleDepth()- Depth_Keller msg: 1730861363.44 epoch sec; seqNo:12; depth 3.60 2024-11-06T02:49:23Z- handleDVL() - DVL msg epochMillisec: 1730861363.46 2024-11-06T02:49:23Z- handleDvl() - RDI_Pathfinder msg: 1730861363.46 sec; seqNo:5; #beams:4 2024-11-06T02:49:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.84 , 78.08 , 76.45 , 100.49 2024-11-06T02:49:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.02 , -0.39 2024-11-06T02:49:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861363.48 epoch sec; seqNo:12; 4061423.58 north; 594641.00 east; 36.69 lat; -121.94 lon; 3.60 depth; utmZone: 10 2024-11-06T02:49:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861361.03 2024-11-06T02:49:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:23Z- waiting for 1730861365.03, time is 1730861363.48 2024-11-06T02:49:23Z- AHRS_M2 msg: 1730861363.83 epoch sec; seqNo:11 2024-11-06T02:49:23Z- AHRS_M2 msg: 0.17 phi; -0.15 theta; 5.30 psi 2024-11-06T02:49:23Z- handleDepth()- Depth_Keller msg: 1730861363.83 epoch sec; seqNo:13; depth 3.64 2024-11-06T02:49:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861363.85 epoch sec; seqNo:13; 4061423.58 north; 594641.00 east; 36.69 lat; -121.94 lon; 3.64 depth; utmZone: 10 2024-11-06T02:49:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861361.03 2024-11-06T02:49:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:23Z- waiting for 1730861365.03, time is 1730861363.85 2024-11-06T02:49:24Z- AHRS_M2 msg: 1730861364.24 epoch sec; seqNo:12 2024-11-06T02:49:24Z- AHRS_M2 msg: 0.18 phi; -0.15 theta; 5.30 psi 2024-11-06T02:49:24Z- handleDepth()- Depth_Keller msg: 1730861364.24 epoch sec; seqNo:14; depth 3.68 2024-11-06T02:49:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861364.25 epoch sec; seqNo:14; 4061423.96 north; 594640.99 east; 36.69 lat; -121.94 lon; 3.68 depth; utmZone: 10 2024-11-06T02:49:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861361.03 2024-11-06T02:49:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:24Z- waiting for 1730861365.03, time is 1730861364.25 2024-11-06T02:49:24Z- handleDepth()- Depth_Keller msg: 1730861364.67 epoch sec; seqNo:15; depth 3.73 2024-11-06T02:49:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861364.68 epoch sec; seqNo:15; 4061424.33 north; 594640.99 east; 36.69 lat; -121.94 lon; 3.73 depth; utmZone: 10 2024-11-06T02:49:24Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:49:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861361.03 2024-11-06T02:49:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:24Z- waiting for 1730861365.03, time is 1730861364.68 2024-11-06T02:49:25Z- AHRS_M2 msg: 1730861365.04 epoch sec; seqNo:13 2024-11-06T02:49:25Z- AHRS_M2 msg: 0.17 phi; -0.15 theta; 5.29 psi 2024-11-06T02:49:25Z- handleDepth()- Depth_Keller msg: 1730861365.05 epoch sec; seqNo:16; depth 3.79 2024-11-06T02:49:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861365.07 epoch sec; seqNo:16; 4061424.32 north; 594639.77 east; 36.69 lat; -121.94 lon; 3.79 depth; utmZone: 10 2024-11-06T02:49:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861361.03 2024-11-06T02:49:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:25Z- waiting for 1730861365.03, time is 1730861365.07 2024-11-06T02:49:25Z- LcmTrn::updateTrn() >>>> pose time 1730861365.07, meas time 1730861363.46 <<<< 2024-11-06T02:49:25Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861365.07 2024-11-06T02:49:25Z- TerrainNav::Measurement type 1 from time = 1730861363.46 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2024-11-06T02:49:25Z- TerrainNav::motionUpdate() - currEstimate = (4061424, 594639.77, 3.79) 2024-11-06T02:49:25Z- TerrainNav::attemptInitFilter is increasing Init Search Window by 0.000000 m 2024-11-06T02:49:25Z- TNAVPF: Successfully initialized particles 2024-11-06T02:49:25Z- TerrainNav:: vehicle motion has been initialized 2024-11-06T02:49:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:49:25Z- variances([0], [2], and psi_berg) = 1206.047691 1199.790071 0.000000 2024-11-06T02:49:25Z- publishEstimatesDecoder() - at 1730861365.07 sec 2024-11-06T02:49:25Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:49:25Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061476.53 594684.72 0.70 2024-11-06T02:49:25Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061424.12 594638.67 3.80 lat/lon: 36.69 -121.94 2024-11-06T02:49:25Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1206.05 1199.79 8.33 0.00 2024-11-06T02:49:25Z- AHRS_M2 msg: 1730861365.45 epoch sec; seqNo:14 2024-11-06T02:49:25Z- AHRS_M2 msg: 0.17 phi; -0.15 theta; 5.29 psi 2024-11-06T02:49:25Z- handleDepth()- Depth_Keller msg: 1730861365.45 epoch sec; seqNo:17; depth 3.85 2024-11-06T02:49:25Z- handleDVL() - DVL msg epochMillisec: 1730861365.47 2024-11-06T02:49:25Z- handleDvl() - RDI_Pathfinder msg: 1730861365.47 sec; seqNo:6; #beams:4 2024-11-06T02:49:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.70 , 76.86 , 77.26 , 97.92 2024-11-06T02:49:25Z- handleDvl() - RDI_Pathfinder msg: velocities 1.25 , -0.08 , 0.29 2024-11-06T02:49:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861365.51 epoch sec; seqNo:17; 4061424.70 north; 594639.77 east; 36.69 lat; -121.94 lon; 3.85 depth; utmZone: 10 2024-11-06T02:49:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861365.07 2024-11-06T02:49:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:25Z- waiting for 1730861369.07, time is 1730861365.51 2024-11-06T02:49:25Z- AHRS_M2 msg: 1730861365.87 epoch sec; seqNo:15 2024-11-06T02:49:25Z- AHRS_M2 msg: 0.16 phi; -0.15 theta; 5.28 psi 2024-11-06T02:49:25Z- handleDepth()- Depth_Keller msg: 1730861365.87 epoch sec; seqNo:18; depth 3.93 2024-11-06T02:49:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861365.90 epoch sec; seqNo:18; 4061424.70 north; 594639.77 east; 36.69 lat; -121.94 lon; 3.93 depth; utmZone: 10 2024-11-06T02:49:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861365.07 2024-11-06T02:49:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:26Z- waiting for 1730861369.07, time is 1730861365.90 2024-11-06T02:49:26Z- AHRS_M2 msg: 1730861366.28 epoch sec; seqNo:16 2024-11-06T02:49:26Z- AHRS_M2 msg: 0.15 phi; -0.16 theta; 5.26 psi 2024-11-06T02:49:26Z- handleDepth()- Depth_Keller msg: 1730861366.29 epoch sec; seqNo:19; depth 3.98 2024-11-06T02:49:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861366.30 epoch sec; seqNo:19; 4061425.06 north; 594638.54 east; 36.69 lat; -121.94 lon; 3.98 depth; utmZone: 10 2024-11-06T02:49:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861365.07 2024-11-06T02:49:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:26Z- waiting for 1730861369.07, time is 1730861366.30 2024-11-06T02:49:26Z- AHRS_M2 msg: 1730861366.79 epoch sec; seqNo:17 2024-11-06T02:49:26Z- AHRS_M2 msg: 0.14 phi; -0.18 theta; 5.25 psi 2024-11-06T02:49:26Z- handleDepth()- Depth_Keller msg: 1730861366.80 epoch sec; seqNo:20; depth 4.06 2024-11-06T02:49:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861366.81 epoch sec; seqNo:20; 4061425.06 north; 594638.54 east; 36.69 lat; -121.94 lon; 4.06 depth; utmZone: 10 2024-11-06T02:49:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861365.07 2024-11-06T02:49:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:26Z- waiting for 1730861369.07, time is 1730861366.81 2024-11-06T02:49:27Z- AHRS_M2 msg: 1730861367.08 epoch sec; seqNo:18 2024-11-06T02:49:27Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.25 psi 2024-11-06T02:49:27Z- handleDepth()- Depth_Keller msg: 1730861367.09 epoch sec; seqNo:21; depth 4.14 2024-11-06T02:49:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861367.10 epoch sec; seqNo:21; 4061425.44 north; 594638.54 east; 36.69 lat; -121.94 lon; 4.14 depth; utmZone: 10 2024-11-06T02:49:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861365.07 2024-11-06T02:49:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:27Z- waiting for 1730861369.07, time is 1730861367.10 2024-11-06T02:49:27Z- AHRS_M2 msg: 1730861367.48 epoch sec; seqNo:19 2024-11-06T02:49:27Z- AHRS_M2 msg: 0.11 phi; -0.22 theta; 5.25 psi 2024-11-06T02:49:27Z- handleDepth()- Depth_Keller msg: 1730861367.49 epoch sec; seqNo:22; depth 4.17 2024-11-06T02:49:27Z- handleDVL() - DVL msg epochMillisec: 1730861367.51 2024-11-06T02:49:27Z- handleDvl() - RDI_Pathfinder msg: 1730861367.51 sec; seqNo:7; #beams:4 2024-11-06T02:49:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.55 , 78.08 , 77.67 , 99.21 2024-11-06T02:49:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.55 , -0.13 , 0.41 2024-11-06T02:49:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861367.54 epoch sec; seqNo:22; 4061425.43 north; 594637.32 east; 36.69 lat; -121.94 lon; 4.17 depth; utmZone: 10 2024-11-06T02:49:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861365.07 2024-11-06T02:49:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:27Z- waiting for 1730861369.07, time is 1730861367.54 2024-11-06T02:49:27Z- AHRS_M2 msg: 1730861367.88 epoch sec; seqNo:20 2024-11-06T02:49:27Z- AHRS_M2 msg: 0.12 phi; -0.21 theta; 5.27 psi 2024-11-06T02:49:27Z- handleDepth()- Depth_Keller msg: 1730861367.90 epoch sec; seqNo:23; depth 4.21 2024-11-06T02:49:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861367.91 epoch sec; seqNo:23; 4061425.43 north; 594637.32 east; 36.69 lat; -121.94 lon; 4.21 depth; utmZone: 10 2024-11-06T02:49:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861365.07 2024-11-06T02:49:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:28Z- waiting for 1730861369.07, time is 1730861367.91 2024-11-06T02:49:28Z- AHRS_M2 msg: 1730861368.30 epoch sec; seqNo:21 2024-11-06T02:49:28Z- AHRS_M2 msg: 0.13 phi; -0.20 theta; 5.28 psi 2024-11-06T02:49:28Z- handleDepth()- Depth_Keller msg: 1730861368.30 epoch sec; seqNo:24; depth 4.25 2024-11-06T02:49:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861368.32 epoch sec; seqNo:24; 4061425.43 north; 594637.32 east; 36.69 lat; -121.94 lon; 4.25 depth; utmZone: 10 2024-11-06T02:49:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861365.07 2024-11-06T02:49:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:28Z- waiting for 1730861369.07, time is 1730861368.32 2024-11-06T02:49:28Z- AHRS_M2 msg: 1730861368.71 epoch sec; seqNo:22 2024-11-06T02:49:28Z- AHRS_M2 msg: 0.15 phi; -0.19 theta; 5.28 psi 2024-11-06T02:49:28Z- handleDepth()- Depth_Keller msg: 1730861368.71 epoch sec; seqNo:25; depth 4.27 2024-11-06T02:49:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861368.73 epoch sec; seqNo:25; 4061425.43 north; 594637.32 east; 36.69 lat; -121.94 lon; 4.27 depth; utmZone: 10 2024-11-06T02:49:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861365.07 2024-11-06T02:49:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:28Z- waiting for 1730861369.07, time is 1730861368.73 2024-11-06T02:49:29Z- handleDepth()- Depth_Keller msg: 1730861369.12 epoch sec; seqNo:26; depth 4.34 2024-11-06T02:49:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861369.13 epoch sec; seqNo:26; 4061425.43 north; 594637.32 east; 36.69 lat; -121.94 lon; 4.34 depth; utmZone: 10 2024-11-06T02:49:29Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:49:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861365.07 2024-11-06T02:49:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:29Z- waiting for 1730861369.07, time is 1730861369.13 2024-11-06T02:49:29Z- LcmTrn::updateTrn() >>>> pose time 1730861369.13, meas time 1730861367.51 <<<< 2024-11-06T02:49:29Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861369.13 2024-11-06T02:49:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861367.51 sec, ping # 0; waiting for INS data more recent than 1730861365.07... 2024-11-06T02:49:29Z- TerrainNav::motionUpdate() - currEstimate = (4061425, 594637.32, 4.34) 2024-11-06T02:49:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:49:29Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:49:29Z- TNavPF::Projecting Measurements 2024-11-06T02:49:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:49:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:49:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:49:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:49:29Z- TNavFilter::4 good beams. 2024-11-06T02:49:29Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:49:29Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:49:29Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:49:29Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:49:29Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:49:29Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:49:29Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:49:29Z- TerrainMapOctree: (vehN, vehE ) = (4061424.99, 594638.30). 2024-11-06T02:49:29Z- TerrainMapOctree: Min Distance = 207.60. 2024-11-06T02:49:29Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:49:29Z- TerrainMapOctree: Switching to tile 2. 2024-11-06T02:49:30Z- TerrainMapOctree::Octree tile load /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile2.bo took 0.902114 seconds. 2024-11-06T02:49:30Z- Forcing Subcloud Comparison 2024-11-06T02:49:30Z- Weighting particles with subcloud comparison 2024-11-06T02:49:30Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:49:30Z- meanExpectedMeasDiff: -16.536478 delta_rms_squared: 23.283636 alpha: 0.999142 2024-11-06T02:49:30Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:49:30Z- meanExpectedMeasDiff: -12.752323 delta_rms_squared: 20.784333 alpha: 0.999039 2024-11-06T02:49:30Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:49:30Z- meanExpectedMeasDiff: -11.666798 delta_rms_squared: 13.945116 alpha: 0.998568 2024-11-06T02:49:30Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:49:30Z- meanExpectedMeasDiff: -15.756243 delta_rms_squared: 18.447249 alpha: 0.998917 2024-11-06T02:49:30Z- Subcloud NIS: 0.140270 2024-11-06T02:49:30Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:49:30Z- TNavPF:: sumWeights = 0.000203 2024-11-06T02:49:30Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:49:30Z- TNavPF::First Term of Map Covariance Matrix is 0.01 2024-11-06T02:49:30Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T02:49:30Z- TerrainNav::Resampling particle filter... 2024-11-06T02:49:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861367.51 sec. 2024-11-06T02:49:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:49:30Z- variances([0], [2], and psi_berg) = 252.150611 746.360117 0.000000 2024-11-06T02:49:30Z- publishEstimatesDecoder() - at 1730861369.13 sec 2024-11-06T02:49:30Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:49:30Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061481.53 594641.82 9.05 2024-11-06T02:49:30Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061468.72 594603.21 9.13 lat/lon: 36.69 -121.94 2024-11-06T02:49:30Z- LcmTrn::publishEstimatesDecoder() - COVAR: 252.15 746.36 0.71 0.00 2024-11-06T02:49:30Z- AHRS_M2 msg: 1730861369.50 epoch sec; seqNo:23 2024-11-06T02:49:30Z- AHRS_M2 msg: 0.17 phi; -0.21 theta; 5.27 psi 2024-11-06T02:49:30Z- handleDepth()- Depth_Keller msg: 1730861369.51 epoch sec; seqNo:27; depth 4.39 2024-11-06T02:49:30Z- handleDVL() - DVL msg epochMillisec: 1730861369.54 2024-11-06T02:49:30Z- handleDvl() - RDI_Pathfinder msg: 1730861369.54 sec; seqNo:8; #beams:4 2024-11-06T02:49:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.55 , 79.31 , 76.45 , 96.77 2024-11-06T02:49:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , 0.04 , -0.02 2024-11-06T02:49:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861369.55 epoch sec; seqNo:27; 4061425.81 north; 594637.31 east; 36.69 lat; -121.94 lon; 4.39 depth; utmZone: 10 2024-11-06T02:49:30Z- AHRS_M2 msg: 1730861369.91 epoch sec; seqNo:24 2024-11-06T02:49:30Z- AHRS_M2 msg: 0.17 phi; -0.22 theta; 5.27 psi 2024-11-06T02:49:30Z- handleDepth()- Depth_Keller msg: 1730861369.91 epoch sec; seqNo:28; depth 4.44 2024-11-06T02:49:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861369.92 epoch sec; seqNo:28; 4061425.80 north; 594636.09 east; 36.69 lat; -121.94 lon; 4.44 depth; utmZone: 10 2024-11-06T02:49:30Z- AHRS_M2 msg: 1730861370.32 epoch sec; seqNo:25 2024-11-06T02:49:30Z- AHRS_M2 msg: 0.17 phi; -0.24 theta; 5.27 psi 2024-11-06T02:49:30Z- handleDepth()- Depth_Keller msg: 1730861370.33 epoch sec; seqNo:29; depth 4.50 2024-11-06T02:49:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861370.34 epoch sec; seqNo:29; 4061426.17 north; 594636.09 east; 36.69 lat; -121.94 lon; 4.50 depth; utmZone: 10 2024-11-06T02:49:30Z- AHRS_M2 msg: 1730861370.72 epoch sec; seqNo:26 2024-11-06T02:49:30Z- AHRS_M2 msg: 0.17 phi; -0.24 theta; 5.28 psi 2024-11-06T02:49:30Z- handleDepth()- Depth_Keller msg: 1730861370.73 epoch sec; seqNo:30; depth 4.57 2024-11-06T02:49:30Z- 12 msgs in 200 ms and 13 handle calls 2024-11-06T02:49:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861369.13 2024-11-06T02:49:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:30Z- waiting for 1730861373.13, time is 1730861370.72 2024-11-06T02:49:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861370.74 epoch sec; seqNo:30; 4061426.17 north; 594636.09 east; 36.69 lat; -121.94 lon; 4.57 depth; utmZone: 10 2024-11-06T02:49:30Z- 1 msgs in 200 ms and 2 handle calls 2024-11-06T02:49:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861369.13 2024-11-06T02:49:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:30Z- waiting for 1730861373.13, time is 1730861370.74 2024-11-06T02:49:31Z- AHRS_M2 msg: 1730861371.12 epoch sec; seqNo:27 2024-11-06T02:49:31Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.28 psi 2024-11-06T02:49:31Z- handleDepth()- Depth_Keller msg: 1730861371.13 epoch sec; seqNo:31; depth 4.63 2024-11-06T02:49:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861371.15 epoch sec; seqNo:31; 4061426.55 north; 594636.08 east; 36.69 lat; -121.94 lon; 4.63 depth; utmZone: 10 2024-11-06T02:49:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861369.13 2024-11-06T02:49:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:31Z- waiting for 1730861373.13, time is 1730861371.15 2024-11-06T02:49:31Z- AHRS_M2 msg: 1730861371.53 epoch sec; seqNo:28 2024-11-06T02:49:31Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.29 psi 2024-11-06T02:49:31Z- handleDepth()- Depth_Keller msg: 1730861371.54 epoch sec; seqNo:32; depth 4.66 2024-11-06T02:49:31Z- handleDVL() - DVL msg epochMillisec: 1730861371.56 2024-11-06T02:49:31Z- handleDvl() - RDI_Pathfinder msg: 1730861371.56 sec; seqNo:9; #beams:4 2024-11-06T02:49:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.84 , 78.08 , 75.63 , 104.14 2024-11-06T02:49:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , 0.05 , -0.29 2024-11-06T02:49:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861371.58 epoch sec; seqNo:32; 4061426.55 north; 594636.08 east; 36.69 lat; -121.94 lon; 4.66 depth; utmZone: 10 2024-11-06T02:49:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861369.13 2024-11-06T02:49:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:31Z- waiting for 1730861373.13, time is 1730861371.58 2024-11-06T02:49:31Z- AHRS_M2 msg: 1730861371.94 epoch sec; seqNo:29 2024-11-06T02:49:31Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.30 psi 2024-11-06T02:49:31Z- handleDepth()- Depth_Keller msg: 1730861371.94 epoch sec; seqNo:33; depth 4.71 2024-11-06T02:49:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861371.95 epoch sec; seqNo:33; 4061426.92 north; 594634.86 east; 36.69 lat; -121.94 lon; 4.71 depth; utmZone: 10 2024-11-06T02:49:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861369.13 2024-11-06T02:49:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:32Z- waiting for 1730861373.13, time is 1730861371.95 2024-11-06T02:49:32Z- AHRS_M2 msg: 1730861372.35 epoch sec; seqNo:30 2024-11-06T02:49:32Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.31 psi 2024-11-06T02:49:32Z- handleDepth()- Depth_Keller msg: 1730861372.36 epoch sec; seqNo:34; depth 4.77 2024-11-06T02:49:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861372.36 epoch sec; seqNo:34; 4061426.92 north; 594634.86 east; 36.69 lat; -121.94 lon; 4.77 depth; utmZone: 10 2024-11-06T02:49:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861369.13 2024-11-06T02:49:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:32Z- waiting for 1730861373.13, time is 1730861372.36 2024-11-06T02:49:32Z- AHRS_M2 msg: 1730861372.74 epoch sec; seqNo:31 2024-11-06T02:49:32Z- AHRS_M2 msg: 0.16 phi; -0.22 theta; 5.31 psi 2024-11-06T02:49:32Z- handleDepth()- Depth_Keller msg: 1730861372.74 epoch sec; seqNo:35; depth 4.82 2024-11-06T02:49:32Z- handleDVL() - DVL msg epochMillisec: 1730861372.75 2024-11-06T02:49:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:49:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:49:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:49:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:49:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:49:32Z- handleDvl() - RDI_Pathfinder msg: 1730861372.75 sec; seqNo:10; #beams:0 2024-11-06T02:49:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:49:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , 0.05 , -0.29 2024-11-06T02:49:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861372.77 epoch sec; seqNo:35; 4061427.30 north; 594634.85 east; 36.69 lat; -121.94 lon; 4.82 depth; utmZone: 10 2024-11-06T02:49:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861369.13 2024-11-06T02:49:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:32Z- waiting for 1730861373.13, time is 1730861372.77 2024-11-06T02:49:33Z- AHRS_M2 msg: 1730861373.15 epoch sec; seqNo:32 2024-11-06T02:49:33Z- AHRS_M2 msg: 0.17 phi; -0.22 theta; 5.31 psi 2024-11-06T02:49:33Z- handleDepth()- Depth_Keller msg: 1730861373.15 epoch sec; seqNo:36; depth 4.91 2024-11-06T02:49:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861373.16 epoch sec; seqNo:36; 4061427.30 north; 594634.85 east; 36.69 lat; -121.94 lon; 4.91 depth; utmZone: 10 2024-11-06T02:49:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861369.13 2024-11-06T02:49:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:33Z- waiting for 1730861373.13, time is 1730861373.16 2024-11-06T02:49:33Z- LcmTrn::updateTrn() >>>> pose time 1730861373.16, meas time 1730861372.75 <<<< 2024-11-06T02:49:33Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861373.16 2024-11-06T02:49:33Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:49:33Z- TerrainNav::motionUpdate() - currEstimate = (4061427, 594634.85, 4.91) 2024-11-06T02:49:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:49:33Z- TerrainNav:: SubcloudNIS is 0.14, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:49:33Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:49:33Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:49:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:49:33Z- variances([0], [2], and psi_berg) = 252.161222 746.346407 0.000000 2024-11-06T02:49:33Z- publishEstimatesDecoder() - at 1730861373.16 sec 2024-11-06T02:49:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:49:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061483.37 594639.39 9.30 2024-11-06T02:49:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061470.59 594600.74 9.70 lat/lon: 36.69 -121.94 2024-11-06T02:49:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 252.16 746.35 0.94 0.00 2024-11-06T02:49:33Z- AHRS_M2 msg: 1730861373.55 epoch sec; seqNo:33 2024-11-06T02:49:33Z- AHRS_M2 msg: 0.17 phi; -0.22 theta; 5.30 psi 2024-11-06T02:49:33Z- handleDepth()- Depth_Keller msg: 1730861373.56 epoch sec; seqNo:37; depth 4.98 2024-11-06T02:49:33Z- handleDVL() - DVL msg epochMillisec: 1730861373.58 2024-11-06T02:49:33Z- handleDvl() - RDI_Pathfinder msg: 1730861373.58 sec; seqNo:11; #beams:4 2024-11-06T02:49:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.42 , 77.26 , 74.40 , 96.64 2024-11-06T02:49:33Z- handleDvl() - RDI_Pathfinder msg: velocities 1.13 , -0.01 , -0.12 2024-11-06T02:49:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861373.59 epoch sec; seqNo:37; 4061427.68 north; 594634.85 east; 36.69 lat; -121.94 lon; 4.98 depth; utmZone: 10 2024-11-06T02:49:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861373.16 2024-11-06T02:49:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:33Z- waiting for 1730861377.16, time is 1730861373.59 2024-11-06T02:49:33Z- AHRS_M2 msg: 1730861373.95 epoch sec; seqNo:34 2024-11-06T02:49:33Z- AHRS_M2 msg: 0.16 phi; -0.23 theta; 5.30 psi 2024-11-06T02:49:33Z- handleDepth()- Depth_Keller msg: 1730861373.96 epoch sec; seqNo:38; depth 5.07 2024-11-06T02:49:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861373.98 epoch sec; seqNo:38; 4061427.66 north; 594633.63 east; 36.69 lat; -121.94 lon; 5.07 depth; utmZone: 10 2024-11-06T02:49:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861373.16 2024-11-06T02:49:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:34Z- waiting for 1730861377.16, time is 1730861373.98 2024-11-06T02:49:34Z- AHRS_M2 msg: 1730861374.36 epoch sec; seqNo:35 2024-11-06T02:49:34Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.29 psi 2024-11-06T02:49:34Z- handleDepth()- Depth_Keller msg: 1730861374.37 epoch sec; seqNo:39; depth 5.14 2024-11-06T02:49:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861374.38 epoch sec; seqNo:39; 4061428.04 north; 594633.63 east; 36.69 lat; -121.94 lon; 5.14 depth; utmZone: 10 2024-11-06T02:49:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861373.16 2024-11-06T02:49:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:34Z- waiting for 1730861377.16, time is 1730861374.38 2024-11-06T02:49:34Z- AHRS_M2 msg: 1730861374.76 epoch sec; seqNo:36 2024-11-06T02:49:34Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.29 psi 2024-11-06T02:49:34Z- handleDepth()- Depth_Keller msg: 1730861374.76 epoch sec; seqNo:40; depth 5.22 2024-11-06T02:49:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861374.77 epoch sec; seqNo:40; 4061428.42 north; 594633.62 east; 36.69 lat; -121.94 lon; 5.22 depth; utmZone: 10 2024-11-06T02:49:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861373.16 2024-11-06T02:49:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:34Z- waiting for 1730861377.16, time is 1730861374.77 2024-11-06T02:49:35Z- AHRS_M2 msg: 1730861375.18 epoch sec; seqNo:37 2024-11-06T02:49:35Z- AHRS_M2 msg: 0.15 phi; -0.26 theta; 5.29 psi 2024-11-06T02:49:35Z- handleDepth()- Depth_Keller msg: 1730861375.18 epoch sec; seqNo:41; depth 5.32 2024-11-06T02:49:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861375.19 epoch sec; seqNo:41; 4061428.41 north; 594632.40 east; 36.69 lat; -121.94 lon; 5.32 depth; utmZone: 10 2024-11-06T02:49:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861373.16 2024-11-06T02:49:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:35Z- waiting for 1730861377.16, time is 1730861375.19 2024-11-06T02:49:35Z- AHRS_M2 msg: 1730861375.57 epoch sec; seqNo:38 2024-11-06T02:49:35Z- AHRS_M2 msg: 0.15 phi; -0.26 theta; 5.30 psi 2024-11-06T02:49:35Z- handleDepth()- Depth_Keller msg: 1730861375.57 epoch sec; seqNo:42; depth 5.39 2024-11-06T02:49:35Z- handleDVL() - DVL msg epochMillisec: 1730861375.59 2024-11-06T02:49:35Z- handleDvl() - RDI_Pathfinder msg: 1730861375.59 sec; seqNo:12; #beams:4 2024-11-06T02:49:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.47 , 77.67 , 73.99 , 101.78 2024-11-06T02:49:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.04 , -0.04 2024-11-06T02:49:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861375.62 epoch sec; seqNo:42; 4061428.79 north; 594632.40 east; 36.69 lat; -121.94 lon; 5.39 depth; utmZone: 10 2024-11-06T02:49:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861373.16 2024-11-06T02:49:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:35Z- waiting for 1730861377.16, time is 1730861375.62 2024-11-06T02:49:35Z- AHRS_M2 msg: 1730861375.97 epoch sec; seqNo:39 2024-11-06T02:49:35Z- AHRS_M2 msg: 0.15 phi; -0.25 theta; 5.31 psi 2024-11-06T02:49:35Z- handleDepth()- Depth_Keller msg: 1730861375.98 epoch sec; seqNo:43; depth 5.46 2024-11-06T02:49:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861375.99 epoch sec; seqNo:43; 4061429.16 north; 594632.39 east; 36.69 lat; -121.94 lon; 5.46 depth; utmZone: 10 2024-11-06T02:49:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861373.16 2024-11-06T02:49:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:36Z- waiting for 1730861377.16, time is 1730861375.99 2024-11-06T02:49:36Z- AHRS_M2 msg: 1730861376.38 epoch sec; seqNo:40 2024-11-06T02:49:36Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.31 psi 2024-11-06T02:49:36Z- handleDepth()- Depth_Keller msg: 1730861376.38 epoch sec; seqNo:44; depth 5.53 2024-11-06T02:49:36Z- handleDVL() - DVL msg epochMillisec: 1730861376.39 2024-11-06T02:49:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:49:36Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:49:36Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:49:36Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:49:36Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:49:36Z- handleDvl() - RDI_Pathfinder msg: 1730861376.39 sec; seqNo:13; #beams:0 2024-11-06T02:49:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:49:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.04 , -0.04 2024-11-06T02:49:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861376.40 epoch sec; seqNo:44; 4061429.16 north; 594632.39 east; 36.69 lat; -121.94 lon; 5.53 depth; utmZone: 10 2024-11-06T02:49:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861373.16 2024-11-06T02:49:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:36Z- waiting for 1730861377.16, time is 1730861376.40 2024-11-06T02:49:36Z- AHRS_M2 msg: 1730861376.78 epoch sec; seqNo:41 2024-11-06T02:49:36Z- AHRS_M2 msg: 0.15 phi; -0.23 theta; 5.31 psi 2024-11-06T02:49:36Z- handleDepth()- Depth_Keller msg: 1730861376.79 epoch sec; seqNo:45; depth 5.62 2024-11-06T02:49:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861376.81 epoch sec; seqNo:45; 4061429.53 north; 594631.17 east; 36.69 lat; -121.94 lon; 5.62 depth; utmZone: 10 2024-11-06T02:49:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861373.16 2024-11-06T02:49:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:36Z- waiting for 1730861377.16, time is 1730861376.81 2024-11-06T02:49:37Z- AHRS_M2 msg: 1730861377.19 epoch sec; seqNo:42 2024-11-06T02:49:37Z- AHRS_M2 msg: 0.14 phi; -0.21 theta; 5.30 psi 2024-11-06T02:49:37Z- handleDepth()- Depth_Keller msg: 1730861377.20 epoch sec; seqNo:46; depth 5.72 2024-11-06T02:49:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861377.24 epoch sec; seqNo:46; 4061429.91 north; 594631.16 east; 36.69 lat; -121.94 lon; 5.72 depth; utmZone: 10 2024-11-06T02:49:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861373.16 2024-11-06T02:49:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:37Z- waiting for 1730861377.16, time is 1730861377.24 2024-11-06T02:49:37Z- LcmTrn::updateTrn() >>>> pose time 1730861377.24, meas time 1730861376.39 <<<< 2024-11-06T02:49:37Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861377.24 2024-11-06T02:49:37Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:49:37Z- TerrainNav::motionUpdate() - currEstimate = (4061430, 594631.16, 5.72) 2024-11-06T02:49:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:49:37Z- TerrainNav:: SubcloudNIS is 0.14, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:49:37Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:49:37Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:49:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:49:37Z- variances([0], [2], and psi_berg) = 252.182161 746.405871 0.000000 2024-11-06T02:49:37Z- publishEstimatesDecoder() - at 1730861377.24 sec 2024-11-06T02:49:37Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:49:37Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061486.04 594635.74 9.73 2024-11-06T02:49:37Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061473.20 594597.05 10.51 lat/lon: 36.69 -121.94 2024-11-06T02:49:37Z- LcmTrn::publishEstimatesDecoder() - COVAR: 252.18 746.41 1.19 0.00 2024-11-06T02:49:37Z- AHRS_M2 msg: 1730861377.59 epoch sec; seqNo:43 2024-11-06T02:49:37Z- AHRS_M2 msg: 0.13 phi; -0.20 theta; 5.30 psi 2024-11-06T02:49:37Z- handleDepth()- Depth_Keller msg: 1730861377.59 epoch sec; seqNo:47; depth 5.83 2024-11-06T02:49:37Z- handleDVL() - DVL msg epochMillisec: 1730861377.61 2024-11-06T02:49:37Z- handleDvl() - RDI_Pathfinder msg: 1730861377.61 sec; seqNo:14; #beams:4 2024-11-06T02:49:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.40 , 76.45 , 73.18 , 101.71 2024-11-06T02:49:37Z- handleDvl() - RDI_Pathfinder msg: velocities 1.33 , -0.05 , 0.04 2024-11-06T02:49:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861377.63 epoch sec; seqNo:47; 4061429.91 north; 594631.16 east; 36.69 lat; -121.94 lon; 5.83 depth; utmZone: 10 2024-11-06T02:49:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861377.24 2024-11-06T02:49:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:37Z- waiting for 1730861381.24, time is 1730861377.63 2024-11-06T02:49:37Z- AHRS_M2 msg: 1730861377.99 epoch sec; seqNo:44 2024-11-06T02:49:37Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.30 psi 2024-11-06T02:49:37Z- handleDepth()- Depth_Keller msg: 1730861378.00 epoch sec; seqNo:48; depth 5.94 2024-11-06T02:49:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861378.02 epoch sec; seqNo:48; 4061430.27 north; 594629.94 east; 36.69 lat; -121.94 lon; 5.94 depth; utmZone: 10 2024-11-06T02:49:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861377.24 2024-11-06T02:49:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:38Z- waiting for 1730861381.24, time is 1730861378.02 2024-11-06T02:49:38Z- AHRS_M2 msg: 1730861378.39 epoch sec; seqNo:45 2024-11-06T02:49:38Z- AHRS_M2 msg: 0.11 phi; -0.16 theta; 5.29 psi 2024-11-06T02:49:38Z- handleDepth()- Depth_Keller msg: 1730861378.40 epoch sec; seqNo:49; depth 6.05 2024-11-06T02:49:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861378.41 epoch sec; seqNo:49; 4061430.65 north; 594629.94 east; 36.69 lat; -121.94 lon; 6.05 depth; utmZone: 10 2024-11-06T02:49:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861377.24 2024-11-06T02:49:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:38Z- waiting for 1730861381.24, time is 1730861378.41 2024-11-06T02:49:38Z- AHRS_M2 msg: 1730861378.80 epoch sec; seqNo:46 2024-11-06T02:49:38Z- AHRS_M2 msg: 0.12 phi; -0.16 theta; 5.29 psi 2024-11-06T02:49:38Z- handleDepth()- Depth_Keller msg: 1730861378.81 epoch sec; seqNo:50; depth 6.14 2024-11-06T02:49:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861378.82 epoch sec; seqNo:50; 4061430.65 north; 594629.94 east; 36.69 lat; -121.94 lon; 6.14 depth; utmZone: 10 2024-11-06T02:49:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861377.24 2024-11-06T02:49:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:38Z- waiting for 1730861381.24, time is 1730861378.82 2024-11-06T02:49:39Z- AHRS_M2 msg: 1730861379.21 epoch sec; seqNo:47 2024-11-06T02:49:39Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.28 psi 2024-11-06T02:49:39Z- handleDepth()- Depth_Keller msg: 1730861379.21 epoch sec; seqNo:51; depth 6.24 2024-11-06T02:49:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861379.28 epoch sec; seqNo:51; 4061431.02 north; 594628.71 east; 36.69 lat; -121.94 lon; 6.24 depth; utmZone: 10 2024-11-06T02:49:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861377.24 2024-11-06T02:49:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:39Z- waiting for 1730861381.24, time is 1730861379.28 2024-11-06T02:49:39Z- AHRS_M2 msg: 1730861379.61 epoch sec; seqNo:48 2024-11-06T02:49:39Z- AHRS_M2 msg: 0.13 phi; -0.16 theta; 5.29 psi 2024-11-06T02:49:39Z- handleDepth()- Depth_Keller msg: 1730861379.62 epoch sec; seqNo:52; depth 6.34 2024-11-06T02:49:39Z- handleDVL() - DVL msg epochMillisec: 1730861379.64 2024-11-06T02:49:39Z- handleDvl() - RDI_Pathfinder msg: 1730861379.64 sec; seqNo:15; #beams:4 2024-11-06T02:49:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.75 , 76.45 , 75.63 , 95.62 2024-11-06T02:49:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.19 , 0.52 2024-11-06T02:49:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861379.66 epoch sec; seqNo:52; 4061431.02 north; 594628.71 east; 36.69 lat; -121.94 lon; 6.34 depth; utmZone: 10 2024-11-06T02:49:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861377.24 2024-11-06T02:49:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:39Z- waiting for 1730861381.24, time is 1730861379.66 2024-11-06T02:49:40Z- AHRS_M2 msg: 1730861380.01 epoch sec; seqNo:49 2024-11-06T02:49:40Z- AHRS_M2 msg: 0.13 phi; -0.16 theta; 5.29 psi 2024-11-06T02:49:40Z- handleDepth()- Depth_Keller msg: 1730861380.02 epoch sec; seqNo:53; depth 6.40 2024-11-06T02:49:40Z- handleDVL() - DVL msg epochMillisec: 1730861380.02 2024-11-06T02:49:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:49:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:49:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:49:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:49:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:49:40Z- handleDvl() - RDI_Pathfinder msg: 1730861380.02 sec; seqNo:16; #beams:0 2024-11-06T02:49:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:49:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.19 , 0.52 2024-11-06T02:49:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861380.04 epoch sec; seqNo:53; 4061431.02 north; 594628.71 east; 36.69 lat; -121.94 lon; 6.40 depth; utmZone: 10 2024-11-06T02:49:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861377.24 2024-11-06T02:49:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:40Z- waiting for 1730861381.24, time is 1730861380.04 2024-11-06T02:49:40Z- AHRS_M2 msg: 1730861380.41 epoch sec; seqNo:50 2024-11-06T02:49:40Z- AHRS_M2 msg: 0.15 phi; -0.15 theta; 5.30 psi 2024-11-06T02:49:40Z- handleDepth()- Depth_Keller msg: 1730861380.43 epoch sec; seqNo:54; depth 6.46 2024-11-06T02:49:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861380.44 epoch sec; seqNo:54; 4061431.40 north; 594628.71 east; 36.69 lat; -121.94 lon; 6.46 depth; utmZone: 10 2024-11-06T02:49:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861377.24 2024-11-06T02:49:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:40Z- waiting for 1730861381.24, time is 1730861380.44 2024-11-06T02:49:40Z- AHRS_M2 msg: 1730861380.82 epoch sec; seqNo:51 2024-11-06T02:49:40Z- AHRS_M2 msg: 0.16 phi; -0.14 theta; 5.31 psi 2024-11-06T02:49:40Z- handleDepth()- Depth_Keller msg: 1730861380.83 epoch sec; seqNo:55; depth 6.51 2024-11-06T02:49:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861380.84 epoch sec; seqNo:55; 4061431.38 north; 594627.49 east; 36.69 lat; -121.94 lon; 6.51 depth; utmZone: 10 2024-11-06T02:49:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861377.24 2024-11-06T02:49:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:40Z- waiting for 1730861381.24, time is 1730861380.84 2024-11-06T02:49:41Z- AHRS_M2 msg: 1730861381.25 epoch sec; seqNo:52 2024-11-06T02:49:41Z- AHRS_M2 msg: 0.18 phi; -0.12 theta; 5.32 psi 2024-11-06T02:49:41Z- handleDepth()- Depth_Keller msg: 1730861381.27 epoch sec; seqNo:56; depth 6.56 2024-11-06T02:49:41Z- handleDVL() - DVL msg epochMillisec: 1730861381.28 2024-11-06T02:49:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:49:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:49:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:49:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:49:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:49:41Z- handleDvl() - RDI_Pathfinder msg: 1730861381.28 sec; seqNo:17; #beams:0 2024-11-06T02:49:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:49:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.19 , 0.52 2024-11-06T02:49:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861381.30 epoch sec; seqNo:56; 4061431.38 north; 594627.49 east; 36.69 lat; -121.94 lon; 6.56 depth; utmZone: 10 2024-11-06T02:49:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861377.24 2024-11-06T02:49:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:41Z- waiting for 1730861381.24, time is 1730861381.30 2024-11-06T02:49:41Z- LcmTrn::updateTrn() >>>> pose time 1730861381.30, meas time 1730861381.28 <<<< 2024-11-06T02:49:41Z- LcmTrn::updateTrn() equating pose 1730861381.28 and meas times 1730861381.28 2024-11-06T02:49:41Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861381.28 2024-11-06T02:49:41Z- TerrainNav::motionUpdate() - currEstimate = (4061431, 594627.49, 6.56) 2024-11-06T02:49:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:49:41Z- TerrainNav:: SubcloudNIS is 0.14, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:49:41Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:49:41Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:49:41Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:49:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:49:41Z- variances([0], [2], and psi_berg) = 252.192298 746.425271 0.000000 2024-11-06T02:49:41Z- publishEstimatesDecoder() - at 1730861381.28 sec 2024-11-06T02:49:41Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:49:41Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061487.55 594632.10 10.84 2024-11-06T02:49:41Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061474.67 594593.38 11.35 lat/lon: 36.69 -121.94 2024-11-06T02:49:41Z- LcmTrn::publishEstimatesDecoder() - COVAR: 252.19 746.43 1.42 0.00 2024-11-06T02:49:41Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T02:49:41Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T02:49:41Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T02:49:41Z- AHRS_M2 msg: 1730861381.62 epoch sec; seqNo:53 2024-11-06T02:49:41Z- AHRS_M2 msg: 0.18 phi; -0.12 theta; 5.32 psi 2024-11-06T02:49:41Z- handleDepth()- Depth_Keller msg: 1730861381.65 epoch sec; seqNo:57; depth 6.60 2024-11-06T02:49:41Z- handleDVL() - DVL msg epochMillisec: 1730861381.68 2024-11-06T02:49:41Z- handleDvl() - RDI_Pathfinder msg: 1730861381.68 sec; seqNo:18; #beams:4 2024-11-06T02:49:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.17 , 76.04 , 76.04 , 95.82 2024-11-06T02:49:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.62 , 0.03 , -0.17 2024-11-06T02:49:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861381.69 epoch sec; seqNo:57; 4061431.76 north; 594627.48 east; 36.69 lat; -121.94 lon; 6.60 depth; utmZone: 10 2024-11-06T02:49:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861381.28 2024-11-06T02:49:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:41Z- waiting for 1730861385.28, time is 1730861381.69 2024-11-06T02:49:42Z- AHRS_M2 msg: 1730861382.03 epoch sec; seqNo:54 2024-11-06T02:49:42Z- AHRS_M2 msg: 0.18 phi; -0.07 theta; 5.34 psi 2024-11-06T02:49:42Z- handleDepth()- Depth_Keller msg: 1730861382.04 epoch sec; seqNo:58; depth 6.65 2024-11-06T02:49:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861382.05 epoch sec; seqNo:58; 4061431.76 north; 594627.48 east; 36.69 lat; -121.94 lon; 6.65 depth; utmZone: 10 2024-11-06T02:49:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861381.28 2024-11-06T02:49:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:42Z- waiting for 1730861385.28, time is 1730861382.05 2024-11-06T02:49:42Z- AHRS_M2 msg: 1730861382.43 epoch sec; seqNo:55 2024-11-06T02:49:42Z- AHRS_M2 msg: 0.17 phi; -0.05 theta; 5.34 psi 2024-11-06T02:49:42Z- handleDepth()- Depth_Keller msg: 1730861382.44 epoch sec; seqNo:59; depth 6.68 2024-11-06T02:49:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861382.46 epoch sec; seqNo:59; 4061431.76 north; 594627.48 east; 36.69 lat; -121.94 lon; 6.68 depth; utmZone: 10 2024-11-06T02:49:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861381.28 2024-11-06T02:49:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:42Z- waiting for 1730861385.28, time is 1730861382.46 2024-11-06T02:49:42Z- AHRS_M2 msg: 1730861382.84 epoch sec; seqNo:56 2024-11-06T02:49:42Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 5.33 psi 2024-11-06T02:49:42Z- handleDepth()- Depth_Keller msg: 1730861382.85 epoch sec; seqNo:60; depth 6.73 2024-11-06T02:49:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861382.87 epoch sec; seqNo:60; 4061432.13 north; 594626.26 east; 36.69 lat; -121.94 lon; 6.73 depth; utmZone: 10 2024-11-06T02:49:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861381.28 2024-11-06T02:49:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:42Z- waiting for 1730861385.28, time is 1730861382.87 2024-11-06T02:49:43Z- handleDepth()- Depth_Keller msg: 1730861383.31 epoch sec; seqNo:61; depth 6.75 2024-11-06T02:49:43Z- handleDVL() - DVL msg epochMillisec: 1730861383.33 2024-11-06T02:49:43Z- handleDvl() - RDI_Pathfinder msg: 1730861383.33 sec; seqNo:19; #beams:4 2024-11-06T02:49:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.10 , 73.58 , 78.08 , 87.23 2024-11-06T02:49:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.05 , -0.03 2024-11-06T02:49:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861383.34 epoch sec; seqNo:61; 4061432.51 north; 594626.25 east; 36.69 lat; -121.94 lon; 6.75 depth; utmZone: 10 2024-11-06T02:49:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861381.28 2024-11-06T02:49:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:43Z- waiting for 1730861385.28, time is 1730861383.34 2024-11-06T02:49:43Z- AHRS_M2 msg: 1730861383.73 epoch sec; seqNo:57 2024-11-06T02:49:43Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 5.32 psi 2024-11-06T02:49:43Z- handleDepth()- Depth_Keller msg: 1730861383.73 epoch sec; seqNo:62; depth 6.79 2024-11-06T02:49:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861383.76 epoch sec; seqNo:62; 4061432.51 north; 594626.25 east; 36.69 lat; -121.94 lon; 6.79 depth; utmZone: 10 2024-11-06T02:49:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861381.28 2024-11-06T02:49:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:43Z- waiting for 1730861385.28, time is 1730861383.76 2024-11-06T02:49:44Z- AHRS_M2 msg: 1730861384.13 epoch sec; seqNo:58 2024-11-06T02:49:44Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 5.32 psi 2024-11-06T02:49:44Z- handleDepth()- Depth_Keller msg: 1730861384.13 epoch sec; seqNo:63; depth 6.81 2024-11-06T02:49:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861384.14 epoch sec; seqNo:63; 4061432.89 north; 594626.25 east; 36.69 lat; -121.94 lon; 6.81 depth; utmZone: 10 2024-11-06T02:49:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861381.28 2024-11-06T02:49:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:44Z- waiting for 1730861385.28, time is 1730861384.14 2024-11-06T02:49:44Z- AHRS_M2 msg: 1730861384.54 epoch sec; seqNo:59 2024-11-06T02:49:44Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 5.32 psi 2024-11-06T02:49:44Z- handleDepth()- Depth_Keller msg: 1730861384.54 epoch sec; seqNo:64; depth 6.82 2024-11-06T02:49:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861384.55 epoch sec; seqNo:64; 4061432.87 north; 594625.03 east; 36.69 lat; -121.94 lon; 6.82 depth; utmZone: 10 2024-11-06T02:49:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861381.28 2024-11-06T02:49:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:44Z- waiting for 1730861385.28, time is 1730861384.55 2024-11-06T02:49:44Z- AHRS_M2 msg: 1730861384.94 epoch sec; seqNo:60 2024-11-06T02:49:44Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 5.33 psi 2024-11-06T02:49:44Z- handleDepth()- Depth_Keller msg: 1730861384.95 epoch sec; seqNo:65; depth 6.82 2024-11-06T02:49:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861384.96 epoch sec; seqNo:65; 4061433.25 north; 594625.02 east; 36.69 lat; -121.94 lon; 6.82 depth; utmZone: 10 2024-11-06T02:49:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861381.28 2024-11-06T02:49:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:45Z- waiting for 1730861385.28, time is 1730861384.96 2024-11-06T02:49:45Z- handleDepth()- Depth_Keller msg: 1730861385.36 epoch sec; seqNo:66; depth 6.82 2024-11-06T02:49:45Z- handleDVL() - DVL msg epochMillisec: 1730861385.39 2024-11-06T02:49:45Z- handleDvl() - RDI_Pathfinder msg: 1730861385.39 sec; seqNo:20; #beams:4 2024-11-06T02:49:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.09 , 73.18 , 79.32 , 83.79 2024-11-06T02:49:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.01 , 0.02 2024-11-06T02:49:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861385.40 epoch sec; seqNo:66; 4061433.63 north; 594625.02 east; 36.69 lat; -121.94 lon; 6.82 depth; utmZone: 10 2024-11-06T02:49:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861381.28 2024-11-06T02:49:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:45Z- waiting for 1730861385.28, time is 1730861385.40 2024-11-06T02:49:45Z- LcmTrn::updateTrn() >>>> pose time 1730861385.40, meas time 1730861385.39 <<<< 2024-11-06T02:49:45Z- LcmTrn::updateTrn() equating pose 1730861385.39 and meas times 1730861385.39 2024-11-06T02:49:45Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861385.39 2024-11-06T02:49:45Z- TerrainNav::motionUpdate() - currEstimate = (4061434, 594625.02, 6.82) 2024-11-06T02:49:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:49:45Z- TerrainNav:: SubcloudNIS is 0.14, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:49:45Z- TNavPF::Projecting Measurements 2024-11-06T02:49:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:49:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:49:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:49:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:49:45Z- TNavFilter::4 good beams. 2024-11-06T02:49:45Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:49:45Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:49:45Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:49:45Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:49:45Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:49:45Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:49:45Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:49:45Z- TerrainMapOctree: (vehN, vehE ) = (4061433.63, 594625.02). 2024-11-06T02:49:45Z- TerrainMapOctree: Min Distance = 222.13. 2024-11-06T02:49:45Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:49:45Z- Forcing Subcloud Comparison 2024-11-06T02:49:45Z- Weighting particles with subcloud comparison 2024-11-06T02:49:45Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:49:45Z- meanExpectedMeasDiff: -3.323545 delta_rms_squared: 7.356807 alpha: 0.997289 2024-11-06T02:49:45Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:49:45Z- meanExpectedMeasDiff: -2.819891 delta_rms_squared: 5.429840 alpha: 0.996330 2024-11-06T02:49:45Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:49:45Z- meanExpectedMeasDiff: -8.115746 delta_rms_squared: 7.253699 alpha: 0.997250 2024-11-06T02:49:45Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:49:45Z- meanExpectedMeasDiff: -3.710450 delta_rms_squared: 3.042713 alpha: 0.993470 2024-11-06T02:49:45Z- Subcloud NIS: 0.076668 2024-11-06T02:49:45Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:49:45Z- TNavPF:: sumWeights = 0.049497 2024-11-06T02:49:45Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:49:45Z- TNavPF::First Term of Map Covariance Matrix is 0.53 2024-11-06T02:49:45Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T02:49:45Z- TerrainNav::Resampling particle filter... 2024-11-06T02:49:45Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861385.39 sec, ping # 0. 2024-11-06T02:49:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:49:45Z- variances([0], [2], and psi_berg) = 103.522011 716.723567 0.000000 2024-11-06T02:49:45Z- publishEstimatesDecoder() - at 1730861385.39 sec 2024-11-06T02:49:45Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:49:45Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061489.71 594629.69 12.27 2024-11-06T02:49:45Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061483.08 594605.82 13.08 lat/lon: 36.69 -121.94 2024-11-06T02:49:45Z- LcmTrn::publishEstimatesDecoder() - COVAR: 103.52 716.72 1.02 0.00 2024-11-06T02:49:45Z- AHRS_M2 msg: 1730861385.81 epoch sec; seqNo:61 2024-11-06T02:49:45Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 5.34 psi 2024-11-06T02:49:45Z- handleDepth()- Depth_Keller msg: 1730861385.81 epoch sec; seqNo:67; depth 6.81 2024-11-06T02:49:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861385.83 epoch sec; seqNo:67; 4061433.63 north; 594625.02 east; 36.69 lat; -121.94 lon; 6.81 depth; utmZone: 10 2024-11-06T02:49:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861385.39 2024-11-06T02:49:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:45Z- waiting for 1730861389.39, time is 1730861385.83 2024-11-06T02:49:46Z- AHRS_M2 msg: 1730861386.19 epoch sec; seqNo:62 2024-11-06T02:49:46Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 5.34 psi 2024-11-06T02:49:46Z- handleDepth()- Depth_Keller msg: 1730861386.20 epoch sec; seqNo:68; depth 6.79 2024-11-06T02:49:46Z- handleDVL() - DVL msg epochMillisec: 1730861386.20 2024-11-06T02:49:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:49:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:49:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:49:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:49:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:49:46Z- handleDvl() - RDI_Pathfinder msg: 1730861386.20 sec; seqNo:21; #beams:0 2024-11-06T02:49:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:49:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.01 , 0.02 2024-11-06T02:49:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861386.21 epoch sec; seqNo:68; 4061433.62 north; 594623.80 east; 36.69 lat; -121.94 lon; 6.79 depth; utmZone: 10 2024-11-06T02:49:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861385.39 2024-11-06T02:49:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:46Z- waiting for 1730861389.39, time is 1730861386.21 2024-11-06T02:49:46Z- AHRS_M2 msg: 1730861386.60 epoch sec; seqNo:63 2024-11-06T02:49:46Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 5.34 psi 2024-11-06T02:49:46Z- handleDepth()- Depth_Keller msg: 1730861386.61 epoch sec; seqNo:69; depth 6.77 2024-11-06T02:49:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861386.62 epoch sec; seqNo:69; 4061434.00 north; 594623.80 east; 36.69 lat; -121.94 lon; 6.77 depth; utmZone: 10 2024-11-06T02:49:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861385.39 2024-11-06T02:49:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:46Z- waiting for 1730861389.39, time is 1730861386.62 2024-11-06T02:49:47Z- AHRS_M2 msg: 1730861387.01 epoch sec; seqNo:64 2024-11-06T02:49:47Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 5.34 psi 2024-11-06T02:49:47Z- handleDepth()- Depth_Keller msg: 1730861387.01 epoch sec; seqNo:70; depth 6.76 2024-11-06T02:49:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861387.02 epoch sec; seqNo:70; 4061434.00 north; 594623.80 east; 36.69 lat; -121.94 lon; 6.76 depth; utmZone: 10 2024-11-06T02:49:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861385.39 2024-11-06T02:49:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:47Z- waiting for 1730861389.39, time is 1730861387.02 2024-11-06T02:49:47Z- AHRS_M2 msg: 1730861387.41 epoch sec; seqNo:65 2024-11-06T02:49:47Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 5.34 psi 2024-11-06T02:49:47Z- handleDepth()- Depth_Keller msg: 1730861387.41 epoch sec; seqNo:71; depth 6.74 2024-11-06T02:49:47Z- handleDVL() - DVL msg epochMillisec: 1730861387.44 2024-11-06T02:49:47Z- handleDvl() - RDI_Pathfinder msg: 1730861387.44 sec; seqNo:22; #beams:4 2024-11-06T02:49:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.30 , 73.59 , 79.72 , 78.91 2024-11-06T02:49:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , -0.11 2024-11-06T02:49:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861387.46 epoch sec; seqNo:71; 4061434.37 north; 594623.79 east; 36.69 lat; -121.94 lon; 6.74 depth; utmZone: 10 2024-11-06T02:49:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861385.39 2024-11-06T02:49:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:47Z- waiting for 1730861389.39, time is 1730861387.46 2024-11-06T02:49:47Z- AHRS_M2 msg: 1730861387.81 epoch sec; seqNo:66 2024-11-06T02:49:47Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 5.34 psi 2024-11-06T02:49:47Z- handleDepth()- Depth_Keller msg: 1730861387.82 epoch sec; seqNo:72; depth 6.71 2024-11-06T02:49:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861387.84 epoch sec; seqNo:72; 4061434.37 north; 594623.79 east; 36.69 lat; -121.94 lon; 6.71 depth; utmZone: 10 2024-11-06T02:49:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861385.39 2024-11-06T02:49:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:47Z- waiting for 1730861389.39, time is 1730861387.84 2024-11-06T02:49:48Z- AHRS_M2 msg: 1730861388.22 epoch sec; seqNo:67 2024-11-06T02:49:48Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 5.34 psi 2024-11-06T02:49:48Z- handleDepth()- Depth_Keller msg: 1730861388.22 epoch sec; seqNo:73; depth 6.70 2024-11-06T02:49:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861388.23 epoch sec; seqNo:73; 4061434.74 north; 594622.57 east; 36.69 lat; -121.94 lon; 6.70 depth; utmZone: 10 2024-11-06T02:49:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861385.39 2024-11-06T02:49:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:48Z- waiting for 1730861389.39, time is 1730861388.23 2024-11-06T02:49:48Z- AHRS_M2 msg: 1730861388.62 epoch sec; seqNo:68 2024-11-06T02:49:48Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 5.34 psi 2024-11-06T02:49:48Z- handleDepth()- Depth_Keller msg: 1730861388.62 epoch sec; seqNo:74; depth 6.67 2024-11-06T02:49:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861388.64 epoch sec; seqNo:74; 4061434.74 north; 594622.57 east; 36.69 lat; -121.94 lon; 6.67 depth; utmZone: 10 2024-11-06T02:49:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861385.39 2024-11-06T02:49:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:48Z- waiting for 1730861389.39, time is 1730861388.64 2024-11-06T02:49:49Z- AHRS_M2 msg: 1730861389.03 epoch sec; seqNo:69 2024-11-06T02:49:49Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 5.33 psi 2024-11-06T02:49:49Z- handleDepth()- Depth_Keller msg: 1730861389.03 epoch sec; seqNo:75; depth 6.65 2024-11-06T02:49:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861389.05 epoch sec; seqNo:75; 4061435.12 north; 594622.56 east; 36.69 lat; -121.94 lon; 6.65 depth; utmZone: 10 2024-11-06T02:49:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861385.39 2024-11-06T02:49:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:49Z- waiting for 1730861389.39, time is 1730861389.05 2024-11-06T02:49:49Z- AHRS_M2 msg: 1730861389.43 epoch sec; seqNo:70 2024-11-06T02:49:49Z- AHRS_M2 msg: 0.11 phi; 0.03 theta; 5.33 psi 2024-11-06T02:49:49Z- handleDepth()- Depth_Keller msg: 1730861389.43 epoch sec; seqNo:76; depth 6.63 2024-11-06T02:49:49Z- handleDVL() - DVL msg epochMillisec: 1730861389.47 2024-11-06T02:49:49Z- handleDvl() - RDI_Pathfinder msg: 1730861389.47 sec; seqNo:23; #beams:4 2024-11-06T02:49:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.28 , 73.58 , 78.49 , 79.72 2024-11-06T02:49:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.08 , -0.16 2024-11-06T02:49:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861389.49 epoch sec; seqNo:76; 4061435.50 north; 594622.56 east; 36.69 lat; -121.94 lon; 6.63 depth; utmZone: 10 2024-11-06T02:49:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861385.39 2024-11-06T02:49:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:49Z- waiting for 1730861389.39, time is 1730861389.49 2024-11-06T02:49:49Z- LcmTrn::updateTrn() >>>> pose time 1730861389.49, meas time 1730861389.47 <<<< 2024-11-06T02:49:49Z- LcmTrn::updateTrn() equating pose 1730861389.47 and meas times 1730861389.47 2024-11-06T02:49:49Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861389.47 2024-11-06T02:49:49Z- TerrainNav::motionUpdate() - currEstimate = (4061435, 594622.56, 6.63) 2024-11-06T02:49:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:49:49Z- TerrainNav:: SubcloudNIS is 0.08, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:49:49Z- TNavPF::Projecting Measurements 2024-11-06T02:49:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:49:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:49:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:49:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:49:49Z- TNavFilter::4 good beams. 2024-11-06T02:49:49Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:49:49Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:49:49Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:49:49Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:49:49Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:49:49Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:49:49Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:49:49Z- TerrainMapOctree: (vehN, vehE ) = (4061435.50, 594622.56). 2024-11-06T02:49:49Z- TerrainMapOctree: Min Distance = 225.06. 2024-11-06T02:49:49Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:49:49Z- Forcing Subcloud Comparison 2024-11-06T02:49:49Z- Weighting particles with subcloud comparison 2024-11-06T02:49:49Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:49:49Z- meanExpectedMeasDiff: -0.832118 delta_rms_squared: 6.450640 alpha: 0.996909 2024-11-06T02:49:49Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:49:49Z- meanExpectedMeasDiff: -0.813379 delta_rms_squared: 3.389486 alpha: 0.994134 2024-11-06T02:49:49Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:49:49Z- meanExpectedMeasDiff: -5.115549 delta_rms_squared: 5.300490 alpha: 0.996241 2024-11-06T02:49:49Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:49:49Z- meanExpectedMeasDiff: -5.510565 delta_rms_squared: 2.791006 alpha: 0.992885 2024-11-06T02:49:49Z- Subcloud NIS: 0.082269 2024-11-06T02:49:49Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:49:49Z- TNavPF:: sumWeights = 0.108062 2024-11-06T02:49:49Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:49:49Z- TNavPF::First Term of Map Covariance Matrix is 0.51 2024-11-06T02:49:49Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T02:49:49Z- TerrainNav::Resampling particle filter... 2024-11-06T02:49:49Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861389.47 sec, ping # 0. 2024-11-06T02:49:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:49:49Z- variances([0], [2], and psi_berg) = 41.624656 452.309224 0.000000 2024-11-06T02:49:49Z- publishEstimatesDecoder() - at 1730861389.47 sec 2024-11-06T02:49:49Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:49:49Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061491.54 594627.21 12.22 2024-11-06T02:49:49Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061488.76 594605.46 13.67 lat/lon: 36.69 -121.94 2024-11-06T02:49:49Z- LcmTrn::publishEstimatesDecoder() - COVAR: 41.62 452.31 0.86 0.00 2024-11-06T02:49:49Z- AHRS_M2 msg: 1730861389.83 epoch sec; seqNo:71 2024-11-06T02:49:49Z- AHRS_M2 msg: 0.11 phi; 0.02 theta; 5.33 psi 2024-11-06T02:49:49Z- handleDepth()- Depth_Keller msg: 1730861389.84 epoch sec; seqNo:77; depth 6.60 2024-11-06T02:49:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861389.86 epoch sec; seqNo:77; 4061435.48 north; 594621.34 east; 36.69 lat; -121.94 lon; 6.60 depth; utmZone: 10 2024-11-06T02:49:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861389.47 2024-11-06T02:49:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:49Z- waiting for 1730861393.47, time is 1730861389.86 2024-11-06T02:49:50Z- AHRS_M2 msg: 1730861390.24 epoch sec; seqNo:72 2024-11-06T02:49:50Z- AHRS_M2 msg: 0.11 phi; 0.02 theta; 5.33 psi 2024-11-06T02:49:50Z- handleDepth()- Depth_Keller msg: 1730861390.24 epoch sec; seqNo:78; depth 6.58 2024-11-06T02:49:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861390.25 epoch sec; seqNo:78; 4061435.86 north; 594621.33 east; 36.69 lat; -121.94 lon; 6.58 depth; utmZone: 10 2024-11-06T02:49:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861389.47 2024-11-06T02:49:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:50Z- waiting for 1730861393.47, time is 1730861390.25 2024-11-06T02:49:50Z- AHRS_M2 msg: 1730861390.65 epoch sec; seqNo:73 2024-11-06T02:49:50Z- AHRS_M2 msg: 0.11 phi; 0.02 theta; 5.33 psi 2024-11-06T02:49:50Z- handleDepth()- Depth_Keller msg: 1730861390.66 epoch sec; seqNo:79; depth 6.57 2024-11-06T02:49:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861390.68 epoch sec; seqNo:79; 4061435.86 north; 594621.33 east; 36.69 lat; -121.94 lon; 6.57 depth; utmZone: 10 2024-11-06T02:49:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861389.47 2024-11-06T02:49:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:50Z- waiting for 1730861393.47, time is 1730861390.68 2024-11-06T02:49:51Z- AHRS_M2 msg: 1730861391.04 epoch sec; seqNo:74 2024-11-06T02:49:51Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.33 psi 2024-11-06T02:49:51Z- handleDepth()- Depth_Keller msg: 1730861391.05 epoch sec; seqNo:80; depth 6.55 2024-11-06T02:49:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861391.06 epoch sec; seqNo:80; 4061436.24 north; 594621.33 east; 36.69 lat; -121.94 lon; 6.55 depth; utmZone: 10 2024-11-06T02:49:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861389.47 2024-11-06T02:49:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:51Z- waiting for 1730861393.47, time is 1730861391.06 2024-11-06T02:49:51Z- AHRS_M2 msg: 1730861391.45 epoch sec; seqNo:75 2024-11-06T02:49:51Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.32 psi 2024-11-06T02:49:51Z- handleDepth()- Depth_Keller msg: 1730861391.46 epoch sec; seqNo:81; depth 6.54 2024-11-06T02:49:51Z- handleDVL() - DVL msg epochMillisec: 1730861391.48 2024-11-06T02:49:51Z- handleDvl() - RDI_Pathfinder msg: 1730861391.48 sec; seqNo:24; #beams:4 2024-11-06T02:49:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.18 , 73.58 , 77.26 , 82.77 2024-11-06T02:49:51Z- handleDvl() - RDI_Pathfinder msg: velocities 1.15 , -0.04 , -0.09 2024-11-06T02:49:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861391.51 epoch sec; seqNo:81; 4061436.23 north; 594620.11 east; 36.69 lat; -121.94 lon; 6.54 depth; utmZone: 10 2024-11-06T02:49:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861389.47 2024-11-06T02:49:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:51Z- waiting for 1730861393.47, time is 1730861391.51 2024-11-06T02:49:51Z- AHRS_M2 msg: 1730861391.85 epoch sec; seqNo:76 2024-11-06T02:49:51Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 5.32 psi 2024-11-06T02:49:51Z- handleDepth()- Depth_Keller msg: 1730861391.86 epoch sec; seqNo:82; depth 6.53 2024-11-06T02:49:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861391.88 epoch sec; seqNo:82; 4061436.61 north; 594620.11 east; 36.69 lat; -121.94 lon; 6.53 depth; utmZone: 10 2024-11-06T02:49:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861389.47 2024-11-06T02:49:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:52Z- waiting for 1730861393.47, time is 1730861391.88 2024-11-06T02:49:52Z- AHRS_M2 msg: 1730861392.26 epoch sec; seqNo:77 2024-11-06T02:49:52Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 5.31 psi 2024-11-06T02:49:52Z- handleDepth()- Depth_Keller msg: 1730861392.26 epoch sec; seqNo:83; depth 6.54 2024-11-06T02:49:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861392.27 epoch sec; seqNo:83; 4061436.99 north; 594620.10 east; 36.69 lat; -121.94 lon; 6.54 depth; utmZone: 10 2024-11-06T02:49:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861389.47 2024-11-06T02:49:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:52Z- waiting for 1730861393.47, time is 1730861392.27 2024-11-06T02:49:52Z- AHRS_M2 msg: 1730861392.66 epoch sec; seqNo:78 2024-11-06T02:49:52Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 5.30 psi 2024-11-06T02:49:52Z- handleDepth()- Depth_Keller msg: 1730861392.67 epoch sec; seqNo:84; depth 6.52 2024-11-06T02:49:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861392.69 epoch sec; seqNo:84; 4061436.97 north; 594618.88 east; 36.69 lat; -121.94 lon; 6.52 depth; utmZone: 10 2024-11-06T02:49:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861389.47 2024-11-06T02:49:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:52Z- waiting for 1730861393.47, time is 1730861392.69 2024-11-06T02:49:53Z- AHRS_M2 msg: 1730861393.06 epoch sec; seqNo:79 2024-11-06T02:49:53Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 5.30 psi 2024-11-06T02:49:53Z- handleDepth()- Depth_Keller msg: 1730861393.07 epoch sec; seqNo:85; depth 6.53 2024-11-06T02:49:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861393.08 epoch sec; seqNo:85; 4061437.35 north; 594618.88 east; 36.69 lat; -121.94 lon; 6.53 depth; utmZone: 10 2024-11-06T02:49:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861389.47 2024-11-06T02:49:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:53Z- waiting for 1730861393.47, time is 1730861393.08 2024-11-06T02:49:53Z- AHRS_M2 msg: 1730861393.48 epoch sec; seqNo:80 2024-11-06T02:49:53Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.29 psi 2024-11-06T02:49:53Z- handleDepth()- Depth_Keller msg: 1730861393.49 epoch sec; seqNo:86; depth 6.55 2024-11-06T02:49:53Z- handleDVL() - DVL msg epochMillisec: 1730861393.52 2024-11-06T02:49:53Z- handleDvl() - RDI_Pathfinder msg: 1730861393.52 sec; seqNo:25; #beams:4 2024-11-06T02:49:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.35 , 73.18 , 77.26 , 84.93 2024-11-06T02:49:53Z- handleDvl() - RDI_Pathfinder msg: velocities 1.14 , -0.01 , 0.21 2024-11-06T02:49:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861393.59 epoch sec; seqNo:86; 4061437.73 north; 594618.87 east; 36.69 lat; -121.94 lon; 6.55 depth; utmZone: 10 2024-11-06T02:49:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861389.47 2024-11-06T02:49:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:53Z- waiting for 1730861393.47, time is 1730861393.59 2024-11-06T02:49:53Z- LcmTrn::updateTrn() >>>> pose time 1730861393.59, meas time 1730861393.52 <<<< 2024-11-06T02:49:53Z- LcmTrn::updateTrn() equating pose 1730861393.52 and meas times 1730861393.52 2024-11-06T02:49:53Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861393.52 2024-11-06T02:49:53Z- TerrainNav::motionUpdate() - currEstimate = (4061438, 594618.87, 6.55) 2024-11-06T02:49:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:49:53Z- TerrainNav:: SubcloudNIS is 0.08, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:49:53Z- TNavPF::Projecting Measurements 2024-11-06T02:49:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:49:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:49:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:49:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:49:53Z- TNavFilter::4 good beams. 2024-11-06T02:49:53Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:49:53Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:49:53Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:49:53Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:49:53Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:49:53Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:49:53Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:49:53Z- TerrainMapOctree: (vehN, vehE ) = (4061437.73, 594618.87). 2024-11-06T02:49:53Z- TerrainMapOctree: Min Distance = 228.99. 2024-11-06T02:49:53Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:49:53Z- Forcing Subcloud Comparison 2024-11-06T02:49:53Z- Weighting particles with subcloud comparison 2024-11-06T02:49:53Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:49:53Z- meanExpectedMeasDiff: -1.101107 delta_rms_squared: 5.416378 alpha: 0.996321 2024-11-06T02:49:53Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:49:53Z- meanExpectedMeasDiff: 0.363715 delta_rms_squared: 2.686700 alpha: 0.992611 2024-11-06T02:49:53Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:49:53Z- meanExpectedMeasDiff: -3.347694 delta_rms_squared: 3.318513 alpha: 0.994009 2024-11-06T02:49:53Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:49:53Z- meanExpectedMeasDiff: -1.472796 delta_rms_squared: 2.342586 alpha: 0.991535 2024-11-06T02:49:53Z- Subcloud NIS: 0.073773 2024-11-06T02:49:53Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:49:53Z- TNavPF:: sumWeights = 0.386243 2024-11-06T02:49:53Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:49:53Z- TNavPF::First Term of Map Covariance Matrix is 1.65 2024-11-06T02:49:53Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T02:49:53Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861393.52 sec, ping # 0. 2024-11-06T02:49:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:49:53Z- variances([0], [2], and psi_berg) = 35.998448 293.372975 0.000000 2024-11-06T02:49:53Z- publishEstimatesDecoder() - at 1730861393.52 sec 2024-11-06T02:49:53Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:49:53Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061496.66 594594.13 14.54 2024-11-06T02:49:53Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061491.90 594600.14 13.91 lat/lon: 36.69 -121.94 2024-11-06T02:49:53Z- LcmTrn::publishEstimatesDecoder() - COVAR: 36.00 293.37 0.81 0.00 2024-11-06T02:49:54Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T02:49:54Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T02:49:54Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T02:49:54Z- AHRS_M2 msg: 1730861393.98 epoch sec; seqNo:81 2024-11-06T02:49:54Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.29 psi 2024-11-06T02:49:54Z- handleDepth()- Depth_Keller msg: 1730861394.01 epoch sec; seqNo:87; depth 6.53 2024-11-06T02:49:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861394.04 epoch sec; seqNo:87; 4061437.72 north; 594617.65 east; 36.69 lat; -121.94 lon; 6.53 depth; utmZone: 10 2024-11-06T02:49:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861393.52 2024-11-06T02:49:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:54Z- waiting for 1730861397.52, time is 1730861394.04 2024-11-06T02:49:54Z- AHRS_M2 msg: 1730861394.40 epoch sec; seqNo:82 2024-11-06T02:49:54Z- AHRS_M2 msg: 0.10 phi; -0.04 theta; 5.27 psi 2024-11-06T02:49:54Z- handleDepth()- Depth_Keller msg: 1730861394.41 epoch sec; seqNo:88; depth 6.54 2024-11-06T02:49:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861394.42 epoch sec; seqNo:88; 4061438.10 north; 594617.65 east; 36.69 lat; -121.94 lon; 6.54 depth; utmZone: 10 2024-11-06T02:49:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861393.52 2024-11-06T02:49:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:54Z- waiting for 1730861397.52, time is 1730861394.42 2024-11-06T02:49:54Z- AHRS_M2 msg: 1730861394.80 epoch sec; seqNo:83 2024-11-06T02:49:54Z- AHRS_M2 msg: 0.10 phi; -0.05 theta; 5.27 psi 2024-11-06T02:49:54Z- handleDepth()- Depth_Keller msg: 1730861394.81 epoch sec; seqNo:89; depth 6.55 2024-11-06T02:49:54Z- handleDVL() - DVL msg epochMillisec: 1730861394.81 2024-11-06T02:49:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:49:54Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:49:54Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:49:54Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:49:54Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:49:54Z- handleDvl() - RDI_Pathfinder msg: 1730861394.81 sec; seqNo:26; #beams:0 2024-11-06T02:49:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:49:54Z- handleDvl() - RDI_Pathfinder msg: velocities 1.14 , -0.01 , 0.21 2024-11-06T02:49:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861394.82 epoch sec; seqNo:89; 4061438.47 north; 594617.64 east; 36.69 lat; -121.94 lon; 6.55 depth; utmZone: 10 2024-11-06T02:49:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861393.52 2024-11-06T02:49:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:54Z- waiting for 1730861397.52, time is 1730861394.82 2024-11-06T02:49:55Z- AHRS_M2 msg: 1730861395.21 epoch sec; seqNo:84 2024-11-06T02:49:55Z- AHRS_M2 msg: 0.11 phi; -0.06 theta; 5.27 psi 2024-11-06T02:49:55Z- handleDepth()- Depth_Keller msg: 1730861395.22 epoch sec; seqNo:90; depth 6.56 2024-11-06T02:49:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861395.24 epoch sec; seqNo:90; 4061438.46 north; 594616.42 east; 36.69 lat; -121.94 lon; 6.56 depth; utmZone: 10 2024-11-06T02:49:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861393.52 2024-11-06T02:49:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:55Z- waiting for 1730861397.52, time is 1730861395.24 2024-11-06T02:49:55Z- AHRS_M2 msg: 1730861395.61 epoch sec; seqNo:85 2024-11-06T02:49:55Z- AHRS_M2 msg: 0.11 phi; -0.07 theta; 5.27 psi 2024-11-06T02:49:55Z- handleDepth()- Depth_Keller msg: 1730861395.61 epoch sec; seqNo:91; depth 6.55 2024-11-06T02:49:55Z- handleDVL() - DVL msg epochMillisec: 1730861395.64 2024-11-06T02:49:55Z- handleDvl() - RDI_Pathfinder msg: 1730861395.64 sec; seqNo:27; #beams:4 2024-11-06T02:49:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.19 , 73.58 , 76.04 , 85.95 2024-11-06T02:49:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.18 2024-11-06T02:49:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861395.66 epoch sec; seqNo:91; 4061438.84 north; 594616.42 east; 36.69 lat; -121.94 lon; 6.55 depth; utmZone: 10 2024-11-06T02:49:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861393.52 2024-11-06T02:49:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:55Z- waiting for 1730861397.52, time is 1730861395.66 2024-11-06T02:49:56Z- AHRS_M2 msg: 1730861396.01 epoch sec; seqNo:86 2024-11-06T02:49:56Z- AHRS_M2 msg: 0.11 phi; -0.08 theta; 5.28 psi 2024-11-06T02:49:56Z- handleDepth()- Depth_Keller msg: 1730861396.02 epoch sec; seqNo:92; depth 6.58 2024-11-06T02:49:56Z- handleDVL() - DVL msg epochMillisec: 1730861396.02 2024-11-06T02:49:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:49:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:49:56Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:49:56Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:49:56Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:49:56Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:49:56Z- handleDvl() - RDI_Pathfinder msg: 1730861396.02 sec; seqNo:28; #beams:0 2024-11-06T02:49:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:49:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.18 2024-11-06T02:49:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861396.04 epoch sec; seqNo:92; 4061438.84 north; 594616.42 east; 36.69 lat; -121.94 lon; 6.58 depth; utmZone: 10 2024-11-06T02:49:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861393.52 2024-11-06T02:49:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:56Z- waiting for 1730861397.52, time is 1730861396.04 2024-11-06T02:49:56Z- AHRS_M2 msg: 1730861396.43 epoch sec; seqNo:87 2024-11-06T02:49:56Z- AHRS_M2 msg: 0.11 phi; -0.08 theta; 5.28 psi 2024-11-06T02:49:56Z- handleDepth()- Depth_Keller msg: 1730861396.43 epoch sec; seqNo:93; depth 6.58 2024-11-06T02:49:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861396.44 epoch sec; seqNo:93; 4061438.84 north; 594616.42 east; 36.69 lat; -121.94 lon; 6.58 depth; utmZone: 10 2024-11-06T02:49:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861393.52 2024-11-06T02:49:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:56Z- waiting for 1730861397.52, time is 1730861396.44 2024-11-06T02:49:56Z- handleDepth()- Depth_Keller msg: 1730861396.84 epoch sec; seqNo:94; depth 6.61 2024-11-06T02:49:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861396.85 epoch sec; seqNo:94; 4061439.20 north; 594615.19 east; 36.69 lat; -121.94 lon; 6.61 depth; utmZone: 10 2024-11-06T02:49:56Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:49:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861393.52 2024-11-06T02:49:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:56Z- waiting for 1730861397.52, time is 1730861396.85 2024-11-06T02:49:57Z- AHRS_M2 msg: 1730861397.23 epoch sec; seqNo:88 2024-11-06T02:49:57Z- AHRS_M2 msg: 0.12 phi; -0.09 theta; 5.28 psi 2024-11-06T02:49:57Z- handleDepth()- Depth_Keller msg: 1730861397.27 epoch sec; seqNo:95; depth 6.61 2024-11-06T02:49:57Z- handleDVL() - DVL msg epochMillisec: 1730861397.31 2024-11-06T02:49:57Z- handleDvl() - RDI_Pathfinder msg: 1730861397.31 sec; seqNo:29; #beams:4 2024-11-06T02:49:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.19 , 74.40 , 75.22 , 89.19 2024-11-06T02:49:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , 0.01 2024-11-06T02:49:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861397.32 epoch sec; seqNo:95; 4061439.20 north; 594615.19 east; 36.69 lat; -121.94 lon; 6.61 depth; utmZone: 10 2024-11-06T02:49:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861393.52 2024-11-06T02:49:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:57Z- waiting for 1730861397.52, time is 1730861397.32 2024-11-06T02:49:57Z- AHRS_M2 msg: 1730861397.63 epoch sec; seqNo:89 2024-11-06T02:49:57Z- AHRS_M2 msg: 0.14 phi; -0.09 theta; 5.28 psi 2024-11-06T02:49:57Z- handleDepth()- Depth_Keller msg: 1730861397.63 epoch sec; seqNo:96; depth 6.66 2024-11-06T02:49:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861397.65 epoch sec; seqNo:96; 4061439.58 north; 594615.19 east; 36.69 lat; -121.94 lon; 6.66 depth; utmZone: 10 2024-11-06T02:49:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861393.52 2024-11-06T02:49:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:57Z- waiting for 1730861397.52, time is 1730861397.65 2024-11-06T02:49:57Z- LcmTrn::updateTrn() >>>> pose time 1730861397.65, meas time 1730861397.31 <<<< 2024-11-06T02:49:57Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861397.65 2024-11-06T02:49:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861397.31 sec, ping # 0; waiting for INS data more recent than 1730861393.52... 2024-11-06T02:49:57Z- TerrainNav::motionUpdate() - currEstimate = (4061440, 594615.19, 6.66) 2024-11-06T02:49:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:49:57Z- TerrainNav:: SubcloudNIS is 0.07, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:49:57Z- TNavPF::Projecting Measurements 2024-11-06T02:49:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:49:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:49:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:49:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:49:57Z- TNavFilter::4 good beams. 2024-11-06T02:49:57Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:49:57Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:49:57Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:49:57Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:49:57Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:49:57Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:49:57Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:49:57Z- TerrainMapOctree: (vehN, vehE ) = (4061439.43, 594615.50). 2024-11-06T02:49:57Z- TerrainMapOctree: Min Distance = 232.32. 2024-11-06T02:49:57Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:49:57Z- Forcing Subcloud Comparison 2024-11-06T02:49:57Z- Weighting particles with subcloud comparison 2024-11-06T02:49:57Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:49:57Z- meanExpectedMeasDiff: -2.321629 delta_rms_squared: 5.043787 alpha: 0.996050 2024-11-06T02:49:57Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:49:57Z- meanExpectedMeasDiff: 1.807795 delta_rms_squared: 2.137594 alpha: 0.990730 2024-11-06T02:49:57Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:49:57Z- meanExpectedMeasDiff: -2.033821 delta_rms_squared: 1.897077 alpha: 0.989567 2024-11-06T02:49:57Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:49:57Z- meanExpectedMeasDiff: -1.053975 delta_rms_squared: 2.211073 alpha: 0.991036 2024-11-06T02:49:57Z- Subcloud NIS: 0.078882 2024-11-06T02:49:57Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:49:57Z- TNavPF:: sumWeights = 0.434950 2024-11-06T02:49:57Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:49:57Z- TNavPF::First Term of Map Covariance Matrix is 1.68 2024-11-06T02:49:57Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T02:49:57Z- TerrainNav::Resampling particle filter... 2024-11-06T02:49:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861397.31 sec. 2024-11-06T02:49:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:49:57Z- variances([0], [2], and psi_berg) = 30.595953 124.738259 0.000000 2024-11-06T02:49:57Z- publishEstimatesDecoder() - at 1730861397.65 sec 2024-11-06T02:49:57Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:49:57Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061495.74 594619.85 14.34 2024-11-06T02:49:57Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061494.75 594591.98 14.13 lat/lon: 36.69 -121.94 2024-11-06T02:49:57Z- LcmTrn::publishEstimatesDecoder() - COVAR: 30.60 124.74 1.04 0.00 2024-11-06T02:49:58Z- AHRS_M2 msg: 1730861398.04 epoch sec; seqNo:90 2024-11-06T02:49:58Z- AHRS_M2 msg: 0.15 phi; -0.09 theta; 5.27 psi 2024-11-06T02:49:58Z- handleDepth()- Depth_Keller msg: 1730861398.06 epoch sec; seqNo:97; depth 6.69 2024-11-06T02:49:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861398.06 epoch sec; seqNo:97; 4061439.58 north; 594615.19 east; 36.69 lat; -121.94 lon; 6.69 depth; utmZone: 10 2024-11-06T02:49:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861397.65 2024-11-06T02:49:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:58Z- waiting for 1730861401.65, time is 1730861398.06 2024-11-06T02:49:58Z- AHRS_M2 msg: 1730861398.44 epoch sec; seqNo:91 2024-11-06T02:49:58Z- AHRS_M2 msg: 0.16 phi; -0.10 theta; 5.26 psi 2024-11-06T02:49:58Z- handleDepth()- Depth_Keller msg: 1730861398.44 epoch sec; seqNo:98; depth 6.75 2024-11-06T02:49:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861398.45 epoch sec; seqNo:98; 4061439.95 north; 594613.97 east; 36.69 lat; -121.94 lon; 6.75 depth; utmZone: 10 2024-11-06T02:49:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861397.65 2024-11-06T02:49:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:58Z- waiting for 1730861401.65, time is 1730861398.45 2024-11-06T02:49:58Z- AHRS_M2 msg: 1730861398.84 epoch sec; seqNo:92 2024-11-06T02:49:58Z- AHRS_M2 msg: 0.15 phi; -0.12 theta; 5.25 psi 2024-11-06T02:49:58Z- handleDepth()- Depth_Keller msg: 1730861398.85 epoch sec; seqNo:99; depth 6.77 2024-11-06T02:49:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861398.86 epoch sec; seqNo:99; 4061439.95 north; 594613.97 east; 36.69 lat; -121.94 lon; 6.77 depth; utmZone: 10 2024-11-06T02:49:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861397.65 2024-11-06T02:49:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:59Z- waiting for 1730861401.65, time is 1730861398.86 2024-11-06T02:49:59Z- AHRS_M2 msg: 1730861399.28 epoch sec; seqNo:93 2024-11-06T02:49:59Z- AHRS_M2 msg: 0.13 phi; -0.14 theta; 5.25 psi 2024-11-06T02:49:59Z- handleDepth()- Depth_Keller msg: 1730861399.29 epoch sec; seqNo:100; depth 6.82 2024-11-06T02:49:59Z- handleDVL() - DVL msg epochMillisec: 1730861399.32 2024-11-06T02:49:59Z- handleDvl() - RDI_Pathfinder msg: 1730861399.32 sec; seqNo:30; #beams:4 2024-11-06T02:49:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.37 , 73.18 , 74.81 , 91.29 2024-11-06T02:49:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.61 , 0.05 , 0.08 2024-11-06T02:49:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861399.33 epoch sec; seqNo:100; 4061439.95 north; 594613.97 east; 36.69 lat; -121.94 lon; 6.82 depth; utmZone: 10 2024-11-06T02:49:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:49:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861397.65 2024-11-06T02:49:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:59Z- waiting for 1730861401.65, time is 1730861399.33 2024-11-06T02:49:59Z- AHRS_M2 msg: 1730861399.65 epoch sec; seqNo:94 2024-11-06T02:49:59Z- AHRS_M2 msg: 0.11 phi; -0.16 theta; 5.24 psi 2024-11-06T02:49:59Z- handleDepth()- Depth_Keller msg: 1730861399.66 epoch sec; seqNo:101; depth 6.84 2024-11-06T02:49:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861399.69 epoch sec; seqNo:101; 4061440.33 north; 594613.96 east; 36.69 lat; -121.94 lon; 6.84 depth; utmZone: 10 2024-11-06T02:49:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:49:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861397.65 2024-11-06T02:49:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:49:59Z- waiting for 1730861401.65, time is 1730861399.69 2024-11-06T02:50:00Z- AHRS_M2 msg: 1730861400.05 epoch sec; seqNo:95 2024-11-06T02:50:00Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.24 psi 2024-11-06T02:50:00Z- handleDepth()- Depth_Keller msg: 1730861400.06 epoch sec; seqNo:102; depth 6.85 2024-11-06T02:50:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861400.06 epoch sec; seqNo:102; 4061440.33 north; 594613.96 east; 36.69 lat; -121.94 lon; 6.85 depth; utmZone: 10 2024-11-06T02:50:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861397.65 2024-11-06T02:50:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:00Z- waiting for 1730861401.65, time is 1730861400.06 2024-11-06T02:50:00Z- handleDepth()- Depth_Keller msg: 1730861400.49 epoch sec; seqNo:103; depth 6.88 2024-11-06T02:50:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861400.49 epoch sec; seqNo:103; 4061440.33 north; 594613.96 east; 36.69 lat; -121.94 lon; 6.88 depth; utmZone: 10 2024-11-06T02:50:00Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:50:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861397.65 2024-11-06T02:50:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:00Z- waiting for 1730861401.65, time is 1730861400.49 2024-11-06T02:50:00Z- AHRS_M2 msg: 1730861400.86 epoch sec; seqNo:96 2024-11-06T02:50:00Z- AHRS_M2 msg: 0.10 phi; -0.16 theta; 5.25 psi 2024-11-06T02:50:00Z- handleDepth()- Depth_Keller msg: 1730861400.88 epoch sec; seqNo:104; depth 6.90 2024-11-06T02:50:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861400.89 epoch sec; seqNo:104; 4061440.69 north; 594612.74 east; 36.69 lat; -121.94 lon; 6.90 depth; utmZone: 10 2024-11-06T02:50:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861397.65 2024-11-06T02:50:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:01Z- waiting for 1730861401.65, time is 1730861400.89 2024-11-06T02:50:01Z- AHRS_M2 msg: 1730861401.29 epoch sec; seqNo:97 2024-11-06T02:50:01Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.25 psi 2024-11-06T02:50:01Z- handleDepth()- Depth_Keller msg: 1730861401.30 epoch sec; seqNo:105; depth 6.93 2024-11-06T02:50:01Z- handleDVL() - DVL msg epochMillisec: 1730861401.32 2024-11-06T02:50:01Z- handleDvl() - RDI_Pathfinder msg: 1730861401.32 sec; seqNo:31; #beams:4 2024-11-06T02:50:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.66 , 74.82 , 73.18 , 94.75 2024-11-06T02:50:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.05 , -0.35 2024-11-06T02:50:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861401.35 epoch sec; seqNo:105; 4061440.69 north; 594612.74 east; 36.69 lat; -121.94 lon; 6.93 depth; utmZone: 10 2024-11-06T02:50:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861397.65 2024-11-06T02:50:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:01Z- waiting for 1730861401.65, time is 1730861401.35 2024-11-06T02:50:01Z- AHRS_M2 msg: 1730861401.68 epoch sec; seqNo:98 2024-11-06T02:50:01Z- AHRS_M2 msg: 0.12 phi; -0.14 theta; 5.25 psi 2024-11-06T02:50:01Z- handleDepth()- Depth_Keller msg: 1730861401.69 epoch sec; seqNo:106; depth 6.99 2024-11-06T02:50:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861401.71 epoch sec; seqNo:106; 4061441.07 north; 594612.73 east; 36.69 lat; -121.94 lon; 6.99 depth; utmZone: 10 2024-11-06T02:50:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861397.65 2024-11-06T02:50:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:01Z- waiting for 1730861401.65, time is 1730861401.71 2024-11-06T02:50:01Z- LcmTrn::updateTrn() >>>> pose time 1730861401.71, meas time 1730861401.32 <<<< 2024-11-06T02:50:01Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861401.71 2024-11-06T02:50:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861401.32 sec, ping # 0; waiting for INS data more recent than 1730861397.65... 2024-11-06T02:50:01Z- TerrainNav::motionUpdate() - currEstimate = (4061441, 594612.73, 6.99) 2024-11-06T02:50:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:50:01Z- TerrainNav:: SubcloudNIS is 0.08, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:50:01Z- TNavPF::Projecting Measurements 2024-11-06T02:50:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:50:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:50:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:50:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:50:01Z- TNavFilter::4 good beams. 2024-11-06T02:50:01Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:50:01Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:50:01Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:50:01Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:50:01Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:50:01Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:50:01Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:50:01Z- TerrainMapOctree: (vehN, vehE ) = (4061440.93, 594612.97). 2024-11-06T02:50:01Z- TerrainMapOctree: Min Distance = 235.01. 2024-11-06T02:50:01Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:50:01Z- Forcing Subcloud Comparison 2024-11-06T02:50:01Z- Weighting particles with subcloud comparison 2024-11-06T02:50:01Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:01Z- meanExpectedMeasDiff: -1.810510 delta_rms_squared: 4.154935 alpha: 0.995210 2024-11-06T02:50:01Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:01Z- meanExpectedMeasDiff: 1.736465 delta_rms_squared: 2.152525 alpha: 0.990794 2024-11-06T02:50:01Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:01Z- meanExpectedMeasDiff: -1.836740 delta_rms_squared: 1.726338 alpha: 0.988547 2024-11-06T02:50:01Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:01Z- meanExpectedMeasDiff: 1.903491 delta_rms_squared: 2.383849 alpha: 0.991680 2024-11-06T02:50:01Z- Subcloud NIS: 0.086105 2024-11-06T02:50:01Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:50:01Z- TNavPF:: sumWeights = 0.433738 2024-11-06T02:50:01Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:50:01Z- TNavPF::First Term of Map Covariance Matrix is 1.58 2024-11-06T02:50:01Z- TNavPF::Calculated NIS Value : 0.19 numBeams normalized NIS: 0.05 2024-11-06T02:50:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861401.32 sec. 2024-11-06T02:50:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:50:02Z- variances([0], [2], and psi_berg) = 26.186091 65.693607 0.000000 2024-11-06T02:50:02Z- publishEstimatesDecoder() - at 1730861401.71 sec 2024-11-06T02:50:02Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:50:02Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061463.88 594579.01 15.94 2024-11-06T02:50:02Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061496.71 594587.72 14.42 lat/lon: 36.69 -121.94 2024-11-06T02:50:02Z- LcmTrn::publishEstimatesDecoder() - COVAR: 26.19 65.69 1.11 0.00 2024-11-06T02:50:02Z- handleDepth()- Depth_Keller msg: 1730861402.10 epoch sec; seqNo:107; depth 7.03 2024-11-06T02:50:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861402.11 epoch sec; seqNo:107; 4061441.45 north; 594612.73 east; 36.69 lat; -121.94 lon; 7.03 depth; utmZone: 10 2024-11-06T02:50:02Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:50:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861401.71 2024-11-06T02:50:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:02Z- waiting for 1730861405.71, time is 1730861402.11 2024-11-06T02:50:02Z- AHRS_M2 msg: 1730861402.48 epoch sec; seqNo:99 2024-11-06T02:50:02Z- AHRS_M2 msg: 0.13 phi; -0.14 theta; 5.24 psi 2024-11-06T02:50:02Z- handleDepth()- Depth_Keller msg: 1730861402.50 epoch sec; seqNo:108; depth 7.07 2024-11-06T02:50:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861402.51 epoch sec; seqNo:108; 4061441.44 north; 594611.51 east; 36.69 lat; -121.94 lon; 7.07 depth; utmZone: 10 2024-11-06T02:50:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861401.71 2024-11-06T02:50:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:02Z- waiting for 1730861405.71, time is 1730861402.51 2024-11-06T02:50:02Z- AHRS_M2 msg: 1730861402.88 epoch sec; seqNo:100 2024-11-06T02:50:02Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.24 psi 2024-11-06T02:50:02Z- handleDepth()- Depth_Keller msg: 1730861402.89 epoch sec; seqNo:109; depth 7.13 2024-11-06T02:50:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861402.90 epoch sec; seqNo:109; 4061441.82 north; 594611.50 east; 36.69 lat; -121.94 lon; 7.13 depth; utmZone: 10 2024-11-06T02:50:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861401.71 2024-11-06T02:50:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:03Z- waiting for 1730861405.71, time is 1730861402.90 2024-11-06T02:50:03Z- AHRS_M2 msg: 1730861403.30 epoch sec; seqNo:101 2024-11-06T02:50:03Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.25 psi 2024-11-06T02:50:03Z- handleDepth()- Depth_Keller msg: 1730861403.30 epoch sec; seqNo:110; depth 7.18 2024-11-06T02:50:03Z- handleDVL() - DVL msg epochMillisec: 1730861403.33 2024-11-06T02:50:03Z- handleDvl() - RDI_Pathfinder msg: 1730861403.33 sec; seqNo:32; #beams:4 2024-11-06T02:50:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.65 , 72.77 , 72.36 , 91.09 2024-11-06T02:50:03Z- handleDvl() - RDI_Pathfinder msg: velocities 1.11 , -0.08 , -0.05 2024-11-06T02:50:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861403.35 epoch sec; seqNo:110; 4061442.20 north; 594611.50 east; 36.69 lat; -121.94 lon; 7.18 depth; utmZone: 10 2024-11-06T02:50:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861401.71 2024-11-06T02:50:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:03Z- waiting for 1730861405.71, time is 1730861403.35 2024-11-06T02:50:03Z- AHRS_M2 msg: 1730861403.69 epoch sec; seqNo:102 2024-11-06T02:50:03Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.26 psi 2024-11-06T02:50:03Z- handleDepth()- Depth_Keller msg: 1730861403.70 epoch sec; seqNo:111; depth 7.22 2024-11-06T02:50:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861403.72 epoch sec; seqNo:111; 4061442.18 north; 594610.28 east; 36.69 lat; -121.94 lon; 7.22 depth; utmZone: 10 2024-11-06T02:50:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861401.71 2024-11-06T02:50:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:03Z- waiting for 1730861405.71, time is 1730861403.72 2024-11-06T02:50:04Z- AHRS_M2 msg: 1730861404.10 epoch sec; seqNo:103 2024-11-06T02:50:04Z- AHRS_M2 msg: 0.14 phi; -0.12 theta; 5.27 psi 2024-11-06T02:50:04Z- handleDepth()- Depth_Keller msg: 1730861404.10 epoch sec; seqNo:112; depth 7.27 2024-11-06T02:50:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861404.12 epoch sec; seqNo:112; 4061442.56 north; 594610.28 east; 36.69 lat; -121.94 lon; 7.27 depth; utmZone: 10 2024-11-06T02:50:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861401.71 2024-11-06T02:50:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:04Z- waiting for 1730861405.71, time is 1730861404.12 2024-11-06T02:50:04Z- AHRS_M2 msg: 1730861404.50 epoch sec; seqNo:104 2024-11-06T02:50:04Z- AHRS_M2 msg: 0.16 phi; -0.12 theta; 5.27 psi 2024-11-06T02:50:04Z- handleDepth()- Depth_Keller msg: 1730861404.51 epoch sec; seqNo:113; depth 7.31 2024-11-06T02:50:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861404.51 epoch sec; seqNo:113; 4061442.56 north; 594610.28 east; 36.69 lat; -121.94 lon; 7.31 depth; utmZone: 10 2024-11-06T02:50:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861401.71 2024-11-06T02:50:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:04Z- waiting for 1730861405.71, time is 1730861404.51 2024-11-06T02:50:04Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T02:50:04Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T02:50:04Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T02:50:04Z- AHRS_M2 msg: 1730861404.90 epoch sec; seqNo:105 2024-11-06T02:50:04Z- AHRS_M2 msg: 0.16 phi; -0.12 theta; 5.27 psi 2024-11-06T02:50:04Z- handleDepth()- Depth_Keller msg: 1730861404.93 epoch sec; seqNo:114; depth 7.34 2024-11-06T02:50:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861404.95 epoch sec; seqNo:114; 4061442.93 north; 594609.05 east; 36.69 lat; -121.94 lon; 7.34 depth; utmZone: 10 2024-11-06T02:50:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861401.71 2024-11-06T02:50:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:05Z- waiting for 1730861405.71, time is 1730861404.95 2024-11-06T02:50:05Z- handleDepth()- Depth_Keller msg: 1730861405.33 epoch sec; seqNo:115; depth 7.40 2024-11-06T02:50:05Z- handleDVL() - DVL msg epochMillisec: 1730861405.36 2024-11-06T02:50:05Z- handleDvl() - RDI_Pathfinder msg: 1730861405.36 sec; seqNo:33; #beams:4 2024-11-06T02:50:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.36 , 71.96 , 72.77 , 89.20 2024-11-06T02:50:05Z- handleDvl() - RDI_Pathfinder msg: velocities 1.08 , -0.10 , 0.09 2024-11-06T02:50:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861405.37 epoch sec; seqNo:115; 4061442.93 north; 594609.05 east; 36.69 lat; -121.94 lon; 7.40 depth; utmZone: 10 2024-11-06T02:50:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861401.71 2024-11-06T02:50:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:05Z- waiting for 1730861405.71, time is 1730861405.37 2024-11-06T02:50:05Z- AHRS_M2 msg: 1730861405.71 epoch sec; seqNo:106 2024-11-06T02:50:05Z- AHRS_M2 msg: 0.18 phi; -0.12 theta; 5.28 psi 2024-11-06T02:50:05Z- handleDepth()- Depth_Keller msg: 1730861405.72 epoch sec; seqNo:116; depth 7.44 2024-11-06T02:50:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861405.74 epoch sec; seqNo:116; 4061443.30 north; 594609.05 east; 36.69 lat; -121.94 lon; 7.44 depth; utmZone: 10 2024-11-06T02:50:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861401.71 2024-11-06T02:50:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:05Z- waiting for 1730861405.71, time is 1730861405.74 2024-11-06T02:50:05Z- LcmTrn::updateTrn() >>>> pose time 1730861405.74, meas time 1730861405.36 <<<< 2024-11-06T02:50:05Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861405.74 2024-11-06T02:50:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861405.36 sec, ping # 0; waiting for INS data more recent than 1730861401.71... 2024-11-06T02:50:05Z- TerrainNav::motionUpdate() - currEstimate = (4061443, 594609.05, 7.44) 2024-11-06T02:50:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:50:05Z- TerrainNav:: SubcloudNIS is 0.09, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:50:05Z- TNavPF::Projecting Measurements 2024-11-06T02:50:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:50:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:50:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:50:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:50:05Z- TNavFilter::4 good beams. 2024-11-06T02:50:05Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:50:05Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:50:05Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:50:05Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:50:05Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:50:05Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:50:05Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:50:05Z- TerrainMapOctree: (vehN, vehE ) = (4061443.09, 594609.40). 2024-11-06T02:50:05Z- TerrainMapOctree: Min Distance = 238.86. 2024-11-06T02:50:05Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:50:05Z- Forcing Subcloud Comparison 2024-11-06T02:50:05Z- Weighting particles with subcloud comparison 2024-11-06T02:50:06Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:06Z- meanExpectedMeasDiff: -0.845063 delta_rms_squared: 3.417927 alpha: 0.994183 2024-11-06T02:50:06Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:06Z- meanExpectedMeasDiff: 0.257342 delta_rms_squared: 1.626747 alpha: 0.987855 2024-11-06T02:50:06Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:06Z- meanExpectedMeasDiff: -3.416629 delta_rms_squared: 2.538804 alpha: 0.992184 2024-11-06T02:50:06Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:06Z- meanExpectedMeasDiff: -2.564271 delta_rms_squared: 3.184540 alpha: 0.993759 2024-11-06T02:50:06Z- Subcloud NIS: 0.101682 2024-11-06T02:50:06Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:50:06Z- TNavPF:: sumWeights = 0.388401 2024-11-06T02:50:06Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:50:06Z- TNavPF::First Term of Map Covariance Matrix is 0.87 2024-11-06T02:50:06Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T02:50:06Z- TerrainNav::Resampling particle filter... 2024-11-06T02:50:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861405.36 sec. 2024-11-06T02:50:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:50:06Z- variances([0], [2], and psi_berg) = 22.984164 44.344265 0.000000 2024-11-06T02:50:06Z- publishEstimatesDecoder() - at 1730861405.74 sec 2024-11-06T02:50:06Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:50:06Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061499.26 594613.66 15.50 2024-11-06T02:50:06Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061499.95 594583.95 15.31 lat/lon: 36.69 -121.94 2024-11-06T02:50:06Z- LcmTrn::publishEstimatesDecoder() - COVAR: 22.98 44.34 1.15 0.00 2024-11-06T02:50:06Z- AHRS_M2 msg: 1730861406.15 epoch sec; seqNo:107 2024-11-06T02:50:06Z- AHRS_M2 msg: 0.17 phi; -0.12 theta; 5.29 psi 2024-11-06T02:50:06Z- handleDepth()- Depth_Keller msg: 1730861406.17 epoch sec; seqNo:117; depth 7.48 2024-11-06T02:50:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861406.18 epoch sec; seqNo:117; 4061443.29 north; 594607.83 east; 36.69 lat; -121.94 lon; 7.48 depth; utmZone: 10 2024-11-06T02:50:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861405.74 2024-11-06T02:50:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:06Z- waiting for 1730861409.74, time is 1730861406.18 2024-11-06T02:50:06Z- handleDepth()- Depth_Keller msg: 1730861406.55 epoch sec; seqNo:118; depth 7.50 2024-11-06T02:50:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861406.56 epoch sec; seqNo:118; 4061443.67 north; 594607.82 east; 36.69 lat; -121.94 lon; 7.50 depth; utmZone: 10 2024-11-06T02:50:06Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:50:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861405.74 2024-11-06T02:50:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:06Z- waiting for 1730861409.74, time is 1730861406.56 2024-11-06T02:50:06Z- AHRS_M2 msg: 1730861406.92 epoch sec; seqNo:108 2024-11-06T02:50:06Z- AHRS_M2 msg: 0.16 phi; -0.11 theta; 5.29 psi 2024-11-06T02:50:06Z- handleDepth()- Depth_Keller msg: 1730861406.94 epoch sec; seqNo:119; depth 7.55 2024-11-06T02:50:06Z- handleDVL() - DVL msg epochMillisec: 1730861406.94 2024-11-06T02:50:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:06Z- handleDvl() - RDI_Pathfinder msg: 1730861406.94 sec; seqNo:34; #beams:0 2024-11-06T02:50:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:06Z- handleDvl() - RDI_Pathfinder msg: velocities 1.08 , -0.10 , 0.09 2024-11-06T02:50:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861406.95 epoch sec; seqNo:119; 4061443.67 north; 594607.82 east; 36.69 lat; -121.94 lon; 7.55 depth; utmZone: 10 2024-11-06T02:50:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861405.74 2024-11-06T02:50:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:07Z- waiting for 1730861409.74, time is 1730861406.95 2024-11-06T02:50:07Z- AHRS_M2 msg: 1730861407.33 epoch sec; seqNo:109 2024-11-06T02:50:07Z- AHRS_M2 msg: 0.15 phi; -0.11 theta; 5.29 psi 2024-11-06T02:50:07Z- handleDepth()- Depth_Keller msg: 1730861407.34 epoch sec; seqNo:120; depth 7.58 2024-11-06T02:50:07Z- handleDVL() - DVL msg epochMillisec: 1730861407.36 2024-11-06T02:50:07Z- handleDvl() - RDI_Pathfinder msg: 1730861407.36 sec; seqNo:35; #beams:4 2024-11-06T02:50:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.36 , 71.55 , 72.37 , 90.28 2024-11-06T02:50:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.02 , 0.11 2024-11-06T02:50:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861407.38 epoch sec; seqNo:120; 4061444.05 north; 594607.82 east; 36.69 lat; -121.94 lon; 7.58 depth; utmZone: 10 2024-11-06T02:50:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861405.74 2024-11-06T02:50:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:07Z- waiting for 1730861409.74, time is 1730861407.38 2024-11-06T02:50:07Z- AHRS_M2 msg: 1730861407.73 epoch sec; seqNo:110 2024-11-06T02:50:07Z- AHRS_M2 msg: 0.15 phi; -0.11 theta; 5.29 psi 2024-11-06T02:50:07Z- handleDepth()- Depth_Keller msg: 1730861407.74 epoch sec; seqNo:121; depth 7.62 2024-11-06T02:50:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861407.75 epoch sec; seqNo:121; 4061444.04 north; 594606.60 east; 36.69 lat; -121.94 lon; 7.62 depth; utmZone: 10 2024-11-06T02:50:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861405.74 2024-11-06T02:50:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:07Z- waiting for 1730861409.74, time is 1730861407.75 2024-11-06T02:50:08Z- AHRS_M2 msg: 1730861408.14 epoch sec; seqNo:111 2024-11-06T02:50:08Z- AHRS_M2 msg: 0.15 phi; -0.11 theta; 5.28 psi 2024-11-06T02:50:08Z- handleDepth()- Depth_Keller msg: 1730861408.14 epoch sec; seqNo:122; depth 7.64 2024-11-06T02:50:08Z- handleDVL() - DVL msg epochMillisec: 1730861408.14 2024-11-06T02:50:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:08Z- handleDvl() - RDI_Pathfinder msg: 1730861408.14 sec; seqNo:36; #beams:0 2024-11-06T02:50:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.02 , 0.11 2024-11-06T02:50:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861408.15 epoch sec; seqNo:122; 4061444.41 north; 594606.59 east; 36.69 lat; -121.94 lon; 7.64 depth; utmZone: 10 2024-11-06T02:50:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861405.74 2024-11-06T02:50:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:08Z- waiting for 1730861409.74, time is 1730861408.15 2024-11-06T02:50:08Z- AHRS_M2 msg: 1730861408.54 epoch sec; seqNo:112 2024-11-06T02:50:08Z- AHRS_M2 msg: 0.14 phi; -0.11 theta; 5.28 psi 2024-11-06T02:50:08Z- handleDepth()- Depth_Keller msg: 1730861408.55 epoch sec; seqNo:123; depth 7.70 2024-11-06T02:50:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861408.58 epoch sec; seqNo:123; 4061444.41 north; 594606.59 east; 36.69 lat; -121.94 lon; 7.70 depth; utmZone: 10 2024-11-06T02:50:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861405.74 2024-11-06T02:50:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:08Z- waiting for 1730861409.74, time is 1730861408.58 2024-11-06T02:50:08Z- AHRS_M2 msg: 1730861408.95 epoch sec; seqNo:113 2024-11-06T02:50:08Z- AHRS_M2 msg: 0.14 phi; -0.11 theta; 5.29 psi 2024-11-06T02:50:08Z- handleDepth()- Depth_Keller msg: 1730861408.95 epoch sec; seqNo:124; depth 7.74 2024-11-06T02:50:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861408.96 epoch sec; seqNo:124; 4061444.78 north; 594605.37 east; 36.69 lat; -121.94 lon; 7.74 depth; utmZone: 10 2024-11-06T02:50:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861405.74 2024-11-06T02:50:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:09Z- waiting for 1730861409.74, time is 1730861408.96 2024-11-06T02:50:09Z- AHRS_M2 msg: 1730861409.35 epoch sec; seqNo:114 2024-11-06T02:50:09Z- AHRS_M2 msg: 0.15 phi; -0.11 theta; 5.29 psi 2024-11-06T02:50:09Z- handleDepth()- Depth_Keller msg: 1730861409.35 epoch sec; seqNo:125; depth 7.78 2024-11-06T02:50:09Z- handleDVL() - DVL msg epochMillisec: 1730861409.38 2024-11-06T02:50:09Z- handleDvl() - RDI_Pathfinder msg: 1730861409.38 sec; seqNo:37; #beams:4 2024-11-06T02:50:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.28 , 71.95 , 72.36 , 89.19 2024-11-06T02:50:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.03 , 0.09 2024-11-06T02:50:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861409.39 epoch sec; seqNo:125; 4061444.78 north; 594605.37 east; 36.69 lat; -121.94 lon; 7.78 depth; utmZone: 10 2024-11-06T02:50:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861405.74 2024-11-06T02:50:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:09Z- waiting for 1730861409.74, time is 1730861409.39 2024-11-06T02:50:09Z- AHRS_M2 msg: 1730861409.75 epoch sec; seqNo:115 2024-11-06T02:50:09Z- AHRS_M2 msg: 0.14 phi; -0.11 theta; 5.29 psi 2024-11-06T02:50:09Z- handleDepth()- Depth_Keller msg: 1730861409.76 epoch sec; seqNo:126; depth 7.81 2024-11-06T02:50:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861409.77 epoch sec; seqNo:126; 4061445.16 north; 594605.36 east; 36.69 lat; -121.94 lon; 7.81 depth; utmZone: 10 2024-11-06T02:50:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861405.74 2024-11-06T02:50:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:09Z- waiting for 1730861409.74, time is 1730861409.77 2024-11-06T02:50:09Z- LcmTrn::updateTrn() >>>> pose time 1730861409.77, meas time 1730861409.38 <<<< 2024-11-06T02:50:09Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861409.77 2024-11-06T02:50:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861409.38 sec, ping # 0; waiting for INS data more recent than 1730861405.74... 2024-11-06T02:50:09Z- TerrainNav::motionUpdate() - currEstimate = (4061445, 594605.36, 7.81) 2024-11-06T02:50:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:50:09Z- TerrainNav:: SubcloudNIS is 0.10, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:50:09Z- TNavPF::Projecting Measurements 2024-11-06T02:50:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:50:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:50:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:50:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:50:09Z- TNavFilter::4 good beams. 2024-11-06T02:50:09Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:50:09Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:50:09Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:50:09Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:50:09Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:50:09Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:50:09Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:50:09Z- TerrainMapOctree: (vehN, vehE ) = (4061444.98, 594605.73). 2024-11-06T02:50:09Z- TerrainMapOctree: Min Distance = 242.55. 2024-11-06T02:50:09Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:50:10Z- Forcing Subcloud Comparison 2024-11-06T02:50:10Z- Weighting particles with subcloud comparison 2024-11-06T02:50:10Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:10Z- meanExpectedMeasDiff: 1.441570 delta_rms_squared: 2.510600 alpha: 0.992097 2024-11-06T02:50:10Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:10Z- meanExpectedMeasDiff: 0.439928 delta_rms_squared: 1.784633 alpha: 0.988917 2024-11-06T02:50:10Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:10Z- meanExpectedMeasDiff: -2.421777 delta_rms_squared: 2.223851 alpha: 0.991087 2024-11-06T02:50:10Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:10Z- meanExpectedMeasDiff: 0.943165 delta_rms_squared: 3.442262 alpha: 0.994223 2024-11-06T02:50:10Z- Subcloud NIS: 0.107459 2024-11-06T02:50:10Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:50:10Z- TNavPF:: sumWeights = 0.496152 2024-11-06T02:50:10Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:50:10Z- TNavPF::First Term of Map Covariance Matrix is 1.07 2024-11-06T02:50:10Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.05 2024-11-06T02:50:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861409.38 sec. 2024-11-06T02:50:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:50:10Z- variances([0], [2], and psi_berg) = 20.444354 40.379672 0.000000 2024-11-06T02:50:10Z- publishEstimatesDecoder() - at 1730861409.77 sec 2024-11-06T02:50:10Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:50:10Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061498.85 594589.60 16.20 2024-11-06T02:50:10Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061502.37 594580.63 15.62 lat/lon: 36.69 -121.94 2024-11-06T02:50:10Z- LcmTrn::publishEstimatesDecoder() - COVAR: 20.44 40.38 1.18 0.00 2024-11-06T02:50:10Z- AHRS_M2 msg: 1730861410.16 epoch sec; seqNo:116 2024-11-06T02:50:10Z- AHRS_M2 msg: 0.14 phi; -0.11 theta; 5.29 psi 2024-11-06T02:50:10Z- handleDepth()- Depth_Keller msg: 1730861410.16 epoch sec; seqNo:127; depth 7.85 2024-11-06T02:50:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861410.17 epoch sec; seqNo:127; 4061445.54 north; 594605.36 east; 36.69 lat; -121.94 lon; 7.85 depth; utmZone: 10 2024-11-06T02:50:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861409.77 2024-11-06T02:50:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:10Z- waiting for 1730861413.77, time is 1730861410.17 2024-11-06T02:50:10Z- AHRS_M2 msg: 1730861410.56 epoch sec; seqNo:117 2024-11-06T02:50:10Z- AHRS_M2 msg: 0.14 phi; -0.11 theta; 5.28 psi 2024-11-06T02:50:10Z- handleDepth()- Depth_Keller msg: 1730861410.57 epoch sec; seqNo:128; depth 7.89 2024-11-06T02:50:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861410.57 epoch sec; seqNo:128; 4061445.52 north; 594604.14 east; 36.69 lat; -121.94 lon; 7.89 depth; utmZone: 10 2024-11-06T02:50:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861409.77 2024-11-06T02:50:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:10Z- waiting for 1730861413.77, time is 1730861410.57 2024-11-06T02:50:10Z- AHRS_M2 msg: 1730861410.97 epoch sec; seqNo:118 2024-11-06T02:50:10Z- AHRS_M2 msg: 0.13 phi; -0.11 theta; 5.28 psi 2024-11-06T02:50:10Z- handleDepth()- Depth_Keller msg: 1730861410.98 epoch sec; seqNo:129; depth 7.91 2024-11-06T02:50:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861410.99 epoch sec; seqNo:129; 4061445.90 north; 594604.14 east; 36.69 lat; -121.94 lon; 7.91 depth; utmZone: 10 2024-11-06T02:50:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861409.77 2024-11-06T02:50:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:11Z- waiting for 1730861413.77, time is 1730861410.99 2024-11-06T02:50:11Z- AHRS_M2 msg: 1730861411.37 epoch sec; seqNo:119 2024-11-06T02:50:11Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 5.27 psi 2024-11-06T02:50:11Z- handleDepth()- Depth_Keller msg: 1730861411.37 epoch sec; seqNo:130; depth 7.97 2024-11-06T02:50:11Z- handleDVL() - DVL msg epochMillisec: 1730861411.39 2024-11-06T02:50:11Z- handleDvl() - RDI_Pathfinder msg: 1730861411.39 sec; seqNo:38; #beams:4 2024-11-06T02:50:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.20 , 71.55 , 71.96 , 89.20 2024-11-06T02:50:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.13 2024-11-06T02:50:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861411.41 epoch sec; seqNo:130; 4061445.90 north; 594604.14 east; 36.69 lat; -121.94 lon; 7.97 depth; utmZone: 10 2024-11-06T02:50:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861409.77 2024-11-06T02:50:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:11Z- waiting for 1730861413.77, time is 1730861411.41 2024-11-06T02:50:11Z- AHRS_M2 msg: 1730861411.80 epoch sec; seqNo:120 2024-11-06T02:50:11Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.27 psi 2024-11-06T02:50:11Z- handleDepth()- Depth_Keller msg: 1730861411.81 epoch sec; seqNo:131; depth 8.00 2024-11-06T02:50:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861411.82 epoch sec; seqNo:131; 4061446.28 north; 594604.13 east; 36.69 lat; -121.94 lon; 8.00 depth; utmZone: 10 2024-11-06T02:50:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861409.77 2024-11-06T02:50:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:11Z- waiting for 1730861413.77, time is 1730861411.82 2024-11-06T02:50:12Z- AHRS_M2 msg: 1730861412.18 epoch sec; seqNo:121 2024-11-06T02:50:12Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.27 psi 2024-11-06T02:50:12Z- handleDepth()- Depth_Keller msg: 1730861412.18 epoch sec; seqNo:132; depth 8.04 2024-11-06T02:50:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861412.19 epoch sec; seqNo:132; 4061446.27 north; 594602.91 east; 36.69 lat; -121.94 lon; 8.04 depth; utmZone: 10 2024-11-06T02:50:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861409.77 2024-11-06T02:50:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:12Z- waiting for 1730861413.77, time is 1730861412.19 2024-11-06T02:50:12Z- AHRS_M2 msg: 1730861412.58 epoch sec; seqNo:122 2024-11-06T02:50:12Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.28 psi 2024-11-06T02:50:12Z- handleDepth()- Depth_Keller msg: 1730861412.59 epoch sec; seqNo:133; depth 8.07 2024-11-06T02:50:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861412.60 epoch sec; seqNo:133; 4061446.27 north; 594602.91 east; 36.69 lat; -121.94 lon; 8.07 depth; utmZone: 10 2024-11-06T02:50:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861409.77 2024-11-06T02:50:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:12Z- waiting for 1730861413.77, time is 1730861412.60 2024-11-06T02:50:12Z- AHRS_M2 msg: 1730861412.98 epoch sec; seqNo:123 2024-11-06T02:50:12Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.29 psi 2024-11-06T02:50:12Z- handleDepth()- Depth_Keller msg: 1730861412.99 epoch sec; seqNo:134; depth 8.09 2024-11-06T02:50:12Z- handleDVL() - DVL msg epochMillisec: 1730861412.99 2024-11-06T02:50:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:12Z- handleDvl() - RDI_Pathfinder msg: 1730861412.99 sec; seqNo:39; #beams:0 2024-11-06T02:50:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.13 2024-11-06T02:50:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861413.01 epoch sec; seqNo:134; 4061446.65 north; 594602.91 east; 36.69 lat; -121.94 lon; 8.09 depth; utmZone: 10 2024-11-06T02:50:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861409.77 2024-11-06T02:50:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:13Z- waiting for 1730861413.77, time is 1730861413.01 2024-11-06T02:50:13Z- AHRS_M2 msg: 1730861413.39 epoch sec; seqNo:124 2024-11-06T02:50:13Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.29 psi 2024-11-06T02:50:13Z- handleDepth()- Depth_Keller msg: 1730861413.39 epoch sec; seqNo:135; depth 8.13 2024-11-06T02:50:13Z- handleDVL() - DVL msg epochMillisec: 1730861413.41 2024-11-06T02:50:13Z- handleDvl() - RDI_Pathfinder msg: 1730861413.41 sec; seqNo:40; #beams:4 2024-11-06T02:50:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.18 , 71.95 , 71.13 , 90.35 2024-11-06T02:50:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.00 , -0.06 2024-11-06T02:50:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861413.44 epoch sec; seqNo:135; 4061446.65 north; 594602.91 east; 36.69 lat; -121.94 lon; 8.13 depth; utmZone: 10 2024-11-06T02:50:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861409.77 2024-11-06T02:50:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:13Z- waiting for 1730861413.77, time is 1730861413.44 2024-11-06T02:50:13Z- AHRS_M2 msg: 1730861413.80 epoch sec; seqNo:125 2024-11-06T02:50:13Z- AHRS_M2 msg: 0.14 phi; -0.13 theta; 5.30 psi 2024-11-06T02:50:13Z- handleDepth()- Depth_Keller msg: 1730861413.81 epoch sec; seqNo:136; depth 8.17 2024-11-06T02:50:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861413.84 epoch sec; seqNo:136; 4061447.03 north; 594602.90 east; 36.69 lat; -121.94 lon; 8.17 depth; utmZone: 10 2024-11-06T02:50:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861409.77 2024-11-06T02:50:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:13Z- waiting for 1730861413.77, time is 1730861413.84 2024-11-06T02:50:13Z- LcmTrn::updateTrn() >>>> pose time 1730861413.84, meas time 1730861413.41 <<<< 2024-11-06T02:50:13Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861413.84 2024-11-06T02:50:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861413.41 sec, ping # 0; waiting for INS data more recent than 1730861409.77... 2024-11-06T02:50:13Z- TerrainNav::motionUpdate() - currEstimate = (4061447, 594602.90, 8.17) 2024-11-06T02:50:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:50:13Z- TerrainNav:: SubcloudNIS is 0.11, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:50:13Z- TNavPF::Projecting Measurements 2024-11-06T02:50:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:50:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:50:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:50:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:50:13Z- TNavFilter::4 good beams. 2024-11-06T02:50:13Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:50:13Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:50:13Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:50:13Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:50:13Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:50:13Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:50:13Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:50:13Z- TerrainMapOctree: (vehN, vehE ) = (4061446.83, 594603.16). 2024-11-06T02:50:13Z- TerrainMapOctree: Min Distance = 245.57. 2024-11-06T02:50:13Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:50:14Z- Forcing Subcloud Comparison 2024-11-06T02:50:14Z- Weighting particles with subcloud comparison 2024-11-06T02:50:14Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:14Z- meanExpectedMeasDiff: -0.271668 delta_rms_squared: 2.324026 alpha: 0.991468 2024-11-06T02:50:14Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:14Z- meanExpectedMeasDiff: 0.476069 delta_rms_squared: 1.730024 alpha: 0.988572 2024-11-06T02:50:14Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:14Z- meanExpectedMeasDiff: -2.481749 delta_rms_squared: 1.986258 alpha: 0.990031 2024-11-06T02:50:14Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:14Z- meanExpectedMeasDiff: 1.665888 delta_rms_squared: 3.525063 alpha: 0.994358 2024-11-06T02:50:14Z- Subcloud NIS: 0.119681 2024-11-06T02:50:14Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:50:14Z- TNavPF:: sumWeights = 0.491005 2024-11-06T02:50:14Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:50:14Z- TNavPF::First Term of Map Covariance Matrix is 0.73 2024-11-06T02:50:14Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T02:50:14Z- TerrainNav::Resampling particle filter... 2024-11-06T02:50:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861413.41 sec. 2024-11-06T02:50:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:50:14Z- variances([0], [2], and psi_berg) = 18.487563 35.398595 0.000000 2024-11-06T02:50:14Z- publishEstimatesDecoder() - at 1730861413.84 sec 2024-11-06T02:50:14Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:50:14Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061503.01 594607.55 17.06 2024-11-06T02:50:14Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061504.60 594578.42 16.02 lat/lon: 36.69 -121.94 2024-11-06T02:50:14Z- LcmTrn::publishEstimatesDecoder() - COVAR: 18.49 35.40 1.19 0.00 2024-11-06T02:50:14Z- AHRS_M2 msg: 1730861414.19 epoch sec; seqNo:126 2024-11-06T02:50:14Z- AHRS_M2 msg: 0.15 phi; -0.14 theta; 5.30 psi 2024-11-06T02:50:14Z- handleDepth()- Depth_Keller msg: 1730861414.21 epoch sec; seqNo:137; depth 8.20 2024-11-06T02:50:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861414.22 epoch sec; seqNo:137; 4061447.01 north; 594601.68 east; 36.69 lat; -121.94 lon; 8.20 depth; utmZone: 10 2024-11-06T02:50:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861413.84 2024-11-06T02:50:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:14Z- waiting for 1730861417.84, time is 1730861414.22 2024-11-06T02:50:14Z- AHRS_M2 msg: 1730861414.60 epoch sec; seqNo:127 2024-11-06T02:50:14Z- AHRS_M2 msg: 0.16 phi; -0.14 theta; 5.30 psi 2024-11-06T02:50:14Z- handleDepth()- Depth_Keller msg: 1730861414.60 epoch sec; seqNo:138; depth 8.25 2024-11-06T02:50:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861414.61 epoch sec; seqNo:138; 4061447.39 north; 594601.68 east; 36.69 lat; -121.94 lon; 8.25 depth; utmZone: 10 2024-11-06T02:50:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861413.84 2024-11-06T02:50:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:14Z- waiting for 1730861417.84, time is 1730861414.61 2024-11-06T02:50:15Z- AHRS_M2 msg: 1730861415.00 epoch sec; seqNo:128 2024-11-06T02:50:15Z- AHRS_M2 msg: 0.16 phi; -0.15 theta; 5.30 psi 2024-11-06T02:50:15Z- handleDepth()- Depth_Keller msg: 1730861415.01 epoch sec; seqNo:139; depth 8.29 2024-11-06T02:50:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861415.02 epoch sec; seqNo:139; 4061447.39 north; 594601.68 east; 36.69 lat; -121.94 lon; 8.29 depth; utmZone: 10 2024-11-06T02:50:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861413.84 2024-11-06T02:50:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:15Z- waiting for 1730861417.84, time is 1730861415.02 2024-11-06T02:50:15Z- handleDepth()- Depth_Keller msg: 1730861415.43 epoch sec; seqNo:140; depth 8.33 2024-11-06T02:50:15Z- handleDVL() - DVL msg epochMillisec: 1730861415.45 2024-11-06T02:50:15Z- handleDvl() - RDI_Pathfinder msg: 1730861415.45 sec; seqNo:41; #beams:4 2024-11-06T02:50:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 78.44 , 70.32 , 91.43 2024-11-06T02:50:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.07 , -0.09 2024-11-06T02:50:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861415.48 epoch sec; seqNo:140; 4061447.77 north; 594601.67 east; 36.69 lat; -121.94 lon; 8.33 depth; utmZone: 10 2024-11-06T02:50:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861413.84 2024-11-06T02:50:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:15Z- waiting for 1730861417.84, time is 1730861415.48 2024-11-06T02:50:15Z- AHRS_M2 msg: 1730861415.81 epoch sec; seqNo:129 2024-11-06T02:50:15Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.30 psi 2024-11-06T02:50:15Z- handleDepth()- Depth_Keller msg: 1730861415.84 epoch sec; seqNo:141; depth 8.38 2024-11-06T02:50:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861415.85 epoch sec; seqNo:141; 4061447.76 north; 594600.45 east; 36.69 lat; -121.94 lon; 8.38 depth; utmZone: 10 2024-11-06T02:50:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861413.84 2024-11-06T02:50:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:15Z- waiting for 1730861417.84, time is 1730861415.85 2024-11-06T02:50:16Z- AHRS_M2 msg: 1730861416.21 epoch sec; seqNo:130 2024-11-06T02:50:16Z- AHRS_M2 msg: 0.15 phi; -0.17 theta; 5.31 psi 2024-11-06T02:50:16Z- handleDepth()- Depth_Keller msg: 1730861416.22 epoch sec; seqNo:142; depth 8.42 2024-11-06T02:50:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861416.23 epoch sec; seqNo:142; 4061447.76 north; 594600.45 east; 36.69 lat; -121.94 lon; 8.42 depth; utmZone: 10 2024-11-06T02:50:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861413.84 2024-11-06T02:50:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:16Z- waiting for 1730861417.84, time is 1730861416.23 2024-11-06T02:50:16Z- AHRS_M2 msg: 1730861416.62 epoch sec; seqNo:131 2024-11-06T02:50:16Z- AHRS_M2 msg: 0.15 phi; -0.17 theta; 5.31 psi 2024-11-06T02:50:16Z- handleDepth()- Depth_Keller msg: 1730861416.63 epoch sec; seqNo:143; depth 8.46 2024-11-06T02:50:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861416.64 epoch sec; seqNo:143; 4061448.14 north; 594600.45 east; 36.69 lat; -121.94 lon; 8.46 depth; utmZone: 10 2024-11-06T02:50:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861413.84 2024-11-06T02:50:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:16Z- waiting for 1730861417.84, time is 1730861416.64 2024-11-06T02:50:17Z- AHRS_M2 msg: 1730861417.03 epoch sec; seqNo:132 2024-11-06T02:50:17Z- AHRS_M2 msg: 0.14 phi; -0.17 theta; 5.31 psi 2024-11-06T02:50:17Z- handleDepth()- Depth_Keller msg: 1730861417.03 epoch sec; seqNo:144; depth 8.51 2024-11-06T02:50:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861417.04 epoch sec; seqNo:144; 4061448.14 north; 594600.45 east; 36.69 lat; -121.94 lon; 8.51 depth; utmZone: 10 2024-11-06T02:50:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861413.84 2024-11-06T02:50:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:17Z- waiting for 1730861417.84, time is 1730861417.04 2024-11-06T02:50:17Z- AHRS_M2 msg: 1730861417.43 epoch sec; seqNo:133 2024-11-06T02:50:17Z- AHRS_M2 msg: 0.13 phi; -0.17 theta; 5.31 psi 2024-11-06T02:50:17Z- handleDepth()- Depth_Keller msg: 1730861417.43 epoch sec; seqNo:145; depth 8.56 2024-11-06T02:50:17Z- handleDVL() - DVL msg epochMillisec: 1730861417.46 2024-11-06T02:50:17Z- handleDvl() - RDI_Pathfinder msg: 1730861417.46 sec; seqNo:42; #beams:4 2024-11-06T02:50:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.35 , 71.13 , 69.50 , 92.51 2024-11-06T02:50:17Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.03 , -0.20 2024-11-06T02:50:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861417.47 epoch sec; seqNo:145; 4061448.50 north; 594599.23 east; 36.69 lat; -121.94 lon; 8.56 depth; utmZone: 10 2024-11-06T02:50:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861413.84 2024-11-06T02:50:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:17Z- waiting for 1730861417.84, time is 1730861417.47 2024-11-06T02:50:17Z- AHRS_M2 msg: 1730861417.83 epoch sec; seqNo:134 2024-11-06T02:50:17Z- AHRS_M2 msg: 0.13 phi; -0.16 theta; 5.30 psi 2024-11-06T02:50:17Z- handleDepth()- Depth_Keller msg: 1730861417.84 epoch sec; seqNo:146; depth 8.62 2024-11-06T02:50:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861417.87 epoch sec; seqNo:146; 4061448.88 north; 594599.22 east; 36.69 lat; -121.94 lon; 8.62 depth; utmZone: 10 2024-11-06T02:50:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861413.84 2024-11-06T02:50:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:17Z- waiting for 1730861417.84, time is 1730861417.87 2024-11-06T02:50:17Z- LcmTrn::updateTrn() >>>> pose time 1730861417.87, meas time 1730861417.46 <<<< 2024-11-06T02:50:17Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861417.87 2024-11-06T02:50:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861417.46 sec, ping # 0; waiting for INS data more recent than 1730861413.84... 2024-11-06T02:50:17Z- TerrainNav::motionUpdate() - currEstimate = (4061449, 594599.22, 8.62) 2024-11-06T02:50:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:50:17Z- TerrainNav:: SubcloudNIS is 0.12, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:50:18Z- TNavPF::Projecting Measurements 2024-11-06T02:50:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:50:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:50:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:50:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:50:18Z- TNavFilter::4 good beams. 2024-11-06T02:50:18Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:50:18Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:50:18Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:50:18Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:50:18Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:50:18Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:50:18Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:50:18Z- TerrainMapOctree: (vehN, vehE ) = (4061448.69, 594599.60). 2024-11-06T02:50:18Z- TerrainMapOctree: Min Distance = 249.19. 2024-11-06T02:50:18Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:50:18Z- Forcing Subcloud Comparison 2024-11-06T02:50:18Z- Weighting particles with subcloud comparison 2024-11-06T02:50:18Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:18Z- meanExpectedMeasDiff: 2.651562 delta_rms_squared: 2.062896 alpha: 0.990398 2024-11-06T02:50:18Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:18Z- meanExpectedMeasDiff: -0.615550 delta_rms_squared: 1.764419 alpha: 0.988792 2024-11-06T02:50:18Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:18Z- meanExpectedMeasDiff: -2.424954 delta_rms_squared: 1.826015 alpha: 0.989166 2024-11-06T02:50:18Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:18Z- meanExpectedMeasDiff: 2.882086 delta_rms_squared: 3.141258 alpha: 0.993673 2024-11-06T02:50:18Z- Subcloud NIS: 0.137514 2024-11-06T02:50:18Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:50:18Z- TNavPF:: sumWeights = 0.339314 2024-11-06T02:50:18Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:50:18Z- TNavPF::First Term of Map Covariance Matrix is 1.30 2024-11-06T02:50:18Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T02:50:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861417.46 sec. 2024-11-06T02:50:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:50:18Z- variances([0], [2], and psi_berg) = 18.033445 39.671858 0.000000 2024-11-06T02:50:18Z- publishEstimatesDecoder() - at 1730861417.87 sec 2024-11-06T02:50:18Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:50:18Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061495.04 594599.12 17.29 2024-11-06T02:50:18Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061506.81 594575.37 16.33 lat/lon: 36.69 -121.94 2024-11-06T02:50:18Z- LcmTrn::publishEstimatesDecoder() - COVAR: 18.03 39.67 1.20 0.00 2024-11-06T02:50:18Z- AHRS_M2 msg: 1730861418.23 epoch sec; seqNo:135 2024-11-06T02:50:18Z- AHRS_M2 msg: 0.12 phi; -0.16 theta; 5.29 psi 2024-11-06T02:50:18Z- handleDepth()- Depth_Keller msg: 1730861418.25 epoch sec; seqNo:147; depth 8.67 2024-11-06T02:50:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861418.26 epoch sec; seqNo:147; 4061448.88 north; 594599.22 east; 36.69 lat; -121.94 lon; 8.67 depth; utmZone: 10 2024-11-06T02:50:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861417.87 2024-11-06T02:50:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:18Z- waiting for 1730861421.87, time is 1730861418.26 2024-11-06T02:50:18Z- AHRS_M2 msg: 1730861418.64 epoch sec; seqNo:136 2024-11-06T02:50:18Z- AHRS_M2 msg: 0.12 phi; -0.16 theta; 5.29 psi 2024-11-06T02:50:18Z- handleDepth()- Depth_Keller msg: 1730861418.65 epoch sec; seqNo:148; depth 8.75 2024-11-06T02:50:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861418.67 epoch sec; seqNo:148; 4061449.26 north; 594599.22 east; 36.69 lat; -121.94 lon; 8.75 depth; utmZone: 10 2024-11-06T02:50:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861417.87 2024-11-06T02:50:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:18Z- waiting for 1730861421.87, time is 1730861418.67 2024-11-06T02:50:19Z- AHRS_M2 msg: 1730861419.04 epoch sec; seqNo:137 2024-11-06T02:50:19Z- AHRS_M2 msg: 0.12 phi; -0.16 theta; 5.28 psi 2024-11-06T02:50:19Z- handleDepth()- Depth_Keller msg: 1730861419.05 epoch sec; seqNo:149; depth 8.81 2024-11-06T02:50:19Z- handleDVL() - DVL msg epochMillisec: 1730861419.05 2024-11-06T02:50:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:19Z- handleDvl() - RDI_Pathfinder msg: 1730861419.05 sec; seqNo:43; #beams:0 2024-11-06T02:50:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:19Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.03 , -0.20 2024-11-06T02:50:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861419.06 epoch sec; seqNo:149; 4061449.62 north; 594597.99 east; 36.69 lat; -121.94 lon; 8.81 depth; utmZone: 10 2024-11-06T02:50:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861417.87 2024-11-06T02:50:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:19Z- waiting for 1730861421.87, time is 1730861419.06 2024-11-06T02:50:19Z- AHRS_M2 msg: 1730861419.45 epoch sec; seqNo:138 2024-11-06T02:50:19Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.27 psi 2024-11-06T02:50:19Z- handleDepth()- Depth_Keller msg: 1730861419.46 epoch sec; seqNo:150; depth 8.88 2024-11-06T02:50:19Z- handleDVL() - DVL msg epochMillisec: 1730861419.49 2024-11-06T02:50:19Z- handleDvl() - RDI_Pathfinder msg: 1730861419.49 sec; seqNo:44; #beams:4 2024-11-06T02:50:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.12 , 70.73 , 68.28 , 91.36 2024-11-06T02:50:19Z- handleDvl() - RDI_Pathfinder msg: velocities 1.14 , -0.01 , 0.11 2024-11-06T02:50:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861419.51 epoch sec; seqNo:150; 4061449.62 north; 594597.99 east; 36.69 lat; -121.94 lon; 8.88 depth; utmZone: 10 2024-11-06T02:50:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861417.87 2024-11-06T02:50:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:19Z- waiting for 1730861421.87, time is 1730861419.51 2024-11-06T02:50:19Z- AHRS_M2 msg: 1730861419.85 epoch sec; seqNo:139 2024-11-06T02:50:19Z- AHRS_M2 msg: 0.10 phi; -0.17 theta; 5.26 psi 2024-11-06T02:50:19Z- handleDepth()- Depth_Keller msg: 1730861419.86 epoch sec; seqNo:151; depth 8.97 2024-11-06T02:50:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861419.88 epoch sec; seqNo:151; 4061450.00 north; 594597.99 east; 36.69 lat; -121.94 lon; 8.97 depth; utmZone: 10 2024-11-06T02:50:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861417.87 2024-11-06T02:50:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:20Z- waiting for 1730861421.87, time is 1730861419.88 2024-11-06T02:50:20Z- AHRS_M2 msg: 1730861420.26 epoch sec; seqNo:140 2024-11-06T02:50:20Z- AHRS_M2 msg: 0.10 phi; -0.18 theta; 5.25 psi 2024-11-06T02:50:20Z- handleDepth()- Depth_Keller msg: 1730861420.26 epoch sec; seqNo:152; depth 9.02 2024-11-06T02:50:20Z- handleDVL() - DVL msg epochMillisec: 1730861420.26 2024-11-06T02:50:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:20Z- handleDvl() - RDI_Pathfinder msg: 1730861420.26 sec; seqNo:45; #beams:0 2024-11-06T02:50:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:20Z- handleDvl() - RDI_Pathfinder msg: velocities 1.14 , -0.01 , 0.11 2024-11-06T02:50:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861420.28 epoch sec; seqNo:152; 4061449.99 north; 594596.77 east; 36.69 lat; -121.94 lon; 9.02 depth; utmZone: 10 2024-11-06T02:50:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861417.87 2024-11-06T02:50:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:20Z- waiting for 1730861421.87, time is 1730861420.28 2024-11-06T02:50:20Z- handleDepth()- Depth_Keller msg: 1730861420.69 epoch sec; seqNo:153; depth 9.08 2024-11-06T02:50:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861420.69 epoch sec; seqNo:153; 4061450.37 north; 594596.76 east; 36.69 lat; -121.94 lon; 9.08 depth; utmZone: 10 2024-11-06T02:50:20Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:50:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861417.87 2024-11-06T02:50:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:20Z- waiting for 1730861421.87, time is 1730861420.69 2024-11-06T02:50:21Z- AHRS_M2 msg: 1730861421.06 epoch sec; seqNo:141 2024-11-06T02:50:21Z- AHRS_M2 msg: 0.10 phi; -0.18 theta; 5.25 psi 2024-11-06T02:50:21Z- handleDepth()- Depth_Keller msg: 1730861421.07 epoch sec; seqNo:154; depth 9.16 2024-11-06T02:50:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861421.08 epoch sec; seqNo:154; 4061450.75 north; 594596.76 east; 36.69 lat; -121.94 lon; 9.16 depth; utmZone: 10 2024-11-06T02:50:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861417.87 2024-11-06T02:50:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:21Z- waiting for 1730861421.87, time is 1730861421.08 2024-11-06T02:50:21Z- AHRS_M2 msg: 1730861421.47 epoch sec; seqNo:142 2024-11-06T02:50:21Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.26 psi 2024-11-06T02:50:21Z- handleDepth()- Depth_Keller msg: 1730861421.47 epoch sec; seqNo:155; depth 9.22 2024-11-06T02:50:21Z- handleDVL() - DVL msg epochMillisec: 1730861421.49 2024-11-06T02:50:21Z- handleDvl() - RDI_Pathfinder msg: 1730861421.49 sec; seqNo:46; #beams:4 2024-11-06T02:50:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.17 , 71.13 , 68.27 , 91.50 2024-11-06T02:50:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , 0.16 2024-11-06T02:50:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861421.51 epoch sec; seqNo:155; 4061450.73 north; 594595.54 east; 36.69 lat; -121.94 lon; 9.22 depth; utmZone: 10 2024-11-06T02:50:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861417.87 2024-11-06T02:50:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:21Z- waiting for 1730861421.87, time is 1730861421.51 2024-11-06T02:50:21Z- AHRS_M2 msg: 1730861421.87 epoch sec; seqNo:143 2024-11-06T02:50:21Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.26 psi 2024-11-06T02:50:21Z- handleDepth()- Depth_Keller msg: 1730861421.88 epoch sec; seqNo:156; depth 9.29 2024-11-06T02:50:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861421.90 epoch sec; seqNo:156; 4061450.73 north; 594595.54 east; 36.69 lat; -121.94 lon; 9.29 depth; utmZone: 10 2024-11-06T02:50:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861417.87 2024-11-06T02:50:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:22Z- waiting for 1730861421.87, time is 1730861421.90 2024-11-06T02:50:22Z- LcmTrn::updateTrn() >>>> pose time 1730861421.90, meas time 1730861421.49 <<<< 2024-11-06T02:50:22Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861421.90 2024-11-06T02:50:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861421.49 sec, ping # 0; waiting for INS data more recent than 1730861417.87... 2024-11-06T02:50:22Z- TerrainNav::motionUpdate() - currEstimate = (4061451, 594595.54, 9.29) 2024-11-06T02:50:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:50:22Z- TerrainNav:: SubcloudNIS is 0.14, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:50:22Z- TNavPF::Projecting Measurements 2024-11-06T02:50:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:50:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:50:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:50:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:50:22Z- TNavFilter::4 good beams. 2024-11-06T02:50:22Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:50:22Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:50:22Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:50:22Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:50:22Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:50:22Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:50:22Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:50:22Z- TerrainMapOctree: (vehN, vehE ) = (4061450.55, 594595.91). 2024-11-06T02:50:22Z- TerrainMapOctree: Min Distance = 252.91. 2024-11-06T02:50:22Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:50:22Z- Forcing Subcloud Comparison 2024-11-06T02:50:22Z- Weighting particles with subcloud comparison 2024-11-06T02:50:22Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:22Z- meanExpectedMeasDiff: 0.065469 delta_rms_squared: 2.362202 alpha: 0.991604 2024-11-06T02:50:22Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:22Z- meanExpectedMeasDiff: -0.757774 delta_rms_squared: 1.846794 alpha: 0.989286 2024-11-06T02:50:22Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:22Z- meanExpectedMeasDiff: -2.151093 delta_rms_squared: 1.764208 alpha: 0.988791 2024-11-06T02:50:22Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:22Z- meanExpectedMeasDiff: 0.992825 delta_rms_squared: 3.123440 alpha: 0.993638 2024-11-06T02:50:22Z- Subcloud NIS: 0.148423 2024-11-06T02:50:22Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:50:22Z- TNavPF:: sumWeights = 0.563111 2024-11-06T02:50:22Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:50:22Z- TNavPF::First Term of Map Covariance Matrix is 0.87 2024-11-06T02:50:22Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.05 2024-11-06T02:50:22Z- TerrainNav::Resampling particle filter... 2024-11-06T02:50:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861421.49 sec. 2024-11-06T02:50:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:50:22Z- variances([0], [2], and psi_berg) = 15.131036 42.584015 0.000000 2024-11-06T02:50:22Z- publishEstimatesDecoder() - at 1730861421.90 sec 2024-11-06T02:50:22Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:50:22Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061506.65 594600.34 17.02 2024-11-06T02:50:22Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061508.97 594572.00 17.17 lat/lon: 36.69 -121.94 2024-11-06T02:50:22Z- LcmTrn::publishEstimatesDecoder() - COVAR: 15.13 42.58 1.15 0.00 2024-11-06T02:50:22Z- AHRS_M2 msg: 1730861422.30 epoch sec; seqNo:144 2024-11-06T02:50:22Z- AHRS_M2 msg: 0.13 phi; -0.19 theta; 5.26 psi 2024-11-06T02:50:22Z- handleDepth()- Depth_Keller msg: 1730861422.31 epoch sec; seqNo:157; depth 9.34 2024-11-06T02:50:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861422.33 epoch sec; seqNo:157; 4061451.11 north; 594595.53 east; 36.69 lat; -121.94 lon; 9.34 depth; utmZone: 10 2024-11-06T02:50:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861421.90 2024-11-06T02:50:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:22Z- waiting for 1730861425.90, time is 1730861422.33 2024-11-06T02:50:22Z- AHRS_M2 msg: 1730861422.68 epoch sec; seqNo:145 2024-11-06T02:50:22Z- AHRS_M2 msg: 0.14 phi; -0.20 theta; 5.27 psi 2024-11-06T02:50:22Z- handleDepth()- Depth_Keller msg: 1730861422.69 epoch sec; seqNo:158; depth 9.40 2024-11-06T02:50:22Z- handleDVL() - DVL msg epochMillisec: 1730861422.69 2024-11-06T02:50:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:22Z- handleDvl() - RDI_Pathfinder msg: 1730861422.69 sec; seqNo:47; #beams:0 2024-11-06T02:50:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , 0.16 2024-11-06T02:50:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861422.70 epoch sec; seqNo:158; 4061451.11 north; 594595.53 east; 36.69 lat; -121.94 lon; 9.40 depth; utmZone: 10 2024-11-06T02:50:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861421.90 2024-11-06T02:50:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:22Z- waiting for 1730861425.90, time is 1730861422.70 2024-11-06T02:50:23Z- AHRS_M2 msg: 1730861423.09 epoch sec; seqNo:146 2024-11-06T02:50:23Z- AHRS_M2 msg: 0.14 phi; -0.19 theta; 5.27 psi 2024-11-06T02:50:23Z- handleDepth()- Depth_Keller msg: 1730861423.09 epoch sec; seqNo:159; depth 9.45 2024-11-06T02:50:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861423.10 epoch sec; seqNo:159; 4061451.11 north; 594595.53 east; 36.69 lat; -121.94 lon; 9.45 depth; utmZone: 10 2024-11-06T02:50:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861421.90 2024-11-06T02:50:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:23Z- waiting for 1730861425.90, time is 1730861423.10 2024-11-06T02:50:23Z- AHRS_M2 msg: 1730861423.49 epoch sec; seqNo:147 2024-11-06T02:50:23Z- AHRS_M2 msg: 0.14 phi; -0.19 theta; 5.27 psi 2024-11-06T02:50:23Z- handleDepth()- Depth_Keller msg: 1730861423.49 epoch sec; seqNo:160; depth 9.51 2024-11-06T02:50:23Z- handleDVL() - DVL msg epochMillisec: 1730861423.52 2024-11-06T02:50:23Z- handleDvl() - RDI_Pathfinder msg: 1730861423.52 sec; seqNo:48; #beams:4 2024-11-06T02:50:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.33 , 70.72 , 67.45 , 92.58 2024-11-06T02:50:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.13 2024-11-06T02:50:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861423.54 epoch sec; seqNo:160; 4061451.48 north; 594594.31 east; 36.69 lat; -121.94 lon; 9.51 depth; utmZone: 10 2024-11-06T02:50:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861421.90 2024-11-06T02:50:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:23Z- waiting for 1730861425.90, time is 1730861423.54 2024-11-06T02:50:23Z- AHRS_M2 msg: 1730861423.89 epoch sec; seqNo:148 2024-11-06T02:50:23Z- AHRS_M2 msg: 0.14 phi; -0.18 theta; 5.27 psi 2024-11-06T02:50:23Z- handleDepth()- Depth_Keller msg: 1730861423.90 epoch sec; seqNo:161; depth 9.57 2024-11-06T02:50:23Z- handleDVL() - DVL msg epochMillisec: 1730861423.90 2024-11-06T02:50:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:23Z- handleDvl() - RDI_Pathfinder msg: 1730861423.90 sec; seqNo:49; #beams:0 2024-11-06T02:50:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.13 2024-11-06T02:50:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861423.93 epoch sec; seqNo:161; 4061451.86 north; 594594.31 east; 36.69 lat; -121.94 lon; 9.57 depth; utmZone: 10 2024-11-06T02:50:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861421.90 2024-11-06T02:50:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:24Z- waiting for 1730861425.90, time is 1730861423.93 2024-11-06T02:50:24Z- AHRS_M2 msg: 1730861424.32 epoch sec; seqNo:149 2024-11-06T02:50:24Z- AHRS_M2 msg: 0.14 phi; -0.16 theta; 5.27 psi 2024-11-06T02:50:24Z- handleDepth()- Depth_Keller msg: 1730861424.33 epoch sec; seqNo:162; depth 9.63 2024-11-06T02:50:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861424.36 epoch sec; seqNo:162; 4061451.86 north; 594594.31 east; 36.69 lat; -121.94 lon; 9.63 depth; utmZone: 10 2024-11-06T02:50:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861421.90 2024-11-06T02:50:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:24Z- waiting for 1730861425.90, time is 1730861424.36 2024-11-06T02:50:24Z- AHRS_M2 msg: 1730861424.70 epoch sec; seqNo:150 2024-11-06T02:50:24Z- AHRS_M2 msg: 0.13 phi; -0.15 theta; 5.27 psi 2024-11-06T02:50:24Z- handleDepth()- Depth_Keller msg: 1730861424.71 epoch sec; seqNo:163; depth 9.70 2024-11-06T02:50:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861424.72 epoch sec; seqNo:163; 4061452.24 north; 594594.30 east; 36.69 lat; -121.94 lon; 9.70 depth; utmZone: 10 2024-11-06T02:50:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861421.90 2024-11-06T02:50:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:24Z- waiting for 1730861425.90, time is 1730861424.72 2024-11-06T02:50:25Z- AHRS_M2 msg: 1730861425.11 epoch sec; seqNo:151 2024-11-06T02:50:25Z- AHRS_M2 msg: 0.13 phi; -0.15 theta; 5.26 psi 2024-11-06T02:50:25Z- handleDepth()- Depth_Keller msg: 1730861425.12 epoch sec; seqNo:164; depth 9.76 2024-11-06T02:50:25Z- handleDVL() - DVL msg epochMillisec: 1730861425.12 2024-11-06T02:50:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:25Z- handleDvl() - RDI_Pathfinder msg: 1730861425.12 sec; seqNo:50; #beams:0 2024-11-06T02:50:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.13 2024-11-06T02:50:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861425.14 epoch sec; seqNo:164; 4061452.22 north; 594593.08 east; 36.69 lat; -121.94 lon; 9.76 depth; utmZone: 10 2024-11-06T02:50:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861421.90 2024-11-06T02:50:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:25Z- waiting for 1730861425.90, time is 1730861425.14 2024-11-06T02:50:25Z- AHRS_M2 msg: 1730861425.51 epoch sec; seqNo:152 2024-11-06T02:50:25Z- AHRS_M2 msg: 0.13 phi; -0.15 theta; 5.25 psi 2024-11-06T02:50:25Z- handleDepth()- Depth_Keller msg: 1730861425.52 epoch sec; seqNo:165; depth 9.82 2024-11-06T02:50:25Z- handleDVL() - DVL msg epochMillisec: 1730861425.56 2024-11-06T02:50:25Z- handleDvl() - RDI_Pathfinder msg: 1730861425.56 sec; seqNo:51; #beams:4 2024-11-06T02:50:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.17 , 68.68 , 68.68 , 89.33 2024-11-06T02:50:25Z- handleDvl() - RDI_Pathfinder msg: velocities 1.11 , -0.06 , 0.10 2024-11-06T02:50:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861425.57 epoch sec; seqNo:165; 4061452.60 north; 594593.08 east; 36.69 lat; -121.94 lon; 9.82 depth; utmZone: 10 2024-11-06T02:50:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861421.90 2024-11-06T02:50:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:25Z- waiting for 1730861425.90, time is 1730861425.57 2024-11-06T02:50:25Z- handleDepth()- Depth_Keller msg: 1730861425.93 epoch sec; seqNo:166; depth 9.90 2024-11-06T02:50:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861425.95 epoch sec; seqNo:166; 4061452.60 north; 594593.08 east; 36.69 lat; -121.94 lon; 9.90 depth; utmZone: 10 2024-11-06T02:50:26Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:50:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861421.90 2024-11-06T02:50:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:26Z- waiting for 1730861425.90, time is 1730861425.95 2024-11-06T02:50:26Z- LcmTrn::updateTrn() >>>> pose time 1730861425.95, meas time 1730861425.56 <<<< 2024-11-06T02:50:26Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861425.95 2024-11-06T02:50:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861425.56 sec, ping # 0; waiting for INS data more recent than 1730861421.90... 2024-11-06T02:50:26Z- TerrainNav::motionUpdate() - currEstimate = (4061453, 594593.08, 9.90) 2024-11-06T02:50:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:50:26Z- TerrainNav:: SubcloudNIS is 0.15, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:50:26Z- TNavPF::Projecting Measurements 2024-11-06T02:50:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:50:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:50:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:50:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:50:26Z- TNavFilter::4 good beams. 2024-11-06T02:50:26Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:50:26Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:50:26Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:50:26Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:50:26Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:50:26Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:50:26Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:50:26Z- TerrainMapOctree: (vehN, vehE ) = (4061452.42, 594593.32). 2024-11-06T02:50:26Z- TerrainMapOctree: Min Distance = 255.97. 2024-11-06T02:50:26Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:50:26Z- Forcing Subcloud Comparison 2024-11-06T02:50:26Z- Weighting particles with subcloud comparison 2024-11-06T02:50:26Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:26Z- meanExpectedMeasDiff: 0.823178 delta_rms_squared: 2.620643 alpha: 0.992426 2024-11-06T02:50:26Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:26Z- meanExpectedMeasDiff: -2.520813 delta_rms_squared: 2.081693 alpha: 0.990484 2024-11-06T02:50:26Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:26Z- meanExpectedMeasDiff: -1.659754 delta_rms_squared: 1.710227 alpha: 0.988441 2024-11-06T02:50:26Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:26Z- meanExpectedMeasDiff: 3.141486 delta_rms_squared: 2.736099 alpha: 0.992743 2024-11-06T02:50:26Z- Subcloud NIS: 0.166370 2024-11-06T02:50:26Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:50:26Z- TNavPF:: sumWeights = 0.322296 2024-11-06T02:50:26Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:50:26Z- TNavPF::First Term of Map Covariance Matrix is 0.69 2024-11-06T02:50:26Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T02:50:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861425.56 sec. 2024-11-06T02:50:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:50:26Z- variances([0], [2], and psi_berg) = 17.525857 65.454464 0.000000 2024-11-06T02:50:26Z- publishEstimatesDecoder() - at 1730861425.95 sec 2024-11-06T02:50:26Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:50:26Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061503.20 594592.33 18.33 2024-11-06T02:50:26Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061510.58 594571.03 17.89 lat/lon: 36.69 -121.94 2024-11-06T02:50:26Z- LcmTrn::publishEstimatesDecoder() - COVAR: 17.53 65.45 1.11 0.00 2024-11-06T02:50:26Z- AHRS_M2 msg: 1730861426.31 epoch sec; seqNo:153 2024-11-06T02:50:26Z- AHRS_M2 msg: 0.14 phi; -0.17 theta; 5.25 psi 2024-11-06T02:50:26Z- handleDepth()- Depth_Keller msg: 1730861426.37 epoch sec; seqNo:167; depth 9.95 2024-11-06T02:50:26Z- handleDVL() - DVL msg epochMillisec: 1730861426.37 2024-11-06T02:50:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:26Z- handleDvl() - RDI_Pathfinder msg: 1730861426.37 sec; seqNo:52; #beams:0 2024-11-06T02:50:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:26Z- handleDvl() - RDI_Pathfinder msg: velocities 1.11 , -0.06 , 0.10 2024-11-06T02:50:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861426.38 epoch sec; seqNo:167; 4061452.97 north; 594591.85 east; 36.69 lat; -121.94 lon; 9.95 depth; utmZone: 10 2024-11-06T02:50:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861425.95 2024-11-06T02:50:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:26Z- waiting for 1730861429.95, time is 1730861426.38 2024-11-06T02:50:26Z- AHRS_M2 msg: 1730861426.72 epoch sec; seqNo:154 2024-11-06T02:50:26Z- AHRS_M2 msg: 0.14 phi; -0.18 theta; 5.25 psi 2024-11-06T02:50:26Z- handleDepth()- Depth_Keller msg: 1730861426.73 epoch sec; seqNo:168; depth 10.02 2024-11-06T02:50:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861426.74 epoch sec; seqNo:168; 4061452.97 north; 594591.85 east; 36.69 lat; -121.94 lon; 10.02 depth; utmZone: 10 2024-11-06T02:50:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861425.95 2024-11-06T02:50:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:26Z- waiting for 1730861429.95, time is 1730861426.74 2024-11-06T02:50:27Z- AHRS_M2 msg: 1730861427.12 epoch sec; seqNo:155 2024-11-06T02:50:27Z- AHRS_M2 msg: 0.15 phi; -0.19 theta; 5.25 psi 2024-11-06T02:50:27Z- handleDepth()- Depth_Keller msg: 1730861427.13 epoch sec; seqNo:169; depth 10.08 2024-11-06T02:50:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861427.15 epoch sec; seqNo:169; 4061453.34 north; 594591.85 east; 36.69 lat; -121.94 lon; 10.08 depth; utmZone: 10 2024-11-06T02:50:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861425.95 2024-11-06T02:50:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:27Z- waiting for 1730861429.95, time is 1730861427.15 2024-11-06T02:50:27Z- AHRS_M2 msg: 1730861427.53 epoch sec; seqNo:156 2024-11-06T02:50:27Z- AHRS_M2 msg: 0.16 phi; -0.19 theta; 5.26 psi 2024-11-06T02:50:27Z- handleDepth()- Depth_Keller msg: 1730861427.53 epoch sec; seqNo:170; depth 10.11 2024-11-06T02:50:27Z- handleDVL() - DVL msg epochMillisec: 1730861427.56 2024-11-06T02:50:27Z- handleDvl() - RDI_Pathfinder msg: 1730861427.56 sec; seqNo:53; #beams:4 2024-11-06T02:50:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.17 , 69.50 , 68.68 , 90.41 2024-11-06T02:50:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.10 , 0.17 2024-11-06T02:50:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861427.59 epoch sec; seqNo:170; 4061453.33 north; 594590.63 east; 36.69 lat; -121.94 lon; 10.11 depth; utmZone: 10 2024-11-06T02:50:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861425.95 2024-11-06T02:50:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:27Z- waiting for 1730861429.95, time is 1730861427.59 2024-11-06T02:50:27Z- AHRS_M2 msg: 1730861427.95 epoch sec; seqNo:157 2024-11-06T02:50:27Z- AHRS_M2 msg: 0.17 phi; -0.19 theta; 5.27 psi 2024-11-06T02:50:27Z- handleDepth()- Depth_Keller msg: 1730861427.96 epoch sec; seqNo:171; depth 10.19 2024-11-06T02:50:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861427.97 epoch sec; seqNo:171; 4061453.33 north; 594590.63 east; 36.69 lat; -121.94 lon; 10.19 depth; utmZone: 10 2024-11-06T02:50:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861425.95 2024-11-06T02:50:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:28Z- waiting for 1730861429.95, time is 1730861427.97 2024-11-06T02:50:28Z- AHRS_M2 msg: 1730861428.35 epoch sec; seqNo:158 2024-11-06T02:50:28Z- AHRS_M2 msg: 0.17 phi; -0.19 theta; 5.28 psi 2024-11-06T02:50:28Z- handleDepth()- Depth_Keller msg: 1730861428.36 epoch sec; seqNo:172; depth 10.22 2024-11-06T02:50:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861428.37 epoch sec; seqNo:172; 4061453.71 north; 594590.62 east; 36.69 lat; -121.94 lon; 10.22 depth; utmZone: 10 2024-11-06T02:50:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861425.95 2024-11-06T02:50:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:28Z- waiting for 1730861429.95, time is 1730861428.37 2024-11-06T02:50:28Z- AHRS_M2 msg: 1730861428.76 epoch sec; seqNo:159 2024-11-06T02:50:28Z- AHRS_M2 msg: 0.17 phi; -0.19 theta; 5.28 psi 2024-11-06T02:50:28Z- handleDepth()- Depth_Keller msg: 1730861428.76 epoch sec; seqNo:173; depth 10.27 2024-11-06T02:50:28Z- handleDVL() - DVL msg epochMillisec: 1730861428.76 2024-11-06T02:50:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:28Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:28Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:28Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:28Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:28Z- handleDvl() - RDI_Pathfinder msg: 1730861428.76 sec; seqNo:54; #beams:0 2024-11-06T02:50:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.10 , 0.17 2024-11-06T02:50:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861428.79 epoch sec; seqNo:173; 4061453.71 north; 594590.62 east; 36.69 lat; -121.94 lon; 10.27 depth; utmZone: 10 2024-11-06T02:50:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861425.95 2024-11-06T02:50:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:28Z- waiting for 1730861429.95, time is 1730861428.79 2024-11-06T02:50:29Z- handleDepth()- Depth_Keller msg: 1730861429.18 epoch sec; seqNo:174; depth 10.34 2024-11-06T02:50:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861429.19 epoch sec; seqNo:174; 4061453.71 north; 594590.62 east; 36.69 lat; -121.94 lon; 10.34 depth; utmZone: 10 2024-11-06T02:50:29Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:50:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861425.95 2024-11-06T02:50:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:29Z- waiting for 1730861429.95, time is 1730861429.19 2024-11-06T02:50:29Z- AHRS_M2 msg: 1730861429.57 epoch sec; seqNo:160 2024-11-06T02:50:29Z- AHRS_M2 msg: 0.17 phi; -0.17 theta; 5.30 psi 2024-11-06T02:50:29Z- handleDepth()- Depth_Keller msg: 1730861429.58 epoch sec; seqNo:175; depth 10.38 2024-11-06T02:50:29Z- handleDVL() - DVL msg epochMillisec: 1730861429.60 2024-11-06T02:50:29Z- handleDvl() - RDI_Pathfinder msg: 1730861429.60 sec; seqNo:55; #beams:4 2024-11-06T02:50:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 68.68 , 67.86 , 91.50 2024-11-06T02:50:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.01 , -0.16 2024-11-06T02:50:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861429.65 epoch sec; seqNo:175; 4061454.08 north; 594589.40 east; 36.69 lat; -121.94 lon; 10.38 depth; utmZone: 10 2024-11-06T02:50:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861425.95 2024-11-06T02:50:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:29Z- waiting for 1730861429.95, time is 1730861429.65 2024-11-06T02:50:29Z- AHRS_M2 msg: 1730861429.97 epoch sec; seqNo:161 2024-11-06T02:50:29Z- AHRS_M2 msg: 0.18 phi; -0.17 theta; 5.30 psi 2024-11-06T02:50:29Z- handleDepth()- Depth_Keller msg: 1730861429.97 epoch sec; seqNo:176; depth 10.45 2024-11-06T02:50:29Z- handleDVL() - DVL msg epochMillisec: 1730861429.97 2024-11-06T02:50:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:29Z- handleDvl() - RDI_Pathfinder msg: 1730861429.97 sec; seqNo:56; #beams:0 2024-11-06T02:50:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.01 , -0.16 2024-11-06T02:50:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861430.00 epoch sec; seqNo:176; 4061454.08 north; 594589.40 east; 36.69 lat; -121.94 lon; 10.45 depth; utmZone: 10 2024-11-06T02:50:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861425.95 2024-11-06T02:50:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:30Z- waiting for 1730861429.95, time is 1730861430.00 2024-11-06T02:50:30Z- LcmTrn::updateTrn() >>>> pose time 1730861430.00, meas time 1730861429.97 <<<< 2024-11-06T02:50:30Z- LcmTrn::updateTrn() equating pose 1730861429.97 and meas times 1730861429.97 2024-11-06T02:50:30Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861429.97 2024-11-06T02:50:30Z- TerrainNav::motionUpdate() - currEstimate = (4061454, 594589.40, 10.45) 2024-11-06T02:50:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:50:30Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:50:30Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:50:30Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:50:30Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:50:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:50:30Z- variances([0], [2], and psi_berg) = 17.526343 65.483664 0.000000 2024-11-06T02:50:30Z- publishEstimatesDecoder() - at 1730861429.97 sec 2024-11-06T02:50:30Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:50:30Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061504.64 594588.65 18.40 2024-11-06T02:50:30Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061512.06 594567.35 18.43 lat/lon: 36.69 -121.94 2024-11-06T02:50:30Z- LcmTrn::publishEstimatesDecoder() - COVAR: 17.53 65.48 1.36 0.00 2024-11-06T02:50:30Z- AHRS_M2 msg: 1730861430.37 epoch sec; seqNo:162 2024-11-06T02:50:30Z- AHRS_M2 msg: 0.17 phi; -0.16 theta; 5.31 psi 2024-11-06T02:50:30Z- handleDepth()- Depth_Keller msg: 1730861430.38 epoch sec; seqNo:177; depth 10.49 2024-11-06T02:50:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861430.39 epoch sec; seqNo:177; 4061454.45 north; 594589.40 east; 36.69 lat; -121.94 lon; 10.49 depth; utmZone: 10 2024-11-06T02:50:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861429.97 2024-11-06T02:50:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:30Z- waiting for 1730861433.97, time is 1730861430.39 2024-11-06T02:50:30Z- AHRS_M2 msg: 1730861430.78 epoch sec; seqNo:163 2024-11-06T02:50:30Z- AHRS_M2 msg: 0.17 phi; -0.16 theta; 5.31 psi 2024-11-06T02:50:30Z- handleDepth()- Depth_Keller msg: 1730861430.79 epoch sec; seqNo:178; depth 10.55 2024-11-06T02:50:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861430.80 epoch sec; seqNo:178; 4061454.45 north; 594589.40 east; 36.69 lat; -121.94 lon; 10.55 depth; utmZone: 10 2024-11-06T02:50:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861429.97 2024-11-06T02:50:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:30Z- waiting for 1730861433.97, time is 1730861430.80 2024-11-06T02:50:31Z- AHRS_M2 msg: 1730861431.18 epoch sec; seqNo:164 2024-11-06T02:50:31Z- AHRS_M2 msg: 0.17 phi; -0.16 theta; 5.30 psi 2024-11-06T02:50:31Z- handleDepth()- Depth_Keller msg: 1730861431.19 epoch sec; seqNo:179; depth 10.61 2024-11-06T02:50:31Z- handleDVL() - DVL msg epochMillisec: 1730861431.19 2024-11-06T02:50:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:31Z- handleDvl() - RDI_Pathfinder msg: 1730861431.19 sec; seqNo:57; #beams:0 2024-11-06T02:50:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.01 , -0.16 2024-11-06T02:50:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861431.20 epoch sec; seqNo:179; 4061454.45 north; 594589.40 east; 36.69 lat; -121.94 lon; 10.61 depth; utmZone: 10 2024-11-06T02:50:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861429.97 2024-11-06T02:50:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:31Z- waiting for 1730861433.97, time is 1730861431.20 2024-11-06T02:50:31Z- AHRS_M2 msg: 1730861431.62 epoch sec; seqNo:165 2024-11-06T02:50:31Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.30 psi 2024-11-06T02:50:31Z- handleDepth()- Depth_Keller msg: 1730861431.63 epoch sec; seqNo:180; depth 10.64 2024-11-06T02:50:31Z- handleDVL() - DVL msg epochMillisec: 1730861431.67 2024-11-06T02:50:31Z- handleDvl() - RDI_Pathfinder msg: 1730861431.67 sec; seqNo:58; #beams:4 2024-11-06T02:50:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.41 , 68.27 , 68.27 , 90.41 2024-11-06T02:50:31Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.03 , -0.09 2024-11-06T02:50:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861431.68 epoch sec; seqNo:180; 4061454.82 north; 594588.17 east; 36.69 lat; -121.94 lon; 10.64 depth; utmZone: 10 2024-11-06T02:50:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861429.97 2024-11-06T02:50:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:31Z- waiting for 1730861433.97, time is 1730861431.68 2024-11-06T02:50:32Z- handleDepth()- Depth_Keller msg: 1730861432.04 epoch sec; seqNo:181; depth 10.72 2024-11-06T02:50:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861432.06 epoch sec; seqNo:181; 4061455.20 north; 594588.17 east; 36.69 lat; -121.94 lon; 10.72 depth; utmZone: 10 2024-11-06T02:50:32Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:50:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861429.97 2024-11-06T02:50:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:32Z- waiting for 1730861433.97, time is 1730861432.06 2024-11-06T02:50:32Z- AHRS_M2 msg: 1730861432.42 epoch sec; seqNo:166 2024-11-06T02:50:32Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.31 psi 2024-11-06T02:50:32Z- handleDepth()- Depth_Keller msg: 1730861432.43 epoch sec; seqNo:182; depth 10.78 2024-11-06T02:50:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861432.45 epoch sec; seqNo:182; 4061455.20 north; 594588.17 east; 36.69 lat; -121.94 lon; 10.78 depth; utmZone: 10 2024-11-06T02:50:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861429.97 2024-11-06T02:50:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:32Z- waiting for 1730861433.97, time is 1730861432.45 2024-11-06T02:50:32Z- AHRS_M2 msg: 1730861432.83 epoch sec; seqNo:167 2024-11-06T02:50:32Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.31 psi 2024-11-06T02:50:32Z- handleDepth()- Depth_Keller msg: 1730861432.83 epoch sec; seqNo:183; depth 10.83 2024-11-06T02:50:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861432.84 epoch sec; seqNo:183; 4061455.56 north; 594586.94 east; 36.69 lat; -121.94 lon; 10.83 depth; utmZone: 10 2024-11-06T02:50:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861429.97 2024-11-06T02:50:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:32Z- waiting for 1730861433.97, time is 1730861432.84 2024-11-06T02:50:33Z- AHRS_M2 msg: 1730861433.26 epoch sec; seqNo:168 2024-11-06T02:50:33Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.31 psi 2024-11-06T02:50:33Z- handleDepth()- Depth_Keller msg: 1730861433.29 epoch sec; seqNo:184; depth 10.88 2024-11-06T02:50:33Z- handleDVL() - DVL msg epochMillisec: 1730861433.32 2024-11-06T02:50:33Z- handleDvl() - RDI_Pathfinder msg: 1730861433.32 sec; seqNo:59; #beams:4 2024-11-06T02:50:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.17 , 67.87 , 67.46 , 89.33 2024-11-06T02:50:33Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.01 , -0.03 2024-11-06T02:50:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861433.34 epoch sec; seqNo:184; 4061455.94 north; 594586.94 east; 36.69 lat; -121.94 lon; 10.88 depth; utmZone: 10 2024-11-06T02:50:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861429.97 2024-11-06T02:50:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:33Z- waiting for 1730861433.97, time is 1730861433.34 2024-11-06T02:50:33Z- AHRS_M2 msg: 1730861433.64 epoch sec; seqNo:169 2024-11-06T02:50:33Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.31 psi 2024-11-06T02:50:33Z- handleDepth()- Depth_Keller msg: 1730861433.64 epoch sec; seqNo:185; depth 10.95 2024-11-06T02:50:33Z- handleDVL() - DVL msg epochMillisec: 1730861433.65 2024-11-06T02:50:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:33Z- handleDvl() - RDI_Pathfinder msg: 1730861433.65 sec; seqNo:60; #beams:0 2024-11-06T02:50:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:33Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.01 , -0.03 2024-11-06T02:50:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861433.67 epoch sec; seqNo:185; 4061455.94 north; 594586.94 east; 36.69 lat; -121.94 lon; 10.95 depth; utmZone: 10 2024-11-06T02:50:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861429.97 2024-11-06T02:50:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:33Z- waiting for 1730861433.97, time is 1730861433.67 2024-11-06T02:50:34Z- AHRS_M2 msg: 1730861434.04 epoch sec; seqNo:170 2024-11-06T02:50:34Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.31 psi 2024-11-06T02:50:34Z- handleDepth()- Depth_Keller msg: 1730861434.05 epoch sec; seqNo:186; depth 10.99 2024-11-06T02:50:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861434.07 epoch sec; seqNo:186; 4061456.32 north; 594586.93 east; 36.69 lat; -121.94 lon; 10.99 depth; utmZone: 10 2024-11-06T02:50:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861429.97 2024-11-06T02:50:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:34Z- waiting for 1730861433.97, time is 1730861434.07 2024-11-06T02:50:34Z- LcmTrn::updateTrn() >>>> pose time 1730861434.07, meas time 1730861433.65 <<<< 2024-11-06T02:50:34Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861434.07 2024-11-06T02:50:34Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:50:34Z- TerrainNav::motionUpdate() - currEstimate = (4061456, 594586.93, 10.99) 2024-11-06T02:50:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:50:34Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:50:34Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:50:34Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:50:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:50:34Z- variances([0], [2], and psi_berg) = 17.526343 65.496406 0.000000 2024-11-06T02:50:34Z- publishEstimatesDecoder() - at 1730861434.07 sec 2024-11-06T02:50:34Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:50:34Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061506.90 594586.20 18.52 2024-11-06T02:50:34Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061514.31 594564.88 18.97 lat/lon: 36.69 -121.94 2024-11-06T02:50:34Z- LcmTrn::publishEstimatesDecoder() - COVAR: 17.53 65.50 1.59 0.00 2024-11-06T02:50:34Z- AHRS_M2 msg: 1730861434.44 epoch sec; seqNo:171 2024-11-06T02:50:34Z- AHRS_M2 msg: 0.15 phi; -0.16 theta; 5.31 psi 2024-11-06T02:50:34Z- handleDepth()- Depth_Keller msg: 1730861434.45 epoch sec; seqNo:187; depth 11.04 2024-11-06T02:50:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861434.46 epoch sec; seqNo:187; 4061456.69 north; 594585.71 east; 36.69 lat; -121.94 lon; 11.04 depth; utmZone: 10 2024-11-06T02:50:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861434.07 2024-11-06T02:50:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:34Z- waiting for 1730861438.07, time is 1730861434.46 2024-11-06T02:50:34Z- AHRS_M2 msg: 1730861434.85 epoch sec; seqNo:172 2024-11-06T02:50:34Z- AHRS_M2 msg: 0.14 phi; -0.16 theta; 5.31 psi 2024-11-06T02:50:34Z- handleDepth()- Depth_Keller msg: 1730861434.85 epoch sec; seqNo:188; depth 11.08 2024-11-06T02:50:34Z- handleDVL() - DVL msg epochMillisec: 1730861434.86 2024-11-06T02:50:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:34Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:34Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:34Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:34Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:34Z- handleDvl() - RDI_Pathfinder msg: 1730861434.86 sec; seqNo:61; #beams:0 2024-11-06T02:50:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:34Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.01 , -0.03 2024-11-06T02:50:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861434.86 epoch sec; seqNo:188; 4061456.69 north; 594585.71 east; 36.69 lat; -121.94 lon; 11.08 depth; utmZone: 10 2024-11-06T02:50:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861434.07 2024-11-06T02:50:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:35Z- waiting for 1730861438.07, time is 1730861434.86 2024-11-06T02:50:35Z- AHRS_M2 msg: 1730861435.29 epoch sec; seqNo:173 2024-11-06T02:50:35Z- AHRS_M2 msg: 0.14 phi; -0.16 theta; 5.31 psi 2024-11-06T02:50:35Z- handleDepth()- Depth_Keller msg: 1730861435.30 epoch sec; seqNo:189; depth 11.15 2024-11-06T02:50:35Z- handleDVL() - DVL msg epochMillisec: 1730861435.32 2024-11-06T02:50:35Z- handleDvl() - RDI_Pathfinder msg: 1730861435.32 sec; seqNo:62; #beams:4 2024-11-06T02:50:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.72 , 67.45 , 67.45 , 89.33 2024-11-06T02:50:35Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , -0.00 , -0.04 2024-11-06T02:50:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861435.34 epoch sec; seqNo:189; 4061457.07 north; 594585.71 east; 36.69 lat; -121.94 lon; 11.15 depth; utmZone: 10 2024-11-06T02:50:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861434.07 2024-11-06T02:50:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:35Z- waiting for 1730861438.07, time is 1730861435.34 2024-11-06T02:50:35Z- AHRS_M2 msg: 1730861435.66 epoch sec; seqNo:174 2024-11-06T02:50:35Z- AHRS_M2 msg: 0.13 phi; -0.17 theta; 5.31 psi 2024-11-06T02:50:35Z- handleDepth()- Depth_Keller msg: 1730861435.67 epoch sec; seqNo:190; depth 11.21 2024-11-06T02:50:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861435.69 epoch sec; seqNo:190; 4061457.07 north; 594585.71 east; 36.69 lat; -121.94 lon; 11.21 depth; utmZone: 10 2024-11-06T02:50:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861434.07 2024-11-06T02:50:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:35Z- waiting for 1730861438.07, time is 1730861435.69 2024-11-06T02:50:36Z- AHRS_M2 msg: 1730861436.06 epoch sec; seqNo:175 2024-11-06T02:50:36Z- AHRS_M2 msg: 0.13 phi; -0.17 theta; 5.30 psi 2024-11-06T02:50:36Z- handleDepth()- Depth_Keller msg: 1730861436.07 epoch sec; seqNo:191; depth 11.25 2024-11-06T02:50:36Z- handleDVL() - DVL msg epochMillisec: 1730861436.07 2024-11-06T02:50:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:36Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:36Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:36Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:36Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:36Z- handleDvl() - RDI_Pathfinder msg: 1730861436.07 sec; seqNo:63; #beams:0 2024-11-06T02:50:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:36Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , -0.00 , -0.04 2024-11-06T02:50:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861436.08 epoch sec; seqNo:191; 4061457.43 north; 594584.48 east; 36.69 lat; -121.94 lon; 11.25 depth; utmZone: 10 2024-11-06T02:50:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861434.07 2024-11-06T02:50:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:36Z- waiting for 1730861438.07, time is 1730861436.08 2024-11-06T02:50:36Z- AHRS_M2 msg: 1730861436.47 epoch sec; seqNo:176 2024-11-06T02:50:36Z- AHRS_M2 msg: 0.13 phi; -0.17 theta; 5.30 psi 2024-11-06T02:50:36Z- handleDepth()- Depth_Keller msg: 1730861436.47 epoch sec; seqNo:192; depth 11.33 2024-11-06T02:50:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861436.48 epoch sec; seqNo:192; 4061457.81 north; 594584.48 east; 36.69 lat; -121.94 lon; 11.33 depth; utmZone: 10 2024-11-06T02:50:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861434.07 2024-11-06T02:50:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:36Z- waiting for 1730861438.07, time is 1730861436.48 2024-11-06T02:50:36Z- handleDepth()- Depth_Keller msg: 1730861436.90 epoch sec; seqNo:193; depth 11.37 2024-11-06T02:50:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861436.91 epoch sec; seqNo:193; 4061457.81 north; 594584.48 east; 36.69 lat; -121.94 lon; 11.37 depth; utmZone: 10 2024-11-06T02:50:37Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:50:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861434.07 2024-11-06T02:50:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:37Z- waiting for 1730861438.07, time is 1730861436.91 2024-11-06T02:50:37Z- AHRS_M2 msg: 1730861437.29 epoch sec; seqNo:177 2024-11-06T02:50:37Z- AHRS_M2 msg: 0.14 phi; -0.17 theta; 5.29 psi 2024-11-06T02:50:37Z- handleDepth()- Depth_Keller msg: 1730861437.30 epoch sec; seqNo:194; depth 11.46 2024-11-06T02:50:37Z- handleDVL() - DVL msg epochMillisec: 1730861437.33 2024-11-06T02:50:37Z- handleDvl() - RDI_Pathfinder msg: 1730861437.33 sec; seqNo:64; #beams:4 2024-11-06T02:50:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.90 , 67.45 , 67.45 , 89.33 2024-11-06T02:50:37Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , -0.06 , 0.19 2024-11-06T02:50:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861437.35 epoch sec; seqNo:194; 4061458.18 north; 594583.25 east; 36.69 lat; -121.94 lon; 11.46 depth; utmZone: 10 2024-11-06T02:50:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861434.07 2024-11-06T02:50:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:37Z- waiting for 1730861438.07, time is 1730861437.35 2024-11-06T02:50:37Z- AHRS_M2 msg: 1730861437.68 epoch sec; seqNo:178 2024-11-06T02:50:37Z- AHRS_M2 msg: 0.14 phi; -0.18 theta; 5.28 psi 2024-11-06T02:50:37Z- handleDepth()- Depth_Keller msg: 1730861437.68 epoch sec; seqNo:195; depth 11.54 2024-11-06T02:50:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861437.70 epoch sec; seqNo:195; 4061458.18 north; 594583.25 east; 36.69 lat; -121.94 lon; 11.54 depth; utmZone: 10 2024-11-06T02:50:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861434.07 2024-11-06T02:50:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:37Z- waiting for 1730861438.07, time is 1730861437.70 2024-11-06T02:50:38Z- handleDepth()- Depth_Keller msg: 1730861438.10 epoch sec; seqNo:196; depth 11.59 2024-11-06T02:50:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861438.12 epoch sec; seqNo:196; 4061458.55 north; 594583.25 east; 36.69 lat; -121.94 lon; 11.59 depth; utmZone: 10 2024-11-06T02:50:38Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:50:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861434.07 2024-11-06T02:50:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:38Z- waiting for 1730861438.07, time is 1730861438.12 2024-11-06T02:50:38Z- LcmTrn::updateTrn() >>>> pose time 1730861438.12, meas time 1730861437.33 <<<< 2024-11-06T02:50:38Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861438.12 2024-11-06T02:50:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861437.33 sec, ping # 0; waiting for INS data more recent than 1730861434.07... 2024-11-06T02:50:38Z- TerrainNav::motionUpdate() - currEstimate = (4061459, 594583.25, 11.59) 2024-11-06T02:50:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:50:38Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:50:38Z- TNavPF::Projecting Measurements 2024-11-06T02:50:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:50:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:50:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:50:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:50:38Z- TNavFilter::4 good beams. 2024-11-06T02:50:38Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:50:38Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:50:38Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:50:38Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:50:38Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:50:38Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:50:38Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:50:38Z- TerrainMapOctree: (vehN, vehE ) = (4061458.12, 594583.97). 2024-11-06T02:50:38Z- TerrainMapOctree: Min Distance = 266.23. 2024-11-06T02:50:38Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:50:38Z- Forcing Subcloud Comparison 2024-11-06T02:50:38Z- Weighting particles with subcloud comparison 2024-11-06T02:50:38Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:38Z- meanExpectedMeasDiff: -6.707227 delta_rms_squared: 4.548802 alpha: 0.995622 2024-11-06T02:50:38Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:38Z- meanExpectedMeasDiff: -1.115326 delta_rms_squared: 2.638106 alpha: 0.992476 2024-11-06T02:50:38Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:38Z- meanExpectedMeasDiff: -1.366084 delta_rms_squared: 2.578416 alpha: 0.992303 2024-11-06T02:50:38Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:38Z- meanExpectedMeasDiff: 4.361401 delta_rms_squared: 3.887727 alpha: 0.994882 2024-11-06T02:50:38Z- Subcloud NIS: 0.203653 2024-11-06T02:50:38Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:50:38Z- TNavPF:: sumWeights = 0.063361 2024-11-06T02:50:38Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:50:38Z- TNavPF::First Term of Map Covariance Matrix is 1.69 2024-11-06T02:50:38Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T02:50:38Z- TerrainNav::Resampling particle filter... 2024-11-06T02:50:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861437.33 sec. 2024-11-06T02:50:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:50:38Z- variances([0], [2], and psi_berg) = 58.444062 112.380903 0.000000 2024-11-06T02:50:38Z- publishEstimatesDecoder() - at 1730861438.12 sec 2024-11-06T02:50:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:50:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061514.41 594588.08 19.93 2024-11-06T02:50:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061513.61 594565.62 20.32 lat/lon: 36.69 -121.94 2024-11-06T02:50:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 58.44 112.38 1.18 0.00 2024-11-06T02:50:38Z- handleDepth()- Depth_Keller msg: 1730861438.51 epoch sec; seqNo:197; depth 11.66 2024-11-06T02:50:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861438.53 epoch sec; seqNo:197; 4061458.54 north; 594582.03 east; 36.69 lat; -121.94 lon; 11.66 depth; utmZone: 10 2024-11-06T02:50:38Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:50:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861438.12 2024-11-06T02:50:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:38Z- waiting for 1730861442.12, time is 1730861438.53 2024-11-06T02:50:38Z- AHRS_M2 msg: 1730861438.89 epoch sec; seqNo:179 2024-11-06T02:50:38Z- AHRS_M2 msg: 0.13 phi; -0.19 theta; 5.26 psi 2024-11-06T02:50:38Z- handleDepth()- Depth_Keller msg: 1730861438.91 epoch sec; seqNo:198; depth 11.74 2024-11-06T02:50:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861438.91 epoch sec; seqNo:198; 4061458.92 north; 594582.02 east; 36.69 lat; -121.94 lon; 11.74 depth; utmZone: 10 2024-11-06T02:50:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861438.12 2024-11-06T02:50:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:39Z- waiting for 1730861442.12, time is 1730861438.91 2024-11-06T02:50:39Z- AHRS_M2 msg: 1730861439.31 epoch sec; seqNo:180 2024-11-06T02:50:39Z- AHRS_M2 msg: 0.12 phi; -0.21 theta; 5.26 psi 2024-11-06T02:50:39Z- handleDepth()- Depth_Keller msg: 1730861439.31 epoch sec; seqNo:199; depth 11.80 2024-11-06T02:50:39Z- handleDVL() - DVL msg epochMillisec: 1730861439.33 2024-11-06T02:50:39Z- handleDvl() - RDI_Pathfinder msg: 1730861439.33 sec; seqNo:65; #beams:4 2024-11-06T02:50:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.91 , 67.46 , 66.64 , 89.33 2024-11-06T02:50:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.06 , 0.20 2024-11-06T02:50:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861439.36 epoch sec; seqNo:199; 4061458.92 north; 594582.02 east; 36.69 lat; -121.94 lon; 11.80 depth; utmZone: 10 2024-11-06T02:50:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861438.12 2024-11-06T02:50:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:39Z- waiting for 1730861442.12, time is 1730861439.36 2024-11-06T02:50:39Z- AHRS_M2 msg: 1730861439.70 epoch sec; seqNo:181 2024-11-06T02:50:39Z- AHRS_M2 msg: 0.12 phi; -0.21 theta; 5.26 psi 2024-11-06T02:50:39Z- handleDepth()- Depth_Keller msg: 1730861439.71 epoch sec; seqNo:200; depth 11.87 2024-11-06T02:50:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861439.73 epoch sec; seqNo:200; 4061458.92 north; 594582.02 east; 36.69 lat; -121.94 lon; 11.87 depth; utmZone: 10 2024-11-06T02:50:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861438.12 2024-11-06T02:50:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:39Z- waiting for 1730861442.12, time is 1730861439.73 2024-11-06T02:50:40Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T02:50:40Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T02:50:40Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T02:50:40Z- AHRS_M2 msg: 1730861440.09 epoch sec; seqNo:182 2024-11-06T02:50:40Z- AHRS_M2 msg: 0.12 phi; -0.21 theta; 5.26 psi 2024-11-06T02:50:40Z- handleDepth()- Depth_Keller msg: 1730861440.13 epoch sec; seqNo:201; depth 11.91 2024-11-06T02:50:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861440.14 epoch sec; seqNo:201; 4061459.30 north; 594582.02 east; 36.69 lat; -121.94 lon; 11.91 depth; utmZone: 10 2024-11-06T02:50:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861438.12 2024-11-06T02:50:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:40Z- waiting for 1730861442.12, time is 1730861440.14 2024-11-06T02:50:40Z- AHRS_M2 msg: 1730861440.51 epoch sec; seqNo:183 2024-11-06T02:50:40Z- AHRS_M2 msg: 0.12 phi; -0.22 theta; 5.27 psi 2024-11-06T02:50:40Z- handleDepth()- Depth_Keller msg: 1730861440.52 epoch sec; seqNo:202; depth 11.99 2024-11-06T02:50:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861440.53 epoch sec; seqNo:202; 4061459.29 north; 594580.80 east; 36.69 lat; -121.94 lon; 11.99 depth; utmZone: 10 2024-11-06T02:50:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861438.12 2024-11-06T02:50:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:40Z- waiting for 1730861442.12, time is 1730861440.53 2024-11-06T02:50:40Z- AHRS_M2 msg: 1730861440.92 epoch sec; seqNo:184 2024-11-06T02:50:40Z- AHRS_M2 msg: 0.13 phi; -0.22 theta; 5.28 psi 2024-11-06T02:50:40Z- handleDepth()- Depth_Keller msg: 1730861440.92 epoch sec; seqNo:203; depth 12.05 2024-11-06T02:50:40Z- handleDVL() - DVL msg epochMillisec: 1730861440.92 2024-11-06T02:50:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:40Z- handleDvl() - RDI_Pathfinder msg: 1730861440.92 sec; seqNo:66; #beams:0 2024-11-06T02:50:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.06 , 0.20 2024-11-06T02:50:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861440.93 epoch sec; seqNo:203; 4061459.29 north; 594580.80 east; 36.69 lat; -121.94 lon; 12.05 depth; utmZone: 10 2024-11-06T02:50:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861438.12 2024-11-06T02:50:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:41Z- waiting for 1730861442.12, time is 1730861440.93 2024-11-06T02:50:41Z- handleDepth()- Depth_Keller msg: 1730861441.34 epoch sec; seqNo:204; depth 12.10 2024-11-06T02:50:41Z- handleDVL() - DVL msg epochMillisec: 1730861441.36 2024-11-06T02:50:41Z- handleDvl() - RDI_Pathfinder msg: 1730861441.36 sec; seqNo:67; #beams:4 2024-11-06T02:50:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.91 , 67.87 , 66.23 , 91.50 2024-11-06T02:50:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.00 , -0.17 2024-11-06T02:50:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861441.38 epoch sec; seqNo:204; 4061459.66 north; 594580.79 east; 36.69 lat; -121.94 lon; 12.10 depth; utmZone: 10 2024-11-06T02:50:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861438.12 2024-11-06T02:50:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:41Z- waiting for 1730861442.12, time is 1730861441.38 2024-11-06T02:50:41Z- AHRS_M2 msg: 1730861441.73 epoch sec; seqNo:185 2024-11-06T02:50:41Z- AHRS_M2 msg: 0.14 phi; -0.21 theta; 5.29 psi 2024-11-06T02:50:41Z- handleDepth()- Depth_Keller msg: 1730861441.73 epoch sec; seqNo:205; depth 12.19 2024-11-06T02:50:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861441.76 epoch sec; seqNo:205; 4061459.66 north; 594580.79 east; 36.69 lat; -121.94 lon; 12.19 depth; utmZone: 10 2024-11-06T02:50:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861438.12 2024-11-06T02:50:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:41Z- waiting for 1730861442.12, time is 1730861441.76 2024-11-06T02:50:42Z- AHRS_M2 msg: 1730861442.13 epoch sec; seqNo:186 2024-11-06T02:50:42Z- AHRS_M2 msg: 0.14 phi; -0.21 theta; 5.30 psi 2024-11-06T02:50:42Z- handleDepth()- Depth_Keller msg: 1730861442.14 epoch sec; seqNo:206; depth 12.24 2024-11-06T02:50:42Z- handleDVL() - DVL msg epochMillisec: 1730861442.14 2024-11-06T02:50:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:42Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:42Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:42Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:42Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:42Z- handleDvl() - RDI_Pathfinder msg: 1730861442.14 sec; seqNo:68; #beams:0 2024-11-06T02:50:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.69 , -0.00 , -0.17 2024-11-06T02:50:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861442.15 epoch sec; seqNo:206; 4061460.04 north; 594580.79 east; 36.69 lat; -121.94 lon; 12.24 depth; utmZone: 10 2024-11-06T02:50:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861438.12 2024-11-06T02:50:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:42Z- waiting for 1730861442.12, time is 1730861442.15 2024-11-06T02:50:42Z- LcmTrn::updateTrn() >>>> pose time 1730861442.15, meas time 1730861442.14 <<<< 2024-11-06T02:50:42Z- LcmTrn::updateTrn() equating pose 1730861442.14 and meas times 1730861442.14 2024-11-06T02:50:42Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861442.14 2024-11-06T02:50:42Z- TerrainNav::motionUpdate() - currEstimate = (4061460, 594580.79, 12.24) 2024-11-06T02:50:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:50:42Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:50:42Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:50:42Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:50:42Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:50:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:50:42Z- variances([0], [2], and psi_berg) = 58.447994 112.375292 0.000000 2024-11-06T02:50:42Z- publishEstimatesDecoder() - at 1730861442.14 sec 2024-11-06T02:50:42Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:50:42Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061515.88 594585.58 21.21 2024-11-06T02:50:42Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061515.10 594563.16 20.97 lat/lon: 36.69 -121.94 2024-11-06T02:50:42Z- LcmTrn::publishEstimatesDecoder() - COVAR: 58.45 112.38 1.45 0.00 2024-11-06T02:50:42Z- AHRS_M2 msg: 1730861442.54 epoch sec; seqNo:187 2024-11-06T02:50:42Z- AHRS_M2 msg: 0.14 phi; -0.20 theta; 5.30 psi 2024-11-06T02:50:42Z- handleDepth()- Depth_Keller msg: 1730861442.55 epoch sec; seqNo:207; depth 12.30 2024-11-06T02:50:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861442.56 epoch sec; seqNo:207; 4061460.03 north; 594579.57 east; 36.69 lat; -121.94 lon; 12.30 depth; utmZone: 10 2024-11-06T02:50:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861442.14 2024-11-06T02:50:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:42Z- waiting for 1730861446.14, time is 1730861442.56 2024-11-06T02:50:42Z- AHRS_M2 msg: 1730861442.94 epoch sec; seqNo:188 2024-11-06T02:50:42Z- AHRS_M2 msg: 0.14 phi; -0.20 theta; 5.30 psi 2024-11-06T02:50:42Z- handleDepth()- Depth_Keller msg: 1730861442.95 epoch sec; seqNo:208; depth 12.38 2024-11-06T02:50:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861442.97 epoch sec; seqNo:208; 4061460.03 north; 594579.57 east; 36.69 lat; -121.94 lon; 12.38 depth; utmZone: 10 2024-11-06T02:50:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861442.14 2024-11-06T02:50:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:43Z- waiting for 1730861446.14, time is 1730861442.97 2024-11-06T02:50:43Z- AHRS_M2 msg: 1730861443.34 epoch sec; seqNo:189 2024-11-06T02:50:43Z- AHRS_M2 msg: 0.14 phi; -0.19 theta; 5.29 psi 2024-11-06T02:50:43Z- handleDepth()- Depth_Keller msg: 1730861443.35 epoch sec; seqNo:209; depth 12.45 2024-11-06T02:50:43Z- handleDVL() - DVL msg epochMillisec: 1730861443.37 2024-11-06T02:50:43Z- handleDvl() - RDI_Pathfinder msg: 1730861443.37 sec; seqNo:69; #beams:4 2024-11-06T02:50:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 85.00 , 66.23 , 65.41 , 92.58 2024-11-06T02:50:43Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.01 , -0.15 2024-11-06T02:50:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861443.40 epoch sec; seqNo:209; 4061460.41 north; 594579.57 east; 36.69 lat; -121.94 lon; 12.45 depth; utmZone: 10 2024-11-06T02:50:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861442.14 2024-11-06T02:50:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:43Z- waiting for 1730861446.14, time is 1730861443.40 2024-11-06T02:50:43Z- AHRS_M2 msg: 1730861443.75 epoch sec; seqNo:190 2024-11-06T02:50:43Z- AHRS_M2 msg: 0.15 phi; -0.19 theta; 5.29 psi 2024-11-06T02:50:43Z- handleDepth()- Depth_Keller msg: 1730861443.76 epoch sec; seqNo:210; depth 12.52 2024-11-06T02:50:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861443.77 epoch sec; seqNo:210; 4061460.79 north; 594579.56 east; 36.69 lat; -121.94 lon; 12.52 depth; utmZone: 10 2024-11-06T02:50:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861442.14 2024-11-06T02:50:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:43Z- waiting for 1730861446.14, time is 1730861443.77 2024-11-06T02:50:44Z- handleDepth()- Depth_Keller msg: 1730861444.18 epoch sec; seqNo:211; depth 12.59 2024-11-06T02:50:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861444.19 epoch sec; seqNo:211; 4061460.77 north; 594578.34 east; 36.69 lat; -121.94 lon; 12.59 depth; utmZone: 10 2024-11-06T02:50:44Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:50:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861442.14 2024-11-06T02:50:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:44Z- waiting for 1730861446.14, time is 1730861444.19 2024-11-06T02:50:44Z- AHRS_M2 msg: 1730861444.56 epoch sec; seqNo:191 2024-11-06T02:50:44Z- AHRS_M2 msg: 0.15 phi; -0.18 theta; 5.29 psi 2024-11-06T02:50:44Z- handleDepth()- Depth_Keller msg: 1730861444.57 epoch sec; seqNo:212; depth 12.66 2024-11-06T02:50:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861444.58 epoch sec; seqNo:212; 4061461.15 north; 594578.34 east; 36.69 lat; -121.94 lon; 12.66 depth; utmZone: 10 2024-11-06T02:50:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861442.14 2024-11-06T02:50:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:44Z- waiting for 1730861446.14, time is 1730861444.58 2024-11-06T02:50:44Z- AHRS_M2 msg: 1730861444.96 epoch sec; seqNo:192 2024-11-06T02:50:44Z- AHRS_M2 msg: 0.15 phi; -0.18 theta; 5.29 psi 2024-11-06T02:50:44Z- handleDepth()- Depth_Keller msg: 1730861444.96 epoch sec; seqNo:213; depth 12.73 2024-11-06T02:50:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861444.97 epoch sec; seqNo:213; 4061461.53 north; 594578.33 east; 36.69 lat; -121.94 lon; 12.73 depth; utmZone: 10 2024-11-06T02:50:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861442.14 2024-11-06T02:50:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:45Z- waiting for 1730861446.14, time is 1730861444.97 2024-11-06T02:50:45Z- AHRS_M2 msg: 1730861445.36 epoch sec; seqNo:193 2024-11-06T02:50:45Z- AHRS_M2 msg: 0.15 phi; -0.18 theta; 5.29 psi 2024-11-06T02:50:45Z- handleDepth()- Depth_Keller msg: 1730861445.38 epoch sec; seqNo:214; depth 12.80 2024-11-06T02:50:45Z- handleDVL() - DVL msg epochMillisec: 1730861445.40 2024-11-06T02:50:45Z- handleDvl() - RDI_Pathfinder msg: 1730861445.40 sec; seqNo:70; #beams:4 2024-11-06T02:50:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.68 , 66.23 , 66.23 , 0.00 2024-11-06T02:50:45Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.00 , 0.02 2024-11-06T02:50:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861445.42 epoch sec; seqNo:214; 4061461.52 north; 594577.11 east; 36.69 lat; -121.94 lon; 12.80 depth; utmZone: 10 2024-11-06T02:50:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861442.14 2024-11-06T02:50:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:45Z- waiting for 1730861446.14, time is 1730861445.42 2024-11-06T02:50:45Z- AHRS_M2 msg: 1730861445.77 epoch sec; seqNo:194 2024-11-06T02:50:45Z- AHRS_M2 msg: 0.15 phi; -0.18 theta; 5.29 psi 2024-11-06T02:50:45Z- handleDepth()- Depth_Keller msg: 1730861445.77 epoch sec; seqNo:215; depth 12.87 2024-11-06T02:50:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861445.80 epoch sec; seqNo:215; 4061461.90 north; 594577.11 east; 36.69 lat; -121.94 lon; 12.87 depth; utmZone: 10 2024-11-06T02:50:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861442.14 2024-11-06T02:50:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:45Z- waiting for 1730861446.14, time is 1730861445.80 2024-11-06T02:50:46Z- AHRS_M2 msg: 1730861446.17 epoch sec; seqNo:195 2024-11-06T02:50:46Z- AHRS_M2 msg: 0.15 phi; -0.18 theta; 5.30 psi 2024-11-06T02:50:46Z- handleDepth()- Depth_Keller msg: 1730861446.18 epoch sec; seqNo:216; depth 12.92 2024-11-06T02:50:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861446.19 epoch sec; seqNo:216; 4061461.90 north; 594577.11 east; 36.69 lat; -121.94 lon; 12.92 depth; utmZone: 10 2024-11-06T02:50:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861442.14 2024-11-06T02:50:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:46Z- waiting for 1730861446.14, time is 1730861446.19 2024-11-06T02:50:46Z- LcmTrn::updateTrn() >>>> pose time 1730861446.19, meas time 1730861445.40 <<<< 2024-11-06T02:50:46Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861446.19 2024-11-06T02:50:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861445.40 sec, ping # 0; waiting for INS data more recent than 1730861442.14... 2024-11-06T02:50:46Z- TerrainNav::motionUpdate() - currEstimate = (4061462, 594577.11, 12.92) 2024-11-06T02:50:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:50:46Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:50:46Z- TNavPF::Projecting Measurements 2024-11-06T02:50:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:50:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:50:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:50:46Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2024-11-06T02:50:46Z- TNavFilter::3 good beams. 2024-11-06T02:50:46Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:50:46Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:50:46Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:50:46Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:50:46Z- TNavPF:: beamIndex[3] = 0 2024-11-06T02:50:46Z- TNavPF:: *** beamsVF.Ncols() = 3 *** 2024-11-06T02:50:46Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:50:46Z- TerrainMapOctree: (vehN, vehE ) = (4061461.53, 594577.83). 2024-11-06T02:50:46Z- TerrainMapOctree: Min Distance = 272.74. 2024-11-06T02:50:46Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:50:46Z- Forcing Subcloud Comparison 2024-11-06T02:50:46Z- Weighting particles with subcloud comparison 2024-11-06T02:50:46Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:46Z- meanExpectedMeasDiff: -6.421834 delta_rms_squared: 3.977369 alpha: 0.994997 2024-11-06T02:50:46Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:46Z- meanExpectedMeasDiff: -0.280087 delta_rms_squared: 2.046659 alpha: 0.990323 2024-11-06T02:50:46Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:46Z- meanExpectedMeasDiff: -0.765066 delta_rms_squared: 2.618805 alpha: 0.992421 2024-11-06T02:50:46Z- Subcloud NIS: 0.225524 2024-11-06T02:50:46Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:50:46Z- TNavPF:: sumWeights = 0.178044 2024-11-06T02:50:46Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:50:46Z- TNavPF::First Term of Map Covariance Matrix is 3.37 2024-11-06T02:50:46Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.02 2024-11-06T02:50:46Z- TerrainNav::Resampling particle filter... 2024-11-06T02:50:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861445.40 sec. 2024-11-06T02:50:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:50:46Z- variances([0], [2], and psi_berg) = 92.606659 107.843760 0.000000 2024-11-06T02:50:46Z- publishEstimatesDecoder() - at 1730861446.19 sec 2024-11-06T02:50:46Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:50:46Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061517.75 594581.83 23.69 2024-11-06T02:50:46Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061513.18 594561.82 22.52 lat/lon: 36.69 -121.94 2024-11-06T02:50:46Z- LcmTrn::publishEstimatesDecoder() - COVAR: 92.61 107.84 1.30 0.00 2024-11-06T02:50:46Z- AHRS_M2 msg: 1730861446.58 epoch sec; seqNo:196 2024-11-06T02:50:46Z- AHRS_M2 msg: 0.16 phi; -0.18 theta; 5.30 psi 2024-11-06T02:50:46Z- handleDepth()- Depth_Keller msg: 1730861446.59 epoch sec; seqNo:217; depth 12.99 2024-11-06T02:50:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861446.60 epoch sec; seqNo:217; 4061462.28 north; 594577.10 east; 36.69 lat; -121.94 lon; 12.99 depth; utmZone: 10 2024-11-06T02:50:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861446.19 2024-11-06T02:50:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:46Z- waiting for 1730861450.19, time is 1730861446.60 2024-11-06T02:50:46Z- AHRS_M2 msg: 1730861446.98 epoch sec; seqNo:197 2024-11-06T02:50:46Z- AHRS_M2 msg: 0.16 phi; -0.18 theta; 5.31 psi 2024-11-06T02:50:46Z- handleDepth()- Depth_Keller msg: 1730861446.99 epoch sec; seqNo:218; depth 13.06 2024-11-06T02:50:46Z- handleDVL() - DVL msg epochMillisec: 1730861446.99 2024-11-06T02:50:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:46Z- handleDvl() - RDI_Pathfinder msg: 1730861446.99 sec; seqNo:71; #beams:0 2024-11-06T02:50:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:46Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.00 , 0.02 2024-11-06T02:50:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861447.01 epoch sec; seqNo:218; 4061462.26 north; 594575.88 east; 36.69 lat; -121.94 lon; 13.06 depth; utmZone: 10 2024-11-06T02:50:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861446.19 2024-11-06T02:50:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:47Z- waiting for 1730861450.19, time is 1730861447.01 2024-11-06T02:50:47Z- AHRS_M2 msg: 1730861447.38 epoch sec; seqNo:198 2024-11-06T02:50:47Z- AHRS_M2 msg: 0.16 phi; -0.18 theta; 5.31 psi 2024-11-06T02:50:47Z- handleDepth()- Depth_Keller msg: 1730861447.39 epoch sec; seqNo:219; depth 13.11 2024-11-06T02:50:47Z- handleDVL() - DVL msg epochMillisec: 1730861447.41 2024-11-06T02:50:47Z- handleDvl() - RDI_Pathfinder msg: 1730861447.41 sec; seqNo:72; #beams:4 2024-11-06T02:50:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 79.00 , 65.40 , 66.37 , 0.00 2024-11-06T02:50:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , -0.03 2024-11-06T02:50:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861447.44 epoch sec; seqNo:219; 4061462.64 north; 594575.88 east; 36.69 lat; -121.94 lon; 13.11 depth; utmZone: 10 2024-11-06T02:50:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861446.19 2024-11-06T02:50:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:47Z- waiting for 1730861450.19, time is 1730861447.44 2024-11-06T02:50:47Z- AHRS_M2 msg: 1730861447.79 epoch sec; seqNo:199 2024-11-06T02:50:47Z- AHRS_M2 msg: 0.16 phi; -0.18 theta; 5.32 psi 2024-11-06T02:50:47Z- handleDepth()- Depth_Keller msg: 1730861447.79 epoch sec; seqNo:220; depth 13.17 2024-11-06T02:50:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861447.81 epoch sec; seqNo:220; 4061463.02 north; 594575.88 east; 36.69 lat; -121.94 lon; 13.17 depth; utmZone: 10 2024-11-06T02:50:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861446.19 2024-11-06T02:50:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:47Z- waiting for 1730861450.19, time is 1730861447.81 2024-11-06T02:50:48Z- AHRS_M2 msg: 1730861448.19 epoch sec; seqNo:200 2024-11-06T02:50:48Z- AHRS_M2 msg: 0.16 phi; -0.17 theta; 5.32 psi 2024-11-06T02:50:48Z- handleDepth()- Depth_Keller msg: 1730861448.19 epoch sec; seqNo:221; depth 13.23 2024-11-06T02:50:48Z- handleDVL() - DVL msg epochMillisec: 1730861448.20 2024-11-06T02:50:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:48Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:48Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:48Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:48Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:48Z- handleDvl() - RDI_Pathfinder msg: 1730861448.20 sec; seqNo:73; #beams:0 2024-11-06T02:50:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , -0.03 2024-11-06T02:50:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861448.22 epoch sec; seqNo:221; 4061463.02 north; 594575.88 east; 36.69 lat; -121.94 lon; 13.23 depth; utmZone: 10 2024-11-06T02:50:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861446.19 2024-11-06T02:50:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:48Z- waiting for 1730861450.19, time is 1730861448.22 2024-11-06T02:50:48Z- AHRS_M2 msg: 1730861448.60 epoch sec; seqNo:201 2024-11-06T02:50:48Z- AHRS_M2 msg: 0.16 phi; -0.17 theta; 5.32 psi 2024-11-06T02:50:48Z- handleDepth()- Depth_Keller msg: 1730861448.60 epoch sec; seqNo:222; depth 13.28 2024-11-06T02:50:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861448.61 epoch sec; seqNo:222; 4061463.38 north; 594574.65 east; 36.69 lat; -121.94 lon; 13.28 depth; utmZone: 10 2024-11-06T02:50:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861446.19 2024-11-06T02:50:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:48Z- waiting for 1730861450.19, time is 1730861448.61 2024-11-06T02:50:49Z- AHRS_M2 msg: 1730861449.01 epoch sec; seqNo:202 2024-11-06T02:50:49Z- AHRS_M2 msg: 0.16 phi; -0.17 theta; 5.32 psi 2024-11-06T02:50:49Z- handleDepth()- Depth_Keller msg: 1730861449.01 epoch sec; seqNo:223; depth 13.34 2024-11-06T02:50:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861449.02 epoch sec; seqNo:223; 4061463.38 north; 594574.65 east; 36.69 lat; -121.94 lon; 13.34 depth; utmZone: 10 2024-11-06T02:50:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861446.19 2024-11-06T02:50:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:49Z- waiting for 1730861450.19, time is 1730861449.02 2024-11-06T02:50:49Z- handleDepth()- Depth_Keller msg: 1730861449.43 epoch sec; seqNo:224; depth 13.42 2024-11-06T02:50:49Z- handleDVL() - DVL msg epochMillisec: 1730861449.45 2024-11-06T02:50:49Z- handleDvl() - RDI_Pathfinder msg: 1730861449.45 sec; seqNo:74; #beams:4 2024-11-06T02:50:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.35 , 64.75 , 66.05 , 83.21 2024-11-06T02:50:49Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.04 , 0.02 2024-11-06T02:50:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861449.47 epoch sec; seqNo:224; 4061463.76 north; 594574.65 east; 36.69 lat; -121.94 lon; 13.42 depth; utmZone: 10 2024-11-06T02:50:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861446.19 2024-11-06T02:50:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:49Z- waiting for 1730861450.19, time is 1730861449.47 2024-11-06T02:50:49Z- AHRS_M2 msg: 1730861449.81 epoch sec; seqNo:203 2024-11-06T02:50:49Z- AHRS_M2 msg: 0.14 phi; -0.16 theta; 5.31 psi 2024-11-06T02:50:49Z- handleDepth()- Depth_Keller msg: 1730861449.81 epoch sec; seqNo:225; depth 13.48 2024-11-06T02:50:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861449.84 epoch sec; seqNo:225; 4061463.76 north; 594574.65 east; 36.69 lat; -121.94 lon; 13.48 depth; utmZone: 10 2024-11-06T02:50:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861446.19 2024-11-06T02:50:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:49Z- waiting for 1730861450.19, time is 1730861449.84 2024-11-06T02:50:50Z- AHRS_M2 msg: 1730861450.21 epoch sec; seqNo:204 2024-11-06T02:50:50Z- AHRS_M2 msg: 0.13 phi; -0.16 theta; 5.30 psi 2024-11-06T02:50:50Z- handleDepth()- Depth_Keller msg: 1730861450.22 epoch sec; seqNo:226; depth 13.52 2024-11-06T02:50:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861450.23 epoch sec; seqNo:226; 4061464.13 north; 594573.42 east; 36.69 lat; -121.94 lon; 13.52 depth; utmZone: 10 2024-11-06T02:50:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861446.19 2024-11-06T02:50:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:50Z- waiting for 1730861450.19, time is 1730861450.23 2024-11-06T02:50:50Z- LcmTrn::updateTrn() >>>> pose time 1730861450.23, meas time 1730861449.45 <<<< 2024-11-06T02:50:50Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861450.23 2024-11-06T02:50:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861449.45 sec, ping # 0; waiting for INS data more recent than 1730861446.19... 2024-11-06T02:50:50Z- TerrainNav::motionUpdate() - currEstimate = (4061464, 594573.42, 13.52) 2024-11-06T02:50:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:50:50Z- TerrainNav:: SubcloudNIS is 0.23, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:50:50Z- TNavPF::Projecting Measurements 2024-11-06T02:50:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:50:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:50:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:50:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:50:50Z- TNavFilter::4 good beams. 2024-11-06T02:50:50Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:50:50Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:50:50Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:50:50Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:50:50Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:50:50Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:50:50Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:50:50Z- TerrainMapOctree: (vehN, vehE ) = (4061463.70, 594574.13). 2024-11-06T02:50:50Z- TerrainMapOctree: Min Distance = 276.76. 2024-11-06T02:50:50Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:50:50Z- Forcing Subcloud Comparison 2024-11-06T02:50:50Z- Weighting particles with subcloud comparison 2024-11-06T02:50:50Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:50Z- meanExpectedMeasDiff: -2.440643 delta_rms_squared: 3.290340 alpha: 0.993958 2024-11-06T02:50:50Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:50Z- meanExpectedMeasDiff: -0.429284 delta_rms_squared: 1.970217 alpha: 0.989951 2024-11-06T02:50:50Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:50Z- meanExpectedMeasDiff: 0.462622 delta_rms_squared: 2.582432 alpha: 0.992315 2024-11-06T02:50:50Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:50Z- meanExpectedMeasDiff: 3.063430 delta_rms_squared: 3.824713 alpha: 0.994798 2024-11-06T02:50:50Z- Subcloud NIS: 0.227468 2024-11-06T02:50:50Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:50:50Z- TNavPF:: sumWeights = 0.367125 2024-11-06T02:50:50Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:50:50Z- TNavPF::First Term of Map Covariance Matrix is 1.55 2024-11-06T02:50:50Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T02:50:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861449.45 sec. 2024-11-06T02:50:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:50:50Z- variances([0], [2], and psi_berg) = 112.166732 95.908047 0.000000 2024-11-06T02:50:50Z- publishEstimatesDecoder() - at 1730861450.23 sec 2024-11-06T02:50:50Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:50:50Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061496.76 594557.45 24.16 2024-11-06T02:50:50Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061511.83 594559.48 23.16 lat/lon: 36.69 -121.94 2024-11-06T02:50:50Z- LcmTrn::publishEstimatesDecoder() - COVAR: 112.17 95.91 1.18 0.00 2024-11-06T02:50:50Z- AHRS_M2 msg: 1730861450.62 epoch sec; seqNo:205 2024-11-06T02:50:50Z- AHRS_M2 msg: 0.12 phi; -0.16 theta; 5.29 psi 2024-11-06T02:50:50Z- handleDepth()- Depth_Keller msg: 1730861450.63 epoch sec; seqNo:227; depth 13.58 2024-11-06T02:50:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861450.64 epoch sec; seqNo:227; 4061464.13 north; 594573.42 east; 36.69 lat; -121.94 lon; 13.58 depth; utmZone: 10 2024-11-06T02:50:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861450.23 2024-11-06T02:50:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:50Z- waiting for 1730861454.23, time is 1730861450.64 2024-11-06T02:50:51Z- AHRS_M2 msg: 1730861451.06 epoch sec; seqNo:206 2024-11-06T02:50:51Z- AHRS_M2 msg: 0.11 phi; -0.16 theta; 5.28 psi 2024-11-06T02:50:51Z- handleDepth()- Depth_Keller msg: 1730861451.07 epoch sec; seqNo:228; depth 13.66 2024-11-06T02:50:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861451.08 epoch sec; seqNo:228; 4061464.51 north; 594573.42 east; 36.69 lat; -121.94 lon; 13.66 depth; utmZone: 10 2024-11-06T02:50:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861450.23 2024-11-06T02:50:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:51Z- waiting for 1730861454.23, time is 1730861451.08 2024-11-06T02:50:51Z- AHRS_M2 msg: 1730861451.45 epoch sec; seqNo:207 2024-11-06T02:50:51Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.27 psi 2024-11-06T02:50:51Z- handleDepth()- Depth_Keller msg: 1730861451.46 epoch sec; seqNo:229; depth 13.73 2024-11-06T02:50:51Z- handleDVL() - DVL msg epochMillisec: 1730861451.48 2024-11-06T02:50:51Z- handleDvl() - RDI_Pathfinder msg: 1730861451.48 sec; seqNo:75; #beams:4 2024-11-06T02:50:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.23 , 65.42 , 66.23 , 86.09 2024-11-06T02:50:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , 0.24 2024-11-06T02:50:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861451.49 epoch sec; seqNo:229; 4061464.49 north; 594572.20 east; 36.69 lat; -121.94 lon; 13.73 depth; utmZone: 10 2024-11-06T02:50:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861450.23 2024-11-06T02:50:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:51Z- waiting for 1730861454.23, time is 1730861451.49 2024-11-06T02:50:51Z- AHRS_M2 msg: 1730861451.85 epoch sec; seqNo:208 2024-11-06T02:50:51Z- AHRS_M2 msg: 0.10 phi; -0.17 theta; 5.26 psi 2024-11-06T02:50:51Z- handleDepth()- Depth_Keller msg: 1730861451.86 epoch sec; seqNo:230; depth 13.78 2024-11-06T02:50:51Z- handleDVL() - DVL msg epochMillisec: 1730861451.86 2024-11-06T02:50:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:51Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:51Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:51Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:51Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:51Z- handleDvl() - RDI_Pathfinder msg: 1730861451.86 sec; seqNo:76; #beams:0 2024-11-06T02:50:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , 0.24 2024-11-06T02:50:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861451.89 epoch sec; seqNo:230; 4061464.87 north; 594572.19 east; 36.69 lat; -121.94 lon; 13.78 depth; utmZone: 10 2024-11-06T02:50:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861450.23 2024-11-06T02:50:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:52Z- waiting for 1730861454.23, time is 1730861451.89 2024-11-06T02:50:52Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T02:50:52Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T02:50:52Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T02:50:52Z- AHRS_M2 msg: 1730861452.25 epoch sec; seqNo:209 2024-11-06T02:50:52Z- AHRS_M2 msg: 0.10 phi; -0.17 theta; 5.26 psi 2024-11-06T02:50:52Z- handleDepth()- Depth_Keller msg: 1730861452.27 epoch sec; seqNo:231; depth 13.84 2024-11-06T02:50:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861452.29 epoch sec; seqNo:231; 4061464.87 north; 594572.19 east; 36.69 lat; -121.94 lon; 13.84 depth; utmZone: 10 2024-11-06T02:50:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861450.23 2024-11-06T02:50:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:52Z- waiting for 1730861454.23, time is 1730861452.29 2024-11-06T02:50:52Z- AHRS_M2 msg: 1730861452.67 epoch sec; seqNo:210 2024-11-06T02:50:52Z- AHRS_M2 msg: 0.10 phi; -0.17 theta; 5.26 psi 2024-11-06T02:50:52Z- handleDepth()- Depth_Keller msg: 1730861452.68 epoch sec; seqNo:232; depth 13.90 2024-11-06T02:50:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861452.69 epoch sec; seqNo:232; 4061465.25 north; 594572.19 east; 36.69 lat; -121.94 lon; 13.90 depth; utmZone: 10 2024-11-06T02:50:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861450.23 2024-11-06T02:50:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:52Z- waiting for 1730861454.23, time is 1730861452.69 2024-11-06T02:50:53Z- AHRS_M2 msg: 1730861453.07 epoch sec; seqNo:211 2024-11-06T02:50:53Z- AHRS_M2 msg: 0.10 phi; -0.18 theta; 5.26 psi 2024-11-06T02:50:53Z- handleDepth()- Depth_Keller msg: 1730861453.08 epoch sec; seqNo:233; depth 13.97 2024-11-06T02:50:53Z- handleDVL() - DVL msg epochMillisec: 1730861453.08 2024-11-06T02:50:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:53Z- handleDvl() - RDI_Pathfinder msg: 1730861453.08 sec; seqNo:77; #beams:0 2024-11-06T02:50:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , 0.24 2024-11-06T02:50:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861453.09 epoch sec; seqNo:233; 4061465.24 north; 594570.97 east; 36.69 lat; -121.94 lon; 13.97 depth; utmZone: 10 2024-11-06T02:50:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861450.23 2024-11-06T02:50:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:53Z- waiting for 1730861454.23, time is 1730861453.09 2024-11-06T02:50:53Z- handleDepth()- Depth_Keller msg: 1730861453.49 epoch sec; seqNo:234; depth 14.01 2024-11-06T02:50:53Z- handleDVL() - DVL msg epochMillisec: 1730861453.52 2024-11-06T02:50:53Z- handleDvl() - RDI_Pathfinder msg: 1730861453.52 sec; seqNo:78; #beams:4 2024-11-06T02:50:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 65.82 , 66.23 , 85.00 2024-11-06T02:50:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.01 , 0.11 2024-11-06T02:50:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861453.53 epoch sec; seqNo:234; 4061465.62 north; 594570.96 east; 36.69 lat; -121.94 lon; 14.01 depth; utmZone: 10 2024-11-06T02:50:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861450.23 2024-11-06T02:50:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:53Z- waiting for 1730861454.23, time is 1730861453.53 2024-11-06T02:50:53Z- AHRS_M2 msg: 1730861453.87 epoch sec; seqNo:212 2024-11-06T02:50:53Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.26 psi 2024-11-06T02:50:53Z- handleDepth()- Depth_Keller msg: 1730861453.88 epoch sec; seqNo:235; depth 14.07 2024-11-06T02:50:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861453.89 epoch sec; seqNo:235; 4061465.62 north; 594570.96 east; 36.69 lat; -121.94 lon; 14.07 depth; utmZone: 10 2024-11-06T02:50:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861450.23 2024-11-06T02:50:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:54Z- waiting for 1730861454.23, time is 1730861453.89 2024-11-06T02:50:54Z- AHRS_M2 msg: 1730861454.28 epoch sec; seqNo:213 2024-11-06T02:50:54Z- AHRS_M2 msg: 0.11 phi; -0.19 theta; 5.26 psi 2024-11-06T02:50:54Z- handleDepth()- Depth_Keller msg: 1730861454.28 epoch sec; seqNo:236; depth 14.11 2024-11-06T02:50:54Z- handleDVL() - DVL msg epochMillisec: 1730861454.29 2024-11-06T02:50:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:54Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:54Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:54Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:54Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:54Z- handleDvl() - RDI_Pathfinder msg: 1730861454.29 sec; seqNo:79; #beams:0 2024-11-06T02:50:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.01 , 0.11 2024-11-06T02:50:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861454.30 epoch sec; seqNo:236; 4061465.62 north; 594570.96 east; 36.69 lat; -121.94 lon; 14.11 depth; utmZone: 10 2024-11-06T02:50:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861450.23 2024-11-06T02:50:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:54Z- waiting for 1730861454.23, time is 1730861454.30 2024-11-06T02:50:54Z- LcmTrn::updateTrn() >>>> pose time 1730861454.30, meas time 1730861454.29 <<<< 2024-11-06T02:50:54Z- LcmTrn::updateTrn() equating pose 1730861454.29 and meas times 1730861454.29 2024-11-06T02:50:54Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861454.29 2024-11-06T02:50:54Z- TerrainNav::motionUpdate() - currEstimate = (4061466, 594570.96, 14.11) 2024-11-06T02:50:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:50:54Z- TerrainNav:: SubcloudNIS is 0.23, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:50:54Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:50:54Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:50:54Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:50:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:50:54Z- variances([0], [2], and psi_berg) = 112.174248 95.908744 0.000000 2024-11-06T02:50:54Z- publishEstimatesDecoder() - at 1730861454.29 sec 2024-11-06T02:50:54Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:50:54Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061498.35 594554.97 24.71 2024-11-06T02:50:54Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061513.32 594557.02 23.75 lat/lon: 36.69 -121.94 2024-11-06T02:50:54Z- LcmTrn::publishEstimatesDecoder() - COVAR: 112.17 95.91 1.43 0.00 2024-11-06T02:50:54Z- AHRS_M2 msg: 1730861454.68 epoch sec; seqNo:214 2024-11-06T02:50:54Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.27 psi 2024-11-06T02:50:54Z- handleDepth()- Depth_Keller msg: 1730861454.71 epoch sec; seqNo:237; depth 14.16 2024-11-06T02:50:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861454.72 epoch sec; seqNo:237; 4061466.00 north; 594570.96 east; 36.69 lat; -121.94 lon; 14.16 depth; utmZone: 10 2024-11-06T02:50:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861454.29 2024-11-06T02:50:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:54Z- waiting for 1730861458.29, time is 1730861454.72 2024-11-06T02:50:55Z- AHRS_M2 msg: 1730861455.09 epoch sec; seqNo:215 2024-11-06T02:50:55Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.28 psi 2024-11-06T02:50:55Z- handleDepth()- Depth_Keller msg: 1730861455.09 epoch sec; seqNo:238; depth 14.21 2024-11-06T02:50:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861455.10 epoch sec; seqNo:238; 4061465.98 north; 594569.74 east; 36.69 lat; -121.94 lon; 14.21 depth; utmZone: 10 2024-11-06T02:50:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861454.29 2024-11-06T02:50:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:55Z- waiting for 1730861458.29, time is 1730861455.10 2024-11-06T02:50:55Z- AHRS_M2 msg: 1730861455.50 epoch sec; seqNo:216 2024-11-06T02:50:55Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.28 psi 2024-11-06T02:50:55Z- handleDepth()- Depth_Keller msg: 1730861455.51 epoch sec; seqNo:239; depth 14.27 2024-11-06T02:50:55Z- handleDVL() - DVL msg epochMillisec: 1730861455.53 2024-11-06T02:50:55Z- handleDvl() - RDI_Pathfinder msg: 1730861455.53 sec; seqNo:80; #beams:4 2024-11-06T02:50:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.81 , 65.00 , 65.41 , 86.08 2024-11-06T02:50:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , 0.02 , -0.23 2024-11-06T02:50:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861455.56 epoch sec; seqNo:239; 4061466.36 north; 594569.74 east; 36.69 lat; -121.94 lon; 14.27 depth; utmZone: 10 2024-11-06T02:50:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861454.29 2024-11-06T02:50:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:55Z- waiting for 1730861458.29, time is 1730861455.56 2024-11-06T02:50:55Z- AHRS_M2 msg: 1730861455.89 epoch sec; seqNo:217 2024-11-06T02:50:55Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.29 psi 2024-11-06T02:50:55Z- handleDepth()- Depth_Keller msg: 1730861455.90 epoch sec; seqNo:240; depth 14.32 2024-11-06T02:50:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861455.92 epoch sec; seqNo:240; 4061466.36 north; 594569.74 east; 36.69 lat; -121.94 lon; 14.32 depth; utmZone: 10 2024-11-06T02:50:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861454.29 2024-11-06T02:50:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:56Z- waiting for 1730861458.29, time is 1730861455.92 2024-11-06T02:50:56Z- AHRS_M2 msg: 1730861456.30 epoch sec; seqNo:218 2024-11-06T02:50:56Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.29 psi 2024-11-06T02:50:56Z- handleDepth()- Depth_Keller msg: 1730861456.30 epoch sec; seqNo:241; depth 14.36 2024-11-06T02:50:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861456.31 epoch sec; seqNo:241; 4061466.36 north; 594569.74 east; 36.69 lat; -121.94 lon; 14.36 depth; utmZone: 10 2024-11-06T02:50:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861454.29 2024-11-06T02:50:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:56Z- waiting for 1730861458.29, time is 1730861456.31 2024-11-06T02:50:56Z- AHRS_M2 msg: 1730861456.72 epoch sec; seqNo:219 2024-11-06T02:50:56Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.30 psi 2024-11-06T02:50:56Z- handleDepth()- Depth_Keller msg: 1730861456.73 epoch sec; seqNo:242; depth 14.42 2024-11-06T02:50:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861456.74 epoch sec; seqNo:242; 4061466.74 north; 594569.73 east; 36.69 lat; -121.94 lon; 14.42 depth; utmZone: 10 2024-11-06T02:50:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861454.29 2024-11-06T02:50:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:56Z- waiting for 1730861458.29, time is 1730861456.74 2024-11-06T02:50:57Z- AHRS_M2 msg: 1730861457.11 epoch sec; seqNo:220 2024-11-06T02:50:57Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.30 psi 2024-11-06T02:50:57Z- handleDepth()- Depth_Keller msg: 1730861457.12 epoch sec; seqNo:243; depth 14.45 2024-11-06T02:50:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861457.13 epoch sec; seqNo:243; 4061466.73 north; 594568.51 east; 36.69 lat; -121.94 lon; 14.45 depth; utmZone: 10 2024-11-06T02:50:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861454.29 2024-11-06T02:50:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:57Z- waiting for 1730861458.29, time is 1730861457.13 2024-11-06T02:50:57Z- AHRS_M2 msg: 1730861457.52 epoch sec; seqNo:221 2024-11-06T02:50:57Z- AHRS_M2 msg: 0.12 phi; -0.17 theta; 5.30 psi 2024-11-06T02:50:57Z- handleDepth()- Depth_Keller msg: 1730861457.53 epoch sec; seqNo:244; depth 14.52 2024-11-06T02:50:57Z- handleDVL() - DVL msg epochMillisec: 1730861457.55 2024-11-06T02:50:57Z- handleDvl() - RDI_Pathfinder msg: 1730861457.55 sec; seqNo:81; #beams:4 2024-11-06T02:50:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 80.67 , 63.77 , 64.18 , 0.00 2024-11-06T02:50:57Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.06 , -0.23 2024-11-06T02:50:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861457.56 epoch sec; seqNo:244; 4061467.11 north; 594568.51 east; 36.69 lat; -121.94 lon; 14.52 depth; utmZone: 10 2024-11-06T02:50:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861454.29 2024-11-06T02:50:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:57Z- waiting for 1730861458.29, time is 1730861457.56 2024-11-06T02:50:57Z- AHRS_M2 msg: 1730861457.91 epoch sec; seqNo:222 2024-11-06T02:50:57Z- AHRS_M2 msg: 0.12 phi; -0.17 theta; 5.30 psi 2024-11-06T02:50:57Z- handleDepth()- Depth_Keller msg: 1730861457.92 epoch sec; seqNo:245; depth 14.59 2024-11-06T02:50:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861457.93 epoch sec; seqNo:245; 4061467.48 north; 594568.50 east; 36.69 lat; -121.94 lon; 14.59 depth; utmZone: 10 2024-11-06T02:50:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861454.29 2024-11-06T02:50:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:58Z- waiting for 1730861458.29, time is 1730861457.93 2024-11-06T02:50:58Z- AHRS_M2 msg: 1730861458.32 epoch sec; seqNo:223 2024-11-06T02:50:58Z- AHRS_M2 msg: 0.13 phi; -0.17 theta; 5.30 psi 2024-11-06T02:50:58Z- handleDepth()- Depth_Keller msg: 1730861458.33 epoch sec; seqNo:246; depth 14.65 2024-11-06T02:50:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861458.34 epoch sec; seqNo:246; 4061467.48 north; 594568.50 east; 36.69 lat; -121.94 lon; 14.65 depth; utmZone: 10 2024-11-06T02:50:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861454.29 2024-11-06T02:50:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:58Z- waiting for 1730861458.29, time is 1730861458.34 2024-11-06T02:50:58Z- LcmTrn::updateTrn() >>>> pose time 1730861458.34, meas time 1730861457.55 <<<< 2024-11-06T02:50:58Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861458.34 2024-11-06T02:50:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861457.55 sec, ping # 0; waiting for INS data more recent than 1730861454.29... 2024-11-06T02:50:58Z- TerrainNav::motionUpdate() - currEstimate = (4061467, 594568.50, 14.65) 2024-11-06T02:50:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:50:58Z- TerrainNav:: SubcloudNIS is 0.23, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:50:58Z- TNavPF::Projecting Measurements 2024-11-06T02:50:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:50:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:50:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:50:58Z- TNavFilter::Excluded 1 beam(s) from correlation due to poor sonar data. 2024-11-06T02:50:58Z- TNavFilter::3 good beams. 2024-11-06T02:50:58Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:50:58Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:50:58Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:50:58Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:50:58Z- TNavPF:: beamIndex[3] = 0 2024-11-06T02:50:58Z- TNavPF:: *** beamsVF.Ncols() = 3 *** 2024-11-06T02:50:58Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:50:58Z- TerrainMapOctree: (vehN, vehE ) = (4061467.12, 594568.98). 2024-11-06T02:50:58Z- TerrainMapOctree: Min Distance = 282.68. 2024-11-06T02:50:58Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:50:58Z- Forcing Subcloud Comparison 2024-11-06T02:50:58Z- Weighting particles with subcloud comparison 2024-11-06T02:50:58Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:58Z- meanExpectedMeasDiff: 2.210530 delta_rms_squared: 2.950022 alpha: 0.993266 2024-11-06T02:50:58Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:58Z- meanExpectedMeasDiff: -0.997581 delta_rms_squared: 2.073199 alpha: 0.990445 2024-11-06T02:50:58Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:50:58Z- meanExpectedMeasDiff: -0.288179 delta_rms_squared: 2.770879 alpha: 0.992834 2024-11-06T02:50:58Z- Subcloud NIS: 0.234850 2024-11-06T02:50:58Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:50:58Z- TNavPF:: sumWeights = 0.561991 2024-11-06T02:50:58Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:50:58Z- TNavPF::First Term of Map Covariance Matrix is 1.54 2024-11-06T02:50:58Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.06 2024-11-06T02:50:58Z- TerrainNav::Resampling particle filter... 2024-11-06T02:50:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861457.55 sec. 2024-11-06T02:50:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:50:58Z- variances([0], [2], and psi_berg) = 101.445908 96.386162 0.000000 2024-11-06T02:50:58Z- publishEstimatesDecoder() - at 1730861458.34 sec 2024-11-06T02:50:58Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:50:58Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061523.42 594573.34 24.86 2024-11-06T02:50:58Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061516.07 594554.50 24.20 lat/lon: 36.69 -121.94 2024-11-06T02:50:58Z- LcmTrn::publishEstimatesDecoder() - COVAR: 101.45 96.39 1.43 0.00 2024-11-06T02:50:58Z- AHRS_M2 msg: 1730861458.73 epoch sec; seqNo:224 2024-11-06T02:50:58Z- AHRS_M2 msg: 0.13 phi; -0.17 theta; 5.29 psi 2024-11-06T02:50:58Z- handleDepth()- Depth_Keller msg: 1730861458.74 epoch sec; seqNo:247; depth 14.71 2024-11-06T02:50:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861458.76 epoch sec; seqNo:247; 4061467.85 north; 594567.28 east; 36.69 lat; -121.94 lon; 14.71 depth; utmZone: 10 2024-11-06T02:50:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:50:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861458.34 2024-11-06T02:50:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:58Z- waiting for 1730861462.34, time is 1730861458.76 2024-11-06T02:50:59Z- AHRS_M2 msg: 1730861459.12 epoch sec; seqNo:225 2024-11-06T02:50:59Z- AHRS_M2 msg: 0.13 phi; -0.17 theta; 5.29 psi 2024-11-06T02:50:59Z- handleDepth()- Depth_Keller msg: 1730861459.14 epoch sec; seqNo:248; depth 14.77 2024-11-06T02:50:59Z- handleDVL() - DVL msg epochMillisec: 1730861459.14 2024-11-06T02:50:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:50:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:50:59Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:50:59Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:50:59Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:50:59Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:50:59Z- handleDvl() - RDI_Pathfinder msg: 1730861459.14 sec; seqNo:82; #beams:0 2024-11-06T02:50:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:50:59Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.06 , -0.23 2024-11-06T02:50:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861459.16 epoch sec; seqNo:248; 4061467.85 north; 594567.28 east; 36.69 lat; -121.94 lon; 14.77 depth; utmZone: 10 2024-11-06T02:50:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861458.34 2024-11-06T02:50:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:59Z- waiting for 1730861462.34, time is 1730861459.16 2024-11-06T02:50:59Z- AHRS_M2 msg: 1730861459.53 epoch sec; seqNo:226 2024-11-06T02:50:59Z- AHRS_M2 msg: 0.14 phi; -0.17 theta; 5.29 psi 2024-11-06T02:50:59Z- handleDepth()- Depth_Keller msg: 1730861459.54 epoch sec; seqNo:249; depth 14.84 2024-11-06T02:50:59Z- handleDVL() - DVL msg epochMillisec: 1730861459.58 2024-11-06T02:50:59Z- handleDvl() - RDI_Pathfinder msg: 1730861459.58 sec; seqNo:83; #beams:4 2024-11-06T02:50:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.59 , 63.78 , 64.19 , 83.92 2024-11-06T02:50:59Z- handleDvl() - RDI_Pathfinder msg: velocities 1.20 , 0.02 , 0.02 2024-11-06T02:50:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861459.59 epoch sec; seqNo:249; 4061468.23 north; 594567.27 east; 36.69 lat; -121.94 lon; 14.84 depth; utmZone: 10 2024-11-06T02:50:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:50:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861458.34 2024-11-06T02:50:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:50:59Z- waiting for 1730861462.34, time is 1730861459.59 2024-11-06T02:50:59Z- AHRS_M2 msg: 1730861459.93 epoch sec; seqNo:227 2024-11-06T02:50:59Z- AHRS_M2 msg: 0.14 phi; -0.17 theta; 5.29 psi 2024-11-06T02:50:59Z- handleDepth()- Depth_Keller msg: 1730861459.94 epoch sec; seqNo:250; depth 14.90 2024-11-06T02:50:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861459.95 epoch sec; seqNo:250; 4061468.59 north; 594566.05 east; 36.69 lat; -121.94 lon; 14.90 depth; utmZone: 10 2024-11-06T02:51:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861458.34 2024-11-06T02:51:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:00Z- waiting for 1730861462.34, time is 1730861459.95 2024-11-06T02:51:00Z- AHRS_M2 msg: 1730861460.34 epoch sec; seqNo:228 2024-11-06T02:51:00Z- AHRS_M2 msg: 0.14 phi; -0.17 theta; 5.28 psi 2024-11-06T02:51:00Z- handleDepth()- Depth_Keller msg: 1730861460.34 epoch sec; seqNo:251; depth 14.96 2024-11-06T02:51:00Z- handleDVL() - DVL msg epochMillisec: 1730861460.34 2024-11-06T02:51:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:00Z- handleDvl() - RDI_Pathfinder msg: 1730861460.34 sec; seqNo:84; #beams:0 2024-11-06T02:51:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:00Z- handleDvl() - RDI_Pathfinder msg: velocities 1.20 , 0.02 , 0.02 2024-11-06T02:51:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861460.35 epoch sec; seqNo:251; 4061468.59 north; 594566.05 east; 36.69 lat; -121.94 lon; 14.96 depth; utmZone: 10 2024-11-06T02:51:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861458.34 2024-11-06T02:51:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:00Z- waiting for 1730861462.34, time is 1730861460.35 2024-11-06T02:51:00Z- AHRS_M2 msg: 1730861460.74 epoch sec; seqNo:229 2024-11-06T02:51:00Z- AHRS_M2 msg: 0.14 phi; -0.17 theta; 5.28 psi 2024-11-06T02:51:00Z- handleDepth()- Depth_Keller msg: 1730861460.75 epoch sec; seqNo:252; depth 15.02 2024-11-06T02:51:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861460.77 epoch sec; seqNo:252; 4061468.97 north; 594566.05 east; 36.69 lat; -121.94 lon; 15.02 depth; utmZone: 10 2024-11-06T02:51:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861458.34 2024-11-06T02:51:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:00Z- waiting for 1730861462.34, time is 1730861460.77 2024-11-06T02:51:01Z- AHRS_M2 msg: 1730861461.14 epoch sec; seqNo:230 2024-11-06T02:51:01Z- AHRS_M2 msg: 0.14 phi; -0.17 theta; 5.27 psi 2024-11-06T02:51:01Z- handleDepth()- Depth_Keller msg: 1730861461.15 epoch sec; seqNo:253; depth 15.11 2024-11-06T02:51:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861461.16 epoch sec; seqNo:253; 4061469.34 north; 594564.82 east; 36.69 lat; -121.94 lon; 15.11 depth; utmZone: 10 2024-11-06T02:51:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861458.34 2024-11-06T02:51:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:01Z- waiting for 1730861462.34, time is 1730861461.16 2024-11-06T02:51:01Z- AHRS_M2 msg: 1730861461.55 epoch sec; seqNo:231 2024-11-06T02:51:01Z- AHRS_M2 msg: 0.14 phi; -0.17 theta; 5.27 psi 2024-11-06T02:51:01Z- handleDepth()- Depth_Keller msg: 1730861461.55 epoch sec; seqNo:254; depth 15.16 2024-11-06T02:51:01Z- handleDVL() - DVL msg epochMillisec: 1730861461.58 2024-11-06T02:51:01Z- handleDvl() - RDI_Pathfinder msg: 1730861461.58 sec; seqNo:85; #beams:4 2024-11-06T02:51:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.58 , 63.77 , 65.00 , 78.90 2024-11-06T02:51:01Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.02 , 0.31 2024-11-06T02:51:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861461.61 epoch sec; seqNo:254; 4061469.34 north; 594564.82 east; 36.69 lat; -121.94 lon; 15.16 depth; utmZone: 10 2024-11-06T02:51:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861458.34 2024-11-06T02:51:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:01Z- waiting for 1730861462.34, time is 1730861461.61 2024-11-06T02:51:01Z- AHRS_M2 msg: 1730861461.95 epoch sec; seqNo:232 2024-11-06T02:51:01Z- AHRS_M2 msg: 0.14 phi; -0.18 theta; 5.26 psi 2024-11-06T02:51:01Z- handleDepth()- Depth_Keller msg: 1730861461.96 epoch sec; seqNo:255; depth 15.23 2024-11-06T02:51:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861461.98 epoch sec; seqNo:255; 4061469.72 north; 594564.82 east; 36.69 lat; -121.94 lon; 15.23 depth; utmZone: 10 2024-11-06T02:51:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861458.34 2024-11-06T02:51:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:02Z- waiting for 1730861462.34, time is 1730861461.98 2024-11-06T02:51:02Z- AHRS_M2 msg: 1730861462.36 epoch sec; seqNo:233 2024-11-06T02:51:02Z- AHRS_M2 msg: 0.14 phi; -0.18 theta; 5.26 psi 2024-11-06T02:51:02Z- handleDepth()- Depth_Keller msg: 1730861462.36 epoch sec; seqNo:256; depth 15.28 2024-11-06T02:51:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861462.37 epoch sec; seqNo:256; 4061469.72 north; 594564.82 east; 36.69 lat; -121.94 lon; 15.28 depth; utmZone: 10 2024-11-06T02:51:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861458.34 2024-11-06T02:51:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:02Z- waiting for 1730861462.34, time is 1730861462.37 2024-11-06T02:51:02Z- LcmTrn::updateTrn() >>>> pose time 1730861462.37, meas time 1730861461.58 <<<< 2024-11-06T02:51:02Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861462.37 2024-11-06T02:51:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861461.58 sec, ping # 0; waiting for INS data more recent than 1730861458.34... 2024-11-06T02:51:02Z- TerrainNav::motionUpdate() - currEstimate = (4061470, 594564.82, 15.28) 2024-11-06T02:51:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:51:02Z- TerrainNav:: SubcloudNIS is 0.23, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:51:02Z- TNavPF::Projecting Measurements 2024-11-06T02:51:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:51:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:51:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:51:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:51:02Z- TNavFilter::4 good beams. 2024-11-06T02:51:02Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:51:02Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:51:02Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:51:02Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:51:02Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:51:02Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:51:02Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:51:02Z- TerrainMapOctree: (vehN, vehE ) = (4061469.28, 594565.54). 2024-11-06T02:51:02Z- TerrainMapOctree: Min Distance = 286.55. 2024-11-06T02:51:02Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:51:02Z- Forcing Subcloud Comparison 2024-11-06T02:51:02Z- Weighting particles with subcloud comparison 2024-11-06T02:51:02Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:02Z- meanExpectedMeasDiff: -5.076723 delta_rms_squared: 2.971151 alpha: 0.993314 2024-11-06T02:51:02Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:02Z- meanExpectedMeasDiff: -0.161196 delta_rms_squared: 1.974167 alpha: 0.989971 2024-11-06T02:51:02Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:02Z- meanExpectedMeasDiff: 1.143263 delta_rms_squared: 2.521421 alpha: 0.992130 2024-11-06T02:51:02Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:02Z- meanExpectedMeasDiff: 1.181883 delta_rms_squared: 3.783253 alpha: 0.994741 2024-11-06T02:51:02Z- Subcloud NIS: 0.258169 2024-11-06T02:51:02Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:51:02Z- TNavPF:: sumWeights = 0.199776 2024-11-06T02:51:02Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:51:02Z- TNavPF::First Term of Map Covariance Matrix is 2.75 2024-11-06T02:51:02Z- TNavPF::Calculated NIS Value : 0.07 numBeams normalized NIS: 0.02 2024-11-06T02:51:02Z- TerrainNav::Resampling particle filter... 2024-11-06T02:51:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861461.58 sec. 2024-11-06T02:51:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:51:02Z- variances([0], [2], and psi_berg) = 96.705117 70.671994 0.000000 2024-11-06T02:51:02Z- publishEstimatesDecoder() - at 1730861462.37 sec 2024-11-06T02:51:02Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:51:02Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061525.71 594569.58 25.51 2024-11-06T02:51:02Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061513.55 594551.00 25.31 lat/lon: 36.69 -121.94 2024-11-06T02:51:02Z- LcmTrn::publishEstimatesDecoder() - COVAR: 96.71 70.67 1.22 0.00 2024-11-06T02:51:02Z- AHRS_M2 msg: 1730861462.76 epoch sec; seqNo:234 2024-11-06T02:51:02Z- AHRS_M2 msg: 0.13 phi; -0.19 theta; 5.25 psi 2024-11-06T02:51:02Z- handleDepth()- Depth_Keller msg: 1730861462.77 epoch sec; seqNo:257; depth 15.34 2024-11-06T02:51:02Z- handleDVL() - DVL msg epochMillisec: 1730861462.77 2024-11-06T02:51:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:02Z- handleDvl() - RDI_Pathfinder msg: 1730861462.77 sec; seqNo:86; #beams:0 2024-11-06T02:51:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:02Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.02 , 0.31 2024-11-06T02:51:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861462.78 epoch sec; seqNo:257; 4061470.08 north; 594563.59 east; 36.69 lat; -121.94 lon; 15.34 depth; utmZone: 10 2024-11-06T02:51:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861462.37 2024-11-06T02:51:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:02Z- waiting for 1730861466.37, time is 1730861462.78 2024-11-06T02:51:03Z- AHRS_M2 msg: 1730861463.17 epoch sec; seqNo:235 2024-11-06T02:51:03Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.25 psi 2024-11-06T02:51:03Z- handleDepth()- Depth_Keller msg: 1730861463.17 epoch sec; seqNo:258; depth 15.39 2024-11-06T02:51:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861463.19 epoch sec; seqNo:258; 4061470.08 north; 594563.59 east; 36.69 lat; -121.94 lon; 15.39 depth; utmZone: 10 2024-11-06T02:51:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861462.37 2024-11-06T02:51:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:03Z- waiting for 1730861466.37, time is 1730861463.19 2024-11-06T02:51:03Z- AHRS_M2 msg: 1730861463.57 epoch sec; seqNo:236 2024-11-06T02:51:03Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.25 psi 2024-11-06T02:51:03Z- handleDepth()- Depth_Keller msg: 1730861463.57 epoch sec; seqNo:259; depth 15.44 2024-11-06T02:51:03Z- handleDVL() - DVL msg epochMillisec: 1730861463.60 2024-11-06T02:51:03Z- handleDvl() - RDI_Pathfinder msg: 1730861463.60 sec; seqNo:87; #beams:4 2024-11-06T02:51:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.18 , 64.76 , 65.41 , 80.30 2024-11-06T02:51:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.07 , 0.22 2024-11-06T02:51:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861463.62 epoch sec; seqNo:259; 4061470.46 north; 594563.59 east; 36.69 lat; -121.94 lon; 15.44 depth; utmZone: 10 2024-11-06T02:51:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861462.37 2024-11-06T02:51:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:03Z- waiting for 1730861466.37, time is 1730861463.62 2024-11-06T02:51:03Z- AHRS_M2 msg: 1730861463.97 epoch sec; seqNo:237 2024-11-06T02:51:03Z- AHRS_M2 msg: 0.11 phi; -0.21 theta; 5.26 psi 2024-11-06T02:51:03Z- handleDepth()- Depth_Keller msg: 1730861463.98 epoch sec; seqNo:260; depth 15.49 2024-11-06T02:51:03Z- handleDVL() - DVL msg epochMillisec: 1730861463.98 2024-11-06T02:51:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:03Z- handleDvl() - RDI_Pathfinder msg: 1730861463.98 sec; seqNo:88; #beams:0 2024-11-06T02:51:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.07 , 0.22 2024-11-06T02:51:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861464.01 epoch sec; seqNo:260; 4061470.46 north; 594563.59 east; 36.69 lat; -121.94 lon; 15.49 depth; utmZone: 10 2024-11-06T02:51:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861462.37 2024-11-06T02:51:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:04Z- waiting for 1730861466.37, time is 1730861464.01 2024-11-06T02:51:04Z- AHRS_M2 msg: 1730861464.38 epoch sec; seqNo:238 2024-11-06T02:51:04Z- AHRS_M2 msg: 0.11 phi; -0.22 theta; 5.26 psi 2024-11-06T02:51:04Z- handleDepth()- Depth_Keller msg: 1730861464.38 epoch sec; seqNo:261; depth 15.54 2024-11-06T02:51:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861464.39 epoch sec; seqNo:261; 4061470.45 north; 594562.37 east; 36.69 lat; -121.94 lon; 15.54 depth; utmZone: 10 2024-11-06T02:51:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861462.37 2024-11-06T02:51:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:04Z- waiting for 1730861466.37, time is 1730861464.39 2024-11-06T02:51:04Z- AHRS_M2 msg: 1730861464.79 epoch sec; seqNo:239 2024-11-06T02:51:04Z- AHRS_M2 msg: 0.12 phi; -0.22 theta; 5.27 psi 2024-11-06T02:51:04Z- handleDepth()- Depth_Keller msg: 1730861464.79 epoch sec; seqNo:262; depth 15.60 2024-11-06T02:51:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861464.80 epoch sec; seqNo:262; 4061470.45 north; 594562.37 east; 36.69 lat; -121.94 lon; 15.60 depth; utmZone: 10 2024-11-06T02:51:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861462.37 2024-11-06T02:51:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:04Z- waiting for 1730861466.37, time is 1730861464.80 2024-11-06T02:51:05Z- AHRS_M2 msg: 1730861465.18 epoch sec; seqNo:240 2024-11-06T02:51:05Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.27 psi 2024-11-06T02:51:05Z- handleDepth()- Depth_Keller msg: 1730861465.19 epoch sec; seqNo:263; depth 15.64 2024-11-06T02:51:05Z- handleDVL() - DVL msg epochMillisec: 1730861465.19 2024-11-06T02:51:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:05Z- handleDvl() - RDI_Pathfinder msg: 1730861465.19 sec; seqNo:89; #beams:0 2024-11-06T02:51:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.07 , 0.22 2024-11-06T02:51:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861465.21 epoch sec; seqNo:263; 4061470.83 north; 594562.36 east; 36.69 lat; -121.94 lon; 15.64 depth; utmZone: 10 2024-11-06T02:51:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861462.37 2024-11-06T02:51:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:05Z- waiting for 1730861466.37, time is 1730861465.21 2024-11-06T02:51:05Z- AHRS_M2 msg: 1730861465.59 epoch sec; seqNo:241 2024-11-06T02:51:05Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.28 psi 2024-11-06T02:51:05Z- handleDepth()- Depth_Keller msg: 1730861465.59 epoch sec; seqNo:264; depth 15.70 2024-11-06T02:51:05Z- handleDVL() - DVL msg epochMillisec: 1730861465.62 2024-11-06T02:51:05Z- handleDvl() - RDI_Pathfinder msg: 1730861465.62 sec; seqNo:90; #beams:4 2024-11-06T02:51:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.36 , 64.59 , 63.77 , 87.17 2024-11-06T02:51:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.63 , 0.00 , -0.14 2024-11-06T02:51:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861465.64 epoch sec; seqNo:264; 4061470.83 north; 594562.36 east; 36.69 lat; -121.94 lon; 15.70 depth; utmZone: 10 2024-11-06T02:51:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861462.37 2024-11-06T02:51:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:05Z- waiting for 1730861466.37, time is 1730861465.64 2024-11-06T02:51:05Z- AHRS_M2 msg: 1730861465.99 epoch sec; seqNo:242 2024-11-06T02:51:05Z- AHRS_M2 msg: 0.15 phi; -0.23 theta; 5.28 psi 2024-11-06T02:51:05Z- handleDepth()- Depth_Keller msg: 1730861466.00 epoch sec; seqNo:265; depth 15.76 2024-11-06T02:51:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861466.01 epoch sec; seqNo:265; 4061470.83 north; 594562.36 east; 36.69 lat; -121.94 lon; 15.76 depth; utmZone: 10 2024-11-06T02:51:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861462.37 2024-11-06T02:51:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:06Z- waiting for 1730861466.37, time is 1730861466.01 2024-11-06T02:51:06Z- AHRS_M2 msg: 1730861466.40 epoch sec; seqNo:243 2024-11-06T02:51:06Z- AHRS_M2 msg: 0.15 phi; -0.23 theta; 5.29 psi 2024-11-06T02:51:06Z- handleDepth()- Depth_Keller msg: 1730861466.40 epoch sec; seqNo:266; depth 15.81 2024-11-06T02:51:06Z- handleDVL() - DVL msg epochMillisec: 1730861466.40 2024-11-06T02:51:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:06Z- handleDvl() - RDI_Pathfinder msg: 1730861466.40 sec; seqNo:91; #beams:0 2024-11-06T02:51:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.63 , 0.00 , -0.14 2024-11-06T02:51:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861466.41 epoch sec; seqNo:266; 4061471.21 north; 594562.36 east; 36.69 lat; -121.94 lon; 15.81 depth; utmZone: 10 2024-11-06T02:51:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861462.37 2024-11-06T02:51:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:06Z- waiting for 1730861466.37, time is 1730861466.41 2024-11-06T02:51:06Z- LcmTrn::updateTrn() >>>> pose time 1730861466.41, meas time 1730861466.40 <<<< 2024-11-06T02:51:06Z- LcmTrn::updateTrn() equating pose 1730861466.40 and meas times 1730861466.40 2024-11-06T02:51:06Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861466.40 2024-11-06T02:51:06Z- TerrainNav::motionUpdate() - currEstimate = (4061471, 594562.36, 15.81) 2024-11-06T02:51:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:51:06Z- TerrainNav:: SubcloudNIS is 0.26, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:51:06Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:51:06Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:51:06Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:51:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:51:06Z- variances([0], [2], and psi_berg) = 96.702030 70.671631 0.000000 2024-11-06T02:51:06Z- publishEstimatesDecoder() - at 1730861466.40 sec 2024-11-06T02:51:06Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:51:06Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061527.17 594567.13 26.17 2024-11-06T02:51:06Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061515.04 594548.54 25.83 lat/lon: 36.69 -121.94 2024-11-06T02:51:06Z- LcmTrn::publishEstimatesDecoder() - COVAR: 96.70 70.67 1.46 0.00 2024-11-06T02:51:06Z- AHRS_M2 msg: 1730861466.80 epoch sec; seqNo:244 2024-11-06T02:51:06Z- AHRS_M2 msg: 0.15 phi; -0.22 theta; 5.29 psi 2024-11-06T02:51:06Z- handleDepth()- Depth_Keller msg: 1730861466.81 epoch sec; seqNo:267; depth 15.86 2024-11-06T02:51:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861466.86 epoch sec; seqNo:267; 4061471.19 north; 594561.14 east; 36.69 lat; -121.94 lon; 15.86 depth; utmZone: 10 2024-11-06T02:51:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861466.40 2024-11-06T02:51:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:06Z- waiting for 1730861470.40, time is 1730861466.86 2024-11-06T02:51:07Z- AHRS_M2 msg: 1730861467.21 epoch sec; seqNo:245 2024-11-06T02:51:07Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.29 psi 2024-11-06T02:51:07Z- handleDepth()- Depth_Keller msg: 1730861467.21 epoch sec; seqNo:268; depth 15.93 2024-11-06T02:51:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861467.26 epoch sec; seqNo:268; 4061471.19 north; 594561.14 east; 36.69 lat; -121.94 lon; 15.93 depth; utmZone: 10 2024-11-06T02:51:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861466.40 2024-11-06T02:51:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:07Z- waiting for 1730861470.40, time is 1730861467.26 2024-11-06T02:51:07Z- AHRS_M2 msg: 1730861467.61 epoch sec; seqNo:246 2024-11-06T02:51:07Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.29 psi 2024-11-06T02:51:07Z- handleDepth()- Depth_Keller msg: 1730861467.61 epoch sec; seqNo:269; depth 16.00 2024-11-06T02:51:07Z- handleDVL() - DVL msg epochMillisec: 1730861467.64 2024-11-06T02:51:07Z- handleDvl() - RDI_Pathfinder msg: 1730861467.64 sec; seqNo:92; #beams:4 2024-11-06T02:51:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.40 , 64.18 , 62.96 , 88.25 2024-11-06T02:51:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.10 , -0.22 2024-11-06T02:51:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861467.67 epoch sec; seqNo:269; 4061471.57 north; 594561.13 east; 36.69 lat; -121.94 lon; 16.00 depth; utmZone: 10 2024-11-06T02:51:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861466.40 2024-11-06T02:51:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:07Z- waiting for 1730861470.40, time is 1730861467.67 2024-11-06T02:51:08Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T02:51:08Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T02:51:08Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T02:51:08Z- AHRS_M2 msg: 1730861468.00 epoch sec; seqNo:247 2024-11-06T02:51:08Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.29 psi 2024-11-06T02:51:08Z- handleDepth()- Depth_Keller msg: 1730861468.03 epoch sec; seqNo:270; depth 16.07 2024-11-06T02:51:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861468.05 epoch sec; seqNo:270; 4061471.95 north; 594561.13 east; 36.69 lat; -121.94 lon; 16.07 depth; utmZone: 10 2024-11-06T02:51:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861466.40 2024-11-06T02:51:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:08Z- waiting for 1730861470.40, time is 1730861468.05 2024-11-06T02:51:08Z- AHRS_M2 msg: 1730861468.41 epoch sec; seqNo:248 2024-11-06T02:51:08Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.28 psi 2024-11-06T02:51:08Z- handleDepth()- Depth_Keller msg: 1730861468.42 epoch sec; seqNo:271; depth 16.13 2024-11-06T02:51:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861468.44 epoch sec; seqNo:271; 4061472.32 north; 594559.91 east; 36.69 lat; -121.94 lon; 16.13 depth; utmZone: 10 2024-11-06T02:51:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861466.40 2024-11-06T02:51:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:08Z- waiting for 1730861470.40, time is 1730861468.44 2024-11-06T02:51:08Z- AHRS_M2 msg: 1730861468.82 epoch sec; seqNo:249 2024-11-06T02:51:08Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.27 psi 2024-11-06T02:51:08Z- handleDepth()- Depth_Keller msg: 1730861468.83 epoch sec; seqNo:272; depth 16.20 2024-11-06T02:51:08Z- handleDVL() - DVL msg epochMillisec: 1730861468.83 2024-11-06T02:51:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:08Z- handleDvl() - RDI_Pathfinder msg: 1730861468.83 sec; seqNo:93; #beams:0 2024-11-06T02:51:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.10 , -0.22 2024-11-06T02:51:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861468.84 epoch sec; seqNo:272; 4061472.32 north; 594559.91 east; 36.69 lat; -121.94 lon; 16.20 depth; utmZone: 10 2024-11-06T02:51:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861466.40 2024-11-06T02:51:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:08Z- waiting for 1730861470.40, time is 1730861468.84 2024-11-06T02:51:09Z- AHRS_M2 msg: 1730861469.24 epoch sec; seqNo:250 2024-11-06T02:51:09Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.26 psi 2024-11-06T02:51:09Z- handleDepth()- Depth_Keller msg: 1730861469.26 epoch sec; seqNo:273; depth 16.27 2024-11-06T02:51:09Z- handleDVL() - DVL msg epochMillisec: 1730861469.32 2024-11-06T02:51:09Z- handleDvl() - RDI_Pathfinder msg: 1730861469.32 sec; seqNo:94; #beams:4 2024-11-06T02:51:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.18 , 64.18 , 62.14 , 87.17 2024-11-06T02:51:09Z- handleDvl() - RDI_Pathfinder msg: velocities 1.12 , 0.07 , -0.04 2024-11-06T02:51:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861469.33 epoch sec; seqNo:273; 4061472.69 north; 594559.90 east; 36.69 lat; -121.94 lon; 16.27 depth; utmZone: 10 2024-11-06T02:51:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861466.40 2024-11-06T02:51:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:09Z- waiting for 1730861470.40, time is 1730861469.33 2024-11-06T02:51:09Z- AHRS_M2 msg: 1730861469.68 epoch sec; seqNo:251 2024-11-06T02:51:09Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.26 psi 2024-11-06T02:51:09Z- handleDepth()- Depth_Keller msg: 1730861469.69 epoch sec; seqNo:274; depth 16.36 2024-11-06T02:51:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861469.71 epoch sec; seqNo:274; 4061473.07 north; 594559.90 east; 36.69 lat; -121.94 lon; 16.36 depth; utmZone: 10 2024-11-06T02:51:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861466.40 2024-11-06T02:51:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:09Z- waiting for 1730861470.40, time is 1730861469.71 2024-11-06T02:51:10Z- AHRS_M2 msg: 1730861470.08 epoch sec; seqNo:252 2024-11-06T02:51:10Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.25 psi 2024-11-06T02:51:10Z- handleDepth()- Depth_Keller msg: 1730861470.09 epoch sec; seqNo:275; depth 16.43 2024-11-06T02:51:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861470.10 epoch sec; seqNo:275; 4061473.06 north; 594558.68 east; 36.69 lat; -121.94 lon; 16.43 depth; utmZone: 10 2024-11-06T02:51:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861466.40 2024-11-06T02:51:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:10Z- waiting for 1730861470.40, time is 1730861470.10 2024-11-06T02:51:10Z- AHRS_M2 msg: 1730861470.49 epoch sec; seqNo:253 2024-11-06T02:51:10Z- AHRS_M2 msg: 0.13 phi; -0.24 theta; 5.24 psi 2024-11-06T02:51:10Z- handleDepth()- Depth_Keller msg: 1730861470.49 epoch sec; seqNo:276; depth 16.51 2024-11-06T02:51:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861470.50 epoch sec; seqNo:276; 4061473.44 north; 594558.67 east; 36.69 lat; -121.94 lon; 16.51 depth; utmZone: 10 2024-11-06T02:51:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861466.40 2024-11-06T02:51:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:10Z- waiting for 1730861470.40, time is 1730861470.50 2024-11-06T02:51:10Z- LcmTrn::updateTrn() >>>> pose time 1730861470.50, meas time 1730861469.32 <<<< 2024-11-06T02:51:10Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861470.50 2024-11-06T02:51:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861469.32 sec, ping # 0; waiting for INS data more recent than 1730861466.40... 2024-11-06T02:51:10Z- TerrainNav::motionUpdate() - currEstimate = (4061473, 594558.67, 16.51) 2024-11-06T02:51:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:51:10Z- TerrainNav:: SubcloudNIS is 0.26, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:51:10Z- TNavPF::Projecting Measurements 2024-11-06T02:51:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:51:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:51:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:51:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:51:10Z- TNavFilter::4 good beams. 2024-11-06T02:51:10Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:51:10Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:51:10Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:51:10Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:51:10Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:51:10Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:51:10Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:51:10Z- TerrainMapOctree: (vehN, vehE ) = (4061472.79, 594559.74). 2024-11-06T02:51:10Z- TerrainMapOctree: Min Distance = 292.98. 2024-11-06T02:51:10Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:51:10Z- Forcing Subcloud Comparison 2024-11-06T02:51:10Z- Weighting particles with subcloud comparison 2024-11-06T02:51:10Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:10Z- meanExpectedMeasDiff: -4.488149 delta_rms_squared: 3.194354 alpha: 0.993778 2024-11-06T02:51:10Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:10Z- meanExpectedMeasDiff: 1.452389 delta_rms_squared: 2.151159 alpha: 0.990788 2024-11-06T02:51:10Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:10Z- meanExpectedMeasDiff: 0.304783 delta_rms_squared: 2.616511 alpha: 0.992414 2024-11-06T02:51:10Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:10Z- meanExpectedMeasDiff: 7.226410 delta_rms_squared: 3.744332 alpha: 0.994687 2024-11-06T02:51:10Z- Subcloud NIS: 0.286434 2024-11-06T02:51:10Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:51:10Z- TNavPF:: sumWeights = 0.042476 2024-11-06T02:51:10Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:51:10Z- TNavPF::First Term of Map Covariance Matrix is 0.87 2024-11-06T02:51:10Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T02:51:10Z- TerrainNav::Resampling particle filter... 2024-11-06T02:51:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861469.32 sec. 2024-11-06T02:51:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:51:10Z- variances([0], [2], and psi_berg) = 64.140864 51.200682 0.000000 2024-11-06T02:51:10Z- publishEstimatesDecoder() - at 1730861470.50 sec 2024-11-06T02:51:10Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:51:10Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061529.64 594563.38 24.95 2024-11-06T02:51:10Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061512.36 594546.22 26.34 lat/lon: 36.69 -121.94 2024-11-06T02:51:10Z- LcmTrn::publishEstimatesDecoder() - COVAR: 64.14 51.20 1.24 0.00 2024-11-06T02:51:10Z- handleDepth()- Depth_Keller msg: 1730861470.91 epoch sec; seqNo:277; depth 16.58 2024-11-06T02:51:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861470.92 epoch sec; seqNo:277; 4061473.82 north; 594558.67 east; 36.69 lat; -121.94 lon; 16.58 depth; utmZone: 10 2024-11-06T02:51:11Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:51:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861470.50 2024-11-06T02:51:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:11Z- waiting for 1730861474.50, time is 1730861470.92 2024-11-06T02:51:11Z- AHRS_M2 msg: 1730861471.30 epoch sec; seqNo:254 2024-11-06T02:51:11Z- AHRS_M2 msg: 0.12 phi; -0.24 theta; 5.23 psi 2024-11-06T02:51:11Z- handleDepth()- Depth_Keller msg: 1730861471.31 epoch sec; seqNo:278; depth 16.68 2024-11-06T02:51:11Z- handleDVL() - DVL msg epochMillisec: 1730861471.33 2024-11-06T02:51:11Z- handleDvl() - RDI_Pathfinder msg: 1730861471.33 sec; seqNo:95; #beams:4 2024-11-06T02:51:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.54 , 63.77 , 62.14 , 88.25 2024-11-06T02:51:11Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , -0.01 , 0.17 2024-11-06T02:51:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861471.36 epoch sec; seqNo:278; 4061473.80 north; 594557.45 east; 36.69 lat; -121.94 lon; 16.68 depth; utmZone: 10 2024-11-06T02:51:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861470.50 2024-11-06T02:51:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:11Z- waiting for 1730861474.50, time is 1730861471.36 2024-11-06T02:51:11Z- AHRS_M2 msg: 1730861471.70 epoch sec; seqNo:255 2024-11-06T02:51:11Z- AHRS_M2 msg: 0.12 phi; -0.24 theta; 5.23 psi 2024-11-06T02:51:11Z- handleDepth()- Depth_Keller msg: 1730861471.70 epoch sec; seqNo:279; depth 16.77 2024-11-06T02:51:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861471.72 epoch sec; seqNo:279; 4061474.18 north; 594557.44 east; 36.69 lat; -121.94 lon; 16.77 depth; utmZone: 10 2024-11-06T02:51:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861470.50 2024-11-06T02:51:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:11Z- waiting for 1730861474.50, time is 1730861471.72 2024-11-06T02:51:12Z- AHRS_M2 msg: 1730861472.10 epoch sec; seqNo:256 2024-11-06T02:51:12Z- AHRS_M2 msg: 0.12 phi; -0.25 theta; 5.22 psi 2024-11-06T02:51:12Z- handleDepth()- Depth_Keller msg: 1730861472.11 epoch sec; seqNo:280; depth 16.84 2024-11-06T02:51:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861472.12 epoch sec; seqNo:280; 4061474.18 north; 594557.44 east; 36.69 lat; -121.94 lon; 16.84 depth; utmZone: 10 2024-11-06T02:51:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861470.50 2024-11-06T02:51:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:12Z- waiting for 1730861474.50, time is 1730861472.12 2024-11-06T02:51:12Z- AHRS_M2 msg: 1730861472.51 epoch sec; seqNo:257 2024-11-06T02:51:12Z- AHRS_M2 msg: 0.12 phi; -0.25 theta; 5.22 psi 2024-11-06T02:51:12Z- handleDepth()- Depth_Keller msg: 1730861472.52 epoch sec; seqNo:281; depth 16.93 2024-11-06T02:51:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861472.53 epoch sec; seqNo:281; 4061474.56 north; 594557.44 east; 36.69 lat; -121.94 lon; 16.93 depth; utmZone: 10 2024-11-06T02:51:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861470.50 2024-11-06T02:51:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:12Z- waiting for 1730861474.50, time is 1730861472.53 2024-11-06T02:51:12Z- AHRS_M2 msg: 1730861472.91 epoch sec; seqNo:258 2024-11-06T02:51:12Z- AHRS_M2 msg: 0.13 phi; -0.26 theta; 5.22 psi 2024-11-06T02:51:12Z- handleDepth()- Depth_Keller msg: 1730861472.91 epoch sec; seqNo:282; depth 17.01 2024-11-06T02:51:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861472.92 epoch sec; seqNo:282; 4061474.55 north; 594556.22 east; 36.69 lat; -121.94 lon; 17.01 depth; utmZone: 10 2024-11-06T02:51:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861470.50 2024-11-06T02:51:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:13Z- waiting for 1730861474.50, time is 1730861472.92 2024-11-06T02:51:13Z- AHRS_M2 msg: 1730861473.31 epoch sec; seqNo:259 2024-11-06T02:51:13Z- AHRS_M2 msg: 0.13 phi; -0.26 theta; 5.22 psi 2024-11-06T02:51:13Z- handleDepth()- Depth_Keller msg: 1730861473.32 epoch sec; seqNo:283; depth 17.09 2024-11-06T02:51:13Z- handleDVL() - DVL msg epochMillisec: 1730861473.34 2024-11-06T02:51:13Z- handleDvl() - RDI_Pathfinder msg: 1730861473.34 sec; seqNo:96; #beams:4 2024-11-06T02:51:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.95 , 64.18 , 61.73 , 88.25 2024-11-06T02:51:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.18 2024-11-06T02:51:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861473.37 epoch sec; seqNo:283; 4061474.55 north; 594556.22 east; 36.69 lat; -121.94 lon; 17.09 depth; utmZone: 10 2024-11-06T02:51:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861470.50 2024-11-06T02:51:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:13Z- waiting for 1730861474.50, time is 1730861473.37 2024-11-06T02:51:13Z- AHRS_M2 msg: 1730861473.71 epoch sec; seqNo:260 2024-11-06T02:51:13Z- AHRS_M2 msg: 0.14 phi; -0.26 theta; 5.23 psi 2024-11-06T02:51:13Z- handleDepth()- Depth_Keller msg: 1730861473.73 epoch sec; seqNo:284; depth 17.20 2024-11-06T02:51:13Z- handleDVL() - DVL msg epochMillisec: 1730861473.73 2024-11-06T02:51:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:13Z- handleDvl() - RDI_Pathfinder msg: 1730861473.73 sec; seqNo:97; #beams:0 2024-11-06T02:51:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.18 2024-11-06T02:51:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861473.74 epoch sec; seqNo:284; 4061474.93 north; 594556.22 east; 36.69 lat; -121.94 lon; 17.20 depth; utmZone: 10 2024-11-06T02:51:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861470.50 2024-11-06T02:51:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:13Z- waiting for 1730861474.50, time is 1730861473.74 2024-11-06T02:51:14Z- AHRS_M2 msg: 1730861474.12 epoch sec; seqNo:261 2024-11-06T02:51:14Z- AHRS_M2 msg: 0.14 phi; -0.26 theta; 5.23 psi 2024-11-06T02:51:14Z- handleDepth()- Depth_Keller msg: 1730861474.12 epoch sec; seqNo:285; depth 17.27 2024-11-06T02:51:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861474.14 epoch sec; seqNo:285; 4061474.91 north; 594555.00 east; 36.69 lat; -121.94 lon; 17.27 depth; utmZone: 10 2024-11-06T02:51:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861470.50 2024-11-06T02:51:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:14Z- waiting for 1730861474.50, time is 1730861474.14 2024-11-06T02:51:14Z- AHRS_M2 msg: 1730861474.52 epoch sec; seqNo:262 2024-11-06T02:51:14Z- AHRS_M2 msg: 0.13 phi; -0.26 theta; 5.23 psi 2024-11-06T02:51:14Z- handleDepth()- Depth_Keller msg: 1730861474.53 epoch sec; seqNo:286; depth 17.36 2024-11-06T02:51:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861474.54 epoch sec; seqNo:286; 4061475.29 north; 594554.99 east; 36.69 lat; -121.94 lon; 17.36 depth; utmZone: 10 2024-11-06T02:51:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861470.50 2024-11-06T02:51:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:14Z- waiting for 1730861474.50, time is 1730861474.54 2024-11-06T02:51:14Z- LcmTrn::updateTrn() >>>> pose time 1730861474.54, meas time 1730861473.73 <<<< 2024-11-06T02:51:14Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861474.54 2024-11-06T02:51:14Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:51:14Z- TerrainNav::motionUpdate() - currEstimate = (4061475, 594554.99, 17.36) 2024-11-06T02:51:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:51:14Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:51:14Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:51:14Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:51:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:51:14Z- variances([0], [2], and psi_berg) = 64.152187 51.195116 0.000000 2024-11-06T02:51:14Z- publishEstimatesDecoder() - at 1730861474.54 sec 2024-11-06T02:51:14Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:51:14Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061531.52 594559.72 26.61 2024-11-06T02:51:14Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061514.22 594542.54 27.18 lat/lon: 36.69 -121.94 2024-11-06T02:51:14Z- LcmTrn::publishEstimatesDecoder() - COVAR: 64.15 51.20 1.48 0.00 2024-11-06T02:51:14Z- AHRS_M2 msg: 1730861474.93 epoch sec; seqNo:263 2024-11-06T02:51:14Z- AHRS_M2 msg: 0.13 phi; -0.26 theta; 5.23 psi 2024-11-06T02:51:14Z- handleDepth()- Depth_Keller msg: 1730861474.93 epoch sec; seqNo:287; depth 17.44 2024-11-06T02:51:14Z- handleDVL() - DVL msg epochMillisec: 1730861474.94 2024-11-06T02:51:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:14Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:14Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:14Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:14Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:14Z- handleDvl() - RDI_Pathfinder msg: 1730861474.94 sec; seqNo:98; #beams:0 2024-11-06T02:51:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.18 2024-11-06T02:51:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861474.95 epoch sec; seqNo:287; 4061475.29 north; 594554.99 east; 36.69 lat; -121.94 lon; 17.44 depth; utmZone: 10 2024-11-06T02:51:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861474.54 2024-11-06T02:51:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:15Z- waiting for 1730861478.54, time is 1730861474.95 2024-11-06T02:51:15Z- AHRS_M2 msg: 1730861475.33 epoch sec; seqNo:264 2024-11-06T02:51:15Z- AHRS_M2 msg: 0.12 phi; -0.26 theta; 5.23 psi 2024-11-06T02:51:15Z- handleDepth()- Depth_Keller msg: 1730861475.34 epoch sec; seqNo:288; depth 17.53 2024-11-06T02:51:15Z- handleDVL() - DVL msg epochMillisec: 1730861475.36 2024-11-06T02:51:15Z- handleDvl() - RDI_Pathfinder msg: 1730861475.36 sec; seqNo:99; #beams:4 2024-11-06T02:51:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.36 , 63.37 , 61.32 , 88.25 2024-11-06T02:51:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , 0.03 2024-11-06T02:51:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861475.38 epoch sec; seqNo:288; 4061475.29 north; 594554.99 east; 36.69 lat; -121.94 lon; 17.53 depth; utmZone: 10 2024-11-06T02:51:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861474.54 2024-11-06T02:51:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:15Z- waiting for 1730861478.54, time is 1730861475.38 2024-11-06T02:51:15Z- AHRS_M2 msg: 1730861475.73 epoch sec; seqNo:265 2024-11-06T02:51:15Z- AHRS_M2 msg: 0.12 phi; -0.26 theta; 5.23 psi 2024-11-06T02:51:15Z- handleDepth()- Depth_Keller msg: 1730861475.74 epoch sec; seqNo:289; depth 17.63 2024-11-06T02:51:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861475.76 epoch sec; seqNo:289; 4061475.67 north; 594554.99 east; 36.69 lat; -121.94 lon; 17.63 depth; utmZone: 10 2024-11-06T02:51:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861474.54 2024-11-06T02:51:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:15Z- waiting for 1730861478.54, time is 1730861475.76 2024-11-06T02:51:16Z- AHRS_M2 msg: 1730861476.14 epoch sec; seqNo:266 2024-11-06T02:51:16Z- AHRS_M2 msg: 0.12 phi; -0.26 theta; 5.23 psi 2024-11-06T02:51:16Z- handleDepth()- Depth_Keller msg: 1730861476.15 epoch sec; seqNo:290; depth 17.70 2024-11-06T02:51:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861476.16 epoch sec; seqNo:290; 4061475.66 north; 594553.77 east; 36.69 lat; -121.94 lon; 17.70 depth; utmZone: 10 2024-11-06T02:51:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861474.54 2024-11-06T02:51:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:16Z- waiting for 1730861478.54, time is 1730861476.16 2024-11-06T02:51:16Z- AHRS_M2 msg: 1730861476.54 epoch sec; seqNo:267 2024-11-06T02:51:16Z- AHRS_M2 msg: 0.12 phi; -0.26 theta; 5.23 psi 2024-11-06T02:51:16Z- handleDepth()- Depth_Keller msg: 1730861476.55 epoch sec; seqNo:291; depth 17.80 2024-11-06T02:51:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861476.56 epoch sec; seqNo:291; 4061476.04 north; 594553.76 east; 36.69 lat; -121.94 lon; 17.80 depth; utmZone: 10 2024-11-06T02:51:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861474.54 2024-11-06T02:51:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:16Z- waiting for 1730861478.54, time is 1730861476.56 2024-11-06T02:51:16Z- AHRS_M2 msg: 1730861476.95 epoch sec; seqNo:268 2024-11-06T02:51:16Z- AHRS_M2 msg: 0.12 phi; -0.26 theta; 5.23 psi 2024-11-06T02:51:16Z- handleDepth()- Depth_Keller msg: 1730861476.95 epoch sec; seqNo:292; depth 17.89 2024-11-06T02:51:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861476.97 epoch sec; seqNo:292; 4061476.04 north; 594553.76 east; 36.69 lat; -121.94 lon; 17.89 depth; utmZone: 10 2024-11-06T02:51:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861474.54 2024-11-06T02:51:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:17Z- waiting for 1730861478.54, time is 1730861476.97 2024-11-06T02:51:17Z- AHRS_M2 msg: 1730861477.35 epoch sec; seqNo:269 2024-11-06T02:51:17Z- AHRS_M2 msg: 0.12 phi; -0.26 theta; 5.23 psi 2024-11-06T02:51:17Z- handleDepth()- Depth_Keller msg: 1730861477.36 epoch sec; seqNo:293; depth 17.98 2024-11-06T02:51:17Z- handleDVL() - DVL msg epochMillisec: 1730861477.39 2024-11-06T02:51:17Z- handleDvl() - RDI_Pathfinder msg: 1730861477.39 sec; seqNo:100; #beams:4 2024-11-06T02:51:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.54 , 63.77 , 60.91 , 88.25 2024-11-06T02:51:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.06 , 0.05 2024-11-06T02:51:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861477.40 epoch sec; seqNo:293; 4061476.04 north; 594553.76 east; 36.69 lat; -121.94 lon; 17.98 depth; utmZone: 10 2024-11-06T02:51:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861474.54 2024-11-06T02:51:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:17Z- waiting for 1730861478.54, time is 1730861477.40 2024-11-06T02:51:17Z- AHRS_M2 msg: 1730861477.76 epoch sec; seqNo:270 2024-11-06T02:51:17Z- AHRS_M2 msg: 0.12 phi; -0.25 theta; 5.24 psi 2024-11-06T02:51:17Z- handleDepth()- Depth_Keller msg: 1730861477.76 epoch sec; seqNo:294; depth 18.07 2024-11-06T02:51:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861477.78 epoch sec; seqNo:294; 4061476.40 north; 594552.54 east; 36.69 lat; -121.94 lon; 18.07 depth; utmZone: 10 2024-11-06T02:51:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861474.54 2024-11-06T02:51:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:17Z- waiting for 1730861478.54, time is 1730861477.78 2024-11-06T02:51:18Z- AHRS_M2 msg: 1730861478.16 epoch sec; seqNo:271 2024-11-06T02:51:18Z- AHRS_M2 msg: 0.13 phi; -0.25 theta; 5.24 psi 2024-11-06T02:51:18Z- handleDepth()- Depth_Keller msg: 1730861478.17 epoch sec; seqNo:295; depth 18.15 2024-11-06T02:51:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861478.18 epoch sec; seqNo:295; 4061476.40 north; 594552.54 east; 36.69 lat; -121.94 lon; 18.15 depth; utmZone: 10 2024-11-06T02:51:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861474.54 2024-11-06T02:51:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:18Z- waiting for 1730861478.54, time is 1730861478.18 2024-11-06T02:51:18Z- AHRS_M2 msg: 1730861478.56 epoch sec; seqNo:272 2024-11-06T02:51:18Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.24 psi 2024-11-06T02:51:18Z- handleDepth()- Depth_Keller msg: 1730861478.57 epoch sec; seqNo:296; depth 18.22 2024-11-06T02:51:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861478.59 epoch sec; seqNo:296; 4061476.78 north; 594552.53 east; 36.69 lat; -121.94 lon; 18.22 depth; utmZone: 10 2024-11-06T02:51:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861474.54 2024-11-06T02:51:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:18Z- waiting for 1730861478.54, time is 1730861478.59 2024-11-06T02:51:18Z- LcmTrn::updateTrn() >>>> pose time 1730861478.59, meas time 1730861477.39 <<<< 2024-11-06T02:51:18Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861478.59 2024-11-06T02:51:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861477.39 sec, ping # 0; waiting for INS data more recent than 1730861474.54... 2024-11-06T02:51:18Z- TerrainNav::motionUpdate() - currEstimate = (4061477, 594552.53, 18.22) 2024-11-06T02:51:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:51:18Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:51:18Z- TNavPF::Projecting Measurements 2024-11-06T02:51:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:51:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:51:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:51:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:51:18Z- TNavFilter::4 good beams. 2024-11-06T02:51:18Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:51:18Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:51:18Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:51:18Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:51:18Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:51:18Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:51:18Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:51:18Z- TerrainMapOctree: (vehN, vehE ) = (4061476.34, 594553.26). 2024-11-06T02:51:18Z- TerrainMapOctree: Min Distance = 299.89. 2024-11-06T02:51:18Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:51:18Z- Forcing Subcloud Comparison 2024-11-06T02:51:18Z- Weighting particles with subcloud comparison 2024-11-06T02:51:18Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:18Z- meanExpectedMeasDiff: -4.879608 delta_rms_squared: 2.620874 alpha: 0.992427 2024-11-06T02:51:18Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:18Z- meanExpectedMeasDiff: 2.868033 delta_rms_squared: 3.363683 alpha: 0.994089 2024-11-06T02:51:18Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:18Z- meanExpectedMeasDiff: 0.525193 delta_rms_squared: 2.612831 alpha: 0.992404 2024-11-06T02:51:18Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:18Z- meanExpectedMeasDiff: 8.340126 delta_rms_squared: 3.650446 alpha: 0.994551 2024-11-06T02:51:18Z- Subcloud NIS: 0.320374 2024-11-06T02:51:18Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:51:18Z- TNavPF:: sumWeights = 0.014115 2024-11-06T02:51:18Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:51:18Z- TNavPF::First Term of Map Covariance Matrix is 0.41 2024-11-06T02:51:18Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T02:51:18Z- TerrainNav::Resampling particle filter... 2024-11-06T02:51:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861477.39 sec. 2024-11-06T02:51:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:51:18Z- variances([0], [2], and psi_berg) = 38.468263 46.177824 0.000000 2024-11-06T02:51:18Z- publishEstimatesDecoder() - at 1730861478.59 sec 2024-11-06T02:51:18Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:51:18Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061536.31 594527.38 29.65 2024-11-06T02:51:18Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061513.43 594541.96 27.46 lat/lon: 36.69 -121.94 2024-11-06T02:51:18Z- LcmTrn::publishEstimatesDecoder() - COVAR: 38.47 46.18 1.19 0.00 2024-11-06T02:51:18Z- AHRS_M2 msg: 1730861478.97 epoch sec; seqNo:273 2024-11-06T02:51:18Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.25 psi 2024-11-06T02:51:18Z- handleDepth()- Depth_Keller msg: 1730861478.97 epoch sec; seqNo:297; depth 18.28 2024-11-06T02:51:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861479.01 epoch sec; seqNo:297; 4061476.78 north; 594552.53 east; 36.69 lat; -121.94 lon; 18.28 depth; utmZone: 10 2024-11-06T02:51:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861478.59 2024-11-06T02:51:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:19Z- waiting for 1730861482.59, time is 1730861479.01 2024-11-06T02:51:19Z- AHRS_M2 msg: 1730861479.37 epoch sec; seqNo:274 2024-11-06T02:51:19Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.26 psi 2024-11-06T02:51:19Z- handleDepth()- Depth_Keller msg: 1730861479.38 epoch sec; seqNo:298; depth 18.37 2024-11-06T02:51:19Z- handleDVL() - DVL msg epochMillisec: 1730861479.40 2024-11-06T02:51:19Z- handleDvl() - RDI_Pathfinder msg: 1730861479.40 sec; seqNo:101; #beams:4 2024-11-06T02:51:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.95 , 62.96 , 60.91 , 87.17 2024-11-06T02:51:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.09 , -0.10 2024-11-06T02:51:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861479.43 epoch sec; seqNo:298; 4061477.16 north; 594552.53 east; 36.69 lat; -121.94 lon; 18.37 depth; utmZone: 10 2024-11-06T02:51:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861478.59 2024-11-06T02:51:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:19Z- waiting for 1730861482.59, time is 1730861479.43 2024-11-06T02:51:19Z- AHRS_M2 msg: 1730861479.78 epoch sec; seqNo:275 2024-11-06T02:51:19Z- AHRS_M2 msg: 0.14 phi; -0.24 theta; 5.26 psi 2024-11-06T02:51:19Z- handleDepth()- Depth_Keller msg: 1730861479.79 epoch sec; seqNo:299; depth 18.44 2024-11-06T02:51:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861479.81 epoch sec; seqNo:299; 4061477.15 north; 594551.31 east; 36.69 lat; -121.94 lon; 18.44 depth; utmZone: 10 2024-11-06T02:51:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861478.59 2024-11-06T02:51:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:19Z- waiting for 1730861482.59, time is 1730861479.81 2024-11-06T02:51:20Z- AHRS_M2 msg: 1730861480.18 epoch sec; seqNo:276 2024-11-06T02:51:20Z- AHRS_M2 msg: 0.14 phi; -0.24 theta; 5.26 psi 2024-11-06T02:51:20Z- handleDepth()- Depth_Keller msg: 1730861480.18 epoch sec; seqNo:300; depth 18.51 2024-11-06T02:51:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861480.19 epoch sec; seqNo:300; 4061477.53 north; 594551.30 east; 36.69 lat; -121.94 lon; 18.51 depth; utmZone: 10 2024-11-06T02:51:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861478.59 2024-11-06T02:51:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:20Z- waiting for 1730861482.59, time is 1730861480.19 2024-11-06T02:51:20Z- AHRS_M2 msg: 1730861480.58 epoch sec; seqNo:277 2024-11-06T02:51:20Z- AHRS_M2 msg: 0.14 phi; -0.24 theta; 5.27 psi 2024-11-06T02:51:20Z- handleDepth()- Depth_Keller msg: 1730861480.59 epoch sec; seqNo:301; depth 18.57 2024-11-06T02:51:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861480.60 epoch sec; seqNo:301; 4061477.53 north; 594551.30 east; 36.69 lat; -121.94 lon; 18.57 depth; utmZone: 10 2024-11-06T02:51:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861478.59 2024-11-06T02:51:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:20Z- waiting for 1730861482.59, time is 1730861480.60 2024-11-06T02:51:20Z- AHRS_M2 msg: 1730861480.99 epoch sec; seqNo:278 2024-11-06T02:51:21Z- AHRS_M2 msg: 0.14 phi; -0.24 theta; 5.27 psi 2024-11-06T02:51:21Z- handleDepth()- Depth_Keller msg: 1730861480.99 epoch sec; seqNo:302; depth 18.64 2024-11-06T02:51:21Z- handleDVL() - DVL msg epochMillisec: 1730861480.99 2024-11-06T02:51:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:21Z- handleDvl() - RDI_Pathfinder msg: 1730861480.99 sec; seqNo:102; #beams:0 2024-11-06T02:51:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.09 , -0.10 2024-11-06T02:51:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861481.00 epoch sec; seqNo:302; 4061477.90 north; 594551.30 east; 36.69 lat; -121.94 lon; 18.64 depth; utmZone: 10 2024-11-06T02:51:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861478.59 2024-11-06T02:51:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:21Z- waiting for 1730861482.59, time is 1730861481.00 2024-11-06T02:51:21Z- AHRS_M2 msg: 1730861481.39 epoch sec; seqNo:279 2024-11-06T02:51:21Z- AHRS_M2 msg: 0.14 phi; -0.24 theta; 5.27 psi 2024-11-06T02:51:21Z- handleDepth()- Depth_Keller msg: 1730861481.40 epoch sec; seqNo:303; depth 18.71 2024-11-06T02:51:21Z- handleDVL() - DVL msg epochMillisec: 1730861481.43 2024-11-06T02:51:21Z- handleDvl() - RDI_Pathfinder msg: 1730861481.43 sec; seqNo:103; #beams:4 2024-11-06T02:51:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.13 , 62.55 , 60.09 , 86.08 2024-11-06T02:51:21Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , 0.06 , -0.13 2024-11-06T02:51:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861481.44 epoch sec; seqNo:303; 4061478.27 north; 594550.08 east; 36.69 lat; -121.94 lon; 18.71 depth; utmZone: 10 2024-11-06T02:51:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861478.59 2024-11-06T02:51:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:21Z- waiting for 1730861482.59, time is 1730861481.44 2024-11-06T02:51:21Z- handleDepth()- Depth_Keller msg: 1730861481.86 epoch sec; seqNo:304; depth 18.78 2024-11-06T02:51:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861481.89 epoch sec; seqNo:304; 4061478.27 north; 594550.08 east; 36.69 lat; -121.94 lon; 18.78 depth; utmZone: 10 2024-11-06T02:51:22Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:51:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861478.59 2024-11-06T02:51:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:22Z- waiting for 1730861482.59, time is 1730861481.89 2024-11-06T02:51:22Z- AHRS_M2 msg: 1730861482.20 epoch sec; seqNo:280 2024-11-06T02:51:22Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.26 psi 2024-11-06T02:51:22Z- handleDepth()- Depth_Keller msg: 1730861482.21 epoch sec; seqNo:305; depth 18.88 2024-11-06T02:51:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861482.22 epoch sec; seqNo:305; 4061478.65 north; 594550.07 east; 36.69 lat; -121.94 lon; 18.88 depth; utmZone: 10 2024-11-06T02:51:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861478.59 2024-11-06T02:51:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:22Z- waiting for 1730861482.59, time is 1730861482.22 2024-11-06T02:51:22Z- AHRS_M2 msg: 1730861482.60 epoch sec; seqNo:281 2024-11-06T02:51:22Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.26 psi 2024-11-06T02:51:22Z- handleDepth()- Depth_Keller msg: 1730861482.61 epoch sec; seqNo:306; depth 18.94 2024-11-06T02:51:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861482.64 epoch sec; seqNo:306; 4061478.65 north; 594550.07 east; 36.69 lat; -121.94 lon; 18.94 depth; utmZone: 10 2024-11-06T02:51:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861478.59 2024-11-06T02:51:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:22Z- waiting for 1730861482.59, time is 1730861482.64 2024-11-06T02:51:22Z- LcmTrn::updateTrn() >>>> pose time 1730861482.64, meas time 1730861481.43 <<<< 2024-11-06T02:51:22Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861482.64 2024-11-06T02:51:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861481.43 sec, ping # 0; waiting for INS data more recent than 1730861478.59... 2024-11-06T02:51:22Z- TerrainNav::motionUpdate() - currEstimate = (4061479, 594550.07, 18.94) 2024-11-06T02:51:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:51:22Z- TerrainNav:: SubcloudNIS is 0.32, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:51:22Z- TNavPF::Projecting Measurements 2024-11-06T02:51:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:51:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:51:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:51:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:51:22Z- TNavFilter::4 good beams. 2024-11-06T02:51:22Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:51:22Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:51:22Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:51:22Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:51:22Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:51:22Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:51:22Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:51:22Z- TerrainMapOctree: (vehN, vehE ) = (4061478.09, 594550.81). 2024-11-06T02:51:22Z- TerrainMapOctree: Min Distance = 302.83. 2024-11-06T02:51:22Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:51:22Z- Forcing Subcloud Comparison 2024-11-06T02:51:22Z- Weighting particles with subcloud comparison 2024-11-06T02:51:22Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:22Z- meanExpectedMeasDiff: -5.657911 delta_rms_squared: 2.661824 alpha: 0.992542 2024-11-06T02:51:22Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:22Z- meanExpectedMeasDiff: 2.281615 delta_rms_squared: 2.421837 alpha: 0.991809 2024-11-06T02:51:22Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:22Z- meanExpectedMeasDiff: -0.609268 delta_rms_squared: 2.080705 alpha: 0.990479 2024-11-06T02:51:22Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:22Z- meanExpectedMeasDiff: 6.904314 delta_rms_squared: 3.440349 alpha: 0.994220 2024-11-06T02:51:22Z- Subcloud NIS: 0.356942 2024-11-06T02:51:22Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:51:22Z- TNavPF:: sumWeights = 0.019502 2024-11-06T02:51:22Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:51:22Z- TNavPF::First Term of Map Covariance Matrix is 0.64 2024-11-06T02:51:22Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T02:51:22Z- TerrainNav::Resampling particle filter... 2024-11-06T02:51:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861481.43 sec. 2024-11-06T02:51:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:51:22Z- variances([0], [2], and psi_berg) = 24.464580 36.970208 0.000000 2024-11-06T02:51:22Z- publishEstimatesDecoder() - at 1730861482.64 sec 2024-11-06T02:51:22Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:51:22Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061528.67 594541.33 28.46 2024-11-06T02:51:22Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061514.57 594541.91 27.90 lat/lon: 36.69 -121.94 2024-11-06T02:51:22Z- LcmTrn::publishEstimatesDecoder() - COVAR: 24.46 36.97 1.00 0.00 2024-11-06T02:51:23Z- AHRS_M2 msg: 1730861483.01 epoch sec; seqNo:282 2024-11-06T02:51:23Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.25 psi 2024-11-06T02:51:23Z- handleDepth()- Depth_Keller msg: 1730861483.03 epoch sec; seqNo:307; depth 19.02 2024-11-06T02:51:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861483.05 epoch sec; seqNo:307; 4061479.01 north; 594548.85 east; 36.69 lat; -121.94 lon; 19.02 depth; utmZone: 10 2024-11-06T02:51:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861482.64 2024-11-06T02:51:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:23Z- waiting for 1730861486.64, time is 1730861483.05 2024-11-06T02:51:23Z- AHRS_M2 msg: 1730861483.42 epoch sec; seqNo:283 2024-11-06T02:51:23Z- AHRS_M2 msg: 0.14 phi; -0.24 theta; 5.25 psi 2024-11-06T02:51:23Z- handleDepth()- Depth_Keller msg: 1730861483.42 epoch sec; seqNo:308; depth 19.11 2024-11-06T02:51:23Z- handleDVL() - DVL msg epochMillisec: 1730861483.44 2024-11-06T02:51:23Z- handleDvl() - RDI_Pathfinder msg: 1730861483.44 sec; seqNo:104; #beams:4 2024-11-06T02:51:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.32 , 60.91 , 60.09 , 85.00 2024-11-06T02:51:23Z- handleDvl() - RDI_Pathfinder msg: velocities 1.10 , -0.03 , 0.10 2024-11-06T02:51:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861483.46 epoch sec; seqNo:308; 4061479.39 north; 594548.84 east; 36.69 lat; -121.94 lon; 19.11 depth; utmZone: 10 2024-11-06T02:51:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861482.64 2024-11-06T02:51:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:23Z- waiting for 1730861486.64, time is 1730861483.46 2024-11-06T02:51:23Z- AHRS_M2 msg: 1730861483.87 epoch sec; seqNo:284 2024-11-06T02:51:23Z- AHRS_M2 msg: 0.14 phi; -0.24 theta; 5.25 psi 2024-11-06T02:51:23Z- handleDepth()- Depth_Keller msg: 1730861483.87 epoch sec; seqNo:309; depth 19.18 2024-11-06T02:51:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861483.90 epoch sec; seqNo:309; 4061479.39 north; 594548.84 east; 36.69 lat; -121.94 lon; 19.18 depth; utmZone: 10 2024-11-06T02:51:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861482.64 2024-11-06T02:51:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:24Z- waiting for 1730861486.64, time is 1730861483.90 2024-11-06T02:51:24Z- AHRS_M2 msg: 1730861484.29 epoch sec; seqNo:285 2024-11-06T02:51:24Z- AHRS_M2 msg: 0.14 phi; -0.24 theta; 5.25 psi 2024-11-06T02:51:24Z- handleDepth()- Depth_Keller msg: 1730861484.31 epoch sec; seqNo:310; depth 19.27 2024-11-06T02:51:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861484.32 epoch sec; seqNo:310; 4061479.76 north; 594547.62 east; 36.69 lat; -121.94 lon; 19.27 depth; utmZone: 10 2024-11-06T02:51:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861482.64 2024-11-06T02:51:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:24Z- waiting for 1730861486.64, time is 1730861484.32 2024-11-06T02:51:24Z- AHRS_M2 msg: 1730861484.70 epoch sec; seqNo:286 2024-11-06T02:51:24Z- AHRS_M2 msg: 0.14 phi; -0.24 theta; 5.24 psi 2024-11-06T02:51:24Z- handleDepth()- Depth_Keller msg: 1730861484.70 epoch sec; seqNo:311; depth 19.36 2024-11-06T02:51:24Z- handleDVL() - DVL msg epochMillisec: 1730861484.71 2024-11-06T02:51:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:24Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:24Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:24Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:24Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:24Z- handleDvl() - RDI_Pathfinder msg: 1730861484.71 sec; seqNo:105; #beams:0 2024-11-06T02:51:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.10 , -0.03 , 0.10 2024-11-06T02:51:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861484.72 epoch sec; seqNo:311; 4061479.76 north; 594547.62 east; 36.69 lat; -121.94 lon; 19.36 depth; utmZone: 10 2024-11-06T02:51:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861482.64 2024-11-06T02:51:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:24Z- waiting for 1730861486.64, time is 1730861484.72 2024-11-06T02:51:25Z- AHRS_M2 msg: 1730861485.10 epoch sec; seqNo:287 2024-11-06T02:51:25Z- AHRS_M2 msg: 0.13 phi; -0.25 theta; 5.24 psi 2024-11-06T02:51:25Z- handleDepth()- Depth_Keller msg: 1730861485.11 epoch sec; seqNo:312; depth 19.44 2024-11-06T02:51:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861485.11 epoch sec; seqNo:312; 4061480.14 north; 594547.61 east; 36.69 lat; -121.94 lon; 19.44 depth; utmZone: 10 2024-11-06T02:51:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861482.64 2024-11-06T02:51:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:25Z- waiting for 1730861486.64, time is 1730861485.11 2024-11-06T02:51:25Z- AHRS_M2 msg: 1730861485.50 epoch sec; seqNo:288 2024-11-06T02:51:25Z- AHRS_M2 msg: 0.13 phi; -0.25 theta; 5.24 psi 2024-11-06T02:51:25Z- handleDepth()- Depth_Keller msg: 1730861485.51 epoch sec; seqNo:313; depth 19.52 2024-11-06T02:51:25Z- handleDVL() - DVL msg epochMillisec: 1730861485.53 2024-11-06T02:51:25Z- handleDvl() - RDI_Pathfinder msg: 1730861485.53 sec; seqNo:106; #beams:4 2024-11-06T02:51:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.50 , 61.32 , 58.87 , 83.92 2024-11-06T02:51:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.03 , 0.17 2024-11-06T02:51:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861485.54 epoch sec; seqNo:313; 4061480.14 north; 594547.61 east; 36.69 lat; -121.94 lon; 19.52 depth; utmZone: 10 2024-11-06T02:51:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861482.64 2024-11-06T02:51:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:25Z- waiting for 1730861486.64, time is 1730861485.54 2024-11-06T02:51:25Z- AHRS_M2 msg: 1730861485.90 epoch sec; seqNo:289 2024-11-06T02:51:25Z- AHRS_M2 msg: 0.13 phi; -0.24 theta; 5.24 psi 2024-11-06T02:51:25Z- handleDepth()- Depth_Keller msg: 1730861485.91 epoch sec; seqNo:314; depth 19.61 2024-11-06T02:51:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861485.93 epoch sec; seqNo:314; 4061480.50 north; 594546.39 east; 36.69 lat; -121.94 lon; 19.61 depth; utmZone: 10 2024-11-06T02:51:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861482.64 2024-11-06T02:51:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:26Z- waiting for 1730861486.64, time is 1730861485.93 2024-11-06T02:51:26Z- AHRS_M2 msg: 1730861486.31 epoch sec; seqNo:290 2024-11-06T02:51:26Z- AHRS_M2 msg: 0.13 phi; -0.24 theta; 5.24 psi 2024-11-06T02:51:26Z- handleDepth()- Depth_Keller msg: 1730861486.31 epoch sec; seqNo:315; depth 19.68 2024-11-06T02:51:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861486.32 epoch sec; seqNo:315; 4061480.50 north; 594546.39 east; 36.69 lat; -121.94 lon; 19.68 depth; utmZone: 10 2024-11-06T02:51:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861482.64 2024-11-06T02:51:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:26Z- waiting for 1730861486.64, time is 1730861486.32 2024-11-06T02:51:26Z- AHRS_M2 msg: 1730861486.71 epoch sec; seqNo:291 2024-11-06T02:51:26Z- AHRS_M2 msg: 0.13 phi; -0.24 theta; 5.24 psi 2024-11-06T02:51:26Z- handleDepth()- Depth_Keller msg: 1730861486.72 epoch sec; seqNo:316; depth 19.75 2024-11-06T02:51:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861486.73 epoch sec; seqNo:316; 4061480.50 north; 594546.39 east; 36.69 lat; -121.94 lon; 19.75 depth; utmZone: 10 2024-11-06T02:51:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861482.64 2024-11-06T02:51:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:26Z- waiting for 1730861486.64, time is 1730861486.73 2024-11-06T02:51:26Z- LcmTrn::updateTrn() >>>> pose time 1730861486.73, meas time 1730861485.53 <<<< 2024-11-06T02:51:26Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861486.73 2024-11-06T02:51:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861485.53 sec, ping # 0; waiting for INS data more recent than 1730861482.64... 2024-11-06T02:51:26Z- TerrainNav::motionUpdate() - currEstimate = (4061481, 594546.39, 19.75) 2024-11-06T02:51:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:51:26Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:51:26Z- TNavPF::Projecting Measurements 2024-11-06T02:51:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:51:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:51:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:51:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:51:26Z- TNavFilter::4 good beams. 2024-11-06T02:51:26Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:51:26Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:51:26Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:51:26Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:51:26Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:51:26Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:51:26Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:51:26Z- TerrainMapOctree: (vehN, vehE ) = (4061479.96, 594547.47). 2024-11-06T02:51:26Z- TerrainMapOctree: Min Distance = 306.42. 2024-11-06T02:51:26Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:51:27Z- Forcing Subcloud Comparison 2024-11-06T02:51:27Z- Weighting particles with subcloud comparison 2024-11-06T02:51:27Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:27Z- meanExpectedMeasDiff: -5.789748 delta_rms_squared: 2.515172 alpha: 0.992111 2024-11-06T02:51:27Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:27Z- meanExpectedMeasDiff: 0.989138 delta_rms_squared: 2.141070 alpha: 0.990745 2024-11-06T02:51:27Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:27Z- meanExpectedMeasDiff: -0.781382 delta_rms_squared: 1.922180 alpha: 0.989702 2024-11-06T02:51:27Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:27Z- meanExpectedMeasDiff: 5.763269 delta_rms_squared: 2.043695 alpha: 0.990309 2024-11-06T02:51:27Z- Subcloud NIS: 0.385238 2024-11-06T02:51:27Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:51:27Z- TNavPF:: sumWeights = 0.036716 2024-11-06T02:51:27Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:51:27Z- TNavPF::First Term of Map Covariance Matrix is 1.02 2024-11-06T02:51:27Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T02:51:27Z- TerrainNav::Resampling particle filter... 2024-11-06T02:51:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861485.53 sec. 2024-11-06T02:51:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:51:27Z- variances([0], [2], and psi_berg) = 14.270595 24.911138 0.000000 2024-11-06T02:51:27Z- publishEstimatesDecoder() - at 1730861486.73 sec 2024-11-06T02:51:27Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:51:27Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061530.79 594537.95 29.24 2024-11-06T02:51:27Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061514.67 594540.60 28.82 lat/lon: 36.69 -121.94 2024-11-06T02:51:27Z- LcmTrn::publishEstimatesDecoder() - COVAR: 14.27 24.91 0.92 0.00 2024-11-06T02:51:27Z- AHRS_M2 msg: 1730861487.12 epoch sec; seqNo:292 2024-11-06T02:51:27Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.25 psi 2024-11-06T02:51:27Z- handleDepth()- Depth_Keller msg: 1730861487.13 epoch sec; seqNo:317; depth 19.83 2024-11-06T02:51:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861487.16 epoch sec; seqNo:317; 4061480.87 north; 594545.17 east; 36.69 lat; -121.94 lon; 19.83 depth; utmZone: 10 2024-11-06T02:51:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861486.73 2024-11-06T02:51:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:27Z- waiting for 1730861490.73, time is 1730861487.16 2024-11-06T02:51:27Z- AHRS_M2 msg: 1730861487.52 epoch sec; seqNo:293 2024-11-06T02:51:27Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.25 psi 2024-11-06T02:51:27Z- handleDepth()- Depth_Keller msg: 1730861487.53 epoch sec; seqNo:318; depth 19.89 2024-11-06T02:51:27Z- handleDVL() - DVL msg epochMillisec: 1730861487.55 2024-11-06T02:51:27Z- handleDvl() - RDI_Pathfinder msg: 1730861487.55 sec; seqNo:107; #beams:4 2024-11-06T02:51:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.09 , 61.32 , 58.87 , 79.31 2024-11-06T02:51:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , 0.07 2024-11-06T02:51:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861487.56 epoch sec; seqNo:318; 4061480.87 north; 594545.17 east; 36.69 lat; -121.94 lon; 19.89 depth; utmZone: 10 2024-11-06T02:51:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861486.73 2024-11-06T02:51:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:27Z- waiting for 1730861490.73, time is 1730861487.56 2024-11-06T02:51:27Z- AHRS_M2 msg: 1730861487.92 epoch sec; seqNo:294 2024-11-06T02:51:27Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.25 psi 2024-11-06T02:51:27Z- handleDepth()- Depth_Keller msg: 1730861487.93 epoch sec; seqNo:319; depth 19.97 2024-11-06T02:51:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861487.94 epoch sec; seqNo:319; 4061481.25 north; 594545.16 east; 36.69 lat; -121.94 lon; 19.97 depth; utmZone: 10 2024-11-06T02:51:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861486.73 2024-11-06T02:51:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:28Z- waiting for 1730861490.73, time is 1730861487.94 2024-11-06T02:51:28Z- AHRS_M2 msg: 1730861488.33 epoch sec; seqNo:295 2024-11-06T02:51:28Z- AHRS_M2 msg: 0.13 phi; -0.22 theta; 5.25 psi 2024-11-06T02:51:28Z- handleDepth()- Depth_Keller msg: 1730861488.34 epoch sec; seqNo:320; depth 20.04 2024-11-06T02:51:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861488.35 epoch sec; seqNo:320; 4061481.25 north; 594545.16 east; 36.69 lat; -121.94 lon; 20.04 depth; utmZone: 10 2024-11-06T02:51:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861486.73 2024-11-06T02:51:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:28Z- waiting for 1730861490.73, time is 1730861488.35 2024-11-06T02:51:28Z- AHRS_M2 msg: 1730861488.73 epoch sec; seqNo:296 2024-11-06T02:51:28Z- AHRS_M2 msg: 0.13 phi; -0.22 theta; 5.25 psi 2024-11-06T02:51:28Z- handleDepth()- Depth_Keller msg: 1730861488.74 epoch sec; seqNo:321; depth 20.11 2024-11-06T02:51:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861488.76 epoch sec; seqNo:321; 4061481.63 north; 594545.16 east; 36.69 lat; -121.94 lon; 20.11 depth; utmZone: 10 2024-11-06T02:51:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861486.73 2024-11-06T02:51:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:28Z- waiting for 1730861490.73, time is 1730861488.76 2024-11-06T02:51:29Z- AHRS_M2 msg: 1730861489.14 epoch sec; seqNo:297 2024-11-06T02:51:29Z- AHRS_M2 msg: 0.13 phi; -0.22 theta; 5.25 psi 2024-11-06T02:51:29Z- handleDepth()- Depth_Keller msg: 1730861489.14 epoch sec; seqNo:322; depth 20.16 2024-11-06T02:51:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861489.15 epoch sec; seqNo:322; 4061481.61 north; 594543.94 east; 36.69 lat; -121.94 lon; 20.16 depth; utmZone: 10 2024-11-06T02:51:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861486.73 2024-11-06T02:51:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:29Z- waiting for 1730861490.73, time is 1730861489.15 2024-11-06T02:51:29Z- AHRS_M2 msg: 1730861489.54 epoch sec; seqNo:298 2024-11-06T02:51:29Z- AHRS_M2 msg: 0.13 phi; -0.22 theta; 5.24 psi 2024-11-06T02:51:29Z- handleDepth()- Depth_Keller msg: 1730861489.54 epoch sec; seqNo:323; depth 20.23 2024-11-06T02:51:29Z- handleDVL() - DVL msg epochMillisec: 1730861489.57 2024-11-06T02:51:29Z- handleDvl() - RDI_Pathfinder msg: 1730861489.57 sec; seqNo:108; #beams:4 2024-11-06T02:51:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.01 , 61.04 , 59.51 , 78.52 2024-11-06T02:51:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.12 , 0.05 2024-11-06T02:51:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861489.59 epoch sec; seqNo:323; 4061481.99 north; 594543.93 east; 36.69 lat; -121.94 lon; 20.23 depth; utmZone: 10 2024-11-06T02:51:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861486.73 2024-11-06T02:51:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:29Z- waiting for 1730861490.73, time is 1730861489.59 2024-11-06T02:51:29Z- AHRS_M2 msg: 1730861489.95 epoch sec; seqNo:299 2024-11-06T02:51:29Z- AHRS_M2 msg: 0.12 phi; -0.22 theta; 5.24 psi 2024-11-06T02:51:29Z- handleDepth()- Depth_Keller msg: 1730861489.96 epoch sec; seqNo:324; depth 20.30 2024-11-06T02:51:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861489.98 epoch sec; seqNo:324; 4061481.99 north; 594543.93 east; 36.69 lat; -121.94 lon; 20.30 depth; utmZone: 10 2024-11-06T02:51:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861486.73 2024-11-06T02:51:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:30Z- waiting for 1730861490.73, time is 1730861489.98 2024-11-06T02:51:30Z- AHRS_M2 msg: 1730861490.35 epoch sec; seqNo:300 2024-11-06T02:51:30Z- AHRS_M2 msg: 0.12 phi; -0.22 theta; 5.24 psi 2024-11-06T02:51:30Z- handleDepth()- Depth_Keller msg: 1730861490.35 epoch sec; seqNo:325; depth 20.36 2024-11-06T02:51:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861490.36 epoch sec; seqNo:325; 4061482.37 north; 594543.93 east; 36.69 lat; -121.94 lon; 20.36 depth; utmZone: 10 2024-11-06T02:51:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861486.73 2024-11-06T02:51:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:30Z- waiting for 1730861490.73, time is 1730861490.36 2024-11-06T02:51:30Z- AHRS_M2 msg: 1730861490.75 epoch sec; seqNo:301 2024-11-06T02:51:30Z- AHRS_M2 msg: 0.13 phi; -0.22 theta; 5.24 psi 2024-11-06T02:51:30Z- handleDepth()- Depth_Keller msg: 1730861490.76 epoch sec; seqNo:326; depth 20.42 2024-11-06T02:51:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861490.77 epoch sec; seqNo:326; 4061482.73 north; 594542.70 east; 36.69 lat; -121.94 lon; 20.42 depth; utmZone: 10 2024-11-06T02:51:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861486.73 2024-11-06T02:51:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:30Z- waiting for 1730861490.73, time is 1730861490.77 2024-11-06T02:51:30Z- LcmTrn::updateTrn() >>>> pose time 1730861490.77, meas time 1730861489.57 <<<< 2024-11-06T02:51:30Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861490.77 2024-11-06T02:51:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861489.57 sec, ping # 0; waiting for INS data more recent than 1730861486.73... 2024-11-06T02:51:30Z- TerrainNav::motionUpdate() - currEstimate = (4061483, 594542.70, 20.42) 2024-11-06T02:51:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:51:30Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:51:30Z- TNavPF::Projecting Measurements 2024-11-06T02:51:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:51:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:51:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:51:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:51:30Z- TNavFilter::4 good beams. 2024-11-06T02:51:30Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:51:30Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:51:30Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:51:30Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:51:30Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:51:30Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:51:30Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:51:30Z- TerrainMapOctree: (vehN, vehE ) = (4061482.07, 594543.80). 2024-11-06T02:51:30Z- TerrainMapOctree: Min Distance = 310.45. 2024-11-06T02:51:30Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:51:31Z- Forcing Subcloud Comparison 2024-11-06T02:51:31Z- Weighting particles with subcloud comparison 2024-11-06T02:51:31Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:31Z- meanExpectedMeasDiff: -5.111992 delta_rms_squared: 2.061470 alpha: 0.990391 2024-11-06T02:51:31Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:31Z- meanExpectedMeasDiff: 2.132556 delta_rms_squared: 2.030965 alpha: 0.990248 2024-11-06T02:51:31Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:31Z- meanExpectedMeasDiff: 0.559814 delta_rms_squared: 1.801556 alpha: 0.989020 2024-11-06T02:51:31Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:31Z- meanExpectedMeasDiff: 2.139601 delta_rms_squared: 2.836372 alpha: 0.992998 2024-11-06T02:51:31Z- Subcloud NIS: 0.387758 2024-11-06T02:51:31Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:51:31Z- TNavPF:: sumWeights = 0.105590 2024-11-06T02:51:31Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:51:31Z- TNavPF::First Term of Map Covariance Matrix is 2.25 2024-11-06T02:51:31Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T02:51:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861489.57 sec. 2024-11-06T02:51:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:51:31Z- variances([0], [2], and psi_berg) = 13.134005 23.776584 0.000000 2024-11-06T02:51:31Z- publishEstimatesDecoder() - at 1730861490.77 sec 2024-11-06T02:51:31Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:51:31Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061521.21 594531.66 28.99 2024-11-06T02:51:31Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061517.06 594536.55 29.51 lat/lon: 36.69 -121.94 2024-11-06T02:51:31Z- LcmTrn::publishEstimatesDecoder() - COVAR: 13.13 23.78 0.94 0.00 2024-11-06T02:51:31Z- AHRS_M2 msg: 1730861491.16 epoch sec; seqNo:302 2024-11-06T02:51:31Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.24 psi 2024-11-06T02:51:31Z- handleDepth()- Depth_Keller msg: 1730861491.17 epoch sec; seqNo:327; depth 20.49 2024-11-06T02:51:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861491.19 epoch sec; seqNo:327; 4061482.73 north; 594542.70 east; 36.69 lat; -121.94 lon; 20.49 depth; utmZone: 10 2024-11-06T02:51:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861490.77 2024-11-06T02:51:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:31Z- waiting for 1730861494.77, time is 1730861491.19 2024-11-06T02:51:31Z- AHRS_M2 msg: 1730861491.56 epoch sec; seqNo:303 2024-11-06T02:51:31Z- AHRS_M2 msg: 0.15 phi; -0.23 theta; 5.24 psi 2024-11-06T02:51:31Z- handleDepth()- Depth_Keller msg: 1730861491.56 epoch sec; seqNo:328; depth 20.54 2024-11-06T02:51:31Z- handleDVL() - DVL msg epochMillisec: 1730861491.59 2024-11-06T02:51:31Z- handleDvl() - RDI_Pathfinder msg: 1730861491.59 sec; seqNo:109; #beams:4 2024-11-06T02:51:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.10 , 60.73 , 59.20 , 77.30 2024-11-06T02:51:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.11 , 0.11 2024-11-06T02:51:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861491.61 epoch sec; seqNo:328; 4061483.11 north; 594542.70 east; 36.69 lat; -121.94 lon; 20.54 depth; utmZone: 10 2024-11-06T02:51:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861490.77 2024-11-06T02:51:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:31Z- waiting for 1730861494.77, time is 1730861491.61 2024-11-06T02:51:31Z- AHRS_M2 msg: 1730861491.96 epoch sec; seqNo:304 2024-11-06T02:51:31Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.24 psi 2024-11-06T02:51:31Z- handleDepth()- Depth_Keller msg: 1730861491.97 epoch sec; seqNo:329; depth 20.61 2024-11-06T02:51:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861491.99 epoch sec; seqNo:329; 4061483.11 north; 594542.70 east; 36.69 lat; -121.94 lon; 20.61 depth; utmZone: 10 2024-11-06T02:51:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861490.77 2024-11-06T02:51:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:32Z- waiting for 1730861494.77, time is 1730861491.99 2024-11-06T02:51:32Z- AHRS_M2 msg: 1730861492.37 epoch sec; seqNo:305 2024-11-06T02:51:32Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.24 psi 2024-11-06T02:51:32Z- handleDepth()- Depth_Keller msg: 1730861492.37 epoch sec; seqNo:330; depth 20.69 2024-11-06T02:51:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861492.38 epoch sec; seqNo:330; 4061483.48 north; 594541.48 east; 36.69 lat; -121.94 lon; 20.69 depth; utmZone: 10 2024-11-06T02:51:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861490.77 2024-11-06T02:51:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:32Z- waiting for 1730861494.77, time is 1730861492.38 2024-11-06T02:51:32Z- AHRS_M2 msg: 1730861492.77 epoch sec; seqNo:306 2024-11-06T02:51:32Z- AHRS_M2 msg: 0.15 phi; -0.25 theta; 5.24 psi 2024-11-06T02:51:32Z- handleDepth()- Depth_Keller msg: 1730861492.79 epoch sec; seqNo:331; depth 20.74 2024-11-06T02:51:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861492.81 epoch sec; seqNo:331; 4061483.48 north; 594541.48 east; 36.69 lat; -121.94 lon; 20.74 depth; utmZone: 10 2024-11-06T02:51:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861490.77 2024-11-06T02:51:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:32Z- waiting for 1730861494.77, time is 1730861492.81 2024-11-06T02:51:33Z- AHRS_M2 msg: 1730861493.17 epoch sec; seqNo:307 2024-11-06T02:51:33Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.24 psi 2024-11-06T02:51:33Z- handleDepth()- Depth_Keller msg: 1730861493.18 epoch sec; seqNo:332; depth 20.81 2024-11-06T02:51:33Z- handleDVL() - DVL msg epochMillisec: 1730861493.18 2024-11-06T02:51:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:33Z- handleDvl() - RDI_Pathfinder msg: 1730861493.18 sec; seqNo:110; #beams:0 2024-11-06T02:51:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.11 , 0.11 2024-11-06T02:51:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861493.19 epoch sec; seqNo:332; 4061483.86 north; 594541.47 east; 36.69 lat; -121.94 lon; 20.81 depth; utmZone: 10 2024-11-06T02:51:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861490.77 2024-11-06T02:51:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:33Z- waiting for 1730861494.77, time is 1730861493.19 2024-11-06T02:51:33Z- AHRS_M2 msg: 1730861493.58 epoch sec; seqNo:308 2024-11-06T02:51:33Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.24 psi 2024-11-06T02:51:33Z- handleDepth()- Depth_Keller msg: 1730861493.59 epoch sec; seqNo:333; depth 20.87 2024-11-06T02:51:33Z- handleDVL() - DVL msg epochMillisec: 1730861493.61 2024-11-06T02:51:33Z- handleDvl() - RDI_Pathfinder msg: 1730861493.61 sec; seqNo:111; #beams:4 2024-11-06T02:51:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.71 , 60.43 , 58.89 , 78.83 2024-11-06T02:51:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.05 , 0.04 2024-11-06T02:51:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861493.62 epoch sec; seqNo:333; 4061483.86 north; 594541.47 east; 36.69 lat; -121.94 lon; 20.87 depth; utmZone: 10 2024-11-06T02:51:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861490.77 2024-11-06T02:51:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:33Z- waiting for 1730861494.77, time is 1730861493.62 2024-11-06T02:51:33Z- AHRS_M2 msg: 1730861493.99 epoch sec; seqNo:309 2024-11-06T02:51:33Z- AHRS_M2 msg: 0.13 phi; -0.26 theta; 5.25 psi 2024-11-06T02:51:34Z- handleDepth()- Depth_Keller msg: 1730861494.01 epoch sec; seqNo:334; depth 20.94 2024-11-06T02:51:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861494.03 epoch sec; seqNo:334; 4061484.24 north; 594541.47 east; 36.69 lat; -121.94 lon; 20.94 depth; utmZone: 10 2024-11-06T02:51:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861490.77 2024-11-06T02:51:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:34Z- waiting for 1730861494.77, time is 1730861494.03 2024-11-06T02:51:34Z- AHRS_M2 msg: 1730861494.39 epoch sec; seqNo:310 2024-11-06T02:51:34Z- AHRS_M2 msg: 0.13 phi; -0.26 theta; 5.25 psi 2024-11-06T02:51:34Z- handleDepth()- Depth_Keller msg: 1730861494.40 epoch sec; seqNo:335; depth 21.01 2024-11-06T02:51:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861494.41 epoch sec; seqNo:335; 4061484.22 north; 594540.25 east; 36.69 lat; -121.94 lon; 21.01 depth; utmZone: 10 2024-11-06T02:51:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861490.77 2024-11-06T02:51:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:34Z- waiting for 1730861494.77, time is 1730861494.41 2024-11-06T02:51:34Z- AHRS_M2 msg: 1730861494.79 epoch sec; seqNo:311 2024-11-06T02:51:34Z- AHRS_M2 msg: 0.13 phi; -0.26 theta; 5.25 psi 2024-11-06T02:51:34Z- handleDepth()- Depth_Keller msg: 1730861494.80 epoch sec; seqNo:336; depth 21.07 2024-11-06T02:51:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861494.81 epoch sec; seqNo:336; 4061484.60 north; 594540.24 east; 36.69 lat; -121.94 lon; 21.07 depth; utmZone: 10 2024-11-06T02:51:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861490.77 2024-11-06T02:51:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:34Z- waiting for 1730861494.77, time is 1730861494.81 2024-11-06T02:51:34Z- LcmTrn::updateTrn() >>>> pose time 1730861494.81, meas time 1730861493.61 <<<< 2024-11-06T02:51:34Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861494.81 2024-11-06T02:51:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861493.61 sec, ping # 0; waiting for INS data more recent than 1730861490.77... 2024-11-06T02:51:34Z- TerrainNav::motionUpdate() - currEstimate = (4061485, 594540.24, 21.07) 2024-11-06T02:51:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:51:34Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:51:34Z- TNavPF::Projecting Measurements 2024-11-06T02:51:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:51:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:51:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:51:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:51:34Z- TNavFilter::4 good beams. 2024-11-06T02:51:34Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:51:34Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:51:34Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:51:34Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:51:34Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:51:34Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:51:34Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:51:34Z- TerrainMapOctree: (vehN, vehE ) = (4061484.05, 594540.97). 2024-11-06T02:51:34Z- TerrainMapOctree: Min Distance = 313.80. 2024-11-06T02:51:34Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:51:35Z- Forcing Subcloud Comparison 2024-11-06T02:51:35Z- Weighting particles with subcloud comparison 2024-11-06T02:51:35Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:35Z- meanExpectedMeasDiff: -5.052801 delta_rms_squared: 1.953936 alpha: 0.989868 2024-11-06T02:51:35Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:35Z- meanExpectedMeasDiff: 1.552827 delta_rms_squared: 1.838847 alpha: 0.989241 2024-11-06T02:51:35Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:35Z- meanExpectedMeasDiff: 0.851791 delta_rms_squared: 1.942287 alpha: 0.989808 2024-11-06T02:51:35Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:35Z- meanExpectedMeasDiff: 1.831178 delta_rms_squared: 2.801575 alpha: 0.992912 2024-11-06T02:51:35Z- Subcloud NIS: 0.391553 2024-11-06T02:51:35Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:51:35Z- TNavPF:: sumWeights = 0.126763 2024-11-06T02:51:35Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:51:35Z- TNavPF::First Term of Map Covariance Matrix is 2.51 2024-11-06T02:51:35Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T02:51:35Z- TerrainNav::Resampling particle filter... 2024-11-06T02:51:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861493.61 sec. 2024-11-06T02:51:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:51:35Z- variances([0], [2], and psi_berg) = 11.095756 22.379122 0.000000 2024-11-06T02:51:35Z- publishEstimatesDecoder() - at 1730861494.81 sec 2024-11-06T02:51:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:51:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061533.11 594529.72 28.13 2024-11-06T02:51:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061518.84 594534.13 30.22 lat/lon: 36.69 -121.94 2024-11-06T02:51:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 11.10 22.38 0.95 0.00 2024-11-06T02:51:35Z- handleDepth()- Depth_Keller msg: 1730861495.24 epoch sec; seqNo:337; depth 21.14 2024-11-06T02:51:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861495.27 epoch sec; seqNo:337; 4061484.60 north; 594540.24 east; 36.69 lat; -121.94 lon; 21.14 depth; utmZone: 10 2024-11-06T02:51:35Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:51:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861494.81 2024-11-06T02:51:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:35Z- waiting for 1730861498.81, time is 1730861495.27 2024-11-06T02:51:35Z- AHRS_M2 msg: 1730861495.60 epoch sec; seqNo:312 2024-11-06T02:51:35Z- AHRS_M2 msg: 0.14 phi; -0.27 theta; 5.25 psi 2024-11-06T02:51:35Z- handleDepth()- Depth_Keller msg: 1730861495.61 epoch sec; seqNo:338; depth 21.21 2024-11-06T02:51:35Z- handleDVL() - DVL msg epochMillisec: 1730861495.63 2024-11-06T02:51:35Z- handleDvl() - RDI_Pathfinder msg: 1730861495.63 sec; seqNo:112; #beams:4 2024-11-06T02:51:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.10 , 60.73 , 57.97 , 80.06 2024-11-06T02:51:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , -0.07 2024-11-06T02:51:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861495.65 epoch sec; seqNo:338; 4061484.98 north; 594540.24 east; 36.69 lat; -121.94 lon; 21.21 depth; utmZone: 10 2024-11-06T02:51:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861494.81 2024-11-06T02:51:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:35Z- waiting for 1730861498.81, time is 1730861495.65 2024-11-06T02:51:36Z- handleDepth()- Depth_Keller msg: 1730861496.02 epoch sec; seqNo:339; depth 21.28 2024-11-06T02:51:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861496.05 epoch sec; seqNo:339; 4061484.97 north; 594539.02 east; 36.69 lat; -121.94 lon; 21.28 depth; utmZone: 10 2024-11-06T02:51:36Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:51:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861494.81 2024-11-06T02:51:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:36Z- waiting for 1730861498.81, time is 1730861496.05 2024-11-06T02:51:36Z- AHRS_M2 msg: 1730861496.41 epoch sec; seqNo:313 2024-11-06T02:51:36Z- AHRS_M2 msg: 0.14 phi; -0.27 theta; 5.24 psi 2024-11-06T02:51:36Z- handleDepth()- Depth_Keller msg: 1730861496.41 epoch sec; seqNo:340; depth 21.35 2024-11-06T02:51:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861496.42 epoch sec; seqNo:340; 4061485.35 north; 594539.01 east; 36.69 lat; -121.94 lon; 21.35 depth; utmZone: 10 2024-11-06T02:51:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861494.81 2024-11-06T02:51:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:36Z- waiting for 1730861498.81, time is 1730861496.42 2024-11-06T02:51:36Z- AHRS_M2 msg: 1730861496.81 epoch sec; seqNo:314 2024-11-06T02:51:36Z- AHRS_M2 msg: 0.13 phi; -0.28 theta; 5.24 psi 2024-11-06T02:51:36Z- handleDepth()- Depth_Keller msg: 1730861496.82 epoch sec; seqNo:341; depth 21.43 2024-11-06T02:51:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861496.83 epoch sec; seqNo:341; 4061485.35 north; 594539.01 east; 36.69 lat; -121.94 lon; 21.43 depth; utmZone: 10 2024-11-06T02:51:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861494.81 2024-11-06T02:51:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:36Z- waiting for 1730861498.81, time is 1730861496.83 2024-11-06T02:51:37Z- AHRS_M2 msg: 1730861497.22 epoch sec; seqNo:315 2024-11-06T02:51:37Z- AHRS_M2 msg: 0.13 phi; -0.28 theta; 5.23 psi 2024-11-06T02:51:37Z- handleDepth()- Depth_Keller msg: 1730861497.25 epoch sec; seqNo:342; depth 21.50 2024-11-06T02:51:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861497.32 epoch sec; seqNo:342; 4061485.73 north; 594539.01 east; 36.69 lat; -121.94 lon; 21.50 depth; utmZone: 10 2024-11-06T02:51:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861494.81 2024-11-06T02:51:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:37Z- waiting for 1730861498.81, time is 1730861497.32 2024-11-06T02:51:37Z- AHRS_M2 msg: 1730861497.62 epoch sec; seqNo:316 2024-11-06T02:51:37Z- AHRS_M2 msg: 0.13 phi; -0.28 theta; 5.22 psi 2024-11-06T02:51:37Z- handleDepth()- Depth_Keller msg: 1730861497.62 epoch sec; seqNo:343; depth 21.59 2024-11-06T02:51:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861497.64 epoch sec; seqNo:343; 4061485.71 north; 594537.79 east; 36.69 lat; -121.94 lon; 21.59 depth; utmZone: 10 2024-11-06T02:51:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861494.81 2024-11-06T02:51:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:37Z- waiting for 1730861498.81, time is 1730861497.64 2024-11-06T02:51:38Z- AHRS_M2 msg: 1730861498.02 epoch sec; seqNo:317 2024-11-06T02:51:38Z- AHRS_M2 msg: 0.13 phi; -0.29 theta; 5.22 psi 2024-11-06T02:51:38Z- handleDepth()- Depth_Keller msg: 1730861498.03 epoch sec; seqNo:344; depth 21.65 2024-11-06T02:51:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861498.04 epoch sec; seqNo:344; 4061486.09 north; 594537.78 east; 36.69 lat; -121.94 lon; 21.65 depth; utmZone: 10 2024-11-06T02:51:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861494.81 2024-11-06T02:51:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:38Z- waiting for 1730861498.81, time is 1730861498.04 2024-11-06T02:51:38Z- AHRS_M2 msg: 1730861498.44 epoch sec; seqNo:318 2024-11-06T02:51:38Z- AHRS_M2 msg: 0.13 phi; -0.29 theta; 5.21 psi 2024-11-06T02:51:38Z- handleDepth()- Depth_Keller msg: 1730861498.45 epoch sec; seqNo:345; depth 21.73 2024-11-06T02:51:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861498.46 epoch sec; seqNo:345; 4061486.09 north; 594537.78 east; 36.69 lat; -121.94 lon; 21.73 depth; utmZone: 10 2024-11-06T02:51:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861494.81 2024-11-06T02:51:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:38Z- waiting for 1730861498.81, time is 1730861498.46 2024-11-06T02:51:38Z- AHRS_M2 msg: 1730861498.83 epoch sec; seqNo:319 2024-11-06T02:51:38Z- AHRS_M2 msg: 0.12 phi; -0.29 theta; 5.20 psi 2024-11-06T02:51:38Z- handleDepth()- Depth_Keller msg: 1730861498.84 epoch sec; seqNo:346; depth 21.82 2024-11-06T02:51:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861498.84 epoch sec; seqNo:346; 4061486.47 north; 594537.78 east; 36.69 lat; -121.94 lon; 21.82 depth; utmZone: 10 2024-11-06T02:51:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861494.81 2024-11-06T02:51:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:38Z- waiting for 1730861498.81, time is 1730861498.84 2024-11-06T02:51:38Z- LcmTrn::updateTrn() >>>> pose time 1730861498.84, meas time 1730861495.63 <<<< 2024-11-06T02:51:38Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861498.84 2024-11-06T02:51:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861495.63 sec, ping # 0; waiting for INS data more recent than 1730861494.81... 2024-11-06T02:51:38Z- TerrainNav::motionUpdate() - currEstimate = (4061486, 594537.78, 21.82) 2024-11-06T02:51:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:51:38Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:51:39Z- TNavPF::Projecting Measurements 2024-11-06T02:51:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:51:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:51:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:51:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:51:39Z- TNavFilter::4 good beams. 2024-11-06T02:51:39Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:51:39Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:51:39Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:51:39Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:51:39Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:51:39Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:51:39Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:51:39Z- TerrainMapOctree: (vehN, vehE ) = (4061484.98, 594539.74). 2024-11-06T02:51:39Z- TerrainMapOctree: Min Distance = 315.32. 2024-11-06T02:51:39Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:51:39Z- Forcing Subcloud Comparison 2024-11-06T02:51:39Z- Weighting particles with subcloud comparison 2024-11-06T02:51:39Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:39Z- meanExpectedMeasDiff: -5.443340 delta_rms_squared: 1.912905 alpha: 0.989653 2024-11-06T02:51:39Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:39Z- meanExpectedMeasDiff: 1.951345 delta_rms_squared: 2.283628 alpha: 0.991318 2024-11-06T02:51:39Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:39Z- meanExpectedMeasDiff: 0.672016 delta_rms_squared: 1.840395 alpha: 0.989250 2024-11-06T02:51:39Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:39Z- meanExpectedMeasDiff: 2.240069 delta_rms_squared: 2.795177 alpha: 0.992896 2024-11-06T02:51:39Z- Subcloud NIS: 0.412632 2024-11-06T02:51:39Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:51:39Z- TNavPF:: sumWeights = 0.084981 2024-11-06T02:51:39Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:51:39Z- TNavPF::First Term of Map Covariance Matrix is 2.17 2024-11-06T02:51:39Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T02:51:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861495.63 sec. 2024-11-06T02:51:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:51:39Z- variances([0], [2], and psi_berg) = 9.997424 20.775211 0.000000 2024-11-06T02:51:39Z- publishEstimatesDecoder() - at 1730861498.84 sec 2024-11-06T02:51:39Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:51:39Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061526.77 594536.05 30.95 2024-11-06T02:51:39Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061520.59 594531.97 30.99 lat/lon: 36.69 -121.94 2024-11-06T02:51:39Z- LcmTrn::publishEstimatesDecoder() - COVAR: 10.00 20.78 0.93 0.00 2024-11-06T02:51:39Z- AHRS_M2 msg: 1730861499.27 epoch sec; seqNo:320 2024-11-06T02:51:39Z- AHRS_M2 msg: 0.12 phi; -0.30 theta; 5.20 psi 2024-11-06T02:51:39Z- handleDepth()- Depth_Keller msg: 1730861499.30 epoch sec; seqNo:347; depth 21.92 2024-11-06T02:51:39Z- handleDVL() - DVL msg epochMillisec: 1730861499.30 2024-11-06T02:51:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:39Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:39Z- handleDvl() - RDI_Pathfinder msg: 1730861499.30 sec; seqNo:113; #beams:0 2024-11-06T02:51:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , -0.07 2024-11-06T02:51:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861499.34 epoch sec; seqNo:347; 4061486.83 north; 594536.56 east; 36.69 lat; -121.94 lon; 21.92 depth; utmZone: 10 2024-11-06T02:51:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861498.84 2024-11-06T02:51:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:39Z- waiting for 1730861502.84, time is 1730861499.34 2024-11-06T02:51:39Z- AHRS_M2 msg: 1730861499.74 epoch sec; seqNo:321 2024-11-06T02:51:39Z- AHRS_M2 msg: 0.12 phi; -0.31 theta; 5.19 psi 2024-11-06T02:51:39Z- handleDepth()- Depth_Keller msg: 1730861499.74 epoch sec; seqNo:348; depth 22.01 2024-11-06T02:51:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861499.75 epoch sec; seqNo:348; 4061486.83 north; 594536.56 east; 36.69 lat; -121.94 lon; 22.01 depth; utmZone: 10 2024-11-06T02:51:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861498.84 2024-11-06T02:51:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:39Z- waiting for 1730861502.84, time is 1730861499.75 2024-11-06T02:51:40Z- AHRS_M2 msg: 1730861500.14 epoch sec; seqNo:322 2024-11-06T02:51:40Z- AHRS_M2 msg: 0.12 phi; -0.31 theta; 5.19 psi 2024-11-06T02:51:40Z- handleDepth()- Depth_Keller msg: 1730861500.14 epoch sec; seqNo:349; depth 22.11 2024-11-06T02:51:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861500.15 epoch sec; seqNo:349; 4061486.83 north; 594536.56 east; 36.69 lat; -121.94 lon; 22.11 depth; utmZone: 10 2024-11-06T02:51:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861498.84 2024-11-06T02:51:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:40Z- waiting for 1730861502.84, time is 1730861500.15 2024-11-06T02:51:40Z- AHRS_M2 msg: 1730861500.54 epoch sec; seqNo:323 2024-11-06T02:51:40Z- AHRS_M2 msg: 0.12 phi; -0.31 theta; 5.19 psi 2024-11-06T02:51:40Z- handleDepth()- Depth_Keller msg: 1730861500.55 epoch sec; seqNo:350; depth 22.21 2024-11-06T02:51:40Z- handleDVL() - DVL msg epochMillisec: 1730861500.55 2024-11-06T02:51:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:40Z- handleDvl() - RDI_Pathfinder msg: 1730861500.55 sec; seqNo:114; #beams:0 2024-11-06T02:51:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , -0.07 2024-11-06T02:51:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861500.56 epoch sec; seqNo:350; 4061487.21 north; 594536.55 east; 36.69 lat; -121.94 lon; 22.21 depth; utmZone: 10 2024-11-06T02:51:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861498.84 2024-11-06T02:51:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:40Z- waiting for 1730861502.84, time is 1730861500.56 2024-11-06T02:51:40Z- AHRS_M2 msg: 1730861500.94 epoch sec; seqNo:324 2024-11-06T02:51:40Z- AHRS_M2 msg: 0.12 phi; -0.31 theta; 5.18 psi 2024-11-06T02:51:40Z- handleDepth()- Depth_Keller msg: 1730861500.95 epoch sec; seqNo:351; depth 22.30 2024-11-06T02:51:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861500.97 epoch sec; seqNo:351; 4061487.20 north; 594535.33 east; 36.69 lat; -121.94 lon; 22.30 depth; utmZone: 10 2024-11-06T02:51:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861498.84 2024-11-06T02:51:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:41Z- waiting for 1730861502.84, time is 1730861500.97 2024-11-06T02:51:41Z- AHRS_M2 msg: 1730861501.35 epoch sec; seqNo:325 2024-11-06T02:51:41Z- AHRS_M2 msg: 0.12 phi; -0.31 theta; 5.18 psi 2024-11-06T02:51:41Z- handleDepth()- Depth_Keller msg: 1730861501.35 epoch sec; seqNo:352; depth 22.40 2024-11-06T02:51:41Z- handleDVL() - DVL msg epochMillisec: 1730861501.37 2024-11-06T02:51:41Z- handleDvl() - RDI_Pathfinder msg: 1730861501.37 sec; seqNo:115; #beams:4 2024-11-06T02:51:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.50 , 60.55 , 56.49 , 79.96 2024-11-06T02:51:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.09 , 0.08 2024-11-06T02:51:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861501.40 epoch sec; seqNo:352; 4061487.58 north; 594535.33 east; 36.69 lat; -121.94 lon; 22.40 depth; utmZone: 10 2024-11-06T02:51:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861498.84 2024-11-06T02:51:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:41Z- waiting for 1730861502.84, time is 1730861501.40 2024-11-06T02:51:41Z- AHRS_M2 msg: 1730861501.75 epoch sec; seqNo:326 2024-11-06T02:51:41Z- AHRS_M2 msg: 0.12 phi; -0.31 theta; 5.18 psi 2024-11-06T02:51:41Z- handleDepth()- Depth_Keller msg: 1730861501.76 epoch sec; seqNo:353; depth 22.50 2024-11-06T02:51:41Z- handleDVL() - DVL msg epochMillisec: 1730861501.76 2024-11-06T02:51:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:41Z- handleDvl() - RDI_Pathfinder msg: 1730861501.76 sec; seqNo:116; #beams:0 2024-11-06T02:51:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.09 , 0.08 2024-11-06T02:51:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861501.78 epoch sec; seqNo:353; 4061487.58 north; 594535.33 east; 36.69 lat; -121.94 lon; 22.50 depth; utmZone: 10 2024-11-06T02:51:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861498.84 2024-11-06T02:51:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:41Z- waiting for 1730861502.84, time is 1730861501.78 2024-11-06T02:51:42Z- AHRS_M2 msg: 1730861502.16 epoch sec; seqNo:327 2024-11-06T02:51:42Z- AHRS_M2 msg: 0.13 phi; -0.31 theta; 5.18 psi 2024-11-06T02:51:42Z- handleDepth()- Depth_Keller msg: 1730861502.16 epoch sec; seqNo:354; depth 22.59 2024-11-06T02:51:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861502.17 epoch sec; seqNo:354; 4061487.96 north; 594535.32 east; 36.69 lat; -121.94 lon; 22.59 depth; utmZone: 10 2024-11-06T02:51:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861498.84 2024-11-06T02:51:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:42Z- waiting for 1730861502.84, time is 1730861502.17 2024-11-06T02:51:42Z- AHRS_M2 msg: 1730861502.56 epoch sec; seqNo:328 2024-11-06T02:51:42Z- AHRS_M2 msg: 0.13 phi; -0.31 theta; 5.18 psi 2024-11-06T02:51:42Z- handleDepth()- Depth_Keller msg: 1730861502.56 epoch sec; seqNo:355; depth 22.70 2024-11-06T02:51:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861502.58 epoch sec; seqNo:355; 4061487.94 north; 594534.10 east; 36.69 lat; -121.94 lon; 22.70 depth; utmZone: 10 2024-11-06T02:51:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861498.84 2024-11-06T02:51:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:42Z- waiting for 1730861502.84, time is 1730861502.58 2024-11-06T02:51:42Z- AHRS_M2 msg: 1730861502.96 epoch sec; seqNo:329 2024-11-06T02:51:42Z- AHRS_M2 msg: 0.13 phi; -0.31 theta; 5.17 psi 2024-11-06T02:51:42Z- handleDepth()- Depth_Keller msg: 1730861502.97 epoch sec; seqNo:356; depth 22.78 2024-11-06T02:51:42Z- handleDVL() - DVL msg epochMillisec: 1730861502.97 2024-11-06T02:51:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:42Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:42Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:42Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:42Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:42Z- handleDvl() - RDI_Pathfinder msg: 1730861502.97 sec; seqNo:117; #beams:0 2024-11-06T02:51:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.09 , 0.08 2024-11-06T02:51:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861502.98 epoch sec; seqNo:356; 4061488.32 north; 594534.10 east; 36.69 lat; -121.94 lon; 22.78 depth; utmZone: 10 2024-11-06T02:51:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861498.84 2024-11-06T02:51:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:43Z- waiting for 1730861502.84, time is 1730861502.98 2024-11-06T02:51:43Z- LcmTrn::updateTrn() >>>> pose time 1730861502.98, meas time 1730861502.97 <<<< 2024-11-06T02:51:43Z- LcmTrn::updateTrn() equating pose 1730861502.97 and meas times 1730861502.97 2024-11-06T02:51:43Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861502.97 2024-11-06T02:51:43Z- TerrainNav::motionUpdate() - currEstimate = (4061488, 594534.10, 22.78) 2024-11-06T02:51:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:51:43Z- TerrainNav:: SubcloudNIS is 0.41, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:51:43Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:51:43Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:51:43Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:51:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:51:43Z- variances([0], [2], and psi_berg) = 9.993452 20.773779 0.000000 2024-11-06T02:51:43Z- publishEstimatesDecoder() - at 1730861502.97 sec 2024-11-06T02:51:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:51:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061528.62 594532.40 32.60 2024-11-06T02:51:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061522.44 594528.29 31.95 lat/lon: 36.69 -121.94 2024-11-06T02:51:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 9.99 20.77 1.17 0.00 2024-11-06T02:51:43Z- AHRS_M2 msg: 1730861503.37 epoch sec; seqNo:330 2024-11-06T02:51:43Z- AHRS_M2 msg: 0.13 phi; -0.31 theta; 5.17 psi 2024-11-06T02:51:43Z- handleDepth()- Depth_Keller msg: 1730861503.38 epoch sec; seqNo:357; depth 22.89 2024-11-06T02:51:43Z- handleDVL() - DVL msg epochMillisec: 1730861503.40 2024-11-06T02:51:43Z- handleDvl() - RDI_Pathfinder msg: 1730861503.40 sec; seqNo:118; #beams:4 2024-11-06T02:51:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.86 , 60.11 , 56.13 , 80.36 2024-11-06T02:51:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , 0.04 2024-11-06T02:51:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861503.42 epoch sec; seqNo:357; 4061488.32 north; 594534.10 east; 36.69 lat; -121.94 lon; 22.89 depth; utmZone: 10 2024-11-06T02:51:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861502.97 2024-11-06T02:51:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:43Z- waiting for 1730861506.97, time is 1730861503.42 2024-11-06T02:51:43Z- AHRS_M2 msg: 1730861503.77 epoch sec; seqNo:331 2024-11-06T02:51:43Z- AHRS_M2 msg: 0.13 phi; -0.31 theta; 5.17 psi 2024-11-06T02:51:43Z- handleDepth()- Depth_Keller msg: 1730861503.78 epoch sec; seqNo:358; depth 22.99 2024-11-06T02:51:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861503.80 epoch sec; seqNo:358; 4061488.70 north; 594534.09 east; 36.69 lat; -121.94 lon; 22.99 depth; utmZone: 10 2024-11-06T02:51:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861502.97 2024-11-06T02:51:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:43Z- waiting for 1730861506.97, time is 1730861503.80 2024-11-06T02:51:44Z- AHRS_M2 msg: 1730861504.18 epoch sec; seqNo:332 2024-11-06T02:51:44Z- AHRS_M2 msg: 0.13 phi; -0.31 theta; 5.16 psi 2024-11-06T02:51:44Z- handleDepth()- Depth_Keller msg: 1730861504.19 epoch sec; seqNo:359; depth 23.09 2024-11-06T02:51:44Z- handleDVL() - DVL msg epochMillisec: 1730861504.19 2024-11-06T02:51:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:44Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:44Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:44Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:44Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:44Z- handleDvl() - RDI_Pathfinder msg: 1730861504.19 sec; seqNo:119; #beams:0 2024-11-06T02:51:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , 0.04 2024-11-06T02:51:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861504.20 epoch sec; seqNo:359; 4061488.69 north; 594532.87 east; 36.69 lat; -121.94 lon; 23.09 depth; utmZone: 10 2024-11-06T02:51:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861502.97 2024-11-06T02:51:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:44Z- waiting for 1730861506.97, time is 1730861504.20 2024-11-06T02:51:44Z- AHRS_M2 msg: 1730861504.58 epoch sec; seqNo:333 2024-11-06T02:51:44Z- AHRS_M2 msg: 0.14 phi; -0.31 theta; 5.15 psi 2024-11-06T02:51:44Z- handleDepth()- Depth_Keller msg: 1730861504.59 epoch sec; seqNo:360; depth 23.18 2024-11-06T02:51:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861504.60 epoch sec; seqNo:360; 4061489.07 north; 594532.87 east; 36.69 lat; -121.94 lon; 23.18 depth; utmZone: 10 2024-11-06T02:51:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861502.97 2024-11-06T02:51:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:44Z- waiting for 1730861506.97, time is 1730861504.60 2024-11-06T02:51:44Z- AHRS_M2 msg: 1730861504.98 epoch sec; seqNo:334 2024-11-06T02:51:44Z- AHRS_M2 msg: 0.13 phi; -0.31 theta; 5.15 psi 2024-11-06T02:51:44Z- handleDepth()- Depth_Keller msg: 1730861504.99 epoch sec; seqNo:361; depth 23.29 2024-11-06T02:51:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861505.01 epoch sec; seqNo:361; 4061489.07 north; 594532.87 east; 36.69 lat; -121.94 lon; 23.29 depth; utmZone: 10 2024-11-06T02:51:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861502.97 2024-11-06T02:51:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:45Z- waiting for 1730861506.97, time is 1730861505.01 2024-11-06T02:51:45Z- AHRS_M2 msg: 1730861505.39 epoch sec; seqNo:335 2024-11-06T02:51:45Z- AHRS_M2 msg: 0.13 phi; -0.31 theta; 5.15 psi 2024-11-06T02:51:45Z- handleDepth()- Depth_Keller msg: 1730861505.40 epoch sec; seqNo:362; depth 23.39 2024-11-06T02:51:45Z- handleDVL() - DVL msg epochMillisec: 1730861505.40 2024-11-06T02:51:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:45Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:45Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:45Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:45Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:45Z- handleDvl() - RDI_Pathfinder msg: 1730861505.40 sec; seqNo:120; #beams:0 2024-11-06T02:51:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , 0.04 2024-11-06T02:51:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861505.42 epoch sec; seqNo:362; 4061489.45 north; 594532.87 east; 36.69 lat; -121.94 lon; 23.39 depth; utmZone: 10 2024-11-06T02:51:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861502.97 2024-11-06T02:51:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:45Z- waiting for 1730861506.97, time is 1730861505.42 2024-11-06T02:51:45Z- AHRS_M2 msg: 1730861505.79 epoch sec; seqNo:336 2024-11-06T02:51:45Z- AHRS_M2 msg: 0.13 phi; -0.31 theta; 5.15 psi 2024-11-06T02:51:45Z- handleDepth()- Depth_Keller msg: 1730861505.80 epoch sec; seqNo:363; depth 23.48 2024-11-06T02:51:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861505.81 epoch sec; seqNo:363; 4061489.43 north; 594531.65 east; 36.69 lat; -121.94 lon; 23.48 depth; utmZone: 10 2024-11-06T02:51:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861502.97 2024-11-06T02:51:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:45Z- waiting for 1730861506.97, time is 1730861505.81 2024-11-06T02:51:46Z- AHRS_M2 msg: 1730861506.19 epoch sec; seqNo:337 2024-11-06T02:51:46Z- AHRS_M2 msg: 0.13 phi; -0.31 theta; 5.15 psi 2024-11-06T02:51:46Z- handleDepth()- Depth_Keller msg: 1730861506.20 epoch sec; seqNo:364; depth 23.57 2024-11-06T02:51:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861506.22 epoch sec; seqNo:364; 4061489.81 north; 594531.64 east; 36.69 lat; -121.94 lon; 23.57 depth; utmZone: 10 2024-11-06T02:51:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861502.97 2024-11-06T02:51:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:46Z- waiting for 1730861506.97, time is 1730861506.22 2024-11-06T02:51:46Z- AHRS_M2 msg: 1730861506.66 epoch sec; seqNo:338 2024-11-06T02:51:46Z- AHRS_M2 msg: 0.13 phi; -0.31 theta; 5.15 psi 2024-11-06T02:51:46Z- handleDepth()- Depth_Keller msg: 1730861506.67 epoch sec; seqNo:365; depth 23.67 2024-11-06T02:51:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861506.70 epoch sec; seqNo:365; 4061489.81 north; 594531.64 east; 36.69 lat; -121.94 lon; 23.67 depth; utmZone: 10 2024-11-06T02:51:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861502.97 2024-11-06T02:51:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:46Z- waiting for 1730861506.97, time is 1730861506.70 2024-11-06T02:51:47Z- AHRS_M2 msg: 1730861507.00 epoch sec; seqNo:339 2024-11-06T02:51:47Z- AHRS_M2 msg: 0.14 phi; -0.31 theta; 5.15 psi 2024-11-06T02:51:47Z- handleDepth()- Depth_Keller msg: 1730861507.01 epoch sec; seqNo:366; depth 23.77 2024-11-06T02:51:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861507.02 epoch sec; seqNo:366; 4061490.19 north; 594531.64 east; 36.69 lat; -121.94 lon; 23.77 depth; utmZone: 10 2024-11-06T02:51:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861502.97 2024-11-06T02:51:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:47Z- waiting for 1730861506.97, time is 1730861507.02 2024-11-06T02:51:47Z- LcmTrn::updateTrn() >>>> pose time 1730861507.02, meas time 1730861505.40 <<<< 2024-11-06T02:51:47Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861507.02 2024-11-06T02:51:47Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:51:47Z- TerrainNav::motionUpdate() - currEstimate = (4061490, 594531.64, 23.77) 2024-11-06T02:51:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:51:47Z- TerrainNav:: SubcloudNIS is 0.41, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:51:47Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:51:47Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:51:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:51:47Z- variances([0], [2], and psi_berg) = 9.992556 20.777039 0.000000 2024-11-06T02:51:47Z- publishEstimatesDecoder() - at 1730861507.02 sec 2024-11-06T02:51:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:51:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061530.36 594529.97 33.90 2024-11-06T02:51:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061524.31 594525.83 32.94 lat/lon: 36.69 -121.94 2024-11-06T02:51:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 9.99 20.78 1.43 0.00 2024-11-06T02:51:47Z- AHRS_M2 msg: 1730861507.41 epoch sec; seqNo:340 2024-11-06T02:51:47Z- AHRS_M2 msg: 0.14 phi; -0.31 theta; 5.16 psi 2024-11-06T02:51:47Z- handleDepth()- Depth_Keller msg: 1730861507.41 epoch sec; seqNo:367; depth 23.86 2024-11-06T02:51:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861507.47 epoch sec; seqNo:367; 4061490.18 north; 594530.42 east; 36.69 lat; -121.94 lon; 23.86 depth; utmZone: 10 2024-11-06T02:51:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861507.02 2024-11-06T02:51:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:47Z- waiting for 1730861511.02, time is 1730861507.47 2024-11-06T02:51:47Z- AHRS_M2 msg: 1730861507.81 epoch sec; seqNo:341 2024-11-06T02:51:47Z- AHRS_M2 msg: 0.14 phi; -0.30 theta; 5.16 psi 2024-11-06T02:51:47Z- handleDepth()- Depth_Keller msg: 1730861507.82 epoch sec; seqNo:368; depth 23.96 2024-11-06T02:51:47Z- handleDVL() - DVL msg epochMillisec: 1730861507.82 2024-11-06T02:51:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:47Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:47Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:47Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:47Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:47Z- handleDvl() - RDI_Pathfinder msg: 1730861507.82 sec; seqNo:121; #beams:0 2024-11-06T02:51:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , 0.04 2024-11-06T02:51:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861507.83 epoch sec; seqNo:368; 4061490.56 north; 594530.41 east; 36.69 lat; -121.94 lon; 23.96 depth; utmZone: 10 2024-11-06T02:51:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861507.02 2024-11-06T02:51:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:47Z- waiting for 1730861511.02, time is 1730861507.83 2024-11-06T02:51:48Z- AHRS_M2 msg: 1730861508.22 epoch sec; seqNo:342 2024-11-06T02:51:48Z- AHRS_M2 msg: 0.15 phi; -0.30 theta; 5.16 psi 2024-11-06T02:51:48Z- handleDepth()- Depth_Keller msg: 1730861508.23 epoch sec; seqNo:369; depth 24.04 2024-11-06T02:51:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861508.25 epoch sec; seqNo:369; 4061490.56 north; 594530.41 east; 36.69 lat; -121.94 lon; 24.04 depth; utmZone: 10 2024-11-06T02:51:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861507.02 2024-11-06T02:51:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:48Z- waiting for 1730861511.02, time is 1730861508.25 2024-11-06T02:51:48Z- AHRS_M2 msg: 1730861508.62 epoch sec; seqNo:343 2024-11-06T02:51:48Z- AHRS_M2 msg: 0.15 phi; -0.30 theta; 5.16 psi 2024-11-06T02:51:48Z- handleDepth()- Depth_Keller msg: 1730861508.62 epoch sec; seqNo:370; depth 24.13 2024-11-06T02:51:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861508.65 epoch sec; seqNo:370; 4061490.93 north; 594530.41 east; 36.69 lat; -121.94 lon; 24.13 depth; utmZone: 10 2024-11-06T02:51:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861507.02 2024-11-06T02:51:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:48Z- waiting for 1730861511.02, time is 1730861508.65 2024-11-06T02:51:49Z- AHRS_M2 msg: 1730861509.02 epoch sec; seqNo:344 2024-11-06T02:51:49Z- AHRS_M2 msg: 0.15 phi; -0.30 theta; 5.16 psi 2024-11-06T02:51:49Z- handleDepth()- Depth_Keller msg: 1730861509.03 epoch sec; seqNo:371; depth 24.21 2024-11-06T02:51:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861509.04 epoch sec; seqNo:371; 4061490.92 north; 594529.19 east; 36.69 lat; -121.94 lon; 24.21 depth; utmZone: 10 2024-11-06T02:51:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861507.02 2024-11-06T02:51:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:49Z- waiting for 1730861511.02, time is 1730861509.04 2024-11-06T02:51:49Z- AHRS_M2 msg: 1730861509.43 epoch sec; seqNo:345 2024-11-06T02:51:49Z- AHRS_M2 msg: 0.16 phi; -0.30 theta; 5.16 psi 2024-11-06T02:51:49Z- handleDepth()- Depth_Keller msg: 1730861509.43 epoch sec; seqNo:372; depth 24.30 2024-11-06T02:51:49Z- handleDVL() - DVL msg epochMillisec: 1730861509.45 2024-11-06T02:51:49Z- handleDvl() - RDI_Pathfinder msg: 1730861509.45 sec; seqNo:122; #beams:4 2024-11-06T02:51:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.64 , 57.97 , 54.90 , 76.06 2024-11-06T02:51:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , -0.00 2024-11-06T02:51:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861509.49 epoch sec; seqNo:372; 4061491.30 north; 594529.18 east; 36.69 lat; -121.94 lon; 24.30 depth; utmZone: 10 2024-11-06T02:51:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861507.02 2024-11-06T02:51:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:49Z- waiting for 1730861511.02, time is 1730861509.49 2024-11-06T02:51:49Z- AHRS_M2 msg: 1730861509.83 epoch sec; seqNo:346 2024-11-06T02:51:49Z- AHRS_M2 msg: 0.15 phi; -0.30 theta; 5.16 psi 2024-11-06T02:51:49Z- handleDepth()- Depth_Keller msg: 1730861509.84 epoch sec; seqNo:373; depth 24.39 2024-11-06T02:51:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861509.85 epoch sec; seqNo:373; 4061491.30 north; 594529.18 east; 36.69 lat; -121.94 lon; 24.39 depth; utmZone: 10 2024-11-06T02:51:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861507.02 2024-11-06T02:51:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:49Z- waiting for 1730861511.02, time is 1730861509.85 2024-11-06T02:51:50Z- AHRS_M2 msg: 1730861510.23 epoch sec; seqNo:347 2024-11-06T02:51:50Z- AHRS_M2 msg: 0.15 phi; -0.30 theta; 5.15 psi 2024-11-06T02:51:50Z- handleDepth()- Depth_Keller msg: 1730861510.25 epoch sec; seqNo:374; depth 24.47 2024-11-06T02:51:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861510.26 epoch sec; seqNo:374; 4061491.68 north; 594529.18 east; 36.69 lat; -121.94 lon; 24.47 depth; utmZone: 10 2024-11-06T02:51:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861507.02 2024-11-06T02:51:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:50Z- waiting for 1730861511.02, time is 1730861510.26 2024-11-06T02:51:50Z- AHRS_M2 msg: 1730861510.64 epoch sec; seqNo:348 2024-11-06T02:51:50Z- AHRS_M2 msg: 0.15 phi; -0.30 theta; 5.15 psi 2024-11-06T02:51:50Z- handleDepth()- Depth_Keller msg: 1730861510.65 epoch sec; seqNo:375; depth 24.57 2024-11-06T02:51:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861510.67 epoch sec; seqNo:375; 4061491.67 north; 594527.96 east; 36.69 lat; -121.94 lon; 24.57 depth; utmZone: 10 2024-11-06T02:51:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861507.02 2024-11-06T02:51:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:50Z- waiting for 1730861511.02, time is 1730861510.67 2024-11-06T02:51:51Z- AHRS_M2 msg: 1730861511.04 epoch sec; seqNo:349 2024-11-06T02:51:51Z- AHRS_M2 msg: 0.15 phi; -0.30 theta; 5.15 psi 2024-11-06T02:51:51Z- handleDepth()- Depth_Keller msg: 1730861511.05 epoch sec; seqNo:376; depth 24.66 2024-11-06T02:51:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861511.06 epoch sec; seqNo:376; 4061491.67 north; 594527.96 east; 36.69 lat; -121.94 lon; 24.66 depth; utmZone: 10 2024-11-06T02:51:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861507.02 2024-11-06T02:51:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:51Z- waiting for 1730861511.02, time is 1730861511.06 2024-11-06T02:51:51Z- LcmTrn::updateTrn() >>>> pose time 1730861511.06, meas time 1730861509.45 <<<< 2024-11-06T02:51:51Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861511.06 2024-11-06T02:51:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861509.45 sec, ping # 0; waiting for INS data more recent than 1730861507.02... 2024-11-06T02:51:51Z- TerrainNav::motionUpdate() - currEstimate = (4061492, 594527.96, 24.66) 2024-11-06T02:51:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:51:51Z- TerrainNav:: SubcloudNIS is 0.41, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:51:51Z- TNavPF::Projecting Measurements 2024-11-06T02:51:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:51:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:51:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:51:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:51:51Z- TNavFilter::4 good beams. 2024-11-06T02:51:51Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:51:51Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:51:51Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:51:51Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:51:51Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:51:51Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:51:51Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:51:51Z- TerrainMapOctree: (vehN, vehE ) = (4061491.08, 594529.42). 2024-11-06T02:51:51Z- TerrainMapOctree: Min Distance = 326.76. 2024-11-06T02:51:51Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:51:51Z- Forcing Subcloud Comparison 2024-11-06T02:51:51Z- Weighting particles with subcloud comparison 2024-11-06T02:51:51Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:51Z- meanExpectedMeasDiff: -5.276479 delta_rms_squared: 2.730488 alpha: 0.992729 2024-11-06T02:51:51Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:51Z- meanExpectedMeasDiff: 3.464580 delta_rms_squared: 3.328150 alpha: 0.994027 2024-11-06T02:51:51Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:51Z- meanExpectedMeasDiff: 0.416439 delta_rms_squared: 2.201320 alpha: 0.990996 2024-11-06T02:51:51Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:51:51Z- meanExpectedMeasDiff: 0.611751 delta_rms_squared: 3.979172 alpha: 0.994999 2024-11-06T02:51:51Z- Subcloud NIS: 0.436331 2024-11-06T02:51:51Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:51:51Z- TNavPF:: sumWeights = 0.056972 2024-11-06T02:51:51Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:51:51Z- TNavPF::First Term of Map Covariance Matrix is 1.44 2024-11-06T02:51:51Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T02:51:51Z- TerrainNav::Resampling particle filter... 2024-11-06T02:51:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861509.45 sec. 2024-11-06T02:51:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:51:51Z- variances([0], [2], and psi_berg) = 8.021200 17.547639 0.000000 2024-11-06T02:51:51Z- publishEstimatesDecoder() - at 1730861511.06 sec 2024-11-06T02:51:51Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:51:51Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061529.74 594521.15 32.44 2024-11-06T02:51:51Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061525.09 594523.05 33.73 lat/lon: 36.69 -121.94 2024-11-06T02:51:51Z- LcmTrn::publishEstimatesDecoder() - COVAR: 8.02 17.55 1.00 0.00 2024-11-06T02:51:51Z- AHRS_M2 msg: 1730861511.45 epoch sec; seqNo:350 2024-11-06T02:51:51Z- AHRS_M2 msg: 0.15 phi; -0.30 theta; 5.14 psi 2024-11-06T02:51:51Z- handleDepth()- Depth_Keller msg: 1730861511.45 epoch sec; seqNo:377; depth 24.75 2024-11-06T02:51:51Z- handleDVL() - DVL msg epochMillisec: 1730861511.49 2024-11-06T02:51:51Z- handleDvl() - RDI_Pathfinder msg: 1730861511.49 sec; seqNo:123; #beams:4 2024-11-06T02:51:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.93 , 57.96 , 54.90 , 75.44 2024-11-06T02:51:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , 0.08 2024-11-06T02:51:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861511.51 epoch sec; seqNo:377; 4061492.04 north; 594527.96 east; 36.69 lat; -121.94 lon; 24.75 depth; utmZone: 10 2024-11-06T02:51:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861511.06 2024-11-06T02:51:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:51Z- waiting for 1730861515.06, time is 1730861511.51 2024-11-06T02:51:51Z- AHRS_M2 msg: 1730861511.88 epoch sec; seqNo:351 2024-11-06T02:51:51Z- AHRS_M2 msg: 0.15 phi; -0.31 theta; 5.14 psi 2024-11-06T02:51:51Z- handleDepth()- Depth_Keller msg: 1730861511.88 epoch sec; seqNo:378; depth 24.83 2024-11-06T02:51:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861511.91 epoch sec; seqNo:378; 4061492.04 north; 594527.96 east; 36.69 lat; -121.94 lon; 24.83 depth; utmZone: 10 2024-11-06T02:51:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861511.06 2024-11-06T02:51:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:52Z- waiting for 1730861515.06, time is 1730861511.91 2024-11-06T02:51:52Z- AHRS_M2 msg: 1730861512.26 epoch sec; seqNo:352 2024-11-06T02:51:52Z- AHRS_M2 msg: 0.15 phi; -0.31 theta; 5.14 psi 2024-11-06T02:51:52Z- handleDepth()- Depth_Keller msg: 1730861512.27 epoch sec; seqNo:379; depth 24.93 2024-11-06T02:51:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861512.27 epoch sec; seqNo:379; 4061492.41 north; 594526.73 east; 36.69 lat; -121.94 lon; 24.93 depth; utmZone: 10 2024-11-06T02:51:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861511.06 2024-11-06T02:51:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:52Z- waiting for 1730861515.06, time is 1730861512.27 2024-11-06T02:51:52Z- AHRS_M2 msg: 1730861512.67 epoch sec; seqNo:353 2024-11-06T02:51:52Z- AHRS_M2 msg: 0.15 phi; -0.31 theta; 5.14 psi 2024-11-06T02:51:52Z- handleDepth()- Depth_Keller msg: 1730861512.68 epoch sec; seqNo:380; depth 25.01 2024-11-06T02:51:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861512.69 epoch sec; seqNo:380; 4061492.41 north; 594526.73 east; 36.69 lat; -121.94 lon; 25.01 depth; utmZone: 10 2024-11-06T02:51:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861511.06 2024-11-06T02:51:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:52Z- waiting for 1730861515.06, time is 1730861512.69 2024-11-06T02:51:53Z- AHRS_M2 msg: 1730861513.06 epoch sec; seqNo:354 2024-11-06T02:51:53Z- AHRS_M2 msg: 0.15 phi; -0.31 theta; 5.14 psi 2024-11-06T02:51:53Z- handleDepth()- Depth_Keller msg: 1730861513.08 epoch sec; seqNo:381; depth 25.09 2024-11-06T02:51:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861513.11 epoch sec; seqNo:381; 4061492.79 north; 594526.73 east; 36.69 lat; -121.94 lon; 25.09 depth; utmZone: 10 2024-11-06T02:51:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861511.06 2024-11-06T02:51:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:53Z- waiting for 1730861515.06, time is 1730861513.11 2024-11-06T02:51:53Z- AHRS_M2 msg: 1730861513.47 epoch sec; seqNo:355 2024-11-06T02:51:53Z- AHRS_M2 msg: 0.14 phi; -0.31 theta; 5.14 psi 2024-11-06T02:51:53Z- handleDepth()- Depth_Keller msg: 1730861513.47 epoch sec; seqNo:382; depth 25.19 2024-11-06T02:51:53Z- handleDVL() - DVL msg epochMillisec: 1730861513.50 2024-11-06T02:51:53Z- handleDvl() - RDI_Pathfinder msg: 1730861513.50 sec; seqNo:124; #beams:4 2024-11-06T02:51:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.17 , 57.35 , 53.87 , 74.15 2024-11-06T02:51:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.06 2024-11-06T02:51:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861513.55 epoch sec; seqNo:382; 4061492.79 north; 594526.73 east; 36.69 lat; -121.94 lon; 25.19 depth; utmZone: 10 2024-11-06T02:51:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861511.06 2024-11-06T02:51:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:53Z- waiting for 1730861515.06, time is 1730861513.55 2024-11-06T02:51:53Z- AHRS_M2 msg: 1730861513.87 epoch sec; seqNo:356 2024-11-06T02:51:53Z- AHRS_M2 msg: 0.14 phi; -0.31 theta; 5.14 psi 2024-11-06T02:51:53Z- handleDepth()- Depth_Keller msg: 1730861513.89 epoch sec; seqNo:383; depth 25.28 2024-11-06T02:51:53Z- handleDVL() - DVL msg epochMillisec: 1730861513.89 2024-11-06T02:51:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:53Z- handleDvl() - RDI_Pathfinder msg: 1730861513.89 sec; seqNo:125; #beams:0 2024-11-06T02:51:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.06 2024-11-06T02:51:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861513.91 epoch sec; seqNo:383; 4061493.15 north; 594525.50 east; 36.69 lat; -121.94 lon; 25.28 depth; utmZone: 10 2024-11-06T02:51:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861511.06 2024-11-06T02:51:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:54Z- waiting for 1730861515.06, time is 1730861513.91 2024-11-06T02:51:54Z- handleDepth()- Depth_Keller msg: 1730861514.30 epoch sec; seqNo:384; depth 25.37 2024-11-06T02:51:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861514.31 epoch sec; seqNo:384; 4061493.15 north; 594525.50 east; 36.69 lat; -121.94 lon; 25.37 depth; utmZone: 10 2024-11-06T02:51:54Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:51:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861511.06 2024-11-06T02:51:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:54Z- waiting for 1730861515.06, time is 1730861514.31 2024-11-06T02:51:54Z- AHRS_M2 msg: 1730861514.68 epoch sec; seqNo:357 2024-11-06T02:51:54Z- AHRS_M2 msg: 0.14 phi; -0.31 theta; 5.14 psi 2024-11-06T02:51:54Z- handleDepth()- Depth_Keller msg: 1730861514.69 epoch sec; seqNo:385; depth 25.46 2024-11-06T02:51:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861514.70 epoch sec; seqNo:385; 4061493.15 north; 594525.50 east; 36.69 lat; -121.94 lon; 25.46 depth; utmZone: 10 2024-11-06T02:51:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861511.06 2024-11-06T02:51:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:54Z- waiting for 1730861515.06, time is 1730861514.70 2024-11-06T02:51:55Z- AHRS_M2 msg: 1730861515.09 epoch sec; seqNo:358 2024-11-06T02:51:55Z- AHRS_M2 msg: 0.14 phi; -0.31 theta; 5.14 psi 2024-11-06T02:51:55Z- handleDepth()- Depth_Keller msg: 1730861515.09 epoch sec; seqNo:386; depth 25.54 2024-11-06T02:51:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861515.11 epoch sec; seqNo:386; 4061493.53 north; 594525.50 east; 36.69 lat; -121.94 lon; 25.54 depth; utmZone: 10 2024-11-06T02:51:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861511.06 2024-11-06T02:51:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:55Z- waiting for 1730861515.06, time is 1730861515.11 2024-11-06T02:51:55Z- LcmTrn::updateTrn() >>>> pose time 1730861515.11, meas time 1730861513.89 <<<< 2024-11-06T02:51:55Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861515.11 2024-11-06T02:51:55Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:51:55Z- TerrainNav::motionUpdate() - currEstimate = (4061494, 594525.50, 25.54) 2024-11-06T02:51:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:51:55Z- TerrainNav:: SubcloudNIS is 0.44, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:51:55Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:51:55Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:51:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:51:55Z- variances([0], [2], and psi_berg) = 8.025609 17.546452 0.000000 2024-11-06T02:51:55Z- publishEstimatesDecoder() - at 1730861515.11 sec 2024-11-06T02:51:55Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:51:55Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061531.55 594518.73 32.85 2024-11-06T02:51:55Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061526.96 594520.59 34.61 lat/lon: 36.69 -121.94 2024-11-06T02:51:55Z- LcmTrn::publishEstimatesDecoder() - COVAR: 8.03 17.55 1.23 0.00 2024-11-06T02:51:55Z- AHRS_M2 msg: 1730861515.49 epoch sec; seqNo:359 2024-11-06T02:51:55Z- AHRS_M2 msg: 0.15 phi; -0.31 theta; 5.15 psi 2024-11-06T02:51:55Z- handleDepth()- Depth_Keller msg: 1730861515.50 epoch sec; seqNo:387; depth 25.63 2024-11-06T02:51:55Z- handleDVL() - DVL msg epochMillisec: 1730861515.53 2024-11-06T02:51:55Z- handleDvl() - RDI_Pathfinder msg: 1730861515.53 sec; seqNo:126; #beams:4 2024-11-06T02:51:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.30 , 57.35 , 53.87 , 73.57 2024-11-06T02:51:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , 0.00 2024-11-06T02:51:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861515.55 epoch sec; seqNo:387; 4061493.52 north; 594524.28 east; 36.69 lat; -121.94 lon; 25.63 depth; utmZone: 10 2024-11-06T02:51:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861515.11 2024-11-06T02:51:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:55Z- waiting for 1730861519.11, time is 1730861515.55 2024-11-06T02:51:55Z- AHRS_M2 msg: 1730861515.90 epoch sec; seqNo:360 2024-11-06T02:51:55Z- AHRS_M2 msg: 0.15 phi; -0.31 theta; 5.15 psi 2024-11-06T02:51:55Z- handleDepth()- Depth_Keller msg: 1730861515.90 epoch sec; seqNo:388; depth 25.73 2024-11-06T02:51:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861515.92 epoch sec; seqNo:388; 4061493.90 north; 594524.27 east; 36.69 lat; -121.94 lon; 25.73 depth; utmZone: 10 2024-11-06T02:51:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861515.11 2024-11-06T02:51:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:56Z- waiting for 1730861519.11, time is 1730861515.92 2024-11-06T02:51:56Z- AHRS_M2 msg: 1730861516.30 epoch sec; seqNo:361 2024-11-06T02:51:56Z- AHRS_M2 msg: 0.16 phi; -0.30 theta; 5.15 psi 2024-11-06T02:51:56Z- handleDepth()- Depth_Keller msg: 1730861516.30 epoch sec; seqNo:389; depth 25.81 2024-11-06T02:51:56Z- handleDVL() - DVL msg epochMillisec: 1730861516.31 2024-11-06T02:51:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:56Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:56Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:56Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:56Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:56Z- handleDvl() - RDI_Pathfinder msg: 1730861516.31 sec; seqNo:127; #beams:0 2024-11-06T02:51:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , 0.00 2024-11-06T02:51:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861516.32 epoch sec; seqNo:389; 4061493.90 north; 594524.27 east; 36.69 lat; -121.94 lon; 25.81 depth; utmZone: 10 2024-11-06T02:51:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861515.11 2024-11-06T02:51:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:56Z- waiting for 1730861519.11, time is 1730861516.32 2024-11-06T02:51:56Z- AHRS_M2 msg: 1730861516.70 epoch sec; seqNo:362 2024-11-06T02:51:56Z- AHRS_M2 msg: 0.16 phi; -0.30 theta; 5.15 psi 2024-11-06T02:51:56Z- handleDepth()- Depth_Keller msg: 1730861516.71 epoch sec; seqNo:390; depth 25.89 2024-11-06T02:51:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861516.73 epoch sec; seqNo:390; 4061493.88 north; 594523.05 east; 36.69 lat; -121.94 lon; 25.89 depth; utmZone: 10 2024-11-06T02:51:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861515.11 2024-11-06T02:51:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:56Z- waiting for 1730861519.11, time is 1730861516.73 2024-11-06T02:51:57Z- AHRS_M2 msg: 1730861517.10 epoch sec; seqNo:363 2024-11-06T02:51:57Z- AHRS_M2 msg: 0.16 phi; -0.30 theta; 5.15 psi 2024-11-06T02:51:57Z- handleDepth()- Depth_Keller msg: 1730861517.11 epoch sec; seqNo:391; depth 26.00 2024-11-06T02:51:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861517.12 epoch sec; seqNo:391; 4061494.26 north; 594523.05 east; 36.69 lat; -121.94 lon; 26.00 depth; utmZone: 10 2024-11-06T02:51:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861515.11 2024-11-06T02:51:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:57Z- waiting for 1730861519.11, time is 1730861517.12 2024-11-06T02:51:57Z- AHRS_M2 msg: 1730861517.52 epoch sec; seqNo:364 2024-11-06T02:51:57Z- AHRS_M2 msg: 0.16 phi; -0.29 theta; 5.15 psi 2024-11-06T02:51:57Z- handleDepth()- Depth_Keller msg: 1730861517.52 epoch sec; seqNo:392; depth 26.08 2024-11-06T02:51:57Z- handleDVL() - DVL msg epochMillisec: 1730861517.55 2024-11-06T02:51:57Z- handleDvl() - RDI_Pathfinder msg: 1730861517.55 sec; seqNo:128; #beams:4 2024-11-06T02:51:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.29 , 56.47 , 53.87 , 73.27 2024-11-06T02:51:57Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.04 , 0.06 2024-11-06T02:51:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861517.57 epoch sec; seqNo:392; 4061494.26 north; 594523.05 east; 36.69 lat; -121.94 lon; 26.08 depth; utmZone: 10 2024-11-06T02:51:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861515.11 2024-11-06T02:51:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:57Z- waiting for 1730861519.11, time is 1730861517.57 2024-11-06T02:51:57Z- AHRS_M2 msg: 1730861517.91 epoch sec; seqNo:365 2024-11-06T02:51:57Z- AHRS_M2 msg: 0.16 phi; -0.29 theta; 5.15 psi 2024-11-06T02:51:57Z- handleDepth()- Depth_Keller msg: 1730861517.93 epoch sec; seqNo:393; depth 26.20 2024-11-06T02:51:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861517.94 epoch sec; seqNo:393; 4061494.64 north; 594523.04 east; 36.69 lat; -121.94 lon; 26.20 depth; utmZone: 10 2024-11-06T02:51:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861515.11 2024-11-06T02:51:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:58Z- waiting for 1730861519.11, time is 1730861517.94 2024-11-06T02:51:58Z- AHRS_M2 msg: 1730861518.32 epoch sec; seqNo:366 2024-11-06T02:51:58Z- AHRS_M2 msg: 0.16 phi; -0.29 theta; 5.14 psi 2024-11-06T02:51:58Z- handleDepth()- Depth_Keller msg: 1730861518.32 epoch sec; seqNo:394; depth 26.28 2024-11-06T02:51:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861518.33 epoch sec; seqNo:394; 4061494.63 north; 594521.82 east; 36.69 lat; -121.94 lon; 26.28 depth; utmZone: 10 2024-11-06T02:51:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861515.11 2024-11-06T02:51:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:58Z- waiting for 1730861519.11, time is 1730861518.33 2024-11-06T02:51:58Z- AHRS_M2 msg: 1730861518.73 epoch sec; seqNo:367 2024-11-06T02:51:58Z- AHRS_M2 msg: 0.16 phi; -0.29 theta; 5.14 psi 2024-11-06T02:51:58Z- handleDepth()- Depth_Keller msg: 1730861518.73 epoch sec; seqNo:395; depth 26.37 2024-11-06T02:51:58Z- handleDVL() - DVL msg epochMillisec: 1730861518.74 2024-11-06T02:51:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:58Z- handleDvl() - RDI_Pathfinder msg: 1730861518.74 sec; seqNo:129; #beams:0 2024-11-06T02:51:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:58Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.04 , 0.06 2024-11-06T02:51:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861518.75 epoch sec; seqNo:395; 4061495.01 north; 594521.82 east; 36.69 lat; -121.94 lon; 26.37 depth; utmZone: 10 2024-11-06T02:51:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:51:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861515.11 2024-11-06T02:51:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:58Z- waiting for 1730861519.11, time is 1730861518.75 2024-11-06T02:51:59Z- AHRS_M2 msg: 1730861519.13 epoch sec; seqNo:368 2024-11-06T02:51:59Z- AHRS_M2 msg: 0.16 phi; -0.29 theta; 5.14 psi 2024-11-06T02:51:59Z- handleDepth()- Depth_Keller msg: 1730861519.13 epoch sec; seqNo:396; depth 26.47 2024-11-06T02:51:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861519.14 epoch sec; seqNo:396; 4061495.01 north; 594521.82 east; 36.69 lat; -121.94 lon; 26.47 depth; utmZone: 10 2024-11-06T02:51:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861515.11 2024-11-06T02:51:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:59Z- waiting for 1730861519.11, time is 1730861519.14 2024-11-06T02:51:59Z- LcmTrn::updateTrn() >>>> pose time 1730861519.14, meas time 1730861518.74 <<<< 2024-11-06T02:51:59Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861519.14 2024-11-06T02:51:59Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:51:59Z- TerrainNav::motionUpdate() - currEstimate = (4061495, 594521.82, 26.47) 2024-11-06T02:51:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:51:59Z- TerrainNav:: SubcloudNIS is 0.44, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:51:59Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:51:59Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:51:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:51:59Z- variances([0], [2], and psi_berg) = 8.031807 17.544584 0.000000 2024-11-06T02:51:59Z- publishEstimatesDecoder() - at 1730861519.14 sec 2024-11-06T02:51:59Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:51:59Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061532.94 594515.08 34.85 2024-11-06T02:51:59Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061528.44 594516.91 35.55 lat/lon: 36.69 -121.94 2024-11-06T02:51:59Z- LcmTrn::publishEstimatesDecoder() - COVAR: 8.03 17.54 1.48 0.00 2024-11-06T02:51:59Z- handleDepth()- Depth_Keller msg: 1730861519.55 epoch sec; seqNo:397; depth 26.57 2024-11-06T02:51:59Z- handleDVL() - DVL msg epochMillisec: 1730861519.57 2024-11-06T02:51:59Z- handleDvl() - RDI_Pathfinder msg: 1730861519.57 sec; seqNo:130; #beams:4 2024-11-06T02:51:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.00 , 56.47 , 53.87 , 70.96 2024-11-06T02:51:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.02 , 0.20 2024-11-06T02:51:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861519.59 epoch sec; seqNo:397; 4061494.99 north; 594520.60 east; 36.69 lat; -121.94 lon; 26.57 depth; utmZone: 10 2024-11-06T02:51:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:51:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861519.14 2024-11-06T02:51:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:51:59Z- waiting for 1730861523.14, time is 1730861519.59 2024-11-06T02:51:59Z- AHRS_M2 msg: 1730861519.93 epoch sec; seqNo:369 2024-11-06T02:51:59Z- AHRS_M2 msg: 0.15 phi; -0.28 theta; 5.14 psi 2024-11-06T02:51:59Z- handleDepth()- Depth_Keller msg: 1730861519.94 epoch sec; seqNo:398; depth 26.66 2024-11-06T02:51:59Z- handleDVL() - DVL msg epochMillisec: 1730861519.94 2024-11-06T02:51:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:51:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:51:59Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:51:59Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:51:59Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:51:59Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:51:59Z- handleDvl() - RDI_Pathfinder msg: 1730861519.94 sec; seqNo:131; #beams:0 2024-11-06T02:51:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:51:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.02 , 0.20 2024-11-06T02:51:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861519.95 epoch sec; seqNo:398; 4061495.37 north; 594520.60 east; 36.69 lat; -121.94 lon; 26.66 depth; utmZone: 10 2024-11-06T02:52:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861519.14 2024-11-06T02:52:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:00Z- waiting for 1730861523.14, time is 1730861519.95 2024-11-06T02:52:00Z- AHRS_M2 msg: 1730861520.34 epoch sec; seqNo:370 2024-11-06T02:52:00Z- AHRS_M2 msg: 0.15 phi; -0.28 theta; 5.14 psi 2024-11-06T02:52:00Z- handleDepth()- Depth_Keller msg: 1730861520.34 epoch sec; seqNo:399; depth 26.74 2024-11-06T02:52:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861520.35 epoch sec; seqNo:399; 4061495.37 north; 594520.60 east; 36.69 lat; -121.94 lon; 26.74 depth; utmZone: 10 2024-11-06T02:52:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861519.14 2024-11-06T02:52:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:00Z- waiting for 1730861523.14, time is 1730861520.35 2024-11-06T02:52:00Z- AHRS_M2 msg: 1730861520.74 epoch sec; seqNo:371 2024-11-06T02:52:00Z- AHRS_M2 msg: 0.16 phi; -0.28 theta; 5.14 psi 2024-11-06T02:52:00Z- handleDepth()- Depth_Keller msg: 1730861520.75 epoch sec; seqNo:400; depth 26.82 2024-11-06T02:52:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861520.76 epoch sec; seqNo:400; 4061495.75 north; 594520.59 east; 36.69 lat; -121.94 lon; 26.82 depth; utmZone: 10 2024-11-06T02:52:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861519.14 2024-11-06T02:52:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:00Z- waiting for 1730861523.14, time is 1730861520.76 2024-11-06T02:52:01Z- AHRS_M2 msg: 1730861521.14 epoch sec; seqNo:372 2024-11-06T02:52:01Z- AHRS_M2 msg: 0.16 phi; -0.28 theta; 5.15 psi 2024-11-06T02:52:01Z- handleDepth()- Depth_Keller msg: 1730861521.17 epoch sec; seqNo:401; depth 26.91 2024-11-06T02:52:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861521.18 epoch sec; seqNo:401; 4061495.74 north; 594519.37 east; 36.69 lat; -121.94 lon; 26.91 depth; utmZone: 10 2024-11-06T02:52:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861519.14 2024-11-06T02:52:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:01Z- waiting for 1730861523.14, time is 1730861521.18 2024-11-06T02:52:01Z- AHRS_M2 msg: 1730861521.55 epoch sec; seqNo:373 2024-11-06T02:52:01Z- AHRS_M2 msg: 0.16 phi; -0.28 theta; 5.15 psi 2024-11-06T02:52:01Z- handleDepth()- Depth_Keller msg: 1730861521.55 epoch sec; seqNo:402; depth 26.99 2024-11-06T02:52:01Z- handleDVL() - DVL msg epochMillisec: 1730861521.59 2024-11-06T02:52:01Z- handleDvl() - RDI_Pathfinder msg: 1730861521.59 sec; seqNo:132; #beams:4 2024-11-06T02:52:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.29 , 56.18 , 53.58 , 69.80 2024-11-06T02:52:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.01 , 0.10 2024-11-06T02:52:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861521.60 epoch sec; seqNo:402; 4061495.74 north; 594519.37 east; 36.69 lat; -121.94 lon; 26.99 depth; utmZone: 10 2024-11-06T02:52:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861519.14 2024-11-06T02:52:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:01Z- waiting for 1730861523.14, time is 1730861521.60 2024-11-06T02:52:01Z- AHRS_M2 msg: 1730861521.95 epoch sec; seqNo:374 2024-11-06T02:52:01Z- AHRS_M2 msg: 0.16 phi; -0.28 theta; 5.16 psi 2024-11-06T02:52:01Z- handleDepth()- Depth_Keller msg: 1730861521.96 epoch sec; seqNo:403; depth 27.06 2024-11-06T02:52:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861521.98 epoch sec; seqNo:403; 4061496.12 north; 594519.37 east; 36.69 lat; -121.94 lon; 27.06 depth; utmZone: 10 2024-11-06T02:52:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861519.14 2024-11-06T02:52:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:02Z- waiting for 1730861523.14, time is 1730861521.98 2024-11-06T02:52:02Z- AHRS_M2 msg: 1730861522.36 epoch sec; seqNo:375 2024-11-06T02:52:02Z- AHRS_M2 msg: 0.17 phi; -0.28 theta; 5.17 psi 2024-11-06T02:52:02Z- handleDepth()- Depth_Keller msg: 1730861522.36 epoch sec; seqNo:404; depth 27.14 2024-11-06T02:52:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861522.38 epoch sec; seqNo:404; 4061496.12 north; 594519.37 east; 36.69 lat; -121.94 lon; 27.14 depth; utmZone: 10 2024-11-06T02:52:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861519.14 2024-11-06T02:52:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:02Z- waiting for 1730861523.14, time is 1730861522.38 2024-11-06T02:52:02Z- AHRS_M2 msg: 1730861522.76 epoch sec; seqNo:376 2024-11-06T02:52:02Z- AHRS_M2 msg: 0.17 phi; -0.28 theta; 5.18 psi 2024-11-06T02:52:02Z- handleDepth()- Depth_Keller msg: 1730861522.77 epoch sec; seqNo:405; depth 27.21 2024-11-06T02:52:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861522.79 epoch sec; seqNo:405; 4061496.12 north; 594519.37 east; 36.69 lat; -121.94 lon; 27.21 depth; utmZone: 10 2024-11-06T02:52:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861519.14 2024-11-06T02:52:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:02Z- waiting for 1730861523.14, time is 1730861522.79 2024-11-06T02:52:03Z- AHRS_M2 msg: 1730861523.16 epoch sec; seqNo:377 2024-11-06T02:52:03Z- AHRS_M2 msg: 0.17 phi; -0.28 theta; 5.18 psi 2024-11-06T02:52:03Z- handleDepth()- Depth_Keller msg: 1730861523.17 epoch sec; seqNo:406; depth 27.29 2024-11-06T02:52:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861523.18 epoch sec; seqNo:406; 4061496.48 north; 594518.14 east; 36.69 lat; -121.94 lon; 27.29 depth; utmZone: 10 2024-11-06T02:52:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861519.14 2024-11-06T02:52:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:03Z- waiting for 1730861523.14, time is 1730861523.18 2024-11-06T02:52:03Z- LcmTrn::updateTrn() >>>> pose time 1730861523.18, meas time 1730861521.59 <<<< 2024-11-06T02:52:03Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861523.18 2024-11-06T02:52:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861521.59 sec, ping # 0; waiting for INS data more recent than 1730861519.14... 2024-11-06T02:52:03Z- TerrainNav::motionUpdate() - currEstimate = (4061496, 594518.14, 27.29) 2024-11-06T02:52:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:52:03Z- TerrainNav:: SubcloudNIS is 0.44, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:52:03Z- TNavPF::Projecting Measurements 2024-11-06T02:52:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:52:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:52:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:52:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:52:03Z- TNavFilter::4 good beams. 2024-11-06T02:52:03Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:52:03Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:52:03Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:52:03Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:52:03Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:52:03Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:52:03Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:52:03Z- TerrainMapOctree: (vehN, vehE ) = (4061495.90, 594519.59). 2024-11-06T02:52:03Z- TerrainMapOctree: Min Distance = 336.98. 2024-11-06T02:52:03Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:52:03Z- Forcing Subcloud Comparison 2024-11-06T02:52:03Z- Weighting particles with subcloud comparison 2024-11-06T02:52:03Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:03Z- meanExpectedMeasDiff: -4.092738 delta_rms_squared: 2.968886 alpha: 0.993309 2024-11-06T02:52:03Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:03Z- meanExpectedMeasDiff: 6.512712 delta_rms_squared: 2.695206 alpha: 0.992634 2024-11-06T02:52:03Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:03Z- meanExpectedMeasDiff: 0.735415 delta_rms_squared: 1.891749 alpha: 0.989538 2024-11-06T02:52:03Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:03Z- meanExpectedMeasDiff: -1.316637 delta_rms_squared: 4.234686 alpha: 0.995299 2024-11-06T02:52:03Z- Subcloud NIS: 0.458322 2024-11-06T02:52:03Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:52:03Z- TNavPF:: sumWeights = 0.010905 2024-11-06T02:52:03Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:52:03Z- TNavPF::First Term of Map Covariance Matrix is 0.22 2024-11-06T02:52:03Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T02:52:03Z- TerrainNav::Resampling particle filter... 2024-11-06T02:52:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861521.59 sec. 2024-11-06T02:52:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:52:03Z- variances([0], [2], and psi_berg) = 8.074044 24.777880 0.000000 2024-11-06T02:52:03Z- publishEstimatesDecoder() - at 1730861523.18 sec 2024-11-06T02:52:03Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:52:03Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061534.42 594511.41 36.27 2024-11-06T02:52:03Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061529.84 594512.53 36.17 lat/lon: 36.69 -121.94 2024-11-06T02:52:03Z- LcmTrn::publishEstimatesDecoder() - COVAR: 8.07 24.78 1.08 0.00 2024-11-06T02:52:03Z- handleDepth()- Depth_Keller msg: 1730861523.61 epoch sec; seqNo:407; depth 27.37 2024-11-06T02:52:03Z- handleDVL() - DVL msg epochMillisec: 1730861523.67 2024-11-06T02:52:03Z- handleDvl() - RDI_Pathfinder msg: 1730861523.67 sec; seqNo:133; #beams:4 2024-11-06T02:52:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.87 , 55.60 , 53.29 , 70.38 2024-11-06T02:52:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , -0.04 2024-11-06T02:52:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861523.69 epoch sec; seqNo:407; 4061496.48 north; 594518.14 east; 36.69 lat; -121.94 lon; 27.37 depth; utmZone: 10 2024-11-06T02:52:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861523.18 2024-11-06T02:52:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:03Z- waiting for 1730861527.18, time is 1730861523.69 2024-11-06T02:52:03Z- AHRS_M2 msg: 1730861523.97 epoch sec; seqNo:378 2024-11-06T02:52:03Z- AHRS_M2 msg: 0.17 phi; -0.27 theta; 5.19 psi 2024-11-06T02:52:04Z- handleDepth()- Depth_Keller msg: 1730861524.01 epoch sec; seqNo:408; depth 27.47 2024-11-06T02:52:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861524.03 epoch sec; seqNo:408; 4061496.86 north; 594518.14 east; 36.69 lat; -121.94 lon; 27.47 depth; utmZone: 10 2024-11-06T02:52:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861523.18 2024-11-06T02:52:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:04Z- waiting for 1730861527.18, time is 1730861524.03 2024-11-06T02:52:04Z- AHRS_M2 msg: 1730861524.38 epoch sec; seqNo:379 2024-11-06T02:52:04Z- AHRS_M2 msg: 0.17 phi; -0.27 theta; 5.19 psi 2024-11-06T02:52:04Z- handleDepth()- Depth_Keller msg: 1730861524.39 epoch sec; seqNo:409; depth 27.52 2024-11-06T02:52:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861524.40 epoch sec; seqNo:409; 4061496.86 north; 594518.14 east; 36.69 lat; -121.94 lon; 27.52 depth; utmZone: 10 2024-11-06T02:52:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861523.18 2024-11-06T02:52:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:04Z- waiting for 1730861527.18, time is 1730861524.40 2024-11-06T02:52:04Z- handleDepth()- Depth_Keller msg: 1730861524.80 epoch sec; seqNo:410; depth 27.59 2024-11-06T02:52:04Z- handleDVL() - DVL msg epochMillisec: 1730861524.80 2024-11-06T02:52:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:52:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:52:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:52:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:52:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:52:04Z- handleDvl() - RDI_Pathfinder msg: 1730861524.80 sec; seqNo:134; #beams:0 2024-11-06T02:52:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:52:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , -0.04 2024-11-06T02:52:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861524.81 epoch sec; seqNo:410; 4061497.23 north; 594516.91 east; 36.69 lat; -121.94 lon; 27.59 depth; utmZone: 10 2024-11-06T02:52:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861523.18 2024-11-06T02:52:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:04Z- waiting for 1730861527.18, time is 1730861524.81 2024-11-06T02:52:05Z- AHRS_M2 msg: 1730861525.18 epoch sec; seqNo:380 2024-11-06T02:52:05Z- AHRS_M2 msg: 0.17 phi; -0.27 theta; 5.19 psi 2024-11-06T02:52:05Z- handleDepth()- Depth_Keller msg: 1730861525.19 epoch sec; seqNo:411; depth 27.68 2024-11-06T02:52:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861525.21 epoch sec; seqNo:411; 4061497.23 north; 594516.91 east; 36.69 lat; -121.94 lon; 27.68 depth; utmZone: 10 2024-11-06T02:52:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861523.18 2024-11-06T02:52:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:05Z- waiting for 1730861527.18, time is 1730861525.21 2024-11-06T02:52:05Z- AHRS_M2 msg: 1730861525.59 epoch sec; seqNo:381 2024-11-06T02:52:05Z- AHRS_M2 msg: 0.17 phi; -0.27 theta; 5.19 psi 2024-11-06T02:52:05Z- handleDepth()- Depth_Keller msg: 1730861525.60 epoch sec; seqNo:412; depth 27.75 2024-11-06T02:52:05Z- handleDVL() - DVL msg epochMillisec: 1730861525.63 2024-11-06T02:52:05Z- handleDvl() - RDI_Pathfinder msg: 1730861525.63 sec; seqNo:135; #beams:4 2024-11-06T02:52:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.58 , 55.02 , 52.99 , 69.79 2024-11-06T02:52:05Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.05 , -0.01 2024-11-06T02:52:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861525.65 epoch sec; seqNo:412; 4061497.61 north; 594516.91 east; 36.69 lat; -121.94 lon; 27.75 depth; utmZone: 10 2024-11-06T02:52:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861523.18 2024-11-06T02:52:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:05Z- waiting for 1730861527.18, time is 1730861525.65 2024-11-06T02:52:06Z- AHRS_M2 msg: 1730861526.00 epoch sec; seqNo:382 2024-11-06T02:52:06Z- AHRS_M2 msg: 0.16 phi; -0.27 theta; 5.19 psi 2024-11-06T02:52:06Z- handleDepth()- Depth_Keller msg: 1730861526.00 epoch sec; seqNo:413; depth 27.84 2024-11-06T02:52:06Z- handleDVL() - DVL msg epochMillisec: 1730861526.01 2024-11-06T02:52:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:52:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:52:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:52:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:52:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:52:06Z- handleDvl() - RDI_Pathfinder msg: 1730861526.01 sec; seqNo:136; #beams:0 2024-11-06T02:52:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:52:06Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.05 , -0.01 2024-11-06T02:52:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861526.02 epoch sec; seqNo:413; 4061497.61 north; 594516.91 east; 36.69 lat; -121.94 lon; 27.84 depth; utmZone: 10 2024-11-06T02:52:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861523.18 2024-11-06T02:52:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:06Z- waiting for 1730861527.18, time is 1730861526.02 2024-11-06T02:52:06Z- AHRS_M2 msg: 1730861526.39 epoch sec; seqNo:383 2024-11-06T02:52:06Z- AHRS_M2 msg: 0.17 phi; -0.27 theta; 5.19 psi 2024-11-06T02:52:06Z- handleDepth()- Depth_Keller msg: 1730861526.41 epoch sec; seqNo:414; depth 27.89 2024-11-06T02:52:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861526.42 epoch sec; seqNo:414; 4061497.97 north; 594515.68 east; 36.69 lat; -121.94 lon; 27.89 depth; utmZone: 10 2024-11-06T02:52:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861523.18 2024-11-06T02:52:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:06Z- waiting for 1730861527.18, time is 1730861526.42 2024-11-06T02:52:06Z- AHRS_M2 msg: 1730861526.80 epoch sec; seqNo:384 2024-11-06T02:52:06Z- AHRS_M2 msg: 0.17 phi; -0.27 theta; 5.19 psi 2024-11-06T02:52:06Z- handleDepth()- Depth_Keller msg: 1730861526.81 epoch sec; seqNo:415; depth 27.98 2024-11-06T02:52:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861526.83 epoch sec; seqNo:415; 4061497.97 north; 594515.68 east; 36.69 lat; -121.94 lon; 27.98 depth; utmZone: 10 2024-11-06T02:52:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861523.18 2024-11-06T02:52:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:06Z- waiting for 1730861527.18, time is 1730861526.83 2024-11-06T02:52:07Z- AHRS_M2 msg: 1730861527.21 epoch sec; seqNo:385 2024-11-06T02:52:07Z- AHRS_M2 msg: 0.17 phi; -0.28 theta; 5.19 psi 2024-11-06T02:52:07Z- handleDepth()- Depth_Keller msg: 1730861527.22 epoch sec; seqNo:416; depth 28.08 2024-11-06T02:52:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861527.27 epoch sec; seqNo:416; 4061498.35 north; 594515.68 east; 36.69 lat; -121.94 lon; 28.08 depth; utmZone: 10 2024-11-06T02:52:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861523.18 2024-11-06T02:52:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:07Z- waiting for 1730861527.18, time is 1730861527.27 2024-11-06T02:52:07Z- LcmTrn::updateTrn() >>>> pose time 1730861527.27, meas time 1730861526.01 <<<< 2024-11-06T02:52:07Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861527.27 2024-11-06T02:52:07Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:52:07Z- TerrainNav::motionUpdate() - currEstimate = (4061498, 594515.68, 28.08) 2024-11-06T02:52:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:52:07Z- TerrainNav:: SubcloudNIS is 0.46, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:52:07Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:52:07Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:52:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:52:07Z- variances([0], [2], and psi_berg) = 8.073833 24.777737 0.000000 2024-11-06T02:52:07Z- publishEstimatesDecoder() - at 1730861527.27 sec 2024-11-06T02:52:07Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:52:07Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061536.30 594508.95 36.98 2024-11-06T02:52:07Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061531.71 594510.07 36.95 lat/lon: 36.69 -121.94 2024-11-06T02:52:07Z- LcmTrn::publishEstimatesDecoder() - COVAR: 8.07 24.78 1.33 0.00 2024-11-06T02:52:07Z- AHRS_M2 msg: 1730861527.61 epoch sec; seqNo:386 2024-11-06T02:52:07Z- AHRS_M2 msg: 0.17 phi; -0.28 theta; 5.19 psi 2024-11-06T02:52:07Z- handleDepth()- Depth_Keller msg: 1730861527.62 epoch sec; seqNo:417; depth 28.16 2024-11-06T02:52:07Z- handleDVL() - DVL msg epochMillisec: 1730861527.64 2024-11-06T02:52:07Z- handleDvl() - RDI_Pathfinder msg: 1730861527.64 sec; seqNo:137; #beams:4 2024-11-06T02:52:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.00 , 54.73 , 52.12 , 68.92 2024-11-06T02:52:07Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.03 , 0.09 2024-11-06T02:52:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861527.66 epoch sec; seqNo:417; 4061498.34 north; 594514.46 east; 36.69 lat; -121.94 lon; 28.16 depth; utmZone: 10 2024-11-06T02:52:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861527.27 2024-11-06T02:52:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:07Z- waiting for 1730861531.27, time is 1730861527.66 2024-11-06T02:52:08Z- AHRS_M2 msg: 1730861528.01 epoch sec; seqNo:387 2024-11-06T02:52:08Z- AHRS_M2 msg: 0.17 phi; -0.28 theta; 5.19 psi 2024-11-06T02:52:08Z- handleDepth()- Depth_Keller msg: 1730861528.02 epoch sec; seqNo:418; depth 28.25 2024-11-06T02:52:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861528.05 epoch sec; seqNo:418; 4061498.72 north; 594514.46 east; 36.69 lat; -121.94 lon; 28.25 depth; utmZone: 10 2024-11-06T02:52:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861527.27 2024-11-06T02:52:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:08Z- waiting for 1730861531.27, time is 1730861528.05 2024-11-06T02:52:08Z- AHRS_M2 msg: 1730861528.41 epoch sec; seqNo:388 2024-11-06T02:52:08Z- AHRS_M2 msg: 0.17 phi; -0.28 theta; 5.19 psi 2024-11-06T02:52:08Z- handleDepth()- Depth_Keller msg: 1730861528.43 epoch sec; seqNo:419; depth 28.30 2024-11-06T02:52:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861528.44 epoch sec; seqNo:419; 4061498.72 north; 594514.46 east; 36.69 lat; -121.94 lon; 28.30 depth; utmZone: 10 2024-11-06T02:52:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861527.27 2024-11-06T02:52:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:08Z- waiting for 1730861531.27, time is 1730861528.44 2024-11-06T02:52:08Z- AHRS_M2 msg: 1730861528.82 epoch sec; seqNo:389 2024-11-06T02:52:08Z- AHRS_M2 msg: 0.17 phi; -0.28 theta; 5.19 psi 2024-11-06T02:52:08Z- handleDepth()- Depth_Keller msg: 1730861528.83 epoch sec; seqNo:420; depth 28.40 2024-11-06T02:52:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861528.84 epoch sec; seqNo:420; 4061498.72 north; 594514.46 east; 36.69 lat; -121.94 lon; 28.40 depth; utmZone: 10 2024-11-06T02:52:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861527.27 2024-11-06T02:52:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:08Z- waiting for 1730861531.27, time is 1730861528.84 2024-11-06T02:52:09Z- AHRS_M2 msg: 1730861529.24 epoch sec; seqNo:390 2024-11-06T02:52:09Z- AHRS_M2 msg: 0.17 phi; -0.28 theta; 5.20 psi 2024-11-06T02:52:09Z- handleDepth()- Depth_Keller msg: 1730861529.26 epoch sec; seqNo:421; depth 28.48 2024-11-06T02:52:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861529.31 epoch sec; seqNo:421; 4061499.08 north; 594513.23 east; 36.69 lat; -121.94 lon; 28.48 depth; utmZone: 10 2024-11-06T02:52:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861527.27 2024-11-06T02:52:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:09Z- waiting for 1730861531.27, time is 1730861529.31 2024-11-06T02:52:09Z- AHRS_M2 msg: 1730861529.63 epoch sec; seqNo:391 2024-11-06T02:52:09Z- AHRS_M2 msg: 0.17 phi; -0.28 theta; 5.20 psi 2024-11-06T02:52:09Z- handleDepth()- Depth_Keller msg: 1730861529.63 epoch sec; seqNo:422; depth 28.57 2024-11-06T02:52:09Z- handleDVL() - DVL msg epochMillisec: 1730861529.67 2024-11-06T02:52:09Z- handleDvl() - RDI_Pathfinder msg: 1730861529.67 sec; seqNo:138; #beams:4 2024-11-06T02:52:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.77 , 54.50 , 52.32 , 69.22 2024-11-06T02:52:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , 0.07 2024-11-06T02:52:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861529.69 epoch sec; seqNo:422; 4061499.08 north; 594513.23 east; 36.69 lat; -121.94 lon; 28.57 depth; utmZone: 10 2024-11-06T02:52:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861527.27 2024-11-06T02:52:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:09Z- waiting for 1730861531.27, time is 1730861529.69 2024-11-06T02:52:10Z- AHRS_M2 msg: 1730861530.03 epoch sec; seqNo:392 2024-11-06T02:52:10Z- AHRS_M2 msg: 0.17 phi; -0.28 theta; 5.20 psi 2024-11-06T02:52:10Z- handleDepth()- Depth_Keller msg: 1730861530.04 epoch sec; seqNo:423; depth 28.64 2024-11-06T02:52:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861530.09 epoch sec; seqNo:423; 4061499.46 north; 594513.23 east; 36.69 lat; -121.94 lon; 28.64 depth; utmZone: 10 2024-11-06T02:52:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861527.27 2024-11-06T02:52:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:10Z- waiting for 1730861531.27, time is 1730861530.09 2024-11-06T02:52:10Z- AHRS_M2 msg: 1730861530.43 epoch sec; seqNo:393 2024-11-06T02:52:10Z- AHRS_M2 msg: 0.17 phi; -0.28 theta; 5.21 psi 2024-11-06T02:52:10Z- handleDepth()- Depth_Keller msg: 1730861530.44 epoch sec; seqNo:424; depth 28.72 2024-11-06T02:52:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861530.45 epoch sec; seqNo:424; 4061499.46 north; 594513.23 east; 36.69 lat; -121.94 lon; 28.72 depth; utmZone: 10 2024-11-06T02:52:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861527.27 2024-11-06T02:52:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:10Z- waiting for 1730861531.27, time is 1730861530.45 2024-11-06T02:52:10Z- handleDepth()- Depth_Keller msg: 1730861530.86 epoch sec; seqNo:425; depth 28.81 2024-11-06T02:52:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861530.87 epoch sec; seqNo:425; 4061499.45 north; 594512.01 east; 36.69 lat; -121.94 lon; 28.81 depth; utmZone: 10 2024-11-06T02:52:11Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:52:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861527.27 2024-11-06T02:52:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:11Z- waiting for 1730861531.27, time is 1730861530.87 2024-11-06T02:52:11Z- handleDepth()- Depth_Keller msg: 1730861531.31 epoch sec; seqNo:426; depth 28.86 2024-11-06T02:52:11Z- handleDVL() - DVL msg epochMillisec: 1730861531.33 2024-11-06T02:52:11Z- handleDvl() - RDI_Pathfinder msg: 1730861531.33 sec; seqNo:139; #beams:4 2024-11-06T02:52:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.41 , 54.15 , 52.12 , 69.78 2024-11-06T02:52:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.01 , -0.03 2024-11-06T02:52:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861531.37 epoch sec; seqNo:426; 4061499.82 north; 594512.00 east; 36.69 lat; -121.94 lon; 28.86 depth; utmZone: 10 2024-11-06T02:52:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861527.27 2024-11-06T02:52:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:11Z- waiting for 1730861531.27, time is 1730861531.37 2024-11-06T02:52:11Z- LcmTrn::updateTrn() >>>> pose time 1730861531.37, meas time 1730861531.33 <<<< 2024-11-06T02:52:11Z- LcmTrn::updateTrn() equating pose 1730861531.33 and meas times 1730861531.33 2024-11-06T02:52:11Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861531.33 2024-11-06T02:52:11Z- TerrainNav::motionUpdate() - currEstimate = (4061500, 594512.00, 28.86) 2024-11-06T02:52:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:52:11Z- TerrainNav:: SubcloudNIS is 0.46, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:52:11Z- TNavPF::Projecting Measurements 2024-11-06T02:52:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:52:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:52:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:52:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:52:11Z- TNavFilter::4 good beams. 2024-11-06T02:52:11Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:52:11Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:52:11Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:52:11Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:52:11Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:52:11Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:52:11Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:52:11Z- TerrainMapOctree: (vehN, vehE ) = (4061499.82, 594512.00). 2024-11-06T02:52:11Z- TerrainMapOctree: Min Distance = 345.04. 2024-11-06T02:52:11Z- TerrainMapOctree: Using tile 2. 2024-11-06T02:52:11Z- Forcing Subcloud Comparison 2024-11-06T02:52:11Z- Weighting particles with subcloud comparison 2024-11-06T02:52:11Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:11Z- meanExpectedMeasDiff: -3.328890 delta_rms_squared: 2.724438 alpha: 0.992713 2024-11-06T02:52:11Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:11Z- meanExpectedMeasDiff: 6.319515 delta_rms_squared: 4.069181 alpha: 0.995109 2024-11-06T02:52:11Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:11Z- meanExpectedMeasDiff: 1.043255 delta_rms_squared: 1.726895 alpha: 0.988551 2024-11-06T02:52:11Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:11Z- meanExpectedMeasDiff: -0.311376 delta_rms_squared: 2.652285 alpha: 0.992516 2024-11-06T02:52:11Z- Subcloud NIS: 0.472958 2024-11-06T02:52:11Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:52:11Z- TNavPF:: sumWeights = 0.028008 2024-11-06T02:52:11Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:52:11Z- TNavPF::First Term of Map Covariance Matrix is 0.42 2024-11-06T02:52:11Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T02:52:11Z- TerrainNav::Resampling particle filter... 2024-11-06T02:52:11Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861531.33 sec, ping # 0. 2024-11-06T02:52:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:52:11Z- variances([0], [2], and psi_berg) = 8.048510 29.416639 0.000000 2024-11-06T02:52:11Z- publishEstimatesDecoder() - at 1730861531.33 sec 2024-11-06T02:52:11Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:52:11Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061537.65 594505.20 37.70 2024-11-06T02:52:11Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061533.92 594500.35 37.75 lat/lon: 36.69 -121.94 2024-11-06T02:52:11Z- LcmTrn::publishEstimatesDecoder() - COVAR: 8.05 29.42 0.88 0.00 2024-11-06T02:52:11Z- AHRS_M2 msg: 1730861531.67 epoch sec; seqNo:394 2024-11-06T02:52:11Z- AHRS_M2 msg: 0.16 phi; -0.28 theta; 5.21 psi 2024-11-06T02:52:11Z- handleDepth()- Depth_Keller msg: 1730861531.68 epoch sec; seqNo:427; depth 28.98 2024-11-06T02:52:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861531.72 epoch sec; seqNo:427; 4061499.82 north; 594512.00 east; 36.69 lat; -121.94 lon; 28.98 depth; utmZone: 10 2024-11-06T02:52:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861531.33 2024-11-06T02:52:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:11Z- waiting for 1730861535.33, time is 1730861531.72 2024-11-06T02:52:12Z- handleDepth()- Depth_Keller msg: 1730861532.07 epoch sec; seqNo:428; depth 29.04 2024-11-06T02:52:12Z- handleDVL() - DVL msg epochMillisec: 1730861532.08 2024-11-06T02:52:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:52:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:52:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:52:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:52:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:52:12Z- handleDvl() - RDI_Pathfinder msg: 1730861532.08 sec; seqNo:140; #beams:0 2024-11-06T02:52:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:52:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.01 , -0.03 2024-11-06T02:52:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861532.09 epoch sec; seqNo:428; 4061500.19 north; 594510.78 east; 36.69 lat; -121.94 lon; 29.04 depth; utmZone: 10 2024-11-06T02:52:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861531.33 2024-11-06T02:52:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:12Z- waiting for 1730861535.33, time is 1730861532.09 2024-11-06T02:52:12Z- AHRS_M2 msg: 1730861532.46 epoch sec; seqNo:395 2024-11-06T02:52:12Z- AHRS_M2 msg: 0.16 phi; -0.28 theta; 5.21 psi 2024-11-06T02:52:12Z- handleDepth()- Depth_Keller msg: 1730861532.47 epoch sec; seqNo:429; depth 29.14 2024-11-06T02:52:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861532.48 epoch sec; seqNo:429; 4061500.19 north; 594510.78 east; 36.69 lat; -121.94 lon; 29.14 depth; utmZone: 10 2024-11-06T02:52:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861531.33 2024-11-06T02:52:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:12Z- waiting for 1730861535.33, time is 1730861532.48 2024-11-06T02:52:12Z- AHRS_M2 msg: 1730861532.86 epoch sec; seqNo:396 2024-11-06T02:52:12Z- AHRS_M2 msg: 0.17 phi; -0.28 theta; 5.20 psi 2024-11-06T02:52:12Z- handleDepth()- Depth_Keller msg: 1730861532.87 epoch sec; seqNo:430; depth 29.21 2024-11-06T02:52:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861532.88 epoch sec; seqNo:430; 4061500.57 north; 594510.77 east; 36.69 lat; -121.94 lon; 29.21 depth; utmZone: 10 2024-11-06T02:52:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861531.33 2024-11-06T02:52:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:13Z- waiting for 1730861535.33, time is 1730861532.88 2024-11-06T02:52:13Z- AHRS_M2 msg: 1730861533.29 epoch sec; seqNo:397 2024-11-06T02:52:13Z- AHRS_M2 msg: 0.17 phi; -0.27 theta; 5.20 psi 2024-11-06T02:52:13Z- handleDepth()- Depth_Keller msg: 1730861533.30 epoch sec; seqNo:431; depth 29.30 2024-11-06T02:52:13Z- handleDVL() - DVL msg epochMillisec: 1730861533.32 2024-11-06T02:52:13Z- handleDvl() - RDI_Pathfinder msg: 1730861533.32 sec; seqNo:141; #beams:4 2024-11-06T02:52:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.95 , 53.69 , 51.51 , 68.95 2024-11-06T02:52:13Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , -0.03 , 0.06 2024-11-06T02:52:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861533.34 epoch sec; seqNo:431; 4061500.57 north; 594510.77 east; 36.69 lat; -121.94 lon; 29.30 depth; utmZone: 10 2024-11-06T02:52:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861531.33 2024-11-06T02:52:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:13Z- waiting for 1730861535.33, time is 1730861533.34 2024-11-06T02:52:13Z- AHRS_M2 msg: 1730861533.68 epoch sec; seqNo:398 2024-11-06T02:52:13Z- AHRS_M2 msg: 0.16 phi; -0.27 theta; 5.19 psi 2024-11-06T02:52:13Z- handleDepth()- Depth_Keller msg: 1730861533.68 epoch sec; seqNo:432; depth 29.39 2024-11-06T02:52:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861533.71 epoch sec; seqNo:432; 4061500.93 north; 594509.55 east; 36.69 lat; -121.94 lon; 29.39 depth; utmZone: 10 2024-11-06T02:52:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861531.33 2024-11-06T02:52:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:13Z- waiting for 1730861535.33, time is 1730861533.71 2024-11-06T02:52:14Z- AHRS_M2 msg: 1730861534.08 epoch sec; seqNo:399 2024-11-06T02:52:14Z- AHRS_M2 msg: 0.16 phi; -0.28 theta; 5.19 psi 2024-11-06T02:52:14Z- handleDepth()- Depth_Keller msg: 1730861534.08 epoch sec; seqNo:433; depth 29.48 2024-11-06T02:52:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861534.09 epoch sec; seqNo:433; 4061500.93 north; 594509.55 east; 36.69 lat; -121.94 lon; 29.48 depth; utmZone: 10 2024-11-06T02:52:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861531.33 2024-11-06T02:52:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:14Z- waiting for 1730861535.33, time is 1730861534.09 2024-11-06T02:52:14Z- AHRS_M2 msg: 1730861534.48 epoch sec; seqNo:400 2024-11-06T02:52:14Z- AHRS_M2 msg: 0.16 phi; -0.28 theta; 5.19 psi 2024-11-06T02:52:14Z- handleDepth()- Depth_Keller msg: 1730861534.49 epoch sec; seqNo:434; depth 29.59 2024-11-06T02:52:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861534.51 epoch sec; seqNo:434; 4061500.93 north; 594509.55 east; 36.69 lat; -121.94 lon; 29.59 depth; utmZone: 10 2024-11-06T02:52:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861531.33 2024-11-06T02:52:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:14Z- waiting for 1730861535.33, time is 1730861534.51 2024-11-06T02:52:14Z- AHRS_M2 msg: 1730861534.88 epoch sec; seqNo:401 2024-11-06T02:52:14Z- AHRS_M2 msg: 0.17 phi; -0.28 theta; 5.19 psi 2024-11-06T02:52:14Z- handleDepth()- Depth_Keller msg: 1730861534.88 epoch sec; seqNo:435; depth 29.68 2024-11-06T02:52:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861534.90 epoch sec; seqNo:435; 4061501.30 north; 594508.32 east; 36.69 lat; -121.94 lon; 29.68 depth; utmZone: 10 2024-11-06T02:52:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861531.33 2024-11-06T02:52:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:15Z- waiting for 1730861535.33, time is 1730861534.90 2024-11-06T02:52:15Z- AHRS_M2 msg: 1730861535.30 epoch sec; seqNo:402 2024-11-06T02:52:15Z- AHRS_M2 msg: 0.17 phi; -0.28 theta; 5.19 psi 2024-11-06T02:52:15Z- handleDepth()- Depth_Keller msg: 1730861535.30 epoch sec; seqNo:436; depth 29.76 2024-11-06T02:52:15Z- handleDVL() - DVL msg epochMillisec: 1730861535.33 2024-11-06T02:52:15Z- handleDvl() - RDI_Pathfinder msg: 1730861535.33 sec; seqNo:142; #beams:4 2024-11-06T02:52:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.68 , 53.41 , 51.23 , 67.86 2024-11-06T02:52:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.07 , 0.18 2024-11-06T02:52:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861535.34 epoch sec; seqNo:436; 4061501.30 north; 594508.32 east; 36.69 lat; -121.94 lon; 29.76 depth; utmZone: 10 2024-11-06T02:52:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861531.33 2024-11-06T02:52:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:15Z- waiting for 1730861535.33, time is 1730861535.34 2024-11-06T02:52:15Z- LcmTrn::updateTrn() >>>> pose time 1730861535.34, meas time 1730861535.33 <<<< 2024-11-06T02:52:15Z- LcmTrn::updateTrn() equating pose 1730861535.33 and meas times 1730861535.33 2024-11-06T02:52:15Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861535.33 2024-11-06T02:52:15Z- TerrainNav::motionUpdate() - currEstimate = (4061501, 594508.32, 29.76) 2024-11-06T02:52:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:52:15Z- TerrainNav:: SubcloudNIS is 0.47, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:52:15Z- TNavPF::Projecting Measurements 2024-11-06T02:52:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:52:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:52:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:52:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:52:15Z- TNavFilter::4 good beams. 2024-11-06T02:52:15Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:52:15Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:52:15Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:52:15Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:52:15Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:52:15Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:52:15Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:52:15Z- TerrainMapOctree: (vehN, vehE ) = (4061501.30, 594508.32). 2024-11-06T02:52:15Z- TerrainMapOctree: Min Distance = 346.78. 2024-11-06T02:52:15Z- TerrainMapOctree: Using tile 5. 2024-11-06T02:52:15Z- TerrainMapOctree: Switching to tile 5. 2024-11-06T02:52:16Z- TerrainMapOctree::Octree tile load /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile5.bo took 1.199490 seconds. 2024-11-06T02:52:17Z- Forcing Subcloud Comparison 2024-11-06T02:52:17Z- Weighting particles with subcloud comparison 2024-11-06T02:52:17Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:17Z- meanExpectedMeasDiff: -3.408778 delta_rms_squared: 2.070716 alpha: 0.990434 2024-11-06T02:52:17Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:17Z- meanExpectedMeasDiff: 3.393254 delta_rms_squared: 1.618978 alpha: 0.987797 2024-11-06T02:52:17Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:17Z- meanExpectedMeasDiff: 1.168661 delta_rms_squared: 1.234699 alpha: 0.984060 2024-11-06T02:52:17Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:17Z- meanExpectedMeasDiff: -1.087887 delta_rms_squared: 1.583148 alpha: 0.987525 2024-11-06T02:52:17Z- Subcloud NIS: 0.464228 2024-11-06T02:52:17Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:52:17Z- TNavPF:: sumWeights = 0.143517 2024-11-06T02:52:17Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:52:17Z- TNavPF::First Term of Map Covariance Matrix is 1.23 2024-11-06T02:52:17Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.01 2024-11-06T02:52:17Z- TerrainNav::Resampling particle filter... 2024-11-06T02:52:17Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861535.33 sec, ping # 0. 2024-11-06T02:52:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:52:17Z- variances([0], [2], and psi_berg) = 5.192455 11.952519 0.000000 2024-11-06T02:52:17Z- publishEstimatesDecoder() - at 1730861535.33 sec 2024-11-06T02:52:17Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:52:17Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061539.16 594501.49 38.60 2024-11-06T02:52:17Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061534.97 594494.08 38.83 lat/lon: 36.69 -121.94 2024-11-06T02:52:17Z- LcmTrn::publishEstimatesDecoder() - COVAR: 5.19 11.95 0.62 0.00 2024-11-06T02:52:17Z- AHRS_M2 msg: 1730861535.70 epoch sec; seqNo:403 2024-11-06T02:52:17Z- AHRS_M2 msg: 0.17 phi; -0.28 theta; 5.19 psi 2024-11-06T02:52:17Z- handleDepth()- Depth_Keller msg: 1730861535.71 epoch sec; seqNo:437; depth 29.86 2024-11-06T02:52:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861535.72 epoch sec; seqNo:437; 4061501.68 north; 594508.32 east; 36.69 lat; -121.94 lon; 29.86 depth; utmZone: 10 2024-11-06T02:52:17Z- AHRS_M2 msg: 1730861536.10 epoch sec; seqNo:404 2024-11-06T02:52:17Z- AHRS_M2 msg: 0.17 phi; -0.28 theta; 5.19 psi 2024-11-06T02:52:17Z- handleDepth()- Depth_Keller msg: 1730861536.10 epoch sec; seqNo:438; depth 29.95 2024-11-06T02:52:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861536.11 epoch sec; seqNo:438; 4061501.68 north; 594508.32 east; 36.69 lat; -121.94 lon; 29.95 depth; utmZone: 10 2024-11-06T02:52:17Z- handleDepth()- Depth_Keller msg: 1730861536.52 epoch sec; seqNo:439; depth 30.04 2024-11-06T02:52:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861536.53 epoch sec; seqNo:439; 4061501.67 north; 594507.10 east; 36.69 lat; -121.94 lon; 30.04 depth; utmZone: 10 2024-11-06T02:52:17Z- AHRS_M2 msg: 1730861536.92 epoch sec; seqNo:405 2024-11-06T02:52:17Z- AHRS_M2 msg: 0.17 phi; -0.29 theta; 5.20 psi 2024-11-06T02:52:17Z- handleDepth()- Depth_Keller msg: 1730861536.92 epoch sec; seqNo:440; depth 30.13 2024-11-06T02:52:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861536.93 epoch sec; seqNo:440; 4061502.04 north; 594507.10 east; 36.69 lat; -121.94 lon; 30.13 depth; utmZone: 10 2024-11-06T02:52:17Z- 11 msgs in 200 ms and 12 handle calls 2024-11-06T02:52:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861535.33 2024-11-06T02:52:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:17Z- waiting for 1730861539.33, time is 1730861536.93 2024-11-06T02:52:17Z- AHRS_M2 msg: 1730861537.31 epoch sec; seqNo:406 2024-11-06T02:52:17Z- AHRS_M2 msg: 0.17 phi; -0.29 theta; 5.21 psi 2024-11-06T02:52:17Z- handleDepth()- Depth_Keller msg: 1730861537.31 epoch sec; seqNo:441; depth 30.21 2024-11-06T02:52:17Z- handleDVL() - DVL msg epochMillisec: 1730861537.34 2024-11-06T02:52:17Z- handleDvl() - RDI_Pathfinder msg: 1730861537.34 sec; seqNo:143; #beams:4 2024-11-06T02:52:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.67 , 53.14 , 51.23 , 68.12 2024-11-06T02:52:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.06 , 0.07 2024-11-06T02:52:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861537.35 epoch sec; seqNo:441; 4061502.04 north; 594507.10 east; 36.69 lat; -121.94 lon; 30.21 depth; utmZone: 10 2024-11-06T02:52:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861535.33 2024-11-06T02:52:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:17Z- waiting for 1730861539.33, time is 1730861537.35 2024-11-06T02:52:17Z- AHRS_M2 msg: 1730861537.71 epoch sec; seqNo:407 2024-11-06T02:52:17Z- AHRS_M2 msg: 0.17 phi; -0.29 theta; 5.22 psi 2024-11-06T02:52:17Z- handleDepth()- Depth_Keller msg: 1730861537.71 epoch sec; seqNo:442; depth 30.29 2024-11-06T02:52:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861537.74 epoch sec; seqNo:442; 4061502.04 north; 594507.10 east; 36.69 lat; -121.94 lon; 30.29 depth; utmZone: 10 2024-11-06T02:52:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861535.33 2024-11-06T02:52:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:17Z- waiting for 1730861539.33, time is 1730861537.74 2024-11-06T02:52:18Z- AHRS_M2 msg: 1730861538.11 epoch sec; seqNo:408 2024-11-06T02:52:18Z- AHRS_M2 msg: 0.16 phi; -0.29 theta; 5.23 psi 2024-11-06T02:52:18Z- handleDepth()- Depth_Keller msg: 1730861538.12 epoch sec; seqNo:443; depth 30.38 2024-11-06T02:52:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861538.13 epoch sec; seqNo:443; 4061502.03 north; 594505.88 east; 36.69 lat; -121.94 lon; 30.38 depth; utmZone: 10 2024-11-06T02:52:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861535.33 2024-11-06T02:52:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:18Z- waiting for 1730861539.33, time is 1730861538.13 2024-11-06T02:52:18Z- AHRS_M2 msg: 1730861538.52 epoch sec; seqNo:409 2024-11-06T02:52:18Z- AHRS_M2 msg: 0.16 phi; -0.29 theta; 5.23 psi 2024-11-06T02:52:18Z- handleDepth()- Depth_Keller msg: 1730861538.52 epoch sec; seqNo:444; depth 30.45 2024-11-06T02:52:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861538.53 epoch sec; seqNo:444; 4061502.41 north; 594505.87 east; 36.69 lat; -121.94 lon; 30.45 depth; utmZone: 10 2024-11-06T02:52:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861535.33 2024-11-06T02:52:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:18Z- waiting for 1730861539.33, time is 1730861538.53 2024-11-06T02:52:18Z- AHRS_M2 msg: 1730861538.92 epoch sec; seqNo:410 2024-11-06T02:52:18Z- AHRS_M2 msg: 0.16 phi; -0.28 theta; 5.24 psi 2024-11-06T02:52:18Z- handleDepth()- Depth_Keller msg: 1730861538.92 epoch sec; seqNo:445; depth 30.53 2024-11-06T02:52:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861538.94 epoch sec; seqNo:445; 4061502.41 north; 594505.87 east; 36.69 lat; -121.94 lon; 30.53 depth; utmZone: 10 2024-11-06T02:52:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861535.33 2024-11-06T02:52:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:19Z- waiting for 1730861539.33, time is 1730861538.94 2024-11-06T02:52:19Z- AHRS_M2 msg: 1730861539.32 epoch sec; seqNo:411 2024-11-06T02:52:19Z- AHRS_M2 msg: 0.16 phi; -0.28 theta; 5.25 psi 2024-11-06T02:52:19Z- handleDepth()- Depth_Keller msg: 1730861539.33 epoch sec; seqNo:446; depth 30.61 2024-11-06T02:52:19Z- handleDVL() - DVL msg epochMillisec: 1730861539.35 2024-11-06T02:52:19Z- handleDvl() - RDI_Pathfinder msg: 1730861539.35 sec; seqNo:144; #beams:4 2024-11-06T02:52:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.40 , 53.14 , 50.68 , 68.12 2024-11-06T02:52:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , -0.10 2024-11-06T02:52:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861539.38 epoch sec; seqNo:446; 4061502.79 north; 594505.87 east; 36.69 lat; -121.94 lon; 30.61 depth; utmZone: 10 2024-11-06T02:52:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861535.33 2024-11-06T02:52:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:19Z- waiting for 1730861539.33, time is 1730861539.38 2024-11-06T02:52:19Z- LcmTrn::updateTrn() >>>> pose time 1730861539.38, meas time 1730861539.35 <<<< 2024-11-06T02:52:19Z- LcmTrn::updateTrn() equating pose 1730861539.35 and meas times 1730861539.35 2024-11-06T02:52:19Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861539.35 2024-11-06T02:52:19Z- TerrainNav::motionUpdate() - currEstimate = (4061503, 594505.87, 30.61) 2024-11-06T02:52:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:52:19Z- TerrainNav:: SubcloudNIS is 0.46, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:52:19Z- TNavPF::Projecting Measurements 2024-11-06T02:52:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:52:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:52:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:52:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:52:19Z- TNavFilter::4 good beams. 2024-11-06T02:52:19Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:52:19Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:52:19Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:52:19Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:52:19Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:52:19Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:52:19Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:52:19Z- TerrainMapOctree: (vehN, vehE ) = (4061502.79, 594505.87). 2024-11-06T02:52:19Z- TerrainMapOctree: Min Distance = 347.44. 2024-11-06T02:52:19Z- TerrainMapOctree: Using tile 5. 2024-11-06T02:52:19Z- Forcing Subcloud Comparison 2024-11-06T02:52:19Z- Weighting particles with subcloud comparison 2024-11-06T02:52:19Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:19Z- meanExpectedMeasDiff: -2.255653 delta_rms_squared: 1.580928 alpha: 0.987507 2024-11-06T02:52:19Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:19Z- meanExpectedMeasDiff: 3.504655 delta_rms_squared: 1.249581 alpha: 0.984247 2024-11-06T02:52:19Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:19Z- meanExpectedMeasDiff: 1.670152 delta_rms_squared: 0.943394 alpha: 0.979240 2024-11-06T02:52:19Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:19Z- meanExpectedMeasDiff: -0.382941 delta_rms_squared: 1.813792 alpha: 0.989094 2024-11-06T02:52:19Z- Subcloud NIS: 0.443190 2024-11-06T02:52:19Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:52:19Z- TNavPF:: sumWeights = 0.180808 2024-11-06T02:52:19Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:52:19Z- TNavPF::First Term of Map Covariance Matrix is 0.90 2024-11-06T02:52:19Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T02:52:19Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861539.35 sec, ping # 0. 2024-11-06T02:52:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:52:19Z- variances([0], [2], and psi_berg) = 2.937168 4.317596 0.000000 2024-11-06T02:52:19Z- publishEstimatesDecoder() - at 1730861539.35 sec 2024-11-06T02:52:19Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:52:19Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061536.01 594489.61 39.24 2024-11-06T02:52:19Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061536.09 594490.74 39.52 lat/lon: 36.69 -121.94 2024-11-06T02:52:19Z- LcmTrn::publishEstimatesDecoder() - COVAR: 2.94 4.32 0.51 0.00 2024-11-06T02:52:19Z- AHRS_M2 msg: 1730861539.73 epoch sec; seqNo:412 2024-11-06T02:52:19Z- AHRS_M2 msg: 0.17 phi; -0.28 theta; 5.26 psi 2024-11-06T02:52:19Z- handleDepth()- Depth_Keller msg: 1730861539.73 epoch sec; seqNo:447; depth 30.69 2024-11-06T02:52:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861539.75 epoch sec; seqNo:447; 4061502.77 north; 594504.65 east; 36.69 lat; -121.94 lon; 30.69 depth; utmZone: 10 2024-11-06T02:52:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861539.35 2024-11-06T02:52:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:19Z- waiting for 1730861543.35, time is 1730861539.75 2024-11-06T02:52:20Z- AHRS_M2 msg: 1730861540.13 epoch sec; seqNo:413 2024-11-06T02:52:20Z- AHRS_M2 msg: 0.17 phi; -0.27 theta; 5.26 psi 2024-11-06T02:52:20Z- handleDepth()- Depth_Keller msg: 1730861540.14 epoch sec; seqNo:448; depth 30.77 2024-11-06T02:52:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861540.17 epoch sec; seqNo:448; 4061503.15 north; 594504.64 east; 36.69 lat; -121.94 lon; 30.77 depth; utmZone: 10 2024-11-06T02:52:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861539.35 2024-11-06T02:52:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:20Z- waiting for 1730861543.35, time is 1730861540.17 2024-11-06T02:52:20Z- AHRS_M2 msg: 1730861540.54 epoch sec; seqNo:414 2024-11-06T02:52:20Z- AHRS_M2 msg: 0.17 phi; -0.27 theta; 5.27 psi 2024-11-06T02:52:20Z- handleDepth()- Depth_Keller msg: 1730861540.54 epoch sec; seqNo:449; depth 30.85 2024-11-06T02:52:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861540.55 epoch sec; seqNo:449; 4061503.15 north; 594504.64 east; 36.69 lat; -121.94 lon; 30.85 depth; utmZone: 10 2024-11-06T02:52:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861539.35 2024-11-06T02:52:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:20Z- waiting for 1730861543.35, time is 1730861540.55 2024-11-06T02:52:21Z- AHRS_M2 msg: 1730861541.02 epoch sec; seqNo:415 2024-11-06T02:52:21Z- AHRS_M2 msg: 0.17 phi; -0.27 theta; 5.27 psi 2024-11-06T02:52:21Z- handleDepth()- Depth_Keller msg: 1730861541.03 epoch sec; seqNo:450; depth 30.93 2024-11-06T02:52:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861541.06 epoch sec; seqNo:450; 4061503.53 north; 594504.64 east; 36.69 lat; -121.94 lon; 30.93 depth; utmZone: 10 2024-11-06T02:52:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861539.35 2024-11-06T02:52:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:21Z- waiting for 1730861543.35, time is 1730861541.06 2024-11-06T02:52:21Z- handleDepth()- Depth_Keller msg: 1730861541.36 epoch sec; seqNo:451; depth 31.04 2024-11-06T02:52:21Z- handleDVL() - DVL msg epochMillisec: 1730861541.38 2024-11-06T02:52:21Z- handleDvl() - RDI_Pathfinder msg: 1730861541.38 sec; seqNo:145; #beams:4 2024-11-06T02:52:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.13 , 52.32 , 50.41 , 66.49 2024-11-06T02:52:21Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.06 , -0.04 2024-11-06T02:52:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861541.39 epoch sec; seqNo:451; 4061503.52 north; 594503.42 east; 36.69 lat; -121.94 lon; 31.04 depth; utmZone: 10 2024-11-06T02:52:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861539.35 2024-11-06T02:52:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:21Z- waiting for 1730861543.35, time is 1730861541.39 2024-11-06T02:52:21Z- AHRS_M2 msg: 1730861541.75 epoch sec; seqNo:416 2024-11-06T02:52:21Z- AHRS_M2 msg: 0.16 phi; -0.26 theta; 5.28 psi 2024-11-06T02:52:21Z- handleDepth()- Depth_Keller msg: 1730861541.78 epoch sec; seqNo:452; depth 31.11 2024-11-06T02:52:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861541.80 epoch sec; seqNo:452; 4061503.52 north; 594503.42 east; 36.69 lat; -121.94 lon; 31.11 depth; utmZone: 10 2024-11-06T02:52:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861539.35 2024-11-06T02:52:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:21Z- waiting for 1730861543.35, time is 1730861541.80 2024-11-06T02:52:22Z- AHRS_M2 msg: 1730861542.15 epoch sec; seqNo:417 2024-11-06T02:52:22Z- AHRS_M2 msg: 0.16 phi; -0.26 theta; 5.28 psi 2024-11-06T02:52:22Z- handleDepth()- Depth_Keller msg: 1730861542.16 epoch sec; seqNo:453; depth 31.19 2024-11-06T02:52:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861542.17 epoch sec; seqNo:453; 4061503.90 north; 594503.41 east; 36.69 lat; -121.94 lon; 31.19 depth; utmZone: 10 2024-11-06T02:52:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861539.35 2024-11-06T02:52:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:22Z- waiting for 1730861543.35, time is 1730861542.17 2024-11-06T02:52:22Z- AHRS_M2 msg: 1730861542.56 epoch sec; seqNo:418 2024-11-06T02:52:22Z- AHRS_M2 msg: 0.16 phi; -0.26 theta; 5.28 psi 2024-11-06T02:52:22Z- handleDepth()- Depth_Keller msg: 1730861542.56 epoch sec; seqNo:454; depth 31.26 2024-11-06T02:52:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861542.58 epoch sec; seqNo:454; 4061503.90 north; 594503.41 east; 36.69 lat; -121.94 lon; 31.26 depth; utmZone: 10 2024-11-06T02:52:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861539.35 2024-11-06T02:52:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:22Z- waiting for 1730861543.35, time is 1730861542.58 2024-11-06T02:52:22Z- AHRS_M2 msg: 1730861542.96 epoch sec; seqNo:419 2024-11-06T02:52:22Z- AHRS_M2 msg: 0.16 phi; -0.26 theta; 5.28 psi 2024-11-06T02:52:22Z- handleDepth()- Depth_Keller msg: 1730861542.97 epoch sec; seqNo:455; depth 31.33 2024-11-06T02:52:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861542.98 epoch sec; seqNo:455; 4061504.26 north; 594502.19 east; 36.69 lat; -121.94 lon; 31.33 depth; utmZone: 10 2024-11-06T02:52:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861539.35 2024-11-06T02:52:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:23Z- waiting for 1730861543.35, time is 1730861542.98 2024-11-06T02:52:23Z- AHRS_M2 msg: 1730861543.37 epoch sec; seqNo:420 2024-11-06T02:52:23Z- AHRS_M2 msg: 0.16 phi; -0.26 theta; 5.29 psi 2024-11-06T02:52:23Z- handleDepth()- Depth_Keller msg: 1730861543.37 epoch sec; seqNo:456; depth 31.43 2024-11-06T02:52:23Z- handleDVL() - DVL msg epochMillisec: 1730861543.39 2024-11-06T02:52:23Z- handleDvl() - RDI_Pathfinder msg: 1730861543.39 sec; seqNo:146; #beams:4 2024-11-06T02:52:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.31 , 51.77 , 50.13 , 65.12 2024-11-06T02:52:23Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.10 , 0.07 2024-11-06T02:52:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861543.42 epoch sec; seqNo:456; 4061504.26 north; 594502.19 east; 36.69 lat; -121.94 lon; 31.43 depth; utmZone: 10 2024-11-06T02:52:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861539.35 2024-11-06T02:52:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:23Z- waiting for 1730861543.35, time is 1730861543.42 2024-11-06T02:52:23Z- LcmTrn::updateTrn() >>>> pose time 1730861543.42, meas time 1730861543.39 <<<< 2024-11-06T02:52:23Z- LcmTrn::updateTrn() equating pose 1730861543.39 and meas times 1730861543.39 2024-11-06T02:52:23Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861543.39 2024-11-06T02:52:23Z- TerrainNav::motionUpdate() - currEstimate = (4061504, 594502.19, 31.43) 2024-11-06T02:52:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:52:23Z- TerrainNav:: SubcloudNIS is 0.44, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:52:23Z- TNavPF::Projecting Measurements 2024-11-06T02:52:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:52:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:52:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:52:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:52:23Z- TNavFilter::4 good beams. 2024-11-06T02:52:23Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:52:23Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:52:23Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:52:23Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:52:23Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:52:23Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:52:23Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:52:23Z- TerrainMapOctree: (vehN, vehE ) = (4061504.26, 594502.19). 2024-11-06T02:52:23Z- TerrainMapOctree: Min Distance = 348.99. 2024-11-06T02:52:23Z- TerrainMapOctree: Using tile 5. 2024-11-06T02:52:23Z- Forcing Subcloud Comparison 2024-11-06T02:52:23Z- Weighting particles with subcloud comparison 2024-11-06T02:52:23Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:23Z- meanExpectedMeasDiff: -1.724390 delta_rms_squared: 1.359750 alpha: 0.985505 2024-11-06T02:52:23Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:23Z- meanExpectedMeasDiff: 1.771896 delta_rms_squared: 0.912536 alpha: 0.978553 2024-11-06T02:52:23Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:23Z- meanExpectedMeasDiff: 1.582827 delta_rms_squared: 0.838182 alpha: 0.976695 2024-11-06T02:52:23Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:23Z- meanExpectedMeasDiff: -0.986569 delta_rms_squared: 1.541414 alpha: 0.987191 2024-11-06T02:52:23Z- Subcloud NIS: 0.424071 2024-11-06T02:52:23Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:52:23Z- TNavPF:: sumWeights = 0.386241 2024-11-06T02:52:23Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:52:23Z- TNavPF::First Term of Map Covariance Matrix is 0.78 2024-11-06T02:52:23Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.06 2024-11-06T02:52:23Z- TerrainNav::Resampling particle filter... 2024-11-06T02:52:23Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861543.39 sec, ping # 0. 2024-11-06T02:52:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:52:23Z- variances([0], [2], and psi_berg) = 2.177026 2.329237 0.000000 2024-11-06T02:52:23Z- publishEstimatesDecoder() - at 1730861543.39 sec 2024-11-06T02:52:23Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:52:23Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061542.50 594495.37 40.28 2024-11-06T02:52:23Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061537.44 594486.83 40.31 lat/lon: 36.69 -121.94 2024-11-06T02:52:23Z- LcmTrn::publishEstimatesDecoder() - COVAR: 2.18 2.33 0.47 0.00 2024-11-06T02:52:23Z- handleDepth()- Depth_Keller msg: 1730861543.79 epoch sec; seqNo:457; depth 31.52 2024-11-06T02:52:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861543.80 epoch sec; seqNo:457; 4061504.64 north; 594502.19 east; 36.69 lat; -121.94 lon; 31.52 depth; utmZone: 10 2024-11-06T02:52:23Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:52:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861543.39 2024-11-06T02:52:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:23Z- waiting for 1730861547.39, time is 1730861543.80 2024-11-06T02:52:24Z- AHRS_M2 msg: 1730861544.17 epoch sec; seqNo:421 2024-11-06T02:52:24Z- AHRS_M2 msg: 0.16 phi; -0.25 theta; 5.30 psi 2024-11-06T02:52:24Z- handleDepth()- Depth_Keller msg: 1730861544.18 epoch sec; seqNo:458; depth 31.59 2024-11-06T02:52:24Z- handleDVL() - DVL msg epochMillisec: 1730861544.18 2024-11-06T02:52:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:52:24Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:52:24Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:52:24Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:52:24Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:52:24Z- handleDvl() - RDI_Pathfinder msg: 1730861544.18 sec; seqNo:147; #beams:0 2024-11-06T02:52:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:52:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.10 , 0.07 2024-11-06T02:52:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861544.20 epoch sec; seqNo:458; 4061504.63 north; 594500.97 east; 36.69 lat; -121.94 lon; 31.59 depth; utmZone: 10 2024-11-06T02:52:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861543.39 2024-11-06T02:52:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:24Z- waiting for 1730861547.39, time is 1730861544.20 2024-11-06T02:52:24Z- AHRS_M2 msg: 1730861544.58 epoch sec; seqNo:422 2024-11-06T02:52:24Z- AHRS_M2 msg: 0.16 phi; -0.25 theta; 5.30 psi 2024-11-06T02:52:24Z- handleDepth()- Depth_Keller msg: 1730861544.58 epoch sec; seqNo:459; depth 31.67 2024-11-06T02:52:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861544.59 epoch sec; seqNo:459; 4061505.01 north; 594500.96 east; 36.69 lat; -121.94 lon; 31.67 depth; utmZone: 10 2024-11-06T02:52:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861543.39 2024-11-06T02:52:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:24Z- waiting for 1730861547.39, time is 1730861544.59 2024-11-06T02:52:24Z- AHRS_M2 msg: 1730861544.98 epoch sec; seqNo:423 2024-11-06T02:52:24Z- AHRS_M2 msg: 0.16 phi; -0.25 theta; 5.31 psi 2024-11-06T02:52:24Z- handleDepth()- Depth_Keller msg: 1730861544.98 epoch sec; seqNo:460; depth 31.75 2024-11-06T02:52:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861544.99 epoch sec; seqNo:460; 4061505.01 north; 594500.96 east; 36.69 lat; -121.94 lon; 31.75 depth; utmZone: 10 2024-11-06T02:52:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861543.39 2024-11-06T02:52:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:25Z- waiting for 1730861547.39, time is 1730861544.99 2024-11-06T02:52:25Z- AHRS_M2 msg: 1730861545.38 epoch sec; seqNo:424 2024-11-06T02:52:25Z- AHRS_M2 msg: 0.16 phi; -0.25 theta; 5.31 psi 2024-11-06T02:52:25Z- handleDepth()- Depth_Keller msg: 1730861545.39 epoch sec; seqNo:461; depth 31.83 2024-11-06T02:52:25Z- handleDVL() - DVL msg epochMillisec: 1730861545.41 2024-11-06T02:52:25Z- handleDvl() - RDI_Pathfinder msg: 1730861545.41 sec; seqNo:148; #beams:4 2024-11-06T02:52:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.58 , 51.50 , 50.13 , 64.30 2024-11-06T02:52:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.08 , 0.03 2024-11-06T02:52:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861545.42 epoch sec; seqNo:461; 4061505.39 north; 594500.96 east; 36.69 lat; -121.94 lon; 31.83 depth; utmZone: 10 2024-11-06T02:52:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861543.39 2024-11-06T02:52:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:25Z- waiting for 1730861547.39, time is 1730861545.42 2024-11-06T02:52:25Z- AHRS_M2 msg: 1730861545.81 epoch sec; seqNo:425 2024-11-06T02:52:25Z- AHRS_M2 msg: 0.16 phi; -0.25 theta; 5.32 psi 2024-11-06T02:52:25Z- handleDepth()- Depth_Keller msg: 1730861545.81 epoch sec; seqNo:462; depth 31.91 2024-11-06T02:52:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861545.83 epoch sec; seqNo:462; 4061505.37 north; 594499.74 east; 36.69 lat; -121.94 lon; 31.91 depth; utmZone: 10 2024-11-06T02:52:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861543.39 2024-11-06T02:52:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:25Z- waiting for 1730861547.39, time is 1730861545.83 2024-11-06T02:52:26Z- AHRS_M2 msg: 1730861546.20 epoch sec; seqNo:426 2024-11-06T02:52:26Z- AHRS_M2 msg: 0.16 phi; -0.25 theta; 5.32 psi 2024-11-06T02:52:26Z- handleDepth()- Depth_Keller msg: 1730861546.21 epoch sec; seqNo:463; depth 31.99 2024-11-06T02:52:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861546.23 epoch sec; seqNo:463; 4061505.75 north; 594499.73 east; 36.69 lat; -121.94 lon; 31.99 depth; utmZone: 10 2024-11-06T02:52:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861543.39 2024-11-06T02:52:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:26Z- waiting for 1730861547.39, time is 1730861546.23 2024-11-06T02:52:26Z- AHRS_M2 msg: 1730861546.60 epoch sec; seqNo:427 2024-11-06T02:52:26Z- AHRS_M2 msg: 0.16 phi; -0.24 theta; 5.32 psi 2024-11-06T02:52:26Z- handleDepth()- Depth_Keller msg: 1730861546.60 epoch sec; seqNo:464; depth 32.06 2024-11-06T02:52:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861546.61 epoch sec; seqNo:464; 4061505.75 north; 594499.73 east; 36.69 lat; -121.94 lon; 32.06 depth; utmZone: 10 2024-11-06T02:52:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861543.39 2024-11-06T02:52:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:26Z- waiting for 1730861547.39, time is 1730861546.61 2024-11-06T02:52:27Z- AHRS_M2 msg: 1730861547.01 epoch sec; seqNo:428 2024-11-06T02:52:27Z- AHRS_M2 msg: 0.16 phi; -0.24 theta; 5.33 psi 2024-11-06T02:52:27Z- handleDepth()- Depth_Keller msg: 1730861547.02 epoch sec; seqNo:465; depth 32.13 2024-11-06T02:52:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861547.03 epoch sec; seqNo:465; 4061506.13 north; 594499.73 east; 36.69 lat; -121.94 lon; 32.13 depth; utmZone: 10 2024-11-06T02:52:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861543.39 2024-11-06T02:52:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:27Z- waiting for 1730861547.39, time is 1730861547.03 2024-11-06T02:52:27Z- AHRS_M2 msg: 1730861547.40 epoch sec; seqNo:429 2024-11-06T02:52:27Z- AHRS_M2 msg: 0.16 phi; -0.24 theta; 5.33 psi 2024-11-06T02:52:27Z- handleDepth()- Depth_Keller msg: 1730861547.41 epoch sec; seqNo:466; depth 32.21 2024-11-06T02:52:27Z- handleDVL() - DVL msg epochMillisec: 1730861547.43 2024-11-06T02:52:27Z- handleDvl() - RDI_Pathfinder msg: 1730861547.43 sec; seqNo:149; #beams:4 2024-11-06T02:52:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.58 , 51.22 , 49.86 , 63.49 2024-11-06T02:52:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.06 , 0.02 2024-11-06T02:52:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861547.46 epoch sec; seqNo:466; 4061506.12 north; 594498.51 east; 36.69 lat; -121.94 lon; 32.21 depth; utmZone: 10 2024-11-06T02:52:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861543.39 2024-11-06T02:52:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:27Z- waiting for 1730861547.39, time is 1730861547.46 2024-11-06T02:52:27Z- LcmTrn::updateTrn() >>>> pose time 1730861547.46, meas time 1730861547.43 <<<< 2024-11-06T02:52:27Z- LcmTrn::updateTrn() equating pose 1730861547.43 and meas times 1730861547.43 2024-11-06T02:52:27Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861547.43 2024-11-06T02:52:27Z- TerrainNav::motionUpdate() - currEstimate = (4061506, 594498.51, 32.21) 2024-11-06T02:52:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:52:27Z- TerrainNav:: SubcloudNIS is 0.42, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:52:27Z- TNavPF::Projecting Measurements 2024-11-06T02:52:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:52:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:52:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:52:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:52:27Z- TNavFilter::4 good beams. 2024-11-06T02:52:27Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:52:27Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:52:27Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:52:27Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:52:27Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:52:27Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:52:27Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:52:27Z- TerrainMapOctree: (vehN, vehE ) = (4061506.12, 594498.51). 2024-11-06T02:52:27Z- TerrainMapOctree: Min Distance = 348.19. 2024-11-06T02:52:27Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:52:27Z- TerrainMapOctree: Switching to tile 4. 2024-11-06T02:52:29Z- TerrainMapOctree::Octree tile load /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile4.bo took 1.242323 seconds. 2024-11-06T02:52:29Z- Forcing Subcloud Comparison 2024-11-06T02:52:29Z- Weighting particles with subcloud comparison 2024-11-06T02:52:29Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:29Z- meanExpectedMeasDiff: -0.129914 delta_rms_squared: 1.258169 alpha: 0.984353 2024-11-06T02:52:29Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:29Z- meanExpectedMeasDiff: 2.070972 delta_rms_squared: 0.851227 alpha: 0.977044 2024-11-06T02:52:29Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:29Z- meanExpectedMeasDiff: 1.870632 delta_rms_squared: 0.791067 alpha: 0.975341 2024-11-06T02:52:29Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:29Z- meanExpectedMeasDiff: -0.570554 delta_rms_squared: 1.415000 alpha: 0.986063 2024-11-06T02:52:29Z- Subcloud NIS: 0.418852 2024-11-06T02:52:29Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:52:29Z- TNavPF:: sumWeights = 0.435305 2024-11-06T02:52:29Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:52:29Z- TNavPF::First Term of Map Covariance Matrix is 0.37 2024-11-06T02:52:29Z- TNavPF::Calculated NIS Value : 0.24 numBeams normalized NIS: 0.06 2024-11-06T02:52:29Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861547.43 sec, ping # 0. 2024-11-06T02:52:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:52:29Z- variances([0], [2], and psi_berg) = 2.164866 1.995801 0.000000 2024-11-06T02:52:29Z- publishEstimatesDecoder() - at 1730861547.43 sec 2024-11-06T02:52:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:52:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061538.96 594482.80 40.40 2024-11-06T02:52:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061539.29 594483.11 40.83 lat/lon: 36.69 -121.94 2024-11-06T02:52:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 2.16 2.00 0.45 0.00 2024-11-06T02:52:29Z- AHRS_M2 msg: 1730861547.81 epoch sec; seqNo:430 2024-11-06T02:52:29Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.33 psi 2024-11-06T02:52:29Z- handleDepth()- Depth_Keller msg: 1730861547.82 epoch sec; seqNo:467; depth 32.29 2024-11-06T02:52:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861547.83 epoch sec; seqNo:467; 4061506.50 north; 594498.50 east; 36.69 lat; -121.94 lon; 32.29 depth; utmZone: 10 2024-11-06T02:52:29Z- AHRS_M2 msg: 1730861548.21 epoch sec; seqNo:431 2024-11-06T02:52:29Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.32 psi 2024-11-06T02:52:29Z- handleDepth()- Depth_Keller msg: 1730861548.22 epoch sec; seqNo:468; depth 32.38 2024-11-06T02:52:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861548.22 epoch sec; seqNo:468; 4061506.50 north; 594498.50 east; 36.69 lat; -121.94 lon; 32.38 depth; utmZone: 10 2024-11-06T02:52:29Z- AHRS_M2 msg: 1730861548.62 epoch sec; seqNo:432 2024-11-06T02:52:29Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.32 psi 2024-11-06T02:52:29Z- handleDepth()- Depth_Keller msg: 1730861548.62 epoch sec; seqNo:469; depth 32.45 2024-11-06T02:52:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861548.64 epoch sec; seqNo:469; 4061506.86 north; 594497.28 east; 36.69 lat; -121.94 lon; 32.45 depth; utmZone: 10 2024-11-06T02:52:29Z- AHRS_M2 msg: 1730861549.02 epoch sec; seqNo:433 2024-11-06T02:52:29Z- AHRS_M2 msg: 0.15 phi; -0.23 theta; 5.32 psi 2024-11-06T02:52:29Z- handleDepth()- Depth_Keller msg: 1730861549.04 epoch sec; seqNo:470; depth 32.51 2024-11-06T02:52:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861549.05 epoch sec; seqNo:470; 4061506.86 north; 594497.28 east; 36.69 lat; -121.94 lon; 32.51 depth; utmZone: 10 2024-11-06T02:52:29Z- AHRS_M2 msg: 1730861549.43 epoch sec; seqNo:434 2024-11-06T02:52:29Z- AHRS_M2 msg: 0.15 phi; -0.23 theta; 5.32 psi 2024-11-06T02:52:29Z- handleDepth()- Depth_Keller msg: 1730861549.43 epoch sec; seqNo:471; depth 32.60 2024-11-06T02:52:29Z- 14 msgs in 200 ms and 15 handle calls 2024-11-06T02:52:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861547.43 2024-11-06T02:52:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:29Z- waiting for 1730861551.43, time is 1730861549.43 2024-11-06T02:52:29Z- handleDVL() - DVL msg epochMillisec: 1730861549.46 2024-11-06T02:52:29Z- handleDvl() - RDI_Pathfinder msg: 1730861549.46 sec; seqNo:150; #beams:4 2024-11-06T02:52:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.03 , 50.95 , 49.86 , 62.39 2024-11-06T02:52:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.08 , 0.12 2024-11-06T02:52:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861549.47 epoch sec; seqNo:471; 4061507.24 north; 594497.27 east; 36.69 lat; -121.94 lon; 32.60 depth; utmZone: 10 2024-11-06T02:52:29Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:52:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861547.43 2024-11-06T02:52:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:29Z- waiting for 1730861551.43, time is 1730861549.47 2024-11-06T02:52:29Z- AHRS_M2 msg: 1730861549.83 epoch sec; seqNo:435 2024-11-06T02:52:29Z- AHRS_M2 msg: 0.15 phi; -0.23 theta; 5.31 psi 2024-11-06T02:52:29Z- handleDepth()- Depth_Keller msg: 1730861549.84 epoch sec; seqNo:472; depth 32.66 2024-11-06T02:52:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861549.85 epoch sec; seqNo:472; 4061507.24 north; 594497.27 east; 36.69 lat; -121.94 lon; 32.66 depth; utmZone: 10 2024-11-06T02:52:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861547.43 2024-11-06T02:52:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:29Z- waiting for 1730861551.43, time is 1730861549.85 2024-11-06T02:52:30Z- AHRS_M2 msg: 1730861550.23 epoch sec; seqNo:436 2024-11-06T02:52:30Z- AHRS_M2 msg: 0.15 phi; -0.23 theta; 5.31 psi 2024-11-06T02:52:30Z- handleDepth()- Depth_Keller msg: 1730861550.24 epoch sec; seqNo:473; depth 32.75 2024-11-06T02:52:30Z- handleDVL() - DVL msg epochMillisec: 1730861550.24 2024-11-06T02:52:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:52:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:52:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:52:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:52:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:52:30Z- handleDvl() - RDI_Pathfinder msg: 1730861550.24 sec; seqNo:151; #beams:0 2024-11-06T02:52:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:52:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.08 , 0.12 2024-11-06T02:52:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861550.25 epoch sec; seqNo:473; 4061507.61 north; 594496.05 east; 36.69 lat; -121.94 lon; 32.75 depth; utmZone: 10 2024-11-06T02:52:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861547.43 2024-11-06T02:52:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:30Z- waiting for 1730861551.43, time is 1730861550.25 2024-11-06T02:52:30Z- AHRS_M2 msg: 1730861550.64 epoch sec; seqNo:437 2024-11-06T02:52:30Z- AHRS_M2 msg: 0.15 phi; -0.23 theta; 5.31 psi 2024-11-06T02:52:30Z- handleDepth()- Depth_Keller msg: 1730861550.65 epoch sec; seqNo:474; depth 32.82 2024-11-06T02:52:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861550.66 epoch sec; seqNo:474; 4061507.61 north; 594496.05 east; 36.69 lat; -121.94 lon; 32.82 depth; utmZone: 10 2024-11-06T02:52:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861547.43 2024-11-06T02:52:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:30Z- waiting for 1730861551.43, time is 1730861550.66 2024-11-06T02:52:31Z- AHRS_M2 msg: 1730861551.04 epoch sec; seqNo:438 2024-11-06T02:52:31Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.31 psi 2024-11-06T02:52:31Z- handleDepth()- Depth_Keller msg: 1730861551.05 epoch sec; seqNo:475; depth 32.89 2024-11-06T02:52:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861551.07 epoch sec; seqNo:475; 4061507.98 north; 594496.05 east; 36.69 lat; -121.94 lon; 32.89 depth; utmZone: 10 2024-11-06T02:52:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861547.43 2024-11-06T02:52:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:31Z- waiting for 1730861551.43, time is 1730861551.07 2024-11-06T02:52:31Z- AHRS_M2 msg: 1730861551.45 epoch sec; seqNo:439 2024-11-06T02:52:31Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.31 psi 2024-11-06T02:52:31Z- handleDepth()- Depth_Keller msg: 1730861551.45 epoch sec; seqNo:476; depth 32.97 2024-11-06T02:52:31Z- handleDVL() - DVL msg epochMillisec: 1730861551.47 2024-11-06T02:52:31Z- handleDvl() - RDI_Pathfinder msg: 1730861551.47 sec; seqNo:152; #beams:4 2024-11-06T02:52:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.79 , 51.08 , 49.55 , 61.81 2024-11-06T02:52:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , 0.09 2024-11-06T02:52:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861551.50 epoch sec; seqNo:476; 4061507.98 north; 594496.05 east; 36.69 lat; -121.94 lon; 32.97 depth; utmZone: 10 2024-11-06T02:52:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861547.43 2024-11-06T02:52:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:31Z- waiting for 1730861551.43, time is 1730861551.50 2024-11-06T02:52:31Z- LcmTrn::updateTrn() >>>> pose time 1730861551.50, meas time 1730861551.47 <<<< 2024-11-06T02:52:31Z- LcmTrn::updateTrn() equating pose 1730861551.47 and meas times 1730861551.47 2024-11-06T02:52:31Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861551.47 2024-11-06T02:52:31Z- TerrainNav::motionUpdate() - currEstimate = (4061508, 594496.05, 32.97) 2024-11-06T02:52:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:52:31Z- TerrainNav:: SubcloudNIS is 0.42, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:52:31Z- TNavPF::Projecting Measurements 2024-11-06T02:52:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:52:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:52:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:52:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:52:31Z- TNavFilter::4 good beams. 2024-11-06T02:52:31Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:52:31Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:52:31Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:52:31Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:52:31Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:52:31Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:52:31Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:52:31Z- TerrainMapOctree: (vehN, vehE ) = (4061507.98, 594496.05). 2024-11-06T02:52:31Z- TerrainMapOctree: Min Distance = 345.12. 2024-11-06T02:52:31Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:52:31Z- Forcing Subcloud Comparison 2024-11-06T02:52:31Z- Weighting particles with subcloud comparison 2024-11-06T02:52:31Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:31Z- meanExpectedMeasDiff: 0.544688 delta_rms_squared: 1.193991 alpha: 0.983525 2024-11-06T02:52:31Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:31Z- meanExpectedMeasDiff: 2.158859 delta_rms_squared: 0.640343 alpha: 0.969713 2024-11-06T02:52:31Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:31Z- meanExpectedMeasDiff: 2.051172 delta_rms_squared: 0.772042 alpha: 0.974749 2024-11-06T02:52:31Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:31Z- meanExpectedMeasDiff: -0.835343 delta_rms_squared: 1.347818 alpha: 0.985378 2024-11-06T02:52:31Z- Subcloud NIS: 0.408176 2024-11-06T02:52:31Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:52:31Z- TNavPF:: sumWeights = 0.387760 2024-11-06T02:52:31Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:52:31Z- TNavPF::First Term of Map Covariance Matrix is 0.31 2024-11-06T02:52:31Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.05 2024-11-06T02:52:31Z- TerrainNav::Resampling particle filter... 2024-11-06T02:52:31Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861551.47 sec, ping # 0. 2024-11-06T02:52:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:52:31Z- variances([0], [2], and psi_berg) = 2.038637 1.975659 0.000000 2024-11-06T02:52:31Z- publishEstimatesDecoder() - at 1730861551.47 sec 2024-11-06T02:52:31Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:52:31Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061546.20 594488.68 41.56 2024-11-06T02:52:31Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061541.16 594480.64 41.31 lat/lon: 36.69 -121.94 2024-11-06T02:52:31Z- LcmTrn::publishEstimatesDecoder() - COVAR: 2.04 1.98 0.46 0.00 2024-11-06T02:52:31Z- AHRS_M2 msg: 1730861551.85 epoch sec; seqNo:440 2024-11-06T02:52:31Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.31 psi 2024-11-06T02:52:31Z- handleDepth()- Depth_Keller msg: 1730861551.87 epoch sec; seqNo:477; depth 33.02 2024-11-06T02:52:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861551.93 epoch sec; seqNo:477; 4061507.97 north; 594494.83 east; 36.69 lat; -121.94 lon; 33.02 depth; utmZone: 10 2024-11-06T02:52:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861551.47 2024-11-06T02:52:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:32Z- waiting for 1730861555.47, time is 1730861551.93 2024-11-06T02:52:32Z- AHRS_M2 msg: 1730861552.25 epoch sec; seqNo:441 2024-11-06T02:52:32Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.32 psi 2024-11-06T02:52:32Z- handleDepth()- Depth_Keller msg: 1730861552.26 epoch sec; seqNo:478; depth 33.10 2024-11-06T02:52:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861552.26 epoch sec; seqNo:478; 4061508.35 north; 594494.82 east; 36.69 lat; -121.94 lon; 33.10 depth; utmZone: 10 2024-11-06T02:52:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861551.47 2024-11-06T02:52:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:32Z- waiting for 1730861555.47, time is 1730861552.26 2024-11-06T02:52:32Z- AHRS_M2 msg: 1730861552.66 epoch sec; seqNo:442 2024-11-06T02:52:32Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.32 psi 2024-11-06T02:52:32Z- handleDepth()- Depth_Keller msg: 1730861552.67 epoch sec; seqNo:479; depth 33.15 2024-11-06T02:52:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861552.68 epoch sec; seqNo:479; 4061508.35 north; 594494.82 east; 36.69 lat; -121.94 lon; 33.15 depth; utmZone: 10 2024-11-06T02:52:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861551.47 2024-11-06T02:52:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:32Z- waiting for 1730861555.47, time is 1730861552.68 2024-11-06T02:52:33Z- AHRS_M2 msg: 1730861553.06 epoch sec; seqNo:443 2024-11-06T02:52:33Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.32 psi 2024-11-06T02:52:33Z- handleDepth()- Depth_Keller msg: 1730861553.07 epoch sec; seqNo:480; depth 33.22 2024-11-06T02:52:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861553.08 epoch sec; seqNo:480; 4061508.73 north; 594494.82 east; 36.69 lat; -121.94 lon; 33.22 depth; utmZone: 10 2024-11-06T02:52:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861551.47 2024-11-06T02:52:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:33Z- waiting for 1730861555.47, time is 1730861553.08 2024-11-06T02:52:33Z- AHRS_M2 msg: 1730861553.46 epoch sec; seqNo:444 2024-11-06T02:52:33Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.32 psi 2024-11-06T02:52:33Z- handleDepth()- Depth_Keller msg: 1730861553.47 epoch sec; seqNo:481; depth 33.28 2024-11-06T02:52:33Z- handleDVL() - DVL msg epochMillisec: 1730861553.50 2024-11-06T02:52:33Z- handleDvl() - RDI_Pathfinder msg: 1730861553.50 sec; seqNo:153; #beams:4 2024-11-06T02:52:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.28 , 50.83 , 49.29 , 61.56 2024-11-06T02:52:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , -0.06 2024-11-06T02:52:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861553.52 epoch sec; seqNo:481; 4061508.73 north; 594494.82 east; 36.69 lat; -121.94 lon; 33.28 depth; utmZone: 10 2024-11-06T02:52:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861551.47 2024-11-06T02:52:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:33Z- waiting for 1730861555.47, time is 1730861553.52 2024-11-06T02:52:33Z- AHRS_M2 msg: 1730861553.87 epoch sec; seqNo:445 2024-11-06T02:52:33Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.33 psi 2024-11-06T02:52:33Z- handleDepth()- Depth_Keller msg: 1730861553.87 epoch sec; seqNo:482; depth 33.35 2024-11-06T02:52:33Z- handleDVL() - DVL msg epochMillisec: 1730861553.87 2024-11-06T02:52:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:52:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:52:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:52:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:52:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:52:33Z- handleDvl() - RDI_Pathfinder msg: 1730861553.87 sec; seqNo:154; #beams:0 2024-11-06T02:52:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:52:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , -0.06 2024-11-06T02:52:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861553.89 epoch sec; seqNo:482; 4061509.09 north; 594493.59 east; 36.69 lat; -121.94 lon; 33.35 depth; utmZone: 10 2024-11-06T02:52:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861551.47 2024-11-06T02:52:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:34Z- waiting for 1730861555.47, time is 1730861553.89 2024-11-06T02:52:34Z- AHRS_M2 msg: 1730861554.27 epoch sec; seqNo:446 2024-11-06T02:52:34Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.33 psi 2024-11-06T02:52:34Z- handleDepth()- Depth_Keller msg: 1730861554.28 epoch sec; seqNo:483; depth 33.40 2024-11-06T02:52:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861554.29 epoch sec; seqNo:483; 4061509.09 north; 594493.59 east; 36.69 lat; -121.94 lon; 33.40 depth; utmZone: 10 2024-11-06T02:52:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861551.47 2024-11-06T02:52:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:34Z- waiting for 1730861555.47, time is 1730861554.29 2024-11-06T02:52:34Z- AHRS_M2 msg: 1730861554.68 epoch sec; seqNo:447 2024-11-06T02:52:34Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.33 psi 2024-11-06T02:52:34Z- handleDepth()- Depth_Keller msg: 1730861554.69 epoch sec; seqNo:484; depth 33.46 2024-11-06T02:52:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861554.69 epoch sec; seqNo:484; 4061509.47 north; 594493.59 east; 36.69 lat; -121.94 lon; 33.46 depth; utmZone: 10 2024-11-06T02:52:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861551.47 2024-11-06T02:52:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:34Z- waiting for 1730861555.47, time is 1730861554.69 2024-11-06T02:52:35Z- AHRS_M2 msg: 1730861555.08 epoch sec; seqNo:448 2024-11-06T02:52:35Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.32 psi 2024-11-06T02:52:35Z- handleDepth()- Depth_Keller msg: 1730861555.09 epoch sec; seqNo:485; depth 33.52 2024-11-06T02:52:35Z- handleDVL() - DVL msg epochMillisec: 1730861555.09 2024-11-06T02:52:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:52:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:52:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:52:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:52:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:52:35Z- handleDvl() - RDI_Pathfinder msg: 1730861555.09 sec; seqNo:155; #beams:0 2024-11-06T02:52:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:52:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , -0.06 2024-11-06T02:52:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861555.10 epoch sec; seqNo:485; 4061509.47 north; 594493.59 east; 36.69 lat; -121.94 lon; 33.52 depth; utmZone: 10 2024-11-06T02:52:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861551.47 2024-11-06T02:52:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:35Z- waiting for 1730861555.47, time is 1730861555.10 2024-11-06T02:52:35Z- AHRS_M2 msg: 1730861555.48 epoch sec; seqNo:449 2024-11-06T02:52:35Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.32 psi 2024-11-06T02:52:35Z- handleDepth()- Depth_Keller msg: 1730861555.49 epoch sec; seqNo:486; depth 33.58 2024-11-06T02:52:35Z- handleDVL() - DVL msg epochMillisec: 1730861555.51 2024-11-06T02:52:35Z- handleDvl() - RDI_Pathfinder msg: 1730861555.51 sec; seqNo:156; #beams:4 2024-11-06T02:52:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 59.25 , 50.31 , 48.78 , 60.78 2024-11-06T02:52:35Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.06 , -0.10 2024-11-06T02:52:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861555.53 epoch sec; seqNo:486; 4061509.84 north; 594492.36 east; 36.69 lat; -121.94 lon; 33.58 depth; utmZone: 10 2024-11-06T02:52:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861551.47 2024-11-06T02:52:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:35Z- waiting for 1730861555.47, time is 1730861555.53 2024-11-06T02:52:35Z- LcmTrn::updateTrn() >>>> pose time 1730861555.53, meas time 1730861555.51 <<<< 2024-11-06T02:52:35Z- LcmTrn::updateTrn() equating pose 1730861555.51 and meas times 1730861555.51 2024-11-06T02:52:35Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861555.51 2024-11-06T02:52:35Z- TerrainNav::motionUpdate() - currEstimate = (4061510, 594492.36, 33.58) 2024-11-06T02:52:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:52:35Z- TerrainNav:: SubcloudNIS is 0.41, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:52:35Z- TNavPF::Projecting Measurements 2024-11-06T02:52:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:52:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:52:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:52:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:52:35Z- TNavFilter::4 good beams. 2024-11-06T02:52:35Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:52:35Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:52:35Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:52:35Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:52:35Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:52:35Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:52:35Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:52:35Z- TerrainMapOctree: (vehN, vehE ) = (4061509.84, 594492.36). 2024-11-06T02:52:35Z- TerrainMapOctree: Min Distance = 341.20. 2024-11-06T02:52:35Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:52:35Z- Forcing Subcloud Comparison 2024-11-06T02:52:35Z- Weighting particles with subcloud comparison 2024-11-06T02:52:35Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:35Z- meanExpectedMeasDiff: -1.635615 delta_rms_squared: 1.230193 alpha: 0.984002 2024-11-06T02:52:35Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:35Z- meanExpectedMeasDiff: 1.998182 delta_rms_squared: 0.764201 alpha: 0.974496 2024-11-06T02:52:35Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:35Z- meanExpectedMeasDiff: 1.405537 delta_rms_squared: 0.785090 alpha: 0.975158 2024-11-06T02:52:35Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:35Z- meanExpectedMeasDiff: -0.600782 delta_rms_squared: 1.324898 alpha: 0.985129 2024-11-06T02:52:35Z- Subcloud NIS: 0.413494 2024-11-06T02:52:35Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:52:35Z- TNavPF:: sumWeights = 0.390360 2024-11-06T02:52:35Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:52:35Z- TNavPF::First Term of Map Covariance Matrix is 0.75 2024-11-06T02:52:35Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T02:52:35Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861555.51 sec, ping # 0. 2024-11-06T02:52:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:52:35Z- variances([0], [2], and psi_berg) = 2.211412 2.471785 0.000000 2024-11-06T02:52:35Z- publishEstimatesDecoder() - at 1730861555.51 sec 2024-11-06T02:52:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:52:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061548.04 594485.13 41.39 2024-11-06T02:52:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061543.05 594477.04 41.82 lat/lon: 36.69 -121.94 2024-11-06T02:52:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 2.21 2.47 0.45 0.00 2024-11-06T02:52:35Z- AHRS_M2 msg: 1730861555.89 epoch sec; seqNo:450 2024-11-06T02:52:35Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.32 psi 2024-11-06T02:52:35Z- handleDepth()- Depth_Keller msg: 1730861555.91 epoch sec; seqNo:487; depth 33.65 2024-11-06T02:52:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861555.95 epoch sec; seqNo:487; 4061509.84 north; 594492.36 east; 36.69 lat; -121.94 lon; 33.65 depth; utmZone: 10 2024-11-06T02:52:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861555.51 2024-11-06T02:52:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:36Z- waiting for 1730861559.51, time is 1730861555.95 2024-11-06T02:52:36Z- AHRS_M2 msg: 1730861556.29 epoch sec; seqNo:451 2024-11-06T02:52:36Z- AHRS_M2 msg: 0.15 phi; -0.22 theta; 5.32 psi 2024-11-06T02:52:36Z- handleDepth()- Depth_Keller msg: 1730861556.30 epoch sec; seqNo:488; depth 33.71 2024-11-06T02:52:36Z- handleDVL() - DVL msg epochMillisec: 1730861556.30 2024-11-06T02:52:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:52:36Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:52:36Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:52:36Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:52:36Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:52:36Z- handleDvl() - RDI_Pathfinder msg: 1730861556.30 sec; seqNo:157; #beams:0 2024-11-06T02:52:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:52:36Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.06 , -0.10 2024-11-06T02:52:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861556.31 epoch sec; seqNo:488; 4061510.22 north; 594492.36 east; 36.69 lat; -121.94 lon; 33.71 depth; utmZone: 10 2024-11-06T02:52:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861555.51 2024-11-06T02:52:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:36Z- waiting for 1730861559.51, time is 1730861556.31 2024-11-06T02:52:36Z- AHRS_M2 msg: 1730861556.70 epoch sec; seqNo:452 2024-11-06T02:52:36Z- AHRS_M2 msg: 0.15 phi; -0.22 theta; 5.31 psi 2024-11-06T02:52:36Z- handleDepth()- Depth_Keller msg: 1730861556.70 epoch sec; seqNo:489; depth 33.77 2024-11-06T02:52:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861556.71 epoch sec; seqNo:489; 4061510.20 north; 594491.14 east; 36.69 lat; -121.94 lon; 33.77 depth; utmZone: 10 2024-11-06T02:52:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861555.51 2024-11-06T02:52:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:36Z- waiting for 1730861559.51, time is 1730861556.71 2024-11-06T02:52:37Z- AHRS_M2 msg: 1730861557.10 epoch sec; seqNo:453 2024-11-06T02:52:37Z- AHRS_M2 msg: 0.15 phi; -0.22 theta; 5.31 psi 2024-11-06T02:52:37Z- handleDepth()- Depth_Keller msg: 1730861557.11 epoch sec; seqNo:490; depth 33.85 2024-11-06T02:52:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861557.12 epoch sec; seqNo:490; 4061510.58 north; 594491.14 east; 36.69 lat; -121.94 lon; 33.85 depth; utmZone: 10 2024-11-06T02:52:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861555.51 2024-11-06T02:52:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:37Z- waiting for 1730861559.51, time is 1730861557.12 2024-11-06T02:52:37Z- AHRS_M2 msg: 1730861557.50 epoch sec; seqNo:454 2024-11-06T02:52:37Z- AHRS_M2 msg: 0.15 phi; -0.22 theta; 5.31 psi 2024-11-06T02:52:37Z- handleDepth()- Depth_Keller msg: 1730861557.51 epoch sec; seqNo:491; depth 33.92 2024-11-06T02:52:37Z- handleDVL() - DVL msg epochMillisec: 1730861557.54 2024-11-06T02:52:37Z- handleDvl() - RDI_Pathfinder msg: 1730861557.54 sec; seqNo:158; #beams:4 2024-11-06T02:52:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 58.99 , 49.55 , 48.78 , 60.02 2024-11-06T02:52:37Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , -0.09 , 0.05 2024-11-06T02:52:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861557.55 epoch sec; seqNo:491; 4061510.58 north; 594491.14 east; 36.69 lat; -121.94 lon; 33.92 depth; utmZone: 10 2024-11-06T02:52:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861555.51 2024-11-06T02:52:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:37Z- waiting for 1730861559.51, time is 1730861557.55 2024-11-06T02:52:37Z- handleDepth()- Depth_Keller msg: 1730861557.93 epoch sec; seqNo:492; depth 33.99 2024-11-06T02:52:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861557.94 epoch sec; seqNo:492; 4061510.96 north; 594491.13 east; 36.69 lat; -121.94 lon; 33.99 depth; utmZone: 10 2024-11-06T02:52:38Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:52:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861555.51 2024-11-06T02:52:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:38Z- waiting for 1730861559.51, time is 1730861557.94 2024-11-06T02:52:38Z- AHRS_M2 msg: 1730861558.31 epoch sec; seqNo:455 2024-11-06T02:52:38Z- AHRS_M2 msg: 0.15 phi; -0.22 theta; 5.31 psi 2024-11-06T02:52:38Z- handleDepth()- Depth_Keller msg: 1730861558.33 epoch sec; seqNo:493; depth 34.07 2024-11-06T02:52:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861558.34 epoch sec; seqNo:493; 4061510.95 north; 594489.91 east; 36.69 lat; -121.94 lon; 34.07 depth; utmZone: 10 2024-11-06T02:52:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861555.51 2024-11-06T02:52:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:38Z- waiting for 1730861559.51, time is 1730861558.34 2024-11-06T02:52:38Z- AHRS_M2 msg: 1730861558.71 epoch sec; seqNo:456 2024-11-06T02:52:38Z- AHRS_M2 msg: 0.15 phi; -0.22 theta; 5.31 psi 2024-11-06T02:52:38Z- handleDepth()- Depth_Keller msg: 1730861558.72 epoch sec; seqNo:494; depth 34.15 2024-11-06T02:52:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861558.73 epoch sec; seqNo:494; 4061511.33 north; 594489.91 east; 36.69 lat; -121.94 lon; 34.15 depth; utmZone: 10 2024-11-06T02:52:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861555.51 2024-11-06T02:52:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:38Z- waiting for 1730861559.51, time is 1730861558.73 2024-11-06T02:52:39Z- AHRS_M2 msg: 1730861559.12 epoch sec; seqNo:457 2024-11-06T02:52:39Z- AHRS_M2 msg: 0.15 phi; -0.22 theta; 5.31 psi 2024-11-06T02:52:39Z- handleDepth()- Depth_Keller msg: 1730861559.13 epoch sec; seqNo:495; depth 34.20 2024-11-06T02:52:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861559.14 epoch sec; seqNo:495; 4061511.33 north; 594489.91 east; 36.69 lat; -121.94 lon; 34.20 depth; utmZone: 10 2024-11-06T02:52:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861555.51 2024-11-06T02:52:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:39Z- waiting for 1730861559.51, time is 1730861559.14 2024-11-06T02:52:39Z- handleDepth()- Depth_Keller msg: 1730861559.55 epoch sec; seqNo:496; depth 34.29 2024-11-06T02:52:39Z- handleDVL() - DVL msg epochMillisec: 1730861559.57 2024-11-06T02:52:39Z- handleDvl() - RDI_Pathfinder msg: 1730861559.57 sec; seqNo:159; #beams:4 2024-11-06T02:52:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 59.24 , 49.54 , 48.78 , 59.76 2024-11-06T02:52:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.09 , 0.14 2024-11-06T02:52:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861559.58 epoch sec; seqNo:496; 4061511.69 north; 594488.68 east; 36.69 lat; -121.94 lon; 34.29 depth; utmZone: 10 2024-11-06T02:52:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861555.51 2024-11-06T02:52:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:39Z- waiting for 1730861559.51, time is 1730861559.58 2024-11-06T02:52:39Z- LcmTrn::updateTrn() >>>> pose time 1730861559.58, meas time 1730861559.57 <<<< 2024-11-06T02:52:39Z- LcmTrn::updateTrn() equating pose 1730861559.57 and meas times 1730861559.57 2024-11-06T02:52:39Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861559.57 2024-11-06T02:52:39Z- TerrainNav::motionUpdate() - currEstimate = (4061512, 594488.68, 34.29) 2024-11-06T02:52:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:52:39Z- TerrainNav:: SubcloudNIS is 0.41, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:52:39Z- TNavPF::Projecting Measurements 2024-11-06T02:52:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:52:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:52:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:52:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:52:39Z- TNavFilter::4 good beams. 2024-11-06T02:52:39Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:52:39Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:52:39Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:52:39Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:52:39Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:52:39Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:52:39Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:52:39Z- TerrainMapOctree: (vehN, vehE ) = (4061511.69, 594488.68). 2024-11-06T02:52:39Z- TerrainMapOctree: Min Distance = 337.28. 2024-11-06T02:52:39Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:52:39Z- Forcing Subcloud Comparison 2024-11-06T02:52:39Z- Weighting particles with subcloud comparison 2024-11-06T02:52:39Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:39Z- meanExpectedMeasDiff: 0.223004 delta_rms_squared: 1.530066 alpha: 0.987097 2024-11-06T02:52:39Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:39Z- meanExpectedMeasDiff: 0.966950 delta_rms_squared: 0.849552 alpha: 0.977000 2024-11-06T02:52:39Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:39Z- meanExpectedMeasDiff: 2.051845 delta_rms_squared: 0.787611 alpha: 0.975236 2024-11-06T02:52:39Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:39Z- meanExpectedMeasDiff: -0.831152 delta_rms_squared: 1.317562 alpha: 0.985047 2024-11-06T02:52:39Z- Subcloud NIS: 0.419335 2024-11-06T02:52:39Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:52:39Z- TNavPF:: sumWeights = 0.488113 2024-11-06T02:52:39Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:52:39Z- TNavPF::First Term of Map Covariance Matrix is 0.46 2024-11-06T02:52:39Z- TNavPF::Calculated NIS Value : 0.25 numBeams normalized NIS: 0.06 2024-11-06T02:52:39Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861559.57 sec, ping # 0. 2024-11-06T02:52:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:52:39Z- variances([0], [2], and psi_berg) = 1.901619 2.428421 0.000000 2024-11-06T02:52:39Z- publishEstimatesDecoder() - at 1730861559.57 sec 2024-11-06T02:52:39Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:52:39Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061543.86 594472.91 42.21 2024-11-06T02:52:39Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061544.88 594473.34 42.35 lat/lon: 36.69 -121.94 2024-11-06T02:52:39Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.90 2.43 0.43 0.00 2024-11-06T02:52:39Z- AHRS_M2 msg: 1730861559.93 epoch sec; seqNo:458 2024-11-06T02:52:39Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.31 psi 2024-11-06T02:52:39Z- handleDepth()- Depth_Keller msg: 1730861559.96 epoch sec; seqNo:497; depth 34.36 2024-11-06T02:52:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861559.99 epoch sec; seqNo:497; 4061511.69 north; 594488.68 east; 36.69 lat; -121.94 lon; 34.36 depth; utmZone: 10 2024-11-06T02:52:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861559.57 2024-11-06T02:52:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:40Z- waiting for 1730861563.57, time is 1730861559.99 2024-11-06T02:52:40Z- AHRS_M2 msg: 1730861560.33 epoch sec; seqNo:459 2024-11-06T02:52:40Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.31 psi 2024-11-06T02:52:40Z- handleDepth()- Depth_Keller msg: 1730861560.34 epoch sec; seqNo:498; depth 34.43 2024-11-06T02:52:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861560.36 epoch sec; seqNo:498; 4061512.07 north; 594488.68 east; 36.69 lat; -121.94 lon; 34.43 depth; utmZone: 10 2024-11-06T02:52:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861559.57 2024-11-06T02:52:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:40Z- waiting for 1730861563.57, time is 1730861560.36 2024-11-06T02:52:40Z- AHRS_M2 msg: 1730861560.74 epoch sec; seqNo:460 2024-11-06T02:52:40Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.31 psi 2024-11-06T02:52:40Z- handleDepth()- Depth_Keller msg: 1730861560.74 epoch sec; seqNo:499; depth 34.49 2024-11-06T02:52:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861560.75 epoch sec; seqNo:499; 4061512.07 north; 594488.68 east; 36.69 lat; -121.94 lon; 34.49 depth; utmZone: 10 2024-11-06T02:52:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861559.57 2024-11-06T02:52:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:40Z- waiting for 1730861563.57, time is 1730861560.75 2024-11-06T02:52:41Z- AHRS_M2 msg: 1730861561.14 epoch sec; seqNo:461 2024-11-06T02:52:41Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.31 psi 2024-11-06T02:52:41Z- handleDepth()- Depth_Keller msg: 1730861561.15 epoch sec; seqNo:500; depth 34.56 2024-11-06T02:52:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861561.16 epoch sec; seqNo:500; 4061512.44 north; 594487.45 east; 36.69 lat; -121.94 lon; 34.56 depth; utmZone: 10 2024-11-06T02:52:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861559.57 2024-11-06T02:52:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:41Z- waiting for 1730861563.57, time is 1730861561.16 2024-11-06T02:52:41Z- handleDepth()- Depth_Keller msg: 1730861561.57 epoch sec; seqNo:501; depth 34.62 2024-11-06T02:52:41Z- handleDVL() - DVL msg epochMillisec: 1730861561.60 2024-11-06T02:52:41Z- handleDvl() - RDI_Pathfinder msg: 1730861561.60 sec; seqNo:160; #beams:4 2024-11-06T02:52:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 58.99 , 49.54 , 48.52 , 59.76 2024-11-06T02:52:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , 0.07 2024-11-06T02:52:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861561.61 epoch sec; seqNo:501; 4061512.44 north; 594487.45 east; 36.69 lat; -121.94 lon; 34.62 depth; utmZone: 10 2024-11-06T02:52:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861559.57 2024-11-06T02:52:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:41Z- waiting for 1730861563.57, time is 1730861561.61 2024-11-06T02:52:41Z- handleDepth()- Depth_Keller msg: 1730861561.97 epoch sec; seqNo:502; depth 34.72 2024-11-06T02:52:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861561.98 epoch sec; seqNo:502; 4061512.82 north; 594487.45 east; 36.69 lat; -121.94 lon; 34.72 depth; utmZone: 10 2024-11-06T02:52:42Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:52:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861559.57 2024-11-06T02:52:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:42Z- waiting for 1730861563.57, time is 1730861561.98 2024-11-06T02:52:42Z- AHRS_M2 msg: 1730861562.35 epoch sec; seqNo:462 2024-11-06T02:52:42Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.32 psi 2024-11-06T02:52:42Z- handleDepth()- Depth_Keller msg: 1730861562.37 epoch sec; seqNo:503; depth 34.77 2024-11-06T02:52:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861562.38 epoch sec; seqNo:503; 4061512.82 north; 594487.45 east; 36.69 lat; -121.94 lon; 34.77 depth; utmZone: 10 2024-11-06T02:52:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861559.57 2024-11-06T02:52:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:42Z- waiting for 1730861563.57, time is 1730861562.38 2024-11-06T02:52:42Z- AHRS_M2 msg: 1730861562.76 epoch sec; seqNo:463 2024-11-06T02:52:42Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.32 psi 2024-11-06T02:52:42Z- handleDepth()- Depth_Keller msg: 1730861562.77 epoch sec; seqNo:504; depth 34.85 2024-11-06T02:52:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861562.78 epoch sec; seqNo:504; 4061512.80 north; 594486.23 east; 36.69 lat; -121.94 lon; 34.85 depth; utmZone: 10 2024-11-06T02:52:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861559.57 2024-11-06T02:52:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:42Z- waiting for 1730861563.57, time is 1730861562.78 2024-11-06T02:52:43Z- handleDepth()- Depth_Keller msg: 1730861563.19 epoch sec; seqNo:505; depth 34.91 2024-11-06T02:52:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861563.21 epoch sec; seqNo:505; 4061513.18 north; 594486.22 east; 36.69 lat; -121.94 lon; 34.91 depth; utmZone: 10 2024-11-06T02:52:43Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:52:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861559.57 2024-11-06T02:52:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:43Z- waiting for 1730861563.57, time is 1730861563.21 2024-11-06T02:52:43Z- AHRS_M2 msg: 1730861563.57 epoch sec; seqNo:464 2024-11-06T02:52:43Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.32 psi 2024-11-06T02:52:43Z- handleDepth()- Depth_Keller msg: 1730861563.57 epoch sec; seqNo:506; depth 34.97 2024-11-06T02:52:43Z- handleDVL() - DVL msg epochMillisec: 1730861563.59 2024-11-06T02:52:43Z- handleDvl() - RDI_Pathfinder msg: 1730861563.59 sec; seqNo:161; #beams:4 2024-11-06T02:52:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 58.73 , 49.03 , 48.00 , 59.24 2024-11-06T02:52:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , -0.02 2024-11-06T02:52:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861563.60 epoch sec; seqNo:506; 4061513.18 north; 594486.22 east; 36.69 lat; -121.94 lon; 34.97 depth; utmZone: 10 2024-11-06T02:52:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861559.57 2024-11-06T02:52:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:43Z- waiting for 1730861563.57, time is 1730861563.60 2024-11-06T02:52:43Z- LcmTrn::updateTrn() >>>> pose time 1730861563.60, meas time 1730861563.59 <<<< 2024-11-06T02:52:43Z- LcmTrn::updateTrn() equating pose 1730861563.59 and meas times 1730861563.59 2024-11-06T02:52:43Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861563.59 2024-11-06T02:52:43Z- TerrainNav::motionUpdate() - currEstimate = (4061513, 594486.22, 34.97) 2024-11-06T02:52:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:52:43Z- TerrainNav:: SubcloudNIS is 0.42, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:52:43Z- TNavPF::Projecting Measurements 2024-11-06T02:52:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:52:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:52:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:52:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:52:43Z- TNavFilter::4 good beams. 2024-11-06T02:52:43Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:52:43Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:52:43Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:52:43Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:52:43Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:52:43Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:52:43Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:52:43Z- TerrainMapOctree: (vehN, vehE ) = (4061513.18, 594486.22). 2024-11-06T02:52:43Z- TerrainMapOctree: Min Distance = 334.49. 2024-11-06T02:52:43Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:52:43Z- Forcing Subcloud Comparison 2024-11-06T02:52:43Z- Weighting particles with subcloud comparison 2024-11-06T02:52:43Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:43Z- meanExpectedMeasDiff: 0.700316 delta_rms_squared: 1.163192 alpha: 0.983097 2024-11-06T02:52:43Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:43Z- meanExpectedMeasDiff: 1.283227 delta_rms_squared: 0.886916 alpha: 0.977947 2024-11-06T02:52:43Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:43Z- meanExpectedMeasDiff: 1.817405 delta_rms_squared: 0.766201 alpha: 0.974561 2024-11-06T02:52:43Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:43Z- meanExpectedMeasDiff: -1.768116 delta_rms_squared: 1.143853 alpha: 0.982816 2024-11-06T02:52:43Z- Subcloud NIS: 0.428251 2024-11-06T02:52:43Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:52:43Z- TNavPF:: sumWeights = 0.407388 2024-11-06T02:52:43Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:52:43Z- TNavPF::First Term of Map Covariance Matrix is 0.36 2024-11-06T02:52:43Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T02:52:43Z- TerrainNav::Resampling particle filter... 2024-11-06T02:52:43Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861563.59 sec, ping # 0. 2024-11-06T02:52:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:52:43Z- variances([0], [2], and psi_berg) = 2.121278 3.004510 0.000000 2024-11-06T02:52:43Z- publishEstimatesDecoder() - at 1730861563.59 sec 2024-11-06T02:52:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:52:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061551.22 594479.17 40.92 2024-11-06T02:52:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061546.40 594470.98 42.86 lat/lon: 36.69 -121.94 2024-11-06T02:52:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 2.12 3.00 0.47 0.00 2024-11-06T02:52:43Z- AHRS_M2 msg: 1730861563.97 epoch sec; seqNo:465 2024-11-06T02:52:43Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.32 psi 2024-11-06T02:52:43Z- handleDepth()- Depth_Keller msg: 1730861563.98 epoch sec; seqNo:507; depth 35.05 2024-11-06T02:52:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861564.00 epoch sec; seqNo:507; 4061513.56 north; 594486.22 east; 36.69 lat; -121.94 lon; 35.05 depth; utmZone: 10 2024-11-06T02:52:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861563.59 2024-11-06T02:52:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:44Z- waiting for 1730861567.59, time is 1730861564.00 2024-11-06T02:52:44Z- handleDepth()- Depth_Keller msg: 1730861564.39 epoch sec; seqNo:508; depth 35.11 2024-11-06T02:52:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861564.40 epoch sec; seqNo:508; 4061513.55 north; 594485.00 east; 36.69 lat; -121.94 lon; 35.11 depth; utmZone: 10 2024-11-06T02:52:44Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:52:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861563.59 2024-11-06T02:52:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:44Z- waiting for 1730861567.59, time is 1730861564.40 2024-11-06T02:52:44Z- AHRS_M2 msg: 1730861564.78 epoch sec; seqNo:466 2024-11-06T02:52:44Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.32 psi 2024-11-06T02:52:44Z- handleDepth()- Depth_Keller msg: 1730861564.78 epoch sec; seqNo:509; depth 35.17 2024-11-06T02:52:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861564.79 epoch sec; seqNo:509; 4061513.92 north; 594485.00 east; 36.69 lat; -121.94 lon; 35.17 depth; utmZone: 10 2024-11-06T02:52:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861563.59 2024-11-06T02:52:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:44Z- waiting for 1730861567.59, time is 1730861564.79 2024-11-06T02:52:45Z- AHRS_M2 msg: 1730861565.19 epoch sec; seqNo:467 2024-11-06T02:52:45Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.32 psi 2024-11-06T02:52:45Z- handleDepth()- Depth_Keller msg: 1730861565.19 epoch sec; seqNo:510; depth 35.25 2024-11-06T02:52:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861565.22 epoch sec; seqNo:510; 4061513.92 north; 594485.00 east; 36.69 lat; -121.94 lon; 35.25 depth; utmZone: 10 2024-11-06T02:52:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861563.59 2024-11-06T02:52:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:45Z- waiting for 1730861567.59, time is 1730861565.22 2024-11-06T02:52:45Z- AHRS_M2 msg: 1730861565.58 epoch sec; seqNo:468 2024-11-06T02:52:45Z- AHRS_M2 msg: 0.14 phi; -0.21 theta; 5.32 psi 2024-11-06T02:52:45Z- handleDepth()- Depth_Keller msg: 1730861565.59 epoch sec; seqNo:511; depth 35.31 2024-11-06T02:52:45Z- handleDVL() - DVL msg epochMillisec: 1730861565.62 2024-11-06T02:52:45Z- handleDvl() - RDI_Pathfinder msg: 1730861565.62 sec; seqNo:162; #beams:4 2024-11-06T02:52:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 58.47 , 48.77 , 47.49 , 58.47 2024-11-06T02:52:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.06 , -0.03 2024-11-06T02:52:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861565.63 epoch sec; seqNo:511; 4061514.30 north; 594484.99 east; 36.69 lat; -121.94 lon; 35.31 depth; utmZone: 10 2024-11-06T02:52:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861563.59 2024-11-06T02:52:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:45Z- waiting for 1730861567.59, time is 1730861565.63 2024-11-06T02:52:45Z- AHRS_M2 msg: 1730861565.99 epoch sec; seqNo:469 2024-11-06T02:52:45Z- AHRS_M2 msg: 0.14 phi; -0.21 theta; 5.32 psi 2024-11-06T02:52:45Z- handleDepth()- Depth_Keller msg: 1730861565.99 epoch sec; seqNo:512; depth 35.39 2024-11-06T02:52:46Z- handleDVL() - DVL msg epochMillisec: 1730861566.00 2024-11-06T02:52:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:52:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:52:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:52:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:52:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:52:46Z- handleDvl() - RDI_Pathfinder msg: 1730861566.00 sec; seqNo:163; #beams:0 2024-11-06T02:52:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:52:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.06 , -0.03 2024-11-06T02:52:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861566.02 epoch sec; seqNo:512; 4061514.29 north; 594483.77 east; 36.69 lat; -121.94 lon; 35.39 depth; utmZone: 10 2024-11-06T02:52:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861563.59 2024-11-06T02:52:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:46Z- waiting for 1730861567.59, time is 1730861566.02 2024-11-06T02:52:46Z- AHRS_M2 msg: 1730861566.39 epoch sec; seqNo:470 2024-11-06T02:52:46Z- AHRS_M2 msg: 0.14 phi; -0.21 theta; 5.32 psi 2024-11-06T02:52:46Z- handleDepth()- Depth_Keller msg: 1730861566.41 epoch sec; seqNo:513; depth 35.44 2024-11-06T02:52:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861566.42 epoch sec; seqNo:513; 4061514.67 north; 594483.77 east; 36.69 lat; -121.94 lon; 35.44 depth; utmZone: 10 2024-11-06T02:52:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861563.59 2024-11-06T02:52:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:46Z- waiting for 1730861567.59, time is 1730861566.42 2024-11-06T02:52:46Z- AHRS_M2 msg: 1730861566.81 epoch sec; seqNo:471 2024-11-06T02:52:46Z- AHRS_M2 msg: 0.14 phi; -0.21 theta; 5.32 psi 2024-11-06T02:52:46Z- handleDepth()- Depth_Keller msg: 1730861566.81 epoch sec; seqNo:514; depth 35.51 2024-11-06T02:52:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861566.82 epoch sec; seqNo:514; 4061515.05 north; 594483.76 east; 36.69 lat; -121.94 lon; 35.51 depth; utmZone: 10 2024-11-06T02:52:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861563.59 2024-11-06T02:52:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:46Z- waiting for 1730861567.59, time is 1730861566.82 2024-11-06T02:52:47Z- AHRS_M2 msg: 1730861567.25 epoch sec; seqNo:472 2024-11-06T02:52:47Z- AHRS_M2 msg: 0.14 phi; -0.21 theta; 5.32 psi 2024-11-06T02:52:47Z- handleDepth()- Depth_Keller msg: 1730861567.28 epoch sec; seqNo:515; depth 35.58 2024-11-06T02:52:47Z- handleDVL() - DVL msg epochMillisec: 1730861567.35 2024-11-06T02:52:47Z- handleDvl() - RDI_Pathfinder msg: 1730861567.35 sec; seqNo:164; #beams:4 2024-11-06T02:52:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 57.71 , 48.26 , 47.24 , 58.47 2024-11-06T02:52:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.07 , -0.00 2024-11-06T02:52:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861567.38 epoch sec; seqNo:515; 4061515.03 north; 594482.54 east; 36.69 lat; -121.94 lon; 35.58 depth; utmZone: 10 2024-11-06T02:52:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861563.59 2024-11-06T02:52:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:47Z- waiting for 1730861567.59, time is 1730861567.38 2024-11-06T02:52:47Z- AHRS_M2 msg: 1730861567.60 epoch sec; seqNo:473 2024-11-06T02:52:47Z- AHRS_M2 msg: 0.14 phi; -0.21 theta; 5.32 psi 2024-11-06T02:52:47Z- handleDepth()- Depth_Keller msg: 1730861567.61 epoch sec; seqNo:516; depth 35.67 2024-11-06T02:52:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861567.63 epoch sec; seqNo:516; 4061515.41 north; 594482.54 east; 36.69 lat; -121.94 lon; 35.67 depth; utmZone: 10 2024-11-06T02:52:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861563.59 2024-11-06T02:52:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:47Z- waiting for 1730861567.59, time is 1730861567.63 2024-11-06T02:52:47Z- LcmTrn::updateTrn() >>>> pose time 1730861567.63, meas time 1730861567.35 <<<< 2024-11-06T02:52:47Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861567.63 2024-11-06T02:52:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861567.35 sec, ping # 0; waiting for INS data more recent than 1730861563.59... 2024-11-06T02:52:47Z- TerrainNav::motionUpdate() - currEstimate = (4061515, 594482.54, 35.67) 2024-11-06T02:52:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:52:47Z- TerrainNav:: SubcloudNIS is 0.43, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:52:47Z- TNavPF::Projecting Measurements 2024-11-06T02:52:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:52:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:52:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:52:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:52:47Z- TNavFilter::4 good beams. 2024-11-06T02:52:47Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:52:47Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:52:47Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:52:47Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:52:47Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:52:47Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:52:47Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:52:47Z- TerrainMapOctree: (vehN, vehE ) = (4061515.26, 594482.79). 2024-11-06T02:52:47Z- TerrainMapOctree: Min Distance = 330.60. 2024-11-06T02:52:47Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:52:47Z- Forcing Subcloud Comparison 2024-11-06T02:52:47Z- Weighting particles with subcloud comparison 2024-11-06T02:52:47Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:47Z- meanExpectedMeasDiff: 0.255908 delta_rms_squared: 1.207229 alpha: 0.983703 2024-11-06T02:52:47Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:47Z- meanExpectedMeasDiff: 1.783532 delta_rms_squared: 0.861780 alpha: 0.977319 2024-11-06T02:52:47Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:47Z- meanExpectedMeasDiff: 2.445961 delta_rms_squared: 1.047601 alpha: 0.981266 2024-11-06T02:52:47Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:47Z- meanExpectedMeasDiff: -1.775272 delta_rms_squared: 1.340014 alpha: 0.985294 2024-11-06T02:52:47Z- Subcloud NIS: 0.435205 2024-11-06T02:52:47Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:52:47Z- TNavPF:: sumWeights = 0.286050 2024-11-06T02:52:47Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:52:47Z- TNavPF::First Term of Map Covariance Matrix is 0.21 2024-11-06T02:52:47Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T02:52:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861567.35 sec. 2024-11-06T02:52:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:52:47Z- variances([0], [2], and psi_berg) = 3.570167 5.340373 0.000000 2024-11-06T02:52:47Z- publishEstimatesDecoder() - at 1730861567.63 sec 2024-11-06T02:52:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:52:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061558.67 594476.26 44.03 2024-11-06T02:52:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061548.93 594467.67 43.31 lat/lon: 36.69 -121.94 2024-11-06T02:52:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 3.57 5.34 0.70 0.00 2024-11-06T02:52:48Z- AHRS_M2 msg: 1730861568.02 epoch sec; seqNo:474 2024-11-06T02:52:48Z- AHRS_M2 msg: 0.14 phi; -0.21 theta; 5.32 psi 2024-11-06T02:52:48Z- handleDepth()- Depth_Keller msg: 1730861568.03 epoch sec; seqNo:517; depth 35.72 2024-11-06T02:52:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861568.05 epoch sec; seqNo:517; 4061515.41 north; 594482.54 east; 36.69 lat; -121.94 lon; 35.72 depth; utmZone: 10 2024-11-06T02:52:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861567.63 2024-11-06T02:52:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:48Z- waiting for 1730861571.63, time is 1730861568.05 2024-11-06T02:52:48Z- AHRS_M2 msg: 1730861568.41 epoch sec; seqNo:475 2024-11-06T02:52:48Z- AHRS_M2 msg: 0.14 phi; -0.21 theta; 5.32 psi 2024-11-06T02:52:48Z- handleDepth()- Depth_Keller msg: 1730861568.42 epoch sec; seqNo:518; depth 35.78 2024-11-06T02:52:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861568.43 epoch sec; seqNo:518; 4061515.79 north; 594482.53 east; 36.69 lat; -121.94 lon; 35.78 depth; utmZone: 10 2024-11-06T02:52:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861567.63 2024-11-06T02:52:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:48Z- waiting for 1730861571.63, time is 1730861568.43 2024-11-06T02:52:48Z- AHRS_M2 msg: 1730861568.82 epoch sec; seqNo:476 2024-11-06T02:52:48Z- AHRS_M2 msg: 0.14 phi; -0.21 theta; 5.32 psi 2024-11-06T02:52:48Z- handleDepth()- Depth_Keller msg: 1730861568.82 epoch sec; seqNo:519; depth 35.85 2024-11-06T02:52:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861568.83 epoch sec; seqNo:519; 4061515.78 north; 594481.31 east; 36.69 lat; -121.94 lon; 35.85 depth; utmZone: 10 2024-11-06T02:52:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861567.63 2024-11-06T02:52:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:48Z- waiting for 1730861571.63, time is 1730861568.83 2024-11-06T02:52:49Z- AHRS_M2 msg: 1730861569.25 epoch sec; seqNo:477 2024-11-06T02:52:49Z- AHRS_M2 msg: 0.14 phi; -0.21 theta; 5.32 psi 2024-11-06T02:52:49Z- handleDepth()- Depth_Keller msg: 1730861569.28 epoch sec; seqNo:520; depth 35.92 2024-11-06T02:52:49Z- handleDVL() - DVL msg epochMillisec: 1730861569.33 2024-11-06T02:52:49Z- handleDvl() - RDI_Pathfinder msg: 1730861569.33 sec; seqNo:165; #beams:4 2024-11-06T02:52:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 57.45 , 48.00 , 46.72 , 58.21 2024-11-06T02:52:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.05 , 0.05 2024-11-06T02:52:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861569.35 epoch sec; seqNo:520; 4061516.16 north; 594481.31 east; 36.69 lat; -121.94 lon; 35.92 depth; utmZone: 10 2024-11-06T02:52:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861567.63 2024-11-06T02:52:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:49Z- waiting for 1730861571.63, time is 1730861569.35 2024-11-06T02:52:49Z- AHRS_M2 msg: 1730861569.62 epoch sec; seqNo:478 2024-11-06T02:52:49Z- AHRS_M2 msg: 0.13 phi; -0.21 theta; 5.32 psi 2024-11-06T02:52:49Z- handleDepth()- Depth_Keller msg: 1730861569.63 epoch sec; seqNo:521; depth 36.01 2024-11-06T02:52:49Z- handleDVL() - DVL msg epochMillisec: 1730861569.63 2024-11-06T02:52:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:52:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:52:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:52:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:52:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:52:49Z- handleDvl() - RDI_Pathfinder msg: 1730861569.63 sec; seqNo:166; #beams:0 2024-11-06T02:52:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:52:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.05 , 0.05 2024-11-06T02:52:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861569.65 epoch sec; seqNo:521; 4061516.16 north; 594481.31 east; 36.69 lat; -121.94 lon; 36.01 depth; utmZone: 10 2024-11-06T02:52:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861567.63 2024-11-06T02:52:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:49Z- waiting for 1730861571.63, time is 1730861569.65 2024-11-06T02:52:50Z- AHRS_M2 msg: 1730861570.04 epoch sec; seqNo:479 2024-11-06T02:52:50Z- AHRS_M2 msg: 0.13 phi; -0.21 theta; 5.32 psi 2024-11-06T02:52:50Z- handleDepth()- Depth_Keller msg: 1730861570.05 epoch sec; seqNo:522; depth 36.06 2024-11-06T02:52:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861570.06 epoch sec; seqNo:522; 4061516.54 north; 594481.31 east; 36.69 lat; -121.94 lon; 36.06 depth; utmZone: 10 2024-11-06T02:52:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861567.63 2024-11-06T02:52:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:50Z- waiting for 1730861571.63, time is 1730861570.06 2024-11-06T02:52:50Z- AHRS_M2 msg: 1730861570.43 epoch sec; seqNo:480 2024-11-06T02:52:50Z- AHRS_M2 msg: 0.13 phi; -0.21 theta; 5.32 psi 2024-11-06T02:52:50Z- handleDepth()- Depth_Keller msg: 1730861570.44 epoch sec; seqNo:523; depth 36.13 2024-11-06T02:52:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861570.45 epoch sec; seqNo:523; 4061516.52 north; 594480.09 east; 36.69 lat; -121.94 lon; 36.13 depth; utmZone: 10 2024-11-06T02:52:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861567.63 2024-11-06T02:52:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:50Z- waiting for 1730861571.63, time is 1730861570.45 2024-11-06T02:52:50Z- AHRS_M2 msg: 1730861570.84 epoch sec; seqNo:481 2024-11-06T02:52:50Z- AHRS_M2 msg: 0.13 phi; -0.21 theta; 5.32 psi 2024-11-06T02:52:50Z- handleDepth()- Depth_Keller msg: 1730861570.84 epoch sec; seqNo:524; depth 36.20 2024-11-06T02:52:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861570.85 epoch sec; seqNo:524; 4061516.90 north; 594480.08 east; 36.69 lat; -121.94 lon; 36.20 depth; utmZone: 10 2024-11-06T02:52:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861567.63 2024-11-06T02:52:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:50Z- waiting for 1730861571.63, time is 1730861570.85 2024-11-06T02:52:51Z- AHRS_M2 msg: 1730861571.30 epoch sec; seqNo:482 2024-11-06T02:52:51Z- AHRS_M2 msg: 0.13 phi; -0.21 theta; 5.32 psi 2024-11-06T02:52:51Z- handleDepth()- Depth_Keller msg: 1730861571.30 epoch sec; seqNo:525; depth 36.26 2024-11-06T02:52:51Z- handleDVL() - DVL msg epochMillisec: 1730861571.32 2024-11-06T02:52:51Z- handleDvl() - RDI_Pathfinder msg: 1730861571.32 sec; seqNo:167; #beams:4 2024-11-06T02:52:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 56.71 , 47.90 , 46.47 , 57.43 2024-11-06T02:52:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.06 , 0.06 2024-11-06T02:52:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861571.34 epoch sec; seqNo:525; 4061516.90 north; 594480.08 east; 36.69 lat; -121.94 lon; 36.26 depth; utmZone: 10 2024-11-06T02:52:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861567.63 2024-11-06T02:52:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:51Z- waiting for 1730861571.63, time is 1730861571.34 2024-11-06T02:52:51Z- AHRS_M2 msg: 1730861571.65 epoch sec; seqNo:483 2024-11-06T02:52:51Z- AHRS_M2 msg: 0.13 phi; -0.22 theta; 5.32 psi 2024-11-06T02:52:51Z- handleDepth()- Depth_Keller msg: 1730861571.65 epoch sec; seqNo:526; depth 36.34 2024-11-06T02:52:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861571.68 epoch sec; seqNo:526; 4061517.27 north; 594478.86 east; 36.69 lat; -121.94 lon; 36.34 depth; utmZone: 10 2024-11-06T02:52:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861567.63 2024-11-06T02:52:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:51Z- waiting for 1730861571.63, time is 1730861571.68 2024-11-06T02:52:51Z- LcmTrn::updateTrn() >>>> pose time 1730861571.68, meas time 1730861571.32 <<<< 2024-11-06T02:52:51Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861571.68 2024-11-06T02:52:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861571.32 sec, ping # 0; waiting for INS data more recent than 1730861567.63... 2024-11-06T02:52:51Z- TerrainNav::motionUpdate() - currEstimate = (4061517, 594478.86, 36.34) 2024-11-06T02:52:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:52:51Z- TerrainNav:: SubcloudNIS is 0.44, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:52:51Z- TNavPF::Projecting Measurements 2024-11-06T02:52:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:52:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:52:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:52:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:52:51Z- TNavFilter::4 good beams. 2024-11-06T02:52:51Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:52:51Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:52:51Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:52:51Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:52:51Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:52:51Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:52:51Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:52:51Z- TerrainMapOctree: (vehN, vehE ) = (4061517.10, 594479.18). 2024-11-06T02:52:51Z- TerrainMapOctree: Min Distance = 326.75. 2024-11-06T02:52:51Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:52:51Z- Forcing Subcloud Comparison 2024-11-06T02:52:51Z- Weighting particles with subcloud comparison 2024-11-06T02:52:51Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:51Z- meanExpectedMeasDiff: 0.253642 delta_rms_squared: 1.695983 alpha: 0.988345 2024-11-06T02:52:51Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:51Z- meanExpectedMeasDiff: 1.651215 delta_rms_squared: 1.156241 alpha: 0.982997 2024-11-06T02:52:51Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:51Z- meanExpectedMeasDiff: 1.921144 delta_rms_squared: 0.938754 alpha: 0.979140 2024-11-06T02:52:51Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:51Z- meanExpectedMeasDiff: -2.161956 delta_rms_squared: 2.143509 alpha: 0.990756 2024-11-06T02:52:51Z- Subcloud NIS: 0.453982 2024-11-06T02:52:51Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:52:51Z- TNavPF:: sumWeights = 0.288465 2024-11-06T02:52:51Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:52:51Z- TNavPF::First Term of Map Covariance Matrix is 0.28 2024-11-06T02:52:51Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T02:52:52Z- TerrainNav::Resampling particle filter... 2024-11-06T02:52:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861571.32 sec. 2024-11-06T02:52:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:52:52Z- variances([0], [2], and psi_berg) = 4.228869 6.455238 0.000000 2024-11-06T02:52:52Z- publishEstimatesDecoder() - at 1730861571.68 sec 2024-11-06T02:52:52Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:52:52Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061555.38 594471.74 42.73 2024-11-06T02:52:52Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061550.99 594464.27 43.83 lat/lon: 36.69 -121.94 2024-11-06T02:52:52Z- LcmTrn::publishEstimatesDecoder() - COVAR: 4.23 6.46 0.79 0.00 2024-11-06T02:52:52Z- handleDepth()- Depth_Keller msg: 1730861572.07 epoch sec; seqNo:527; depth 36.41 2024-11-06T02:52:52Z- handleDVL() - DVL msg epochMillisec: 1730861572.08 2024-11-06T02:52:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:52:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:52:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:52:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:52:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:52:52Z- handleDvl() - RDI_Pathfinder msg: 1730861572.08 sec; seqNo:168; #beams:0 2024-11-06T02:52:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:52:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.06 , 0.06 2024-11-06T02:52:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861572.10 epoch sec; seqNo:527; 4061517.27 north; 594478.86 east; 36.69 lat; -121.94 lon; 36.41 depth; utmZone: 10 2024-11-06T02:52:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861571.68 2024-11-06T02:52:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:52Z- waiting for 1730861575.68, time is 1730861572.10 2024-11-06T02:52:52Z- AHRS_M2 msg: 1730861572.45 epoch sec; seqNo:484 2024-11-06T02:52:52Z- AHRS_M2 msg: 0.13 phi; -0.22 theta; 5.33 psi 2024-11-06T02:52:52Z- handleDepth()- Depth_Keller msg: 1730861572.47 epoch sec; seqNo:528; depth 36.50 2024-11-06T02:52:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861572.47 epoch sec; seqNo:528; 4061517.65 north; 594478.85 east; 36.69 lat; -121.94 lon; 36.50 depth; utmZone: 10 2024-11-06T02:52:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861571.68 2024-11-06T02:52:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:52Z- waiting for 1730861575.68, time is 1730861572.47 2024-11-06T02:52:52Z- AHRS_M2 msg: 1730861572.86 epoch sec; seqNo:485 2024-11-06T02:52:52Z- AHRS_M2 msg: 0.13 phi; -0.22 theta; 5.33 psi 2024-11-06T02:52:52Z- handleDepth()- Depth_Keller msg: 1730861572.87 epoch sec; seqNo:529; depth 36.55 2024-11-06T02:52:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861572.88 epoch sec; seqNo:529; 4061517.65 north; 594478.85 east; 36.69 lat; -121.94 lon; 36.55 depth; utmZone: 10 2024-11-06T02:52:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861571.68 2024-11-06T02:52:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:53Z- waiting for 1730861575.68, time is 1730861572.88 2024-11-06T02:52:53Z- AHRS_M2 msg: 1730861573.30 epoch sec; seqNo:486 2024-11-06T02:52:53Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.33 psi 2024-11-06T02:52:53Z- handleDepth()- Depth_Keller msg: 1730861573.30 epoch sec; seqNo:530; depth 36.62 2024-11-06T02:52:53Z- handleDVL() - DVL msg epochMillisec: 1730861573.33 2024-11-06T02:52:53Z- handleDvl() - RDI_Pathfinder msg: 1730861573.33 sec; seqNo:169; #beams:4 2024-11-06T02:52:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 56.71 , 47.66 , 46.23 , 57.67 2024-11-06T02:52:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.07 , 0.00 2024-11-06T02:52:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861573.34 epoch sec; seqNo:530; 4061518.01 north; 594477.63 east; 36.69 lat; -121.94 lon; 36.62 depth; utmZone: 10 2024-11-06T02:52:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861571.68 2024-11-06T02:52:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:53Z- waiting for 1730861575.68, time is 1730861573.34 2024-11-06T02:52:53Z- AHRS_M2 msg: 1730861573.68 epoch sec; seqNo:487 2024-11-06T02:52:53Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.33 psi 2024-11-06T02:52:53Z- handleDepth()- Depth_Keller msg: 1730861573.68 epoch sec; seqNo:531; depth 36.70 2024-11-06T02:52:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861573.70 epoch sec; seqNo:531; 4061518.01 north; 594477.63 east; 36.69 lat; -121.94 lon; 36.70 depth; utmZone: 10 2024-11-06T02:52:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861571.68 2024-11-06T02:52:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:53Z- waiting for 1730861575.68, time is 1730861573.70 2024-11-06T02:52:54Z- AHRS_M2 msg: 1730861574.07 epoch sec; seqNo:488 2024-11-06T02:52:54Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.33 psi 2024-11-06T02:52:54Z- handleDepth()- Depth_Keller msg: 1730861574.08 epoch sec; seqNo:532; depth 36.76 2024-11-06T02:52:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861574.09 epoch sec; seqNo:532; 4061518.39 north; 594477.62 east; 36.69 lat; -121.94 lon; 36.76 depth; utmZone: 10 2024-11-06T02:52:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861571.68 2024-11-06T02:52:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:54Z- waiting for 1730861575.68, time is 1730861574.09 2024-11-06T02:52:54Z- AHRS_M2 msg: 1730861574.47 epoch sec; seqNo:489 2024-11-06T02:52:54Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.33 psi 2024-11-06T02:52:54Z- handleDepth()- Depth_Keller msg: 1730861574.49 epoch sec; seqNo:533; depth 36.82 2024-11-06T02:52:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861574.50 epoch sec; seqNo:533; 4061518.39 north; 594477.62 east; 36.69 lat; -121.94 lon; 36.82 depth; utmZone: 10 2024-11-06T02:52:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861571.68 2024-11-06T02:52:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:54Z- waiting for 1730861575.68, time is 1730861574.50 2024-11-06T02:52:54Z- AHRS_M2 msg: 1730861574.88 epoch sec; seqNo:490 2024-11-06T02:52:54Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.33 psi 2024-11-06T02:52:54Z- handleDepth()- Depth_Keller msg: 1730861574.89 epoch sec; seqNo:534; depth 36.90 2024-11-06T02:52:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861574.90 epoch sec; seqNo:534; 4061518.76 north; 594476.40 east; 36.69 lat; -121.94 lon; 36.90 depth; utmZone: 10 2024-11-06T02:52:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861571.68 2024-11-06T02:52:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:55Z- waiting for 1730861575.68, time is 1730861574.90 2024-11-06T02:52:55Z- AHRS_M2 msg: 1730861575.30 epoch sec; seqNo:491 2024-11-06T02:52:55Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.33 psi 2024-11-06T02:52:55Z- handleDepth()- Depth_Keller msg: 1730861575.31 epoch sec; seqNo:535; depth 36.98 2024-11-06T02:52:55Z- handleDVL() - DVL msg epochMillisec: 1730861575.33 2024-11-06T02:52:55Z- handleDvl() - RDI_Pathfinder msg: 1730861575.33 sec; seqNo:170; #beams:4 2024-11-06T02:52:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 56.47 , 47.18 , 46.22 , 57.66 2024-11-06T02:52:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.07 , -0.01 2024-11-06T02:52:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861575.34 epoch sec; seqNo:535; 4061518.76 north; 594476.40 east; 36.69 lat; -121.94 lon; 36.98 depth; utmZone: 10 2024-11-06T02:52:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861571.68 2024-11-06T02:52:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:55Z- waiting for 1730861575.68, time is 1730861575.34 2024-11-06T02:52:55Z- AHRS_M2 msg: 1730861575.68 epoch sec; seqNo:492 2024-11-06T02:52:55Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.33 psi 2024-11-06T02:52:55Z- handleDepth()- Depth_Keller msg: 1730861575.69 epoch sec; seqNo:536; depth 37.07 2024-11-06T02:52:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861575.71 epoch sec; seqNo:536; 4061519.13 north; 594476.40 east; 36.69 lat; -121.94 lon; 37.07 depth; utmZone: 10 2024-11-06T02:52:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861571.68 2024-11-06T02:52:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:55Z- waiting for 1730861575.68, time is 1730861575.71 2024-11-06T02:52:55Z- LcmTrn::updateTrn() >>>> pose time 1730861575.71, meas time 1730861575.33 <<<< 2024-11-06T02:52:55Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861575.71 2024-11-06T02:52:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861575.33 sec, ping # 0; waiting for INS data more recent than 1730861571.68... 2024-11-06T02:52:55Z- TerrainNav::motionUpdate() - currEstimate = (4061519, 594476.40, 37.07) 2024-11-06T02:52:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:52:55Z- TerrainNav:: SubcloudNIS is 0.45, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:52:55Z- TNavPF::Projecting Measurements 2024-11-06T02:52:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:52:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:52:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:52:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:52:55Z- TNavFilter::4 good beams. 2024-11-06T02:52:55Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:52:55Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:52:55Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:52:55Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:52:55Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:52:55Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:52:55Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:52:55Z- TerrainMapOctree: (vehN, vehE ) = (4061518.96, 594476.63). 2024-11-06T02:52:55Z- TerrainMapOctree: Min Distance = 323.64. 2024-11-06T02:52:55Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:52:55Z- Forcing Subcloud Comparison 2024-11-06T02:52:55Z- Weighting particles with subcloud comparison 2024-11-06T02:52:55Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:55Z- meanExpectedMeasDiff: 0.372997 delta_rms_squared: 1.806921 alpha: 0.989053 2024-11-06T02:52:55Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:55Z- meanExpectedMeasDiff: 0.613222 delta_rms_squared: 1.180931 alpha: 0.983346 2024-11-06T02:52:56Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:56Z- meanExpectedMeasDiff: 2.680043 delta_rms_squared: 1.278743 alpha: 0.984600 2024-11-06T02:52:56Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:52:56Z- meanExpectedMeasDiff: -1.554665 delta_rms_squared: 2.873782 alpha: 0.993089 2024-11-06T02:52:56Z- Subcloud NIS: 0.450461 2024-11-06T02:52:56Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:52:56Z- TNavPF:: sumWeights = 0.288467 2024-11-06T02:52:56Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:52:56Z- TNavPF::First Term of Map Covariance Matrix is 0.27 2024-11-06T02:52:56Z- TNavPF::Calculated NIS Value : 0.14 numBeams normalized NIS: 0.03 2024-11-06T02:52:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861575.33 sec. 2024-11-06T02:52:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:52:56Z- variances([0], [2], and psi_berg) = 4.654296 7.700758 0.000000 2024-11-06T02:52:56Z- publishEstimatesDecoder() - at 1730861575.71 sec 2024-11-06T02:52:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:52:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061557.09 594465.08 44.08 2024-11-06T02:52:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061553.21 594462.23 44.36 lat/lon: 36.69 -121.94 2024-11-06T02:52:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 4.65 7.70 0.80 0.00 2024-11-06T02:52:56Z- AHRS_M2 msg: 1730861576.09 epoch sec; seqNo:493 2024-11-06T02:52:56Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.33 psi 2024-11-06T02:52:56Z- handleDepth()- Depth_Keller msg: 1730861576.09 epoch sec; seqNo:537; depth 37.14 2024-11-06T02:52:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861576.12 epoch sec; seqNo:537; 4061519.51 north; 594476.39 east; 36.69 lat; -121.94 lon; 37.14 depth; utmZone: 10 2024-11-06T02:52:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861575.71 2024-11-06T02:52:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:56Z- waiting for 1730861579.71, time is 1730861576.12 2024-11-06T02:52:56Z- AHRS_M2 msg: 1730861576.50 epoch sec; seqNo:494 2024-11-06T02:52:56Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.33 psi 2024-11-06T02:52:56Z- handleDepth()- Depth_Keller msg: 1730861576.51 epoch sec; seqNo:538; depth 37.22 2024-11-06T02:52:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861576.52 epoch sec; seqNo:538; 4061519.50 north; 594475.17 east; 36.69 lat; -121.94 lon; 37.22 depth; utmZone: 10 2024-11-06T02:52:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861575.71 2024-11-06T02:52:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:56Z- waiting for 1730861579.71, time is 1730861576.52 2024-11-06T02:52:56Z- AHRS_M2 msg: 1730861576.89 epoch sec; seqNo:495 2024-11-06T02:52:56Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.33 psi 2024-11-06T02:52:56Z- handleDepth()- Depth_Keller msg: 1730861576.92 epoch sec; seqNo:539; depth 37.31 2024-11-06T02:52:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861576.95 epoch sec; seqNo:539; 4061519.88 north; 594475.17 east; 36.69 lat; -121.94 lon; 37.31 depth; utmZone: 10 2024-11-06T02:52:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861575.71 2024-11-06T02:52:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:57Z- waiting for 1730861579.71, time is 1730861576.95 2024-11-06T02:52:57Z- AHRS_M2 msg: 1730861577.31 epoch sec; seqNo:496 2024-11-06T02:52:57Z- AHRS_M2 msg: 0.15 phi; -0.23 theta; 5.33 psi 2024-11-06T02:52:57Z- handleDepth()- Depth_Keller msg: 1730861577.32 epoch sec; seqNo:540; depth 37.39 2024-11-06T02:52:57Z- handleDVL() - DVL msg epochMillisec: 1730861577.35 2024-11-06T02:52:57Z- handleDvl() - RDI_Pathfinder msg: 1730861577.35 sec; seqNo:171; #beams:4 2024-11-06T02:52:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 56.47 , 46.70 , 45.75 , 57.42 2024-11-06T02:52:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.06 , 0.04 2024-11-06T02:52:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861577.37 epoch sec; seqNo:540; 4061519.88 north; 594475.17 east; 36.69 lat; -121.94 lon; 37.39 depth; utmZone: 10 2024-11-06T02:52:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861575.71 2024-11-06T02:52:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:57Z- waiting for 1730861579.71, time is 1730861577.37 2024-11-06T02:52:57Z- AHRS_M2 msg: 1730861577.70 epoch sec; seqNo:497 2024-11-06T02:52:57Z- AHRS_M2 msg: 0.15 phi; -0.23 theta; 5.33 psi 2024-11-06T02:52:57Z- handleDepth()- Depth_Keller msg: 1730861577.71 epoch sec; seqNo:541; depth 37.47 2024-11-06T02:52:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861577.72 epoch sec; seqNo:541; 4061520.24 north; 594473.94 east; 36.69 lat; -121.94 lon; 37.47 depth; utmZone: 10 2024-11-06T02:52:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861575.71 2024-11-06T02:52:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:57Z- waiting for 1730861579.71, time is 1730861577.72 2024-11-06T02:52:58Z- AHRS_M2 msg: 1730861578.11 epoch sec; seqNo:498 2024-11-06T02:52:58Z- AHRS_M2 msg: 0.15 phi; -0.23 theta; 5.33 psi 2024-11-06T02:52:58Z- handleDepth()- Depth_Keller msg: 1730861578.11 epoch sec; seqNo:542; depth 37.56 2024-11-06T02:52:58Z- handleDVL() - DVL msg epochMillisec: 1730861578.12 2024-11-06T02:52:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:52:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:52:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:52:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:52:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:52:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:52:58Z- handleDvl() - RDI_Pathfinder msg: 1730861578.12 sec; seqNo:172; #beams:0 2024-11-06T02:52:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:52:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.06 , 0.04 2024-11-06T02:52:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861578.12 epoch sec; seqNo:542; 4061520.24 north; 594473.94 east; 36.69 lat; -121.94 lon; 37.56 depth; utmZone: 10 2024-11-06T02:52:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861575.71 2024-11-06T02:52:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:58Z- waiting for 1730861579.71, time is 1730861578.12 2024-11-06T02:52:58Z- AHRS_M2 msg: 1730861578.51 epoch sec; seqNo:499 2024-11-06T02:52:58Z- AHRS_M2 msg: 0.15 phi; -0.23 theta; 5.33 psi 2024-11-06T02:52:58Z- handleDepth()- Depth_Keller msg: 1730861578.54 epoch sec; seqNo:543; depth 37.62 2024-11-06T02:52:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861578.55 epoch sec; seqNo:543; 4061520.62 north; 594473.94 east; 36.69 lat; -121.94 lon; 37.62 depth; utmZone: 10 2024-11-06T02:52:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861575.71 2024-11-06T02:52:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:58Z- waiting for 1730861579.71, time is 1730861578.55 2024-11-06T02:52:58Z- AHRS_M2 msg: 1730861578.92 epoch sec; seqNo:500 2024-11-06T02:52:58Z- AHRS_M2 msg: 0.15 phi; -0.23 theta; 5.33 psi 2024-11-06T02:52:58Z- handleDepth()- Depth_Keller msg: 1730861578.92 epoch sec; seqNo:544; depth 37.70 2024-11-06T02:52:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861578.93 epoch sec; seqNo:544; 4061520.62 north; 594473.94 east; 36.69 lat; -121.94 lon; 37.70 depth; utmZone: 10 2024-11-06T02:52:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861575.71 2024-11-06T02:52:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:59Z- waiting for 1730861579.71, time is 1730861578.93 2024-11-06T02:52:59Z- AHRS_M2 msg: 1730861579.32 epoch sec; seqNo:501 2024-11-06T02:52:59Z- AHRS_M2 msg: 0.15 phi; -0.23 theta; 5.33 psi 2024-11-06T02:52:59Z- handleDepth()- Depth_Keller msg: 1730861579.33 epoch sec; seqNo:545; depth 37.81 2024-11-06T02:52:59Z- handleDVL() - DVL msg epochMillisec: 1730861579.35 2024-11-06T02:52:59Z- handleDvl() - RDI_Pathfinder msg: 1730861579.35 sec; seqNo:173; #beams:4 2024-11-06T02:52:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 56.23 , 46.46 , 45.51 , 57.18 2024-11-06T02:52:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.04 , 0.05 2024-11-06T02:52:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861579.37 epoch sec; seqNo:545; 4061520.99 north; 594472.71 east; 36.69 lat; -121.94 lon; 37.81 depth; utmZone: 10 2024-11-06T02:52:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:52:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861575.71 2024-11-06T02:52:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:59Z- waiting for 1730861579.71, time is 1730861579.37 2024-11-06T02:52:59Z- AHRS_M2 msg: 1730861579.73 epoch sec; seqNo:502 2024-11-06T02:52:59Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.33 psi 2024-11-06T02:52:59Z- handleDepth()- Depth_Keller msg: 1730861579.73 epoch sec; seqNo:546; depth 37.90 2024-11-06T02:52:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861579.75 epoch sec; seqNo:546; 4061520.99 north; 594472.71 east; 36.69 lat; -121.94 lon; 37.90 depth; utmZone: 10 2024-11-06T02:52:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:52:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861575.71 2024-11-06T02:52:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:52:59Z- waiting for 1730861579.71, time is 1730861579.75 2024-11-06T02:52:59Z- LcmTrn::updateTrn() >>>> pose time 1730861579.75, meas time 1730861579.35 <<<< 2024-11-06T02:52:59Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861579.75 2024-11-06T02:52:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861579.35 sec, ping # 0; waiting for INS data more recent than 1730861575.71... 2024-11-06T02:52:59Z- TerrainNav::motionUpdate() - currEstimate = (4061521, 594472.71, 37.90) 2024-11-06T02:52:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:52:59Z- TerrainNav:: SubcloudNIS is 0.45, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:52:59Z- TNavPF::Projecting Measurements 2024-11-06T02:52:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:52:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:52:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:52:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:52:59Z- TNavFilter::4 good beams. 2024-11-06T02:52:59Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:52:59Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:52:59Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:52:59Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:52:59Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:52:59Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:52:59Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:52:59Z- TerrainMapOctree: (vehN, vehE ) = (4061520.80, 594473.08). 2024-11-06T02:52:59Z- TerrainMapOctree: Min Distance = 319.84. 2024-11-06T02:52:59Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:53:00Z- Forcing Subcloud Comparison 2024-11-06T02:53:00Z- Weighting particles with subcloud comparison 2024-11-06T02:53:00Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:00Z- meanExpectedMeasDiff: 0.462206 delta_rms_squared: 1.910035 alpha: 0.989637 2024-11-06T02:53:00Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:00Z- meanExpectedMeasDiff: 0.473620 delta_rms_squared: 1.278315 alpha: 0.984595 2024-11-06T02:53:00Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:00Z- meanExpectedMeasDiff: 2.766554 delta_rms_squared: 1.175557 alpha: 0.983271 2024-11-06T02:53:00Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:00Z- meanExpectedMeasDiff: -1.759530 delta_rms_squared: 2.441181 alpha: 0.991874 2024-11-06T02:53:00Z- Subcloud NIS: 0.432976 2024-11-06T02:53:00Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:53:00Z- TNavPF:: sumWeights = 0.264970 2024-11-06T02:53:00Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:53:00Z- TNavPF::First Term of Map Covariance Matrix is 0.27 2024-11-06T02:53:00Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T02:53:00Z- TerrainNav::Resampling particle filter... 2024-11-06T02:53:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861579.35 sec. 2024-11-06T02:53:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:53:00Z- variances([0], [2], and psi_berg) = 5.205105 9.503702 0.000000 2024-11-06T02:53:00Z- publishEstimatesDecoder() - at 1730861579.75 sec 2024-11-06T02:53:00Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:53:00Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061559.13 594465.55 45.31 2024-11-06T02:53:00Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061555.44 594459.08 44.95 lat/lon: 36.69 -121.94 2024-11-06T02:53:00Z- LcmTrn::publishEstimatesDecoder() - COVAR: 5.21 9.50 0.79 0.00 2024-11-06T02:53:00Z- AHRS_M2 msg: 1730861580.14 epoch sec; seqNo:503 2024-11-06T02:53:00Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.33 psi 2024-11-06T02:53:00Z- handleDepth()- Depth_Keller msg: 1730861580.17 epoch sec; seqNo:547; depth 37.95 2024-11-06T02:53:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861580.18 epoch sec; seqNo:547; 4061521.37 north; 594472.71 east; 36.69 lat; -121.94 lon; 37.95 depth; utmZone: 10 2024-11-06T02:53:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861579.75 2024-11-06T02:53:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:00Z- waiting for 1730861583.75, time is 1730861580.18 2024-11-06T02:53:00Z- AHRS_M2 msg: 1730861580.53 epoch sec; seqNo:504 2024-11-06T02:53:00Z- AHRS_M2 msg: 0.15 phi; -0.23 theta; 5.33 psi 2024-11-06T02:53:00Z- handleDepth()- Depth_Keller msg: 1730861580.54 epoch sec; seqNo:548; depth 38.05 2024-11-06T02:53:00Z- handleDVL() - DVL msg epochMillisec: 1730861580.55 2024-11-06T02:53:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:53:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:53:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:53:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:53:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:53:00Z- handleDvl() - RDI_Pathfinder msg: 1730861580.55 sec; seqNo:174; #beams:0 2024-11-06T02:53:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:53:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.04 , 0.05 2024-11-06T02:53:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861580.58 epoch sec; seqNo:548; 4061521.37 north; 594472.71 east; 36.69 lat; -121.94 lon; 38.05 depth; utmZone: 10 2024-11-06T02:53:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861579.75 2024-11-06T02:53:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:00Z- waiting for 1730861583.75, time is 1730861580.58 2024-11-06T02:53:00Z- AHRS_M2 msg: 1730861580.94 epoch sec; seqNo:505 2024-11-06T02:53:00Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.33 psi 2024-11-06T02:53:00Z- handleDepth()- Depth_Keller msg: 1730861580.94 epoch sec; seqNo:549; depth 38.12 2024-11-06T02:53:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861580.95 epoch sec; seqNo:549; 4061521.73 north; 594471.48 east; 36.69 lat; -121.94 lon; 38.12 depth; utmZone: 10 2024-11-06T02:53:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861579.75 2024-11-06T02:53:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:01Z- waiting for 1730861583.75, time is 1730861580.95 2024-11-06T02:53:01Z- AHRS_M2 msg: 1730861581.34 epoch sec; seqNo:506 2024-11-06T02:53:01Z- AHRS_M2 msg: 0.15 phi; -0.23 theta; 5.33 psi 2024-11-06T02:53:01Z- handleDepth()- Depth_Keller msg: 1730861581.35 epoch sec; seqNo:550; depth 38.20 2024-11-06T02:53:01Z- handleDVL() - DVL msg epochMillisec: 1730861581.37 2024-11-06T02:53:01Z- handleDvl() - RDI_Pathfinder msg: 1730861581.37 sec; seqNo:175; #beams:4 2024-11-06T02:53:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 56.23 , 46.22 , 45.27 , 56.47 2024-11-06T02:53:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , 0.01 2024-11-06T02:53:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861581.39 epoch sec; seqNo:550; 4061521.73 north; 594471.48 east; 36.69 lat; -121.94 lon; 38.20 depth; utmZone: 10 2024-11-06T02:53:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861579.75 2024-11-06T02:53:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:01Z- waiting for 1730861583.75, time is 1730861581.39 2024-11-06T02:53:01Z- AHRS_M2 msg: 1730861581.74 epoch sec; seqNo:507 2024-11-06T02:53:01Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.33 psi 2024-11-06T02:53:01Z- handleDepth()- Depth_Keller msg: 1730861581.75 epoch sec; seqNo:551; depth 38.29 2024-11-06T02:53:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861581.77 epoch sec; seqNo:551; 4061522.11 north; 594471.48 east; 36.69 lat; -121.94 lon; 38.29 depth; utmZone: 10 2024-11-06T02:53:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861579.75 2024-11-06T02:53:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:01Z- waiting for 1730861583.75, time is 1730861581.77 2024-11-06T02:53:02Z- AHRS_M2 msg: 1730861582.15 epoch sec; seqNo:508 2024-11-06T02:53:02Z- AHRS_M2 msg: 0.15 phi; -0.23 theta; 5.33 psi 2024-11-06T02:53:02Z- handleDepth()- Depth_Keller msg: 1730861582.15 epoch sec; seqNo:552; depth 38.36 2024-11-06T02:53:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861582.16 epoch sec; seqNo:552; 4061522.11 north; 594471.48 east; 36.69 lat; -121.94 lon; 38.36 depth; utmZone: 10 2024-11-06T02:53:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861579.75 2024-11-06T02:53:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:02Z- waiting for 1730861583.75, time is 1730861582.16 2024-11-06T02:53:02Z- AHRS_M2 msg: 1730861582.55 epoch sec; seqNo:509 2024-11-06T02:53:02Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.33 psi 2024-11-06T02:53:02Z- handleDepth()- Depth_Keller msg: 1730861582.56 epoch sec; seqNo:553; depth 38.44 2024-11-06T02:53:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861582.57 epoch sec; seqNo:553; 4061522.48 north; 594470.26 east; 36.69 lat; -121.94 lon; 38.44 depth; utmZone: 10 2024-11-06T02:53:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861579.75 2024-11-06T02:53:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:02Z- waiting for 1730861583.75, time is 1730861582.57 2024-11-06T02:53:02Z- AHRS_M2 msg: 1730861582.96 epoch sec; seqNo:510 2024-11-06T02:53:02Z- AHRS_M2 msg: 0.15 phi; -0.23 theta; 5.34 psi 2024-11-06T02:53:02Z- handleDepth()- Depth_Keller msg: 1730861582.96 epoch sec; seqNo:554; depth 38.50 2024-11-06T02:53:02Z- handleDVL() - DVL msg epochMillisec: 1730861582.97 2024-11-06T02:53:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:53:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:53:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:53:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:53:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:53:02Z- handleDvl() - RDI_Pathfinder msg: 1730861582.97 sec; seqNo:176; #beams:0 2024-11-06T02:53:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:53:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , 0.01 2024-11-06T02:53:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861582.99 epoch sec; seqNo:554; 4061522.86 north; 594470.25 east; 36.69 lat; -121.94 lon; 38.50 depth; utmZone: 10 2024-11-06T02:53:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861579.75 2024-11-06T02:53:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:03Z- waiting for 1730861583.75, time is 1730861582.99 2024-11-06T02:53:03Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T02:53:03Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T02:53:03Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T02:53:03Z- AHRS_M2 msg: 1730861583.35 epoch sec; seqNo:511 2024-11-06T02:53:03Z- AHRS_M2 msg: 0.15 phi; -0.23 theta; 5.34 psi 2024-11-06T02:53:03Z- handleDepth()- Depth_Keller msg: 1730861583.38 epoch sec; seqNo:555; depth 38.58 2024-11-06T02:53:03Z- handleDVL() - DVL msg epochMillisec: 1730861583.41 2024-11-06T02:53:03Z- handleDvl() - RDI_Pathfinder msg: 1730861583.41 sec; seqNo:177; #beams:4 2024-11-06T02:53:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 55.27 , 45.74 , 45.02 , 55.98 2024-11-06T02:53:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.02 , -0.03 2024-11-06T02:53:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861583.42 epoch sec; seqNo:555; 4061522.86 north; 594470.25 east; 36.69 lat; -121.94 lon; 38.58 depth; utmZone: 10 2024-11-06T02:53:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861579.75 2024-11-06T02:53:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:03Z- waiting for 1730861583.75, time is 1730861583.42 2024-11-06T02:53:03Z- AHRS_M2 msg: 1730861583.77 epoch sec; seqNo:512 2024-11-06T02:53:03Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.34 psi 2024-11-06T02:53:03Z- handleDepth()- Depth_Keller msg: 1730861583.78 epoch sec; seqNo:556; depth 38.66 2024-11-06T02:53:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861583.80 epoch sec; seqNo:556; 4061523.23 north; 594470.25 east; 36.69 lat; -121.94 lon; 38.66 depth; utmZone: 10 2024-11-06T02:53:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861579.75 2024-11-06T02:53:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:03Z- waiting for 1730861583.75, time is 1730861583.80 2024-11-06T02:53:03Z- LcmTrn::updateTrn() >>>> pose time 1730861583.80, meas time 1730861583.41 <<<< 2024-11-06T02:53:03Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861583.80 2024-11-06T02:53:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861583.41 sec, ping # 0; waiting for INS data more recent than 1730861579.75... 2024-11-06T02:53:03Z- TerrainNav::motionUpdate() - currEstimate = (4061523, 594470.25, 38.66) 2024-11-06T02:53:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:53:03Z- TerrainNav:: SubcloudNIS is 0.43, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:53:03Z- TNavPF::Projecting Measurements 2024-11-06T02:53:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:53:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:53:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:53:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:53:03Z- TNavFilter::4 good beams. 2024-11-06T02:53:03Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:53:03Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:53:03Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:53:03Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:53:03Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:53:03Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:53:03Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:53:03Z- TerrainMapOctree: (vehN, vehE ) = (4061523.02, 594470.49). 2024-11-06T02:53:03Z- TerrainMapOctree: Min Distance = 316.44. 2024-11-06T02:53:03Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:53:04Z- Forcing Subcloud Comparison 2024-11-06T02:53:04Z- Weighting particles with subcloud comparison 2024-11-06T02:53:04Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:04Z- meanExpectedMeasDiff: 0.562644 delta_rms_squared: 1.941863 alpha: 0.989806 2024-11-06T02:53:04Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:04Z- meanExpectedMeasDiff: 0.380368 delta_rms_squared: 1.275752 alpha: 0.984565 2024-11-06T02:53:04Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:04Z- meanExpectedMeasDiff: 3.802585 delta_rms_squared: 1.137497 alpha: 0.982721 2024-11-06T02:53:04Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:04Z- meanExpectedMeasDiff: -0.752526 delta_rms_squared: 2.028710 alpha: 0.990238 2024-11-06T02:53:04Z- Subcloud NIS: 0.421318 2024-11-06T02:53:04Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:53:04Z- TNavPF:: sumWeights = 0.181437 2024-11-06T02:53:04Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:53:04Z- TNavPF::First Term of Map Covariance Matrix is 0.19 2024-11-06T02:53:04Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T02:53:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861583.41 sec. 2024-11-06T02:53:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:53:04Z- variances([0], [2], and psi_berg) = 5.119979 10.836583 0.000000 2024-11-06T02:53:04Z- publishEstimatesDecoder() - at 1730861583.80 sec 2024-11-06T02:53:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:53:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061555.08 594467.81 44.82 2024-11-06T02:53:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061557.72 594456.81 45.30 lat/lon: 36.69 -121.94 2024-11-06T02:53:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 5.12 10.84 0.78 0.00 2024-11-06T02:53:04Z- AHRS_M2 msg: 1730861584.17 epoch sec; seqNo:513 2024-11-06T02:53:04Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.34 psi 2024-11-06T02:53:04Z- handleDepth()- Depth_Keller msg: 1730861584.21 epoch sec; seqNo:557; depth 38.73 2024-11-06T02:53:04Z- handleDVL() - DVL msg epochMillisec: 1730861584.21 2024-11-06T02:53:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:53:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:53:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:53:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:53:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:53:04Z- handleDvl() - RDI_Pathfinder msg: 1730861584.21 sec; seqNo:178; #beams:0 2024-11-06T02:53:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:53:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.02 , -0.03 2024-11-06T02:53:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861584.23 epoch sec; seqNo:557; 4061523.22 north; 594469.03 east; 36.69 lat; -121.94 lon; 38.73 depth; utmZone: 10 2024-11-06T02:53:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861583.80 2024-11-06T02:53:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:04Z- waiting for 1730861587.80, time is 1730861584.23 2024-11-06T02:53:04Z- AHRS_M2 msg: 1730861584.57 epoch sec; seqNo:514 2024-11-06T02:53:04Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.33 psi 2024-11-06T02:53:04Z- handleDepth()- Depth_Keller msg: 1730861584.58 epoch sec; seqNo:558; depth 38.82 2024-11-06T02:53:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861584.59 epoch sec; seqNo:558; 4061523.60 north; 594469.02 east; 36.69 lat; -121.94 lon; 38.82 depth; utmZone: 10 2024-11-06T02:53:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861583.80 2024-11-06T02:53:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:04Z- waiting for 1730861587.80, time is 1730861584.59 2024-11-06T02:53:04Z- AHRS_M2 msg: 1730861584.98 epoch sec; seqNo:515 2024-11-06T02:53:04Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.33 psi 2024-11-06T02:53:04Z- handleDepth()- Depth_Keller msg: 1730861584.99 epoch sec; seqNo:559; depth 38.90 2024-11-06T02:53:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861585.00 epoch sec; seqNo:559; 4061523.60 north; 594469.02 east; 36.69 lat; -121.94 lon; 38.90 depth; utmZone: 10 2024-11-06T02:53:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861583.80 2024-11-06T02:53:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:05Z- waiting for 1730861587.80, time is 1730861585.00 2024-11-06T02:53:05Z- AHRS_M2 msg: 1730861585.38 epoch sec; seqNo:516 2024-11-06T02:53:05Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.33 psi 2024-11-06T02:53:05Z- handleDepth()- Depth_Keller msg: 1730861585.38 epoch sec; seqNo:560; depth 38.98 2024-11-06T02:53:05Z- handleDVL() - DVL msg epochMillisec: 1730861585.41 2024-11-06T02:53:05Z- handleDvl() - RDI_Pathfinder msg: 1730861585.41 sec; seqNo:179; #beams:4 2024-11-06T02:53:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 55.03 , 45.26 , 44.79 , 55.74 2024-11-06T02:53:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.06 , -0.02 2024-11-06T02:53:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861585.42 epoch sec; seqNo:560; 4061523.98 north; 594469.02 east; 36.69 lat; -121.94 lon; 38.98 depth; utmZone: 10 2024-11-06T02:53:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861583.80 2024-11-06T02:53:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:05Z- waiting for 1730861587.80, time is 1730861585.42 2024-11-06T02:53:05Z- AHRS_M2 msg: 1730861585.78 epoch sec; seqNo:517 2024-11-06T02:53:05Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.33 psi 2024-11-06T02:53:05Z- handleDepth()- Depth_Keller msg: 1730861585.79 epoch sec; seqNo:561; depth 39.05 2024-11-06T02:53:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861585.81 epoch sec; seqNo:561; 4061523.97 north; 594467.80 east; 36.69 lat; -121.94 lon; 39.05 depth; utmZone: 10 2024-11-06T02:53:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861583.80 2024-11-06T02:53:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:05Z- waiting for 1730861587.80, time is 1730861585.81 2024-11-06T02:53:06Z- AHRS_M2 msg: 1730861586.19 epoch sec; seqNo:518 2024-11-06T02:53:06Z- AHRS_M2 msg: 0.14 phi; -0.21 theta; 5.33 psi 2024-11-06T02:53:06Z- handleDepth()- Depth_Keller msg: 1730861586.19 epoch sec; seqNo:562; depth 39.13 2024-11-06T02:53:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861586.20 epoch sec; seqNo:562; 4061524.34 north; 594467.79 east; 36.69 lat; -121.94 lon; 39.13 depth; utmZone: 10 2024-11-06T02:53:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861583.80 2024-11-06T02:53:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:06Z- waiting for 1730861587.80, time is 1730861586.20 2024-11-06T02:53:06Z- AHRS_M2 msg: 1730861586.59 epoch sec; seqNo:519 2024-11-06T02:53:06Z- AHRS_M2 msg: 0.14 phi; -0.21 theta; 5.33 psi 2024-11-06T02:53:06Z- handleDepth()- Depth_Keller msg: 1730861586.61 epoch sec; seqNo:563; depth 39.20 2024-11-06T02:53:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861586.62 epoch sec; seqNo:563; 4061524.72 north; 594467.79 east; 36.69 lat; -121.94 lon; 39.20 depth; utmZone: 10 2024-11-06T02:53:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861583.80 2024-11-06T02:53:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:06Z- waiting for 1730861587.80, time is 1730861586.62 2024-11-06T02:53:07Z- AHRS_M2 msg: 1730861586.99 epoch sec; seqNo:520 2024-11-06T02:53:07Z- AHRS_M2 msg: 0.14 phi; -0.21 theta; 5.33 psi 2024-11-06T02:53:07Z- handleDepth()- Depth_Keller msg: 1730861587.00 epoch sec; seqNo:564; depth 39.29 2024-11-06T02:53:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861587.01 epoch sec; seqNo:564; 4061524.72 north; 594467.79 east; 36.69 lat; -121.94 lon; 39.29 depth; utmZone: 10 2024-11-06T02:53:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861583.80 2024-11-06T02:53:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:07Z- waiting for 1730861587.80, time is 1730861587.01 2024-11-06T02:53:07Z- AHRS_M2 msg: 1730861587.40 epoch sec; seqNo:521 2024-11-06T02:53:07Z- AHRS_M2 msg: 0.14 phi; -0.21 theta; 5.33 psi 2024-11-06T02:53:07Z- handleDepth()- Depth_Keller msg: 1730861587.41 epoch sec; seqNo:565; depth 39.37 2024-11-06T02:53:07Z- handleDVL() - DVL msg epochMillisec: 1730861587.42 2024-11-06T02:53:07Z- handleDvl() - RDI_Pathfinder msg: 1730861587.42 sec; seqNo:180; #beams:4 2024-11-06T02:53:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 54.55 , 44.55 , 44.79 , 54.79 2024-11-06T02:53:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.08 , 0.01 2024-11-06T02:53:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861587.45 epoch sec; seqNo:565; 4061525.09 north; 594466.57 east; 36.69 lat; -121.94 lon; 39.37 depth; utmZone: 10 2024-11-06T02:53:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861583.80 2024-11-06T02:53:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:07Z- waiting for 1730861587.80, time is 1730861587.45 2024-11-06T02:53:07Z- handleDepth()- Depth_Keller msg: 1730861587.83 epoch sec; seqNo:566; depth 39.45 2024-11-06T02:53:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861587.84 epoch sec; seqNo:566; 4061525.09 north; 594466.57 east; 36.69 lat; -121.94 lon; 39.45 depth; utmZone: 10 2024-11-06T02:53:07Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:53:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861583.80 2024-11-06T02:53:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:07Z- waiting for 1730861587.80, time is 1730861587.84 2024-11-06T02:53:07Z- LcmTrn::updateTrn() >>>> pose time 1730861587.84, meas time 1730861587.42 <<<< 2024-11-06T02:53:07Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861587.84 2024-11-06T02:53:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861587.42 sec, ping # 0; waiting for INS data more recent than 1730861583.80... 2024-11-06T02:53:07Z- TerrainNav::motionUpdate() - currEstimate = (4061525, 594466.57, 39.45) 2024-11-06T02:53:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:53:07Z- TerrainNav:: SubcloudNIS is 0.42, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:53:08Z- TNavPF::Projecting Measurements 2024-11-06T02:53:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:53:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:53:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:53:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:53:08Z- TNavFilter::4 good beams. 2024-11-06T02:53:08Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:53:08Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:53:08Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:53:08Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:53:08Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:53:08Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:53:08Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:53:08Z- TerrainMapOctree: (vehN, vehE ) = (4061524.90, 594466.94). 2024-11-06T02:53:08Z- TerrainMapOctree: Min Distance = 312.63. 2024-11-06T02:53:08Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:53:08Z- Forcing Subcloud Comparison 2024-11-06T02:53:08Z- Weighting particles with subcloud comparison 2024-11-06T02:53:08Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:08Z- meanExpectedMeasDiff: 0.560832 delta_rms_squared: 2.037026 alpha: 0.990277 2024-11-06T02:53:08Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:08Z- meanExpectedMeasDiff: -0.217149 delta_rms_squared: 1.217682 alpha: 0.983841 2024-11-06T02:53:08Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:08Z- meanExpectedMeasDiff: 5.156504 delta_rms_squared: 1.105994 alpha: 0.982238 2024-11-06T02:53:08Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:08Z- meanExpectedMeasDiff: -0.587466 delta_rms_squared: 1.868897 alpha: 0.989412 2024-11-06T02:53:08Z- Subcloud NIS: 0.417854 2024-11-06T02:53:08Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:53:08Z- TNavPF:: sumWeights = 0.064487 2024-11-06T02:53:08Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:53:08Z- TNavPF::First Term of Map Covariance Matrix is 0.07 2024-11-06T02:53:08Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T02:53:08Z- TerrainNav::Resampling particle filter... 2024-11-06T02:53:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861587.42 sec. 2024-11-06T02:53:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:53:08Z- variances([0], [2], and psi_berg) = 4.911738 18.237540 0.000000 2024-11-06T02:53:08Z- publishEstimatesDecoder() - at 1730861587.84 sec 2024-11-06T02:53:08Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:53:08Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061563.27 594459.44 46.04 2024-11-06T02:53:08Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061559.16 594453.71 45.70 lat/lon: 36.69 -121.94 2024-11-06T02:53:08Z- LcmTrn::publishEstimatesDecoder() - COVAR: 4.91 18.24 0.76 0.00 2024-11-06T02:53:08Z- AHRS_M2 msg: 1730861588.21 epoch sec; seqNo:522 2024-11-06T02:53:08Z- AHRS_M2 msg: 0.14 phi; -0.20 theta; 5.33 psi 2024-11-06T02:53:08Z- handleDepth()- Depth_Keller msg: 1730861588.21 epoch sec; seqNo:567; depth 39.52 2024-11-06T02:53:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861588.25 epoch sec; seqNo:567; 4061525.47 north; 594466.56 east; 36.69 lat; -121.94 lon; 39.52 depth; utmZone: 10 2024-11-06T02:53:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861587.84 2024-11-06T02:53:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:08Z- waiting for 1730861591.84, time is 1730861588.25 2024-11-06T02:53:08Z- AHRS_M2 msg: 1730861588.62 epoch sec; seqNo:523 2024-11-06T02:53:08Z- AHRS_M2 msg: 0.15 phi; -0.20 theta; 5.33 psi 2024-11-06T02:53:08Z- handleDepth()- Depth_Keller msg: 1730861588.63 epoch sec; seqNo:568; depth 39.61 2024-11-06T02:53:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861588.67 epoch sec; seqNo:568; 4061525.45 north; 594465.34 east; 36.69 lat; -121.94 lon; 39.61 depth; utmZone: 10 2024-11-06T02:53:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861587.84 2024-11-06T02:53:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:08Z- waiting for 1730861591.84, time is 1730861588.67 2024-11-06T02:53:09Z- AHRS_M2 msg: 1730861589.02 epoch sec; seqNo:524 2024-11-06T02:53:09Z- AHRS_M2 msg: 0.15 phi; -0.20 theta; 5.33 psi 2024-11-06T02:53:09Z- handleDepth()- Depth_Keller msg: 1730861589.02 epoch sec; seqNo:569; depth 39.69 2024-11-06T02:53:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861589.03 epoch sec; seqNo:569; 4061525.83 north; 594465.34 east; 36.69 lat; -121.94 lon; 39.69 depth; utmZone: 10 2024-11-06T02:53:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861587.84 2024-11-06T02:53:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:09Z- waiting for 1730861591.84, time is 1730861589.03 2024-11-06T02:53:09Z- AHRS_M2 msg: 1730861589.42 epoch sec; seqNo:525 2024-11-06T02:53:09Z- AHRS_M2 msg: 0.15 phi; -0.20 theta; 5.34 psi 2024-11-06T02:53:09Z- handleDepth()- Depth_Keller msg: 1730861589.43 epoch sec; seqNo:570; depth 39.76 2024-11-06T02:53:09Z- handleDVL() - DVL msg epochMillisec: 1730861589.45 2024-11-06T02:53:09Z- handleDvl() - RDI_Pathfinder msg: 1730861589.45 sec; seqNo:181; #beams:4 2024-11-06T02:53:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 54.31 , 44.07 , 44.78 , 53.83 2024-11-06T02:53:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.07 , 0.03 2024-11-06T02:53:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861589.46 epoch sec; seqNo:570; 4061525.83 north; 594465.34 east; 36.69 lat; -121.94 lon; 39.76 depth; utmZone: 10 2024-11-06T02:53:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861587.84 2024-11-06T02:53:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:09Z- waiting for 1730861591.84, time is 1730861589.46 2024-11-06T02:53:09Z- AHRS_M2 msg: 1730861589.82 epoch sec; seqNo:526 2024-11-06T02:53:09Z- AHRS_M2 msg: 0.15 phi; -0.20 theta; 5.34 psi 2024-11-06T02:53:09Z- handleDepth()- Depth_Keller msg: 1730861589.83 epoch sec; seqNo:571; depth 39.84 2024-11-06T02:53:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861589.85 epoch sec; seqNo:571; 4061526.21 north; 594465.33 east; 36.69 lat; -121.94 lon; 39.84 depth; utmZone: 10 2024-11-06T02:53:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861587.84 2024-11-06T02:53:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:09Z- waiting for 1730861591.84, time is 1730861589.85 2024-11-06T02:53:10Z- AHRS_M2 msg: 1730861590.23 epoch sec; seqNo:527 2024-11-06T02:53:10Z- AHRS_M2 msg: 0.15 phi; -0.19 theta; 5.34 psi 2024-11-06T02:53:10Z- handleDepth()- Depth_Keller msg: 1730861590.23 epoch sec; seqNo:572; depth 39.93 2024-11-06T02:53:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861590.24 epoch sec; seqNo:572; 4061526.20 north; 594464.11 east; 36.69 lat; -121.94 lon; 39.93 depth; utmZone: 10 2024-11-06T02:53:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861587.84 2024-11-06T02:53:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:10Z- waiting for 1730861591.84, time is 1730861590.24 2024-11-06T02:53:10Z- AHRS_M2 msg: 1730861590.64 epoch sec; seqNo:528 2024-11-06T02:53:10Z- AHRS_M2 msg: 0.15 phi; -0.19 theta; 5.34 psi 2024-11-06T02:53:10Z- handleDepth()- Depth_Keller msg: 1730861590.64 epoch sec; seqNo:573; depth 40.00 2024-11-06T02:53:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861590.66 epoch sec; seqNo:573; 4061526.58 north; 594464.11 east; 36.69 lat; -121.94 lon; 40.00 depth; utmZone: 10 2024-11-06T02:53:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861587.84 2024-11-06T02:53:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:10Z- waiting for 1730861591.84, time is 1730861590.66 2024-11-06T02:53:11Z- AHRS_M2 msg: 1730861591.04 epoch sec; seqNo:529 2024-11-06T02:53:11Z- AHRS_M2 msg: 0.15 phi; -0.19 theta; 5.34 psi 2024-11-06T02:53:11Z- handleDepth()- Depth_Keller msg: 1730861591.04 epoch sec; seqNo:574; depth 40.07 2024-11-06T02:53:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861591.05 epoch sec; seqNo:574; 4061526.58 north; 594464.11 east; 36.69 lat; -121.94 lon; 40.07 depth; utmZone: 10 2024-11-06T02:53:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861587.84 2024-11-06T02:53:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:11Z- waiting for 1730861591.84, time is 1730861591.05 2024-11-06T02:53:11Z- AHRS_M2 msg: 1730861591.44 epoch sec; seqNo:530 2024-11-06T02:53:11Z- AHRS_M2 msg: 0.15 phi; -0.19 theta; 5.34 psi 2024-11-06T02:53:11Z- handleDepth()- Depth_Keller msg: 1730861591.45 epoch sec; seqNo:575; depth 40.15 2024-11-06T02:53:11Z- handleDVL() - DVL msg epochMillisec: 1730861591.47 2024-11-06T02:53:11Z- handleDvl() - RDI_Pathfinder msg: 1730861591.47 sec; seqNo:182; #beams:4 2024-11-06T02:53:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 54.07 , 43.59 , 44.78 , 53.36 2024-11-06T02:53:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.06 , 0.04 2024-11-06T02:53:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861591.49 epoch sec; seqNo:575; 4061526.94 north; 594462.88 east; 36.69 lat; -121.94 lon; 40.15 depth; utmZone: 10 2024-11-06T02:53:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861587.84 2024-11-06T02:53:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:11Z- waiting for 1730861591.84, time is 1730861591.49 2024-11-06T02:53:11Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T02:53:11Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T02:53:11Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T02:53:11Z- AHRS_M2 msg: 1730861591.84 epoch sec; seqNo:531 2024-11-06T02:53:11Z- AHRS_M2 msg: 0.15 phi; -0.19 theta; 5.34 psi 2024-11-06T02:53:11Z- handleDepth()- Depth_Keller msg: 1730861591.86 epoch sec; seqNo:576; depth 40.22 2024-11-06T02:53:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861591.88 epoch sec; seqNo:576; 4061526.94 north; 594462.88 east; 36.69 lat; -121.94 lon; 40.22 depth; utmZone: 10 2024-11-06T02:53:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861587.84 2024-11-06T02:53:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:12Z- waiting for 1730861591.84, time is 1730861591.88 2024-11-06T02:53:12Z- LcmTrn::updateTrn() >>>> pose time 1730861591.88, meas time 1730861591.47 <<<< 2024-11-06T02:53:12Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861591.88 2024-11-06T02:53:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861591.47 sec, ping # 0; waiting for INS data more recent than 1730861587.84... 2024-11-06T02:53:12Z- TerrainNav::motionUpdate() - currEstimate = (4061527, 594462.88, 40.22) 2024-11-06T02:53:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:53:12Z- TerrainNav:: SubcloudNIS is 0.42, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:53:12Z- TNavPF::Projecting Measurements 2024-11-06T02:53:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:53:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:53:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:53:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:53:12Z- TNavFilter::4 good beams. 2024-11-06T02:53:12Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:53:12Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:53:12Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:53:12Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:53:12Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:53:12Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:53:12Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:53:12Z- TerrainMapOctree: (vehN, vehE ) = (4061526.76, 594463.26). 2024-11-06T02:53:12Z- TerrainMapOctree: Min Distance = 308.73. 2024-11-06T02:53:12Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:53:12Z- Forcing Subcloud Comparison 2024-11-06T02:53:12Z- Weighting particles with subcloud comparison 2024-11-06T02:53:12Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:12Z- meanExpectedMeasDiff: 0.742628 delta_rms_squared: 2.040046 alpha: 0.990291 2024-11-06T02:53:12Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:12Z- meanExpectedMeasDiff: 0.229148 delta_rms_squared: 1.525095 alpha: 0.987056 2024-11-06T02:53:12Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:12Z- meanExpectedMeasDiff: 6.054016 delta_rms_squared: 1.768329 alpha: 0.988816 2024-11-06T02:53:12Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:12Z- meanExpectedMeasDiff: -0.668267 delta_rms_squared: 1.800432 alpha: 0.989014 2024-11-06T02:53:12Z- Subcloud NIS: 0.453273 2024-11-06T02:53:12Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:53:12Z- TNavPF:: sumWeights = 0.036473 2024-11-06T02:53:12Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:53:12Z- TNavPF::First Term of Map Covariance Matrix is 0.04 2024-11-06T02:53:12Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T02:53:12Z- TerrainNav::Resampling particle filter... 2024-11-06T02:53:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861591.47 sec. 2024-11-06T02:53:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:53:12Z- variances([0], [2], and psi_berg) = 2.177935 39.649668 0.000000 2024-11-06T02:53:12Z- publishEstimatesDecoder() - at 1730861591.88 sec 2024-11-06T02:53:12Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:53:12Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061565.16 594455.82 45.68 2024-11-06T02:53:12Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061559.75 594453.58 46.04 lat/lon: 36.70 -121.94 2024-11-06T02:53:12Z- LcmTrn::publishEstimatesDecoder() - COVAR: 2.18 39.65 0.84 0.00 2024-11-06T02:53:12Z- AHRS_M2 msg: 1730861592.26 epoch sec; seqNo:532 2024-11-06T02:53:12Z- AHRS_M2 msg: 0.14 phi; -0.19 theta; 5.34 psi 2024-11-06T02:53:12Z- handleDepth()- Depth_Keller msg: 1730861592.28 epoch sec; seqNo:577; depth 40.30 2024-11-06T02:53:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861592.30 epoch sec; seqNo:577; 4061527.32 north; 594462.88 east; 36.69 lat; -121.94 lon; 40.30 depth; utmZone: 10 2024-11-06T02:53:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861591.88 2024-11-06T02:53:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:12Z- waiting for 1730861595.88, time is 1730861592.30 2024-11-06T02:53:12Z- AHRS_M2 msg: 1730861592.66 epoch sec; seqNo:533 2024-11-06T02:53:12Z- AHRS_M2 msg: 0.14 phi; -0.18 theta; 5.34 psi 2024-11-06T02:53:12Z- handleDepth()- Depth_Keller msg: 1730861592.67 epoch sec; seqNo:578; depth 40.36 2024-11-06T02:53:12Z- handleDVL() - DVL msg epochMillisec: 1730861592.67 2024-11-06T02:53:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:53:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:53:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:53:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:53:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:53:12Z- handleDvl() - RDI_Pathfinder msg: 1730861592.67 sec; seqNo:183; #beams:0 2024-11-06T02:53:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:53:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.06 , 0.04 2024-11-06T02:53:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861592.68 epoch sec; seqNo:578; 4061527.70 north; 594462.88 east; 36.69 lat; -121.94 lon; 40.36 depth; utmZone: 10 2024-11-06T02:53:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861591.88 2024-11-06T02:53:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:12Z- waiting for 1730861595.88, time is 1730861592.68 2024-11-06T02:53:13Z- AHRS_M2 msg: 1730861593.06 epoch sec; seqNo:534 2024-11-06T02:53:13Z- AHRS_M2 msg: 0.14 phi; -0.18 theta; 5.35 psi 2024-11-06T02:53:13Z- handleDepth()- Depth_Keller msg: 1730861593.06 epoch sec; seqNo:579; depth 40.43 2024-11-06T02:53:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861593.08 epoch sec; seqNo:579; 4061527.69 north; 594461.65 east; 36.69 lat; -121.94 lon; 40.43 depth; utmZone: 10 2024-11-06T02:53:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861591.88 2024-11-06T02:53:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:13Z- waiting for 1730861595.88, time is 1730861593.08 2024-11-06T02:53:13Z- AHRS_M2 msg: 1730861593.46 epoch sec; seqNo:535 2024-11-06T02:53:13Z- AHRS_M2 msg: 0.14 phi; -0.18 theta; 5.35 psi 2024-11-06T02:53:13Z- handleDepth()- Depth_Keller msg: 1730861593.46 epoch sec; seqNo:580; depth 40.51 2024-11-06T02:53:13Z- handleDVL() - DVL msg epochMillisec: 1730861593.49 2024-11-06T02:53:13Z- handleDvl() - RDI_Pathfinder msg: 1730861593.49 sec; seqNo:184; #beams:4 2024-11-06T02:53:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 53.52 , 42.91 , 44.90 , 52.64 2024-11-06T02:53:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , 0.01 2024-11-06T02:53:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861593.50 epoch sec; seqNo:580; 4061528.07 north; 594461.65 east; 36.69 lat; -121.94 lon; 40.51 depth; utmZone: 10 2024-11-06T02:53:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861591.88 2024-11-06T02:53:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:13Z- waiting for 1730861595.88, time is 1730861593.50 2024-11-06T02:53:13Z- AHRS_M2 msg: 1730861593.86 epoch sec; seqNo:536 2024-11-06T02:53:13Z- AHRS_M2 msg: 0.14 phi; -0.17 theta; 5.35 psi 2024-11-06T02:53:13Z- handleDepth()- Depth_Keller msg: 1730861593.87 epoch sec; seqNo:581; depth 40.56 2024-11-06T02:53:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861593.89 epoch sec; seqNo:581; 4061528.07 north; 594461.65 east; 36.69 lat; -121.94 lon; 40.56 depth; utmZone: 10 2024-11-06T02:53:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861591.88 2024-11-06T02:53:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:14Z- waiting for 1730861595.88, time is 1730861593.89 2024-11-06T02:53:14Z- AHRS_M2 msg: 1730861594.28 epoch sec; seqNo:537 2024-11-06T02:53:14Z- AHRS_M2 msg: 0.14 phi; -0.17 theta; 5.35 psi 2024-11-06T02:53:14Z- handleDepth()- Depth_Keller msg: 1730861594.28 epoch sec; seqNo:582; depth 40.61 2024-11-06T02:53:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861594.30 epoch sec; seqNo:582; 4061528.44 north; 594461.65 east; 36.69 lat; -121.94 lon; 40.61 depth; utmZone: 10 2024-11-06T02:53:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861591.88 2024-11-06T02:53:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:14Z- waiting for 1730861595.88, time is 1730861594.30 2024-11-06T02:53:14Z- AHRS_M2 msg: 1730861594.67 epoch sec; seqNo:538 2024-11-06T02:53:14Z- AHRS_M2 msg: 0.14 phi; -0.17 theta; 5.35 psi 2024-11-06T02:53:14Z- handleDepth()- Depth_Keller msg: 1730861594.68 epoch sec; seqNo:583; depth 40.69 2024-11-06T02:53:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861594.70 epoch sec; seqNo:583; 4061528.43 north; 594460.43 east; 36.69 lat; -121.94 lon; 40.69 depth; utmZone: 10 2024-11-06T02:53:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861591.88 2024-11-06T02:53:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:14Z- waiting for 1730861595.88, time is 1730861594.70 2024-11-06T02:53:15Z- AHRS_M2 msg: 1730861595.08 epoch sec; seqNo:539 2024-11-06T02:53:15Z- AHRS_M2 msg: 0.15 phi; -0.16 theta; 5.36 psi 2024-11-06T02:53:15Z- handleDepth()- Depth_Keller msg: 1730861595.08 epoch sec; seqNo:584; depth 40.74 2024-11-06T02:53:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861595.09 epoch sec; seqNo:584; 4061528.81 north; 594460.42 east; 36.69 lat; -121.94 lon; 40.74 depth; utmZone: 10 2024-11-06T02:53:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861591.88 2024-11-06T02:53:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:15Z- waiting for 1730861595.88, time is 1730861595.09 2024-11-06T02:53:15Z- AHRS_M2 msg: 1730861595.48 epoch sec; seqNo:540 2024-11-06T02:53:15Z- AHRS_M2 msg: 0.15 phi; -0.16 theta; 5.36 psi 2024-11-06T02:53:15Z- handleDepth()- Depth_Keller msg: 1730861595.48 epoch sec; seqNo:585; depth 40.81 2024-11-06T02:53:15Z- handleDVL() - DVL msg epochMillisec: 1730861595.51 2024-11-06T02:53:15Z- handleDvl() - RDI_Pathfinder msg: 1730861595.51 sec; seqNo:185; #beams:4 2024-11-06T02:53:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 53.08 , 42.69 , 44.45 , 51.75 2024-11-06T02:53:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , -0.02 2024-11-06T02:53:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861595.52 epoch sec; seqNo:585; 4061528.81 north; 594460.42 east; 36.69 lat; -121.94 lon; 40.81 depth; utmZone: 10 2024-11-06T02:53:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861591.88 2024-11-06T02:53:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:15Z- waiting for 1730861595.88, time is 1730861595.52 2024-11-06T02:53:15Z- AHRS_M2 msg: 1730861595.88 epoch sec; seqNo:541 2024-11-06T02:53:15Z- AHRS_M2 msg: 0.15 phi; -0.15 theta; 5.36 psi 2024-11-06T02:53:15Z- handleDepth()- Depth_Keller msg: 1730861595.89 epoch sec; seqNo:586; depth 40.86 2024-11-06T02:53:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861595.91 epoch sec; seqNo:586; 4061529.19 north; 594460.42 east; 36.69 lat; -121.94 lon; 40.86 depth; utmZone: 10 2024-11-06T02:53:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861591.88 2024-11-06T02:53:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:16Z- waiting for 1730861595.88, time is 1730861595.91 2024-11-06T02:53:16Z- LcmTrn::updateTrn() >>>> pose time 1730861595.91, meas time 1730861595.51 <<<< 2024-11-06T02:53:16Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861595.91 2024-11-06T02:53:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861595.51 sec, ping # 0; waiting for INS data more recent than 1730861591.88... 2024-11-06T02:53:16Z- TerrainNav::motionUpdate() - currEstimate = (4061529, 594460.42, 40.86) 2024-11-06T02:53:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:53:16Z- TerrainNav:: SubcloudNIS is 0.45, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:53:16Z- TNavPF::Projecting Measurements 2024-11-06T02:53:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:53:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:53:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:53:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:53:16Z- TNavFilter::4 good beams. 2024-11-06T02:53:16Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:53:16Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:53:16Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:53:16Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:53:16Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:53:16Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:53:16Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:53:16Z- TerrainMapOctree: (vehN, vehE ) = (4061528.96, 594460.66). 2024-11-06T02:53:16Z- TerrainMapOctree: Min Distance = 305.35. 2024-11-06T02:53:16Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:53:16Z- Forcing Subcloud Comparison 2024-11-06T02:53:16Z- Weighting particles with subcloud comparison 2024-11-06T02:53:16Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:16Z- meanExpectedMeasDiff: 0.794877 delta_rms_squared: 2.976801 alpha: 0.993326 2024-11-06T02:53:16Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:16Z- meanExpectedMeasDiff: -0.374205 delta_rms_squared: 1.273973 alpha: 0.984544 2024-11-06T02:53:16Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:16Z- meanExpectedMeasDiff: 5.335668 delta_rms_squared: 1.010136 alpha: 0.980585 2024-11-06T02:53:16Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:16Z- meanExpectedMeasDiff: -0.447214 delta_rms_squared: 1.831123 alpha: 0.989196 2024-11-06T02:53:16Z- Subcloud NIS: 0.483962 2024-11-06T02:53:16Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:53:16Z- TNavPF:: sumWeights = 0.045268 2024-11-06T02:53:16Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:53:16Z- TNavPF::First Term of Map Covariance Matrix is 0.08 2024-11-06T02:53:16Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T02:53:16Z- TerrainNav::Resampling particle filter... 2024-11-06T02:53:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861595.51 sec. 2024-11-06T02:53:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:53:16Z- variances([0], [2], and psi_berg) = 1.559259 38.215902 0.000000 2024-11-06T02:53:16Z- publishEstimatesDecoder() - at 1730861595.91 sec 2024-11-06T02:53:16Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:53:16Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061567.19 594453.50 45.90 2024-11-06T02:53:16Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061561.95 594450.06 46.19 lat/lon: 36.70 -121.94 2024-11-06T02:53:16Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.56 38.22 0.84 0.00 2024-11-06T02:53:16Z- AHRS_M2 msg: 1730861596.29 epoch sec; seqNo:542 2024-11-06T02:53:16Z- AHRS_M2 msg: 0.15 phi; -0.15 theta; 5.36 psi 2024-11-06T02:53:16Z- handleDepth()- Depth_Keller msg: 1730861596.31 epoch sec; seqNo:587; depth 40.92 2024-11-06T02:53:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861596.33 epoch sec; seqNo:587; 4061529.55 north; 594459.19 east; 36.69 lat; -121.94 lon; 40.92 depth; utmZone: 10 2024-11-06T02:53:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861595.91 2024-11-06T02:53:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:16Z- waiting for 1730861599.91, time is 1730861596.33 2024-11-06T02:53:16Z- AHRS_M2 msg: 1730861596.70 epoch sec; seqNo:543 2024-11-06T02:53:16Z- AHRS_M2 msg: 0.15 phi; -0.15 theta; 5.36 psi 2024-11-06T02:53:16Z- handleDepth()- Depth_Keller msg: 1730861596.71 epoch sec; seqNo:588; depth 40.98 2024-11-06T02:53:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861596.72 epoch sec; seqNo:588; 4061529.55 north; 594459.19 east; 36.69 lat; -121.94 lon; 40.98 depth; utmZone: 10 2024-11-06T02:53:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861595.91 2024-11-06T02:53:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:16Z- waiting for 1730861599.91, time is 1730861596.72 2024-11-06T02:53:17Z- AHRS_M2 msg: 1730861597.10 epoch sec; seqNo:544 2024-11-06T02:53:17Z- AHRS_M2 msg: 0.15 phi; -0.15 theta; 5.36 psi 2024-11-06T02:53:17Z- handleDepth()- Depth_Keller msg: 1730861597.10 epoch sec; seqNo:589; depth 41.04 2024-11-06T02:53:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861597.11 epoch sec; seqNo:589; 4061529.93 north; 594459.19 east; 36.69 lat; -121.94 lon; 41.04 depth; utmZone: 10 2024-11-06T02:53:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861595.91 2024-11-06T02:53:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:17Z- waiting for 1730861599.91, time is 1730861597.11 2024-11-06T02:53:17Z- AHRS_M2 msg: 1730861597.50 epoch sec; seqNo:545 2024-11-06T02:53:17Z- AHRS_M2 msg: 0.14 phi; -0.14 theta; 5.36 psi 2024-11-06T02:53:17Z- handleDepth()- Depth_Keller msg: 1730861597.51 epoch sec; seqNo:590; depth 41.08 2024-11-06T02:53:17Z- handleDVL() - DVL msg epochMillisec: 1730861597.53 2024-11-06T02:53:17Z- handleDvl() - RDI_Pathfinder msg: 1730861597.53 sec; seqNo:186; #beams:4 2024-11-06T02:53:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 52.64 , 42.24 , 44.67 , 50.43 2024-11-06T02:53:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.01 , -0.03 2024-11-06T02:53:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861597.54 epoch sec; seqNo:590; 4061529.93 north; 594459.19 east; 36.69 lat; -121.94 lon; 41.08 depth; utmZone: 10 2024-11-06T02:53:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861595.91 2024-11-06T02:53:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:17Z- waiting for 1730861599.91, time is 1730861597.54 2024-11-06T02:53:17Z- AHRS_M2 msg: 1730861597.90 epoch sec; seqNo:546 2024-11-06T02:53:17Z- AHRS_M2 msg: 0.14 phi; -0.14 theta; 5.36 psi 2024-11-06T02:53:17Z- handleDepth()- Depth_Keller msg: 1730861597.91 epoch sec; seqNo:591; depth 41.12 2024-11-06T02:53:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861597.93 epoch sec; seqNo:591; 4061530.31 north; 594459.18 east; 36.69 lat; -121.94 lon; 41.12 depth; utmZone: 10 2024-11-06T02:53:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861595.91 2024-11-06T02:53:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:18Z- waiting for 1730861599.91, time is 1730861597.93 2024-11-06T02:53:18Z- AHRS_M2 msg: 1730861598.31 epoch sec; seqNo:547 2024-11-06T02:53:18Z- AHRS_M2 msg: 0.14 phi; -0.14 theta; 5.36 psi 2024-11-06T02:53:18Z- handleDepth()- Depth_Keller msg: 1730861598.32 epoch sec; seqNo:592; depth 41.18 2024-11-06T02:53:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861598.33 epoch sec; seqNo:592; 4061530.68 north; 594457.96 east; 36.69 lat; -121.94 lon; 41.18 depth; utmZone: 10 2024-11-06T02:53:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861595.91 2024-11-06T02:53:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:18Z- waiting for 1730861599.91, time is 1730861598.33 2024-11-06T02:53:18Z- AHRS_M2 msg: 1730861598.71 epoch sec; seqNo:548 2024-11-06T02:53:18Z- AHRS_M2 msg: 0.14 phi; -0.13 theta; 5.36 psi 2024-11-06T02:53:18Z- handleDepth()- Depth_Keller msg: 1730861598.72 epoch sec; seqNo:593; depth 41.22 2024-11-06T02:53:18Z- handleDVL() - DVL msg epochMillisec: 1730861598.72 2024-11-06T02:53:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:53:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:53:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:53:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:53:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:53:18Z- handleDvl() - RDI_Pathfinder msg: 1730861598.72 sec; seqNo:187; #beams:0 2024-11-06T02:53:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:53:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.01 , -0.03 2024-11-06T02:53:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861598.73 epoch sec; seqNo:593; 4061530.68 north; 594457.96 east; 36.69 lat; -121.94 lon; 41.22 depth; utmZone: 10 2024-11-06T02:53:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861595.91 2024-11-06T02:53:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:18Z- waiting for 1730861599.91, time is 1730861598.73 2024-11-06T02:53:19Z- AHRS_M2 msg: 1730861599.12 epoch sec; seqNo:549 2024-11-06T02:53:19Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.35 psi 2024-11-06T02:53:19Z- handleDepth()- Depth_Keller msg: 1730861599.13 epoch sec; seqNo:594; depth 41.27 2024-11-06T02:53:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861599.14 epoch sec; seqNo:594; 4061531.06 north; 594457.96 east; 36.69 lat; -121.94 lon; 41.27 depth; utmZone: 10 2024-11-06T02:53:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861595.91 2024-11-06T02:53:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:19Z- waiting for 1730861599.91, time is 1730861599.14 2024-11-06T02:53:19Z- AHRS_M2 msg: 1730861599.52 epoch sec; seqNo:550 2024-11-06T02:53:19Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.35 psi 2024-11-06T02:53:19Z- handleDepth()- Depth_Keller msg: 1730861599.53 epoch sec; seqNo:595; depth 41.30 2024-11-06T02:53:19Z- handleDVL() - DVL msg epochMillisec: 1730861599.55 2024-11-06T02:53:19Z- handleDvl() - RDI_Pathfinder msg: 1730861599.55 sec; seqNo:188; #beams:4 2024-11-06T02:53:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 51.75 , 42.02 , 44.01 , 49.76 2024-11-06T02:53:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.07 , -0.03 2024-11-06T02:53:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861599.58 epoch sec; seqNo:595; 4061531.04 north; 594456.74 east; 36.69 lat; -121.94 lon; 41.30 depth; utmZone: 10 2024-11-06T02:53:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861595.91 2024-11-06T02:53:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:19Z- waiting for 1730861599.91, time is 1730861599.58 2024-11-06T02:53:19Z- AHRS_M2 msg: 1730861599.92 epoch sec; seqNo:551 2024-11-06T02:53:19Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.35 psi 2024-11-06T02:53:19Z- handleDepth()- Depth_Keller msg: 1730861599.93 epoch sec; seqNo:596; depth 41.36 2024-11-06T02:53:19Z- handleDVL() - DVL msg epochMillisec: 1730861599.93 2024-11-06T02:53:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:53:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:53:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:53:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:53:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:53:19Z- handleDvl() - RDI_Pathfinder msg: 1730861599.93 sec; seqNo:189; #beams:0 2024-11-06T02:53:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:53:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.07 , -0.03 2024-11-06T02:53:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861599.96 epoch sec; seqNo:596; 4061531.42 north; 594456.73 east; 36.69 lat; -121.94 lon; 41.36 depth; utmZone: 10 2024-11-06T02:53:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861595.91 2024-11-06T02:53:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:20Z- waiting for 1730861599.91, time is 1730861599.96 2024-11-06T02:53:20Z- LcmTrn::updateTrn() >>>> pose time 1730861599.96, meas time 1730861599.93 <<<< 2024-11-06T02:53:20Z- LcmTrn::updateTrn() equating pose 1730861599.93 and meas times 1730861599.93 2024-11-06T02:53:20Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861599.93 2024-11-06T02:53:20Z- TerrainNav::motionUpdate() - currEstimate = (4061531, 594456.73, 41.36) 2024-11-06T02:53:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:53:20Z- TerrainNav:: SubcloudNIS is 0.48, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:53:20Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:53:20Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:53:20Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:53:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:53:20Z- variances([0], [2], and psi_berg) = 1.564181 38.215316 0.000000 2024-11-06T02:53:20Z- publishEstimatesDecoder() - at 1730861599.93 sec 2024-11-06T02:53:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:53:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061569.43 594449.83 47.04 2024-11-06T02:53:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061564.18 594446.37 46.70 lat/lon: 36.70 -121.94 2024-11-06T02:53:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.56 38.22 1.09 0.00 2024-11-06T02:53:20Z- AHRS_M2 msg: 1730861600.33 epoch sec; seqNo:552 2024-11-06T02:53:20Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.35 psi 2024-11-06T02:53:20Z- handleDepth()- Depth_Keller msg: 1730861600.35 epoch sec; seqNo:597; depth 41.41 2024-11-06T02:53:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861600.37 epoch sec; seqNo:597; 4061531.80 north; 594456.73 east; 36.69 lat; -121.94 lon; 41.41 depth; utmZone: 10 2024-11-06T02:53:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861599.93 2024-11-06T02:53:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:20Z- waiting for 1730861603.93, time is 1730861600.37 2024-11-06T02:53:20Z- AHRS_M2 msg: 1730861600.74 epoch sec; seqNo:553 2024-11-06T02:53:20Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.34 psi 2024-11-06T02:53:20Z- handleDepth()- Depth_Keller msg: 1730861600.75 epoch sec; seqNo:598; depth 41.47 2024-11-06T02:53:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861600.76 epoch sec; seqNo:598; 4061531.80 north; 594456.73 east; 36.69 lat; -121.94 lon; 41.47 depth; utmZone: 10 2024-11-06T02:53:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861599.93 2024-11-06T02:53:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:20Z- waiting for 1730861603.93, time is 1730861600.76 2024-11-06T02:53:21Z- AHRS_M2 msg: 1730861601.15 epoch sec; seqNo:554 2024-11-06T02:53:21Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.34 psi 2024-11-06T02:53:21Z- handleDepth()- Depth_Keller msg: 1730861601.15 epoch sec; seqNo:599; depth 41.50 2024-11-06T02:53:21Z- handleDVL() - DVL msg epochMillisec: 1730861601.16 2024-11-06T02:53:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:53:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:53:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:53:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:53:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:53:21Z- handleDvl() - RDI_Pathfinder msg: 1730861601.16 sec; seqNo:190; #beams:0 2024-11-06T02:53:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:53:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.07 , -0.03 2024-11-06T02:53:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861601.16 epoch sec; seqNo:599; 4061532.17 north; 594455.50 east; 36.69 lat; -121.94 lon; 41.50 depth; utmZone: 10 2024-11-06T02:53:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861599.93 2024-11-06T02:53:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:21Z- waiting for 1730861603.93, time is 1730861601.16 2024-11-06T02:53:21Z- AHRS_M2 msg: 1730861601.55 epoch sec; seqNo:555 2024-11-06T02:53:21Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.34 psi 2024-11-06T02:53:21Z- handleDepth()- Depth_Keller msg: 1730861601.55 epoch sec; seqNo:600; depth 41.56 2024-11-06T02:53:21Z- handleDVL() - DVL msg epochMillisec: 1730861601.57 2024-11-06T02:53:21Z- handleDvl() - RDI_Pathfinder msg: 1730861601.57 sec; seqNo:191; #beams:4 2024-11-06T02:53:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 51.08 , 41.80 , 44.45 , 48.87 2024-11-06T02:53:21Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.06 , -0.03 2024-11-06T02:53:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861601.60 epoch sec; seqNo:600; 4061532.17 north; 594455.50 east; 36.69 lat; -121.94 lon; 41.56 depth; utmZone: 10 2024-11-06T02:53:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861599.93 2024-11-06T02:53:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:21Z- waiting for 1730861603.93, time is 1730861601.60 2024-11-06T02:53:21Z- AHRS_M2 msg: 1730861601.97 epoch sec; seqNo:556 2024-11-06T02:53:21Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 5.34 psi 2024-11-06T02:53:21Z- handleDepth()- Depth_Keller msg: 1730861601.98 epoch sec; seqNo:601; depth 41.61 2024-11-06T02:53:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861601.99 epoch sec; seqNo:601; 4061532.54 north; 594455.50 east; 36.69 lat; -121.94 lon; 41.61 depth; utmZone: 10 2024-11-06T02:53:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861599.93 2024-11-06T02:53:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:22Z- waiting for 1730861603.93, time is 1730861601.99 2024-11-06T02:53:22Z- AHRS_M2 msg: 1730861602.35 epoch sec; seqNo:557 2024-11-06T02:53:22Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 5.33 psi 2024-11-06T02:53:22Z- handleDepth()- Depth_Keller msg: 1730861602.36 epoch sec; seqNo:602; depth 41.65 2024-11-06T02:53:22Z- handleDVL() - DVL msg epochMillisec: 1730861602.36 2024-11-06T02:53:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:53:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:53:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:53:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:53:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:53:22Z- handleDvl() - RDI_Pathfinder msg: 1730861602.36 sec; seqNo:192; #beams:0 2024-11-06T02:53:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:53:22Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.06 , -0.03 2024-11-06T02:53:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861602.38 epoch sec; seqNo:602; 4061532.54 north; 594455.50 east; 36.69 lat; -121.94 lon; 41.65 depth; utmZone: 10 2024-11-06T02:53:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861599.93 2024-11-06T02:53:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:22Z- waiting for 1730861603.93, time is 1730861602.38 2024-11-06T02:53:22Z- AHRS_M2 msg: 1730861602.76 epoch sec; seqNo:558 2024-11-06T02:53:22Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 5.33 psi 2024-11-06T02:53:22Z- handleDepth()- Depth_Keller msg: 1730861602.76 epoch sec; seqNo:603; depth 41.69 2024-11-06T02:53:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861602.77 epoch sec; seqNo:603; 4061532.91 north; 594454.27 east; 36.69 lat; -121.94 lon; 41.69 depth; utmZone: 10 2024-11-06T02:53:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861599.93 2024-11-06T02:53:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:22Z- waiting for 1730861603.93, time is 1730861602.77 2024-11-06T02:53:23Z- AHRS_M2 msg: 1730861603.16 epoch sec; seqNo:559 2024-11-06T02:53:23Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 5.33 psi 2024-11-06T02:53:23Z- handleDepth()- Depth_Keller msg: 1730861603.17 epoch sec; seqNo:604; depth 41.75 2024-11-06T02:53:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861603.18 epoch sec; seqNo:604; 4061533.29 north; 594454.27 east; 36.69 lat; -121.94 lon; 41.75 depth; utmZone: 10 2024-11-06T02:53:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861599.93 2024-11-06T02:53:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:23Z- waiting for 1730861603.93, time is 1730861603.18 2024-11-06T02:53:23Z- AHRS_M2 msg: 1730861603.56 epoch sec; seqNo:560 2024-11-06T02:53:23Z- AHRS_M2 msg: 0.13 phi; -0.11 theta; 5.33 psi 2024-11-06T02:53:23Z- handleDepth()- Depth_Keller msg: 1730861603.57 epoch sec; seqNo:605; depth 41.79 2024-11-06T02:53:23Z- handleDVL() - DVL msg epochMillisec: 1730861603.59 2024-11-06T02:53:23Z- handleDvl() - RDI_Pathfinder msg: 1730861603.59 sec; seqNo:193; #beams:4 2024-11-06T02:53:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.86 , 41.80 , 44.67 , 48.65 2024-11-06T02:53:23Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.05 , 0.02 2024-11-06T02:53:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861603.61 epoch sec; seqNo:605; 4061533.29 north; 594454.27 east; 36.69 lat; -121.94 lon; 41.79 depth; utmZone: 10 2024-11-06T02:53:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861599.93 2024-11-06T02:53:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:23Z- waiting for 1730861603.93, time is 1730861603.61 2024-11-06T02:53:23Z- AHRS_M2 msg: 1730861603.97 epoch sec; seqNo:561 2024-11-06T02:53:23Z- AHRS_M2 msg: 0.13 phi; -0.11 theta; 5.32 psi 2024-11-06T02:53:23Z- handleDepth()- Depth_Keller msg: 1730861603.99 epoch sec; seqNo:606; depth 41.85 2024-11-06T02:53:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861604.01 epoch sec; seqNo:606; 4061533.65 north; 594453.05 east; 36.69 lat; -121.94 lon; 41.85 depth; utmZone: 10 2024-11-06T02:53:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861599.93 2024-11-06T02:53:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:24Z- waiting for 1730861603.93, time is 1730861604.01 2024-11-06T02:53:24Z- LcmTrn::updateTrn() >>>> pose time 1730861604.01, meas time 1730861603.59 <<<< 2024-11-06T02:53:24Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861604.01 2024-11-06T02:53:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861603.59 sec, ping # 0; waiting for INS data more recent than 1730861599.93... 2024-11-06T02:53:24Z- TerrainNav::motionUpdate() - currEstimate = (4061534, 594453.05, 41.85) 2024-11-06T02:53:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:53:24Z- TerrainNav:: SubcloudNIS is 0.48, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:53:24Z- TNavPF::Projecting Measurements 2024-11-06T02:53:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:53:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:53:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:53:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:53:24Z- TNavFilter::4 good beams. 2024-11-06T02:53:24Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:53:24Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:53:24Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:53:24Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:53:24Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:53:24Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:53:24Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:53:24Z- TerrainMapOctree: (vehN, vehE ) = (4061533.43, 594453.42). 2024-11-06T02:53:24Z- TerrainMapOctree: Min Distance = 297.13. 2024-11-06T02:53:24Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:53:24Z- Forcing Subcloud Comparison 2024-11-06T02:53:24Z- Weighting particles with subcloud comparison 2024-11-06T02:53:24Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:24Z- meanExpectedMeasDiff: 3.028635 delta_rms_squared: 3.324676 alpha: 0.994020 2024-11-06T02:53:24Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:24Z- meanExpectedMeasDiff: 1.900894 delta_rms_squared: 3.772482 alpha: 0.994726 2024-11-06T02:53:24Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:24Z- meanExpectedMeasDiff: 5.820950 delta_rms_squared: 2.273554 alpha: 0.991280 2024-11-06T02:53:24Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:24Z- meanExpectedMeasDiff: -0.813668 delta_rms_squared: 2.068213 alpha: 0.990422 2024-11-06T02:53:24Z- Subcloud NIS: 0.483737 2024-11-06T02:53:24Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:53:24Z- TNavPF:: sumWeights = 0.022310 2024-11-06T02:53:24Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:53:24Z- TNavPF::First Term of Map Covariance Matrix is 0.06 2024-11-06T02:53:24Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T02:53:24Z- TerrainNav::Resampling particle filter... 2024-11-06T02:53:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861603.59 sec. 2024-11-06T02:53:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:53:24Z- variances([0], [2], and psi_berg) = 1.271891 36.622664 0.000000 2024-11-06T02:53:24Z- publishEstimatesDecoder() - at 1730861604.01 sec 2024-11-06T02:53:24Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:53:24Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061571.45 594446.35 45.79 2024-11-06T02:53:24Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061566.37 594442.09 45.94 lat/lon: 36.70 -121.94 2024-11-06T02:53:24Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.27 36.62 0.80 0.00 2024-11-06T02:53:24Z- AHRS_M2 msg: 1730861604.38 epoch sec; seqNo:562 2024-11-06T02:53:24Z- AHRS_M2 msg: 0.13 phi; -0.11 theta; 5.32 psi 2024-11-06T02:53:24Z- handleDepth()- Depth_Keller msg: 1730861604.40 epoch sec; seqNo:607; depth 41.89 2024-11-06T02:53:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861604.43 epoch sec; seqNo:607; 4061533.65 north; 594453.05 east; 36.69 lat; -121.94 lon; 41.89 depth; utmZone: 10 2024-11-06T02:53:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861604.01 2024-11-06T02:53:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:24Z- waiting for 1730861608.01, time is 1730861604.43 2024-11-06T02:53:24Z- AHRS_M2 msg: 1730861604.78 epoch sec; seqNo:563 2024-11-06T02:53:24Z- AHRS_M2 msg: 0.13 phi; -0.11 theta; 5.32 psi 2024-11-06T02:53:24Z- handleDepth()- Depth_Keller msg: 1730861604.79 epoch sec; seqNo:608; depth 41.93 2024-11-06T02:53:24Z- handleDVL() - DVL msg epochMillisec: 1730861604.79 2024-11-06T02:53:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:53:24Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:53:24Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:53:24Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:53:24Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:53:24Z- handleDvl() - RDI_Pathfinder msg: 1730861604.79 sec; seqNo:194; #beams:0 2024-11-06T02:53:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:53:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.05 , 0.02 2024-11-06T02:53:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861604.81 epoch sec; seqNo:608; 4061534.03 north; 594453.04 east; 36.69 lat; -121.94 lon; 41.93 depth; utmZone: 10 2024-11-06T02:53:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861604.01 2024-11-06T02:53:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:24Z- waiting for 1730861608.01, time is 1730861604.81 2024-11-06T02:53:25Z- AHRS_M2 msg: 1730861605.18 epoch sec; seqNo:564 2024-11-06T02:53:25Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 5.32 psi 2024-11-06T02:53:25Z- handleDepth()- Depth_Keller msg: 1730861605.18 epoch sec; seqNo:609; depth 42.00 2024-11-06T02:53:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861605.20 epoch sec; seqNo:609; 4061534.03 north; 594453.04 east; 36.69 lat; -121.94 lon; 42.00 depth; utmZone: 10 2024-11-06T02:53:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861604.01 2024-11-06T02:53:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:25Z- waiting for 1730861608.01, time is 1730861605.20 2024-11-06T02:53:25Z- AHRS_M2 msg: 1730861605.58 epoch sec; seqNo:565 2024-11-06T02:53:25Z- AHRS_M2 msg: 0.12 phi; -0.10 theta; 5.32 psi 2024-11-06T02:53:25Z- handleDepth()- Depth_Keller msg: 1730861605.59 epoch sec; seqNo:610; depth 42.02 2024-11-06T02:53:25Z- handleDVL() - DVL msg epochMillisec: 1730861605.61 2024-11-06T02:53:25Z- handleDvl() - RDI_Pathfinder msg: 1730861605.61 sec; seqNo:195; #beams:4 2024-11-06T02:53:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 51.08 , 41.57 , 44.89 , 48.20 2024-11-06T02:53:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , 0.07 2024-11-06T02:53:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861605.63 epoch sec; seqNo:610; 4061534.40 north; 594451.82 east; 36.69 lat; -121.94 lon; 42.02 depth; utmZone: 10 2024-11-06T02:53:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861604.01 2024-11-06T02:53:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:25Z- waiting for 1730861608.01, time is 1730861605.63 2024-11-06T02:53:25Z- AHRS_M2 msg: 1730861605.99 epoch sec; seqNo:566 2024-11-06T02:53:25Z- AHRS_M2 msg: 0.12 phi; -0.10 theta; 5.32 psi 2024-11-06T02:53:26Z- handleDepth()- Depth_Keller msg: 1730861606.00 epoch sec; seqNo:611; depth 42.06 2024-11-06T02:53:26Z- handleDVL() - DVL msg epochMillisec: 1730861606.01 2024-11-06T02:53:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:53:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:53:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:53:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:53:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:53:26Z- handleDvl() - RDI_Pathfinder msg: 1730861606.01 sec; seqNo:196; #beams:0 2024-11-06T02:53:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:53:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , 0.07 2024-11-06T02:53:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861606.02 epoch sec; seqNo:611; 4061534.40 north; 594451.82 east; 36.69 lat; -121.94 lon; 42.06 depth; utmZone: 10 2024-11-06T02:53:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861604.01 2024-11-06T02:53:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:26Z- waiting for 1730861608.01, time is 1730861606.02 2024-11-06T02:53:26Z- AHRS_M2 msg: 1730861606.39 epoch sec; seqNo:567 2024-11-06T02:53:26Z- AHRS_M2 msg: 0.12 phi; -0.10 theta; 5.32 psi 2024-11-06T02:53:26Z- handleDepth()- Depth_Keller msg: 1730861606.40 epoch sec; seqNo:612; depth 42.11 2024-11-06T02:53:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861606.41 epoch sec; seqNo:612; 4061534.78 north; 594451.81 east; 36.69 lat; -121.94 lon; 42.11 depth; utmZone: 10 2024-11-06T02:53:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861604.01 2024-11-06T02:53:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:26Z- waiting for 1730861608.01, time is 1730861606.41 2024-11-06T02:53:26Z- AHRS_M2 msg: 1730861606.80 epoch sec; seqNo:568 2024-11-06T02:53:26Z- AHRS_M2 msg: 0.12 phi; -0.10 theta; 5.32 psi 2024-11-06T02:53:26Z- handleDepth()- Depth_Keller msg: 1730861606.81 epoch sec; seqNo:613; depth 42.15 2024-11-06T02:53:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861606.83 epoch sec; seqNo:613; 4061535.14 north; 594450.59 east; 36.69 lat; -121.94 lon; 42.15 depth; utmZone: 10 2024-11-06T02:53:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861604.01 2024-11-06T02:53:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:26Z- waiting for 1730861608.01, time is 1730861606.83 2024-11-06T02:53:27Z- AHRS_M2 msg: 1730861607.20 epoch sec; seqNo:569 2024-11-06T02:53:27Z- AHRS_M2 msg: 0.12 phi; -0.10 theta; 5.32 psi 2024-11-06T02:53:27Z- handleDepth()- Depth_Keller msg: 1730861607.21 epoch sec; seqNo:614; depth 42.17 2024-11-06T02:53:27Z- handleDVL() - DVL msg epochMillisec: 1730861607.22 2024-11-06T02:53:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:53:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:53:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:53:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:53:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:53:27Z- handleDvl() - RDI_Pathfinder msg: 1730861607.22 sec; seqNo:197; #beams:0 2024-11-06T02:53:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:53:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , 0.07 2024-11-06T02:53:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861607.27 epoch sec; seqNo:614; 4061535.14 north; 594450.59 east; 36.69 lat; -121.94 lon; 42.17 depth; utmZone: 10 2024-11-06T02:53:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861604.01 2024-11-06T02:53:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:27Z- waiting for 1730861608.01, time is 1730861607.27 2024-11-06T02:53:27Z- AHRS_M2 msg: 1730861607.61 epoch sec; seqNo:570 2024-11-06T02:53:27Z- AHRS_M2 msg: 0.12 phi; -0.10 theta; 5.32 psi 2024-11-06T02:53:27Z- handleDepth()- Depth_Keller msg: 1730861607.61 epoch sec; seqNo:615; depth 42.22 2024-11-06T02:53:27Z- handleDVL() - DVL msg epochMillisec: 1730861607.63 2024-11-06T02:53:27Z- handleDvl() - RDI_Pathfinder msg: 1730861607.63 sec; seqNo:198; #beams:4 2024-11-06T02:53:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.64 , 41.57 , 44.89 , 47.98 2024-11-06T02:53:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.03 2024-11-06T02:53:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861607.65 epoch sec; seqNo:615; 4061535.52 north; 594450.58 east; 36.69 lat; -121.94 lon; 42.22 depth; utmZone: 10 2024-11-06T02:53:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861604.01 2024-11-06T02:53:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:27Z- waiting for 1730861608.01, time is 1730861607.65 2024-11-06T02:53:28Z- AHRS_M2 msg: 1730861608.01 epoch sec; seqNo:571 2024-11-06T02:53:28Z- AHRS_M2 msg: 0.12 phi; -0.10 theta; 5.32 psi 2024-11-06T02:53:28Z- handleDepth()- Depth_Keller msg: 1730861608.01 epoch sec; seqNo:616; depth 42.26 2024-11-06T02:53:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861608.04 epoch sec; seqNo:616; 4061535.52 north; 594450.58 east; 36.69 lat; -121.94 lon; 42.26 depth; utmZone: 10 2024-11-06T02:53:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861604.01 2024-11-06T02:53:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:28Z- waiting for 1730861608.01, time is 1730861608.04 2024-11-06T02:53:28Z- LcmTrn::updateTrn() >>>> pose time 1730861608.04, meas time 1730861607.63 <<<< 2024-11-06T02:53:28Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861608.04 2024-11-06T02:53:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861607.63 sec, ping # 0; waiting for INS data more recent than 1730861604.01... 2024-11-06T02:53:28Z- TerrainNav::motionUpdate() - currEstimate = (4061536, 594450.58, 42.26) 2024-11-06T02:53:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:53:28Z- TerrainNav:: SubcloudNIS is 0.48, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:53:28Z- TNavPF::Projecting Measurements 2024-11-06T02:53:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:53:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:53:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:53:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:53:28Z- TNavFilter::4 good beams. 2024-11-06T02:53:28Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:53:28Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:53:28Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:53:28Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:53:28Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:53:28Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:53:28Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:53:28Z- TerrainMapOctree: (vehN, vehE ) = (4061535.33, 594450.83). 2024-11-06T02:53:28Z- TerrainMapOctree: Min Distance = 293.97. 2024-11-06T02:53:28Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:53:28Z- Forcing Subcloud Comparison 2024-11-06T02:53:28Z- Weighting particles with subcloud comparison 2024-11-06T02:53:28Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:28Z- meanExpectedMeasDiff: 2.755938 delta_rms_squared: 3.094034 alpha: 0.993577 2024-11-06T02:53:28Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:28Z- meanExpectedMeasDiff: 1.822770 delta_rms_squared: 3.119733 alpha: 0.993630 2024-11-06T02:53:28Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:28Z- meanExpectedMeasDiff: 4.784299 delta_rms_squared: 1.291649 alpha: 0.984752 2024-11-06T02:53:28Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:28Z- meanExpectedMeasDiff: -2.254388 delta_rms_squared: 1.616383 alpha: 0.987778 2024-11-06T02:53:28Z- Subcloud NIS: 0.488205 2024-11-06T02:53:28Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:53:28Z- TNavPF:: sumWeights = 0.026171 2024-11-06T02:53:28Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:53:28Z- TNavPF::First Term of Map Covariance Matrix is 0.08 2024-11-06T02:53:28Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T02:53:28Z- TerrainNav::Resampling particle filter... 2024-11-06T02:53:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861607.63 sec. 2024-11-06T02:53:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:53:28Z- variances([0], [2], and psi_berg) = 1.069301 40.276565 0.000000 2024-11-06T02:53:28Z- publishEstimatesDecoder() - at 1730861608.04 sec 2024-11-06T02:53:28Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:53:28Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061573.73 594443.77 44.23 2024-11-06T02:53:28Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061568.09 594440.73 45.66 lat/lon: 36.70 -121.94 2024-11-06T02:53:28Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.07 40.28 0.84 0.00 2024-11-06T02:53:28Z- AHRS_M2 msg: 1730861608.41 epoch sec; seqNo:572 2024-11-06T02:53:28Z- AHRS_M2 msg: 0.12 phi; -0.10 theta; 5.32 psi 2024-11-06T02:53:28Z- handleDepth()- Depth_Keller msg: 1730861608.42 epoch sec; seqNo:617; depth 42.28 2024-11-06T02:53:28Z- handleDVL() - DVL msg epochMillisec: 1730861608.42 2024-11-06T02:53:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:53:28Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:53:28Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:53:28Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:53:28Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:53:28Z- handleDvl() - RDI_Pathfinder msg: 1730861608.42 sec; seqNo:199; #beams:0 2024-11-06T02:53:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:53:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.03 2024-11-06T02:53:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861608.43 epoch sec; seqNo:617; 4061535.89 north; 594449.36 east; 36.69 lat; -121.94 lon; 42.28 depth; utmZone: 10 2024-11-06T02:53:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861608.04 2024-11-06T02:53:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:28Z- waiting for 1730861612.04, time is 1730861608.43 2024-11-06T02:53:28Z- AHRS_M2 msg: 1730861608.82 epoch sec; seqNo:573 2024-11-06T02:53:28Z- AHRS_M2 msg: 0.12 phi; -0.10 theta; 5.32 psi 2024-11-06T02:53:28Z- handleDepth()- Depth_Keller msg: 1730861608.82 epoch sec; seqNo:618; depth 42.31 2024-11-06T02:53:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861608.83 epoch sec; seqNo:618; 4061535.89 north; 594449.36 east; 36.69 lat; -121.94 lon; 42.31 depth; utmZone: 10 2024-11-06T02:53:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861608.04 2024-11-06T02:53:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:28Z- waiting for 1730861612.04, time is 1730861608.83 2024-11-06T02:53:29Z- AHRS_M2 msg: 1730861609.23 epoch sec; seqNo:574 2024-11-06T02:53:29Z- AHRS_M2 msg: 0.12 phi; -0.10 theta; 5.32 psi 2024-11-06T02:53:29Z- handleDepth()- Depth_Keller msg: 1730861609.24 epoch sec; seqNo:619; depth 42.33 2024-11-06T02:53:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861609.30 epoch sec; seqNo:619; 4061536.27 north; 594449.36 east; 36.69 lat; -121.94 lon; 42.33 depth; utmZone: 10 2024-11-06T02:53:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861608.04 2024-11-06T02:53:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:29Z- waiting for 1730861612.04, time is 1730861609.30 2024-11-06T02:53:29Z- AHRS_M2 msg: 1730861609.62 epoch sec; seqNo:575 2024-11-06T02:53:29Z- AHRS_M2 msg: 0.12 phi; -0.10 theta; 5.32 psi 2024-11-06T02:53:29Z- handleDepth()- Depth_Keller msg: 1730861609.63 epoch sec; seqNo:620; depth 42.37 2024-11-06T02:53:29Z- handleDVL() - DVL msg epochMillisec: 1730861609.66 2024-11-06T02:53:29Z- handleDvl() - RDI_Pathfinder msg: 1730861609.66 sec; seqNo:200; #beams:4 2024-11-06T02:53:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.19 , 41.35 , 44.44 , 48.20 2024-11-06T02:53:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , -0.07 2024-11-06T02:53:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861609.68 epoch sec; seqNo:620; 4061536.27 north; 594449.36 east; 36.69 lat; -121.94 lon; 42.37 depth; utmZone: 10 2024-11-06T02:53:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861608.04 2024-11-06T02:53:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:29Z- waiting for 1730861612.04, time is 1730861609.68 2024-11-06T02:53:30Z- handleDepth()- Depth_Keller msg: 1730861610.05 epoch sec; seqNo:621; depth 42.39 2024-11-06T02:53:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861610.08 epoch sec; seqNo:621; 4061536.64 north; 594449.35 east; 36.69 lat; -121.94 lon; 42.39 depth; utmZone: 10 2024-11-06T02:53:30Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:53:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861608.04 2024-11-06T02:53:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:30Z- waiting for 1730861612.04, time is 1730861610.08 2024-11-06T02:53:30Z- AHRS_M2 msg: 1730861610.43 epoch sec; seqNo:576 2024-11-06T02:53:30Z- AHRS_M2 msg: 0.12 phi; -0.10 theta; 5.32 psi 2024-11-06T02:53:30Z- handleDepth()- Depth_Keller msg: 1730861610.44 epoch sec; seqNo:622; depth 42.40 2024-11-06T02:53:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861610.45 epoch sec; seqNo:622; 4061536.63 north; 594448.13 east; 36.69 lat; -121.94 lon; 42.40 depth; utmZone: 10 2024-11-06T02:53:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861608.04 2024-11-06T02:53:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:30Z- waiting for 1730861612.04, time is 1730861610.45 2024-11-06T02:53:30Z- AHRS_M2 msg: 1730861610.84 epoch sec; seqNo:577 2024-11-06T02:53:30Z- AHRS_M2 msg: 0.12 phi; -0.10 theta; 5.32 psi 2024-11-06T02:53:30Z- handleDepth()- Depth_Keller msg: 1730861610.84 epoch sec; seqNo:623; depth 42.44 2024-11-06T02:53:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861610.86 epoch sec; seqNo:623; 4061537.01 north; 594448.13 east; 36.69 lat; -121.94 lon; 42.44 depth; utmZone: 10 2024-11-06T02:53:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861608.04 2024-11-06T02:53:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:30Z- waiting for 1730861612.04, time is 1730861610.86 2024-11-06T02:53:31Z- AHRS_M2 msg: 1730861611.28 epoch sec; seqNo:578 2024-11-06T02:53:31Z- AHRS_M2 msg: 0.12 phi; -0.10 theta; 5.31 psi 2024-11-06T02:53:31Z- handleDepth()- Depth_Keller msg: 1730861611.29 epoch sec; seqNo:624; depth 42.48 2024-11-06T02:53:31Z- handleDVL() - DVL msg epochMillisec: 1730861611.31 2024-11-06T02:53:31Z- handleDvl() - RDI_Pathfinder msg: 1730861611.31 sec; seqNo:201; #beams:4 2024-11-06T02:53:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.19 , 40.90 , 43.78 , 47.98 2024-11-06T02:53:31Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.01 , -0.08 2024-11-06T02:53:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861611.34 epoch sec; seqNo:624; 4061537.01 north; 594448.13 east; 36.69 lat; -121.94 lon; 42.48 depth; utmZone: 10 2024-11-06T02:53:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861608.04 2024-11-06T02:53:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:31Z- waiting for 1730861612.04, time is 1730861611.34 2024-11-06T02:53:31Z- AHRS_M2 msg: 1730861611.67 epoch sec; seqNo:579 2024-11-06T02:53:31Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 5.31 psi 2024-11-06T02:53:31Z- handleDepth()- Depth_Keller msg: 1730861611.69 epoch sec; seqNo:625; depth 42.51 2024-11-06T02:53:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861611.79 epoch sec; seqNo:625; 4061537.37 north; 594446.90 east; 36.69 lat; -121.94 lon; 42.51 depth; utmZone: 10 2024-11-06T02:53:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861608.04 2024-11-06T02:53:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:31Z- waiting for 1730861612.04, time is 1730861611.79 2024-11-06T02:53:32Z- handleDepth()- Depth_Keller msg: 1730861612.07 epoch sec; seqNo:626; depth 42.55 2024-11-06T02:53:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861612.08 epoch sec; seqNo:626; 4061537.37 north; 594446.90 east; 36.69 lat; -121.94 lon; 42.55 depth; utmZone: 10 2024-11-06T02:53:32Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:53:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861608.04 2024-11-06T02:53:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:32Z- waiting for 1730861612.04, time is 1730861612.08 2024-11-06T02:53:32Z- LcmTrn::updateTrn() >>>> pose time 1730861612.08, meas time 1730861611.31 <<<< 2024-11-06T02:53:32Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861612.08 2024-11-06T02:53:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861611.31 sec, ping # 0; waiting for INS data more recent than 1730861608.04... 2024-11-06T02:53:32Z- TerrainNav::motionUpdate() - currEstimate = (4061537, 594446.90, 42.55) 2024-11-06T02:53:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:53:32Z- TerrainNav:: SubcloudNIS is 0.49, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:53:32Z- TNavPF::Projecting Measurements 2024-11-06T02:53:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:53:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:53:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:53:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:53:32Z- TNavFilter::4 good beams. 2024-11-06T02:53:32Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:53:32Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:53:32Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:53:32Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:53:32Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:53:32Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:53:32Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:53:32Z- TerrainMapOctree: (vehN, vehE ) = (4061537.02, 594447.60). 2024-11-06T02:53:32Z- TerrainMapOctree: Min Distance = 290.53. 2024-11-06T02:53:32Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:53:32Z- Forcing Subcloud Comparison 2024-11-06T02:53:32Z- Weighting particles with subcloud comparison 2024-11-06T02:53:32Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:32Z- meanExpectedMeasDiff: 1.568750 delta_rms_squared: 2.247865 alpha: 0.991181 2024-11-06T02:53:32Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:32Z- meanExpectedMeasDiff: 2.056553 delta_rms_squared: 4.344921 alpha: 0.995418 2024-11-06T02:53:32Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:32Z- meanExpectedMeasDiff: 2.228669 delta_rms_squared: 3.447076 alpha: 0.994231 2024-11-06T02:53:32Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:32Z- meanExpectedMeasDiff: -2.823593 delta_rms_squared: 1.750045 alpha: 0.988701 2024-11-06T02:53:32Z- Subcloud NIS: 0.503721 2024-11-06T02:53:32Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:53:32Z- TNavPF:: sumWeights = 0.064960 2024-11-06T02:53:32Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:53:32Z- TNavPF::First Term of Map Covariance Matrix is 0.14 2024-11-06T02:53:32Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T02:53:32Z- TerrainNav::Resampling particle filter... 2024-11-06T02:53:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861611.31 sec. 2024-11-06T02:53:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:53:32Z- variances([0], [2], and psi_berg) = 0.706143 41.850148 0.000000 2024-11-06T02:53:32Z- publishEstimatesDecoder() - at 1730861612.08 sec 2024-11-06T02:53:32Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:53:32Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061575.58 594440.11 44.92 2024-11-06T02:53:32Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061569.74 594438.20 45.71 lat/lon: 36.70 -121.94 2024-11-06T02:53:32Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.71 41.85 0.84 0.00 2024-11-06T02:53:32Z- AHRS_M2 msg: 1730861612.46 epoch sec; seqNo:580 2024-11-06T02:53:32Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 5.30 psi 2024-11-06T02:53:32Z- handleDepth()- Depth_Keller msg: 1730861612.47 epoch sec; seqNo:627; depth 42.58 2024-11-06T02:53:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861612.48 epoch sec; seqNo:627; 4061537.75 north; 594446.90 east; 36.69 lat; -121.94 lon; 42.58 depth; utmZone: 10 2024-11-06T02:53:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861612.08 2024-11-06T02:53:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:32Z- waiting for 1730861616.08, time is 1730861612.48 2024-11-06T02:53:32Z- AHRS_M2 msg: 1730861612.86 epoch sec; seqNo:581 2024-11-06T02:53:32Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 5.30 psi 2024-11-06T02:53:32Z- handleDepth()- Depth_Keller msg: 1730861612.86 epoch sec; seqNo:628; depth 42.60 2024-11-06T02:53:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861612.87 epoch sec; seqNo:628; 4061538.13 north; 594446.89 east; 36.69 lat; -121.94 lon; 42.60 depth; utmZone: 10 2024-11-06T02:53:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861612.08 2024-11-06T02:53:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:33Z- waiting for 1730861616.08, time is 1730861612.87 2024-11-06T02:53:33Z- AHRS_M2 msg: 1730861613.29 epoch sec; seqNo:582 2024-11-06T02:53:33Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 5.29 psi 2024-11-06T02:53:33Z- handleDepth()- Depth_Keller msg: 1730861613.30 epoch sec; seqNo:629; depth 42.63 2024-11-06T02:53:33Z- handleDVL() - DVL msg epochMillisec: 1730861613.32 2024-11-06T02:53:33Z- handleDvl() - RDI_Pathfinder msg: 1730861613.32 sec; seqNo:202; #beams:4 2024-11-06T02:53:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.53 , 40.68 , 44.22 , 47.31 2024-11-06T02:53:33Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , -0.05 , 0.00 2024-11-06T02:53:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861613.34 epoch sec; seqNo:629; 4061538.12 north; 594445.67 east; 36.69 lat; -121.94 lon; 42.63 depth; utmZone: 10 2024-11-06T02:53:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861612.08 2024-11-06T02:53:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:33Z- waiting for 1730861616.08, time is 1730861613.34 2024-11-06T02:53:33Z- AHRS_M2 msg: 1730861613.67 epoch sec; seqNo:583 2024-11-06T02:53:33Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 5.29 psi 2024-11-06T02:53:33Z- handleDepth()- Depth_Keller msg: 1730861613.67 epoch sec; seqNo:630; depth 42.66 2024-11-06T02:53:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861613.69 epoch sec; seqNo:630; 4061538.50 north; 594445.67 east; 36.69 lat; -121.94 lon; 42.66 depth; utmZone: 10 2024-11-06T02:53:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861612.08 2024-11-06T02:53:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:33Z- waiting for 1730861616.08, time is 1730861613.69 2024-11-06T02:53:34Z- AHRS_M2 msg: 1730861614.07 epoch sec; seqNo:584 2024-11-06T02:53:34Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 5.29 psi 2024-11-06T02:53:34Z- handleDepth()- Depth_Keller msg: 1730861614.08 epoch sec; seqNo:631; depth 42.71 2024-11-06T02:53:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861614.09 epoch sec; seqNo:631; 4061538.50 north; 594445.67 east; 36.69 lat; -121.94 lon; 42.71 depth; utmZone: 10 2024-11-06T02:53:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861612.08 2024-11-06T02:53:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:34Z- waiting for 1730861616.08, time is 1730861614.09 2024-11-06T02:53:34Z- AHRS_M2 msg: 1730861614.47 epoch sec; seqNo:585 2024-11-06T02:53:34Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.28 psi 2024-11-06T02:53:34Z- handleDepth()- Depth_Keller msg: 1730861614.48 epoch sec; seqNo:632; depth 42.76 2024-11-06T02:53:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861614.49 epoch sec; seqNo:632; 4061538.86 north; 594444.44 east; 36.69 lat; -121.94 lon; 42.76 depth; utmZone: 10 2024-11-06T02:53:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861612.08 2024-11-06T02:53:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:34Z- waiting for 1730861616.08, time is 1730861614.49 2024-11-06T02:53:34Z- AHRS_M2 msg: 1730861614.88 epoch sec; seqNo:586 2024-11-06T02:53:34Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.28 psi 2024-11-06T02:53:34Z- handleDepth()- Depth_Keller msg: 1730861614.90 epoch sec; seqNo:633; depth 42.81 2024-11-06T02:53:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861614.91 epoch sec; seqNo:633; 4061538.86 north; 594444.44 east; 36.69 lat; -121.94 lon; 42.81 depth; utmZone: 10 2024-11-06T02:53:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861612.08 2024-11-06T02:53:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:35Z- waiting for 1730861616.08, time is 1730861614.91 2024-11-06T02:53:35Z- AHRS_M2 msg: 1730861615.30 epoch sec; seqNo:587 2024-11-06T02:53:35Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.28 psi 2024-11-06T02:53:35Z- handleDepth()- Depth_Keller msg: 1730861615.30 epoch sec; seqNo:634; depth 42.84 2024-11-06T02:53:35Z- handleDVL() - DVL msg epochMillisec: 1730861615.32 2024-11-06T02:53:35Z- handleDvl() - RDI_Pathfinder msg: 1730861615.32 sec; seqNo:203; #beams:4 2024-11-06T02:53:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.75 , 40.46 , 44.00 , 46.87 2024-11-06T02:53:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.06 , 0.06 2024-11-06T02:53:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861615.35 epoch sec; seqNo:634; 4061539.24 north; 594444.44 east; 36.69 lat; -121.94 lon; 42.84 depth; utmZone: 10 2024-11-06T02:53:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861612.08 2024-11-06T02:53:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:35Z- waiting for 1730861616.08, time is 1730861615.35 2024-11-06T02:53:35Z- AHRS_M2 msg: 1730861615.69 epoch sec; seqNo:588 2024-11-06T02:53:35Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.28 psi 2024-11-06T02:53:35Z- handleDepth()- Depth_Keller msg: 1730861615.70 epoch sec; seqNo:635; depth 42.89 2024-11-06T02:53:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861615.72 epoch sec; seqNo:635; 4061539.24 north; 594444.44 east; 36.69 lat; -121.94 lon; 42.89 depth; utmZone: 10 2024-11-06T02:53:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861612.08 2024-11-06T02:53:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:35Z- waiting for 1730861616.08, time is 1730861615.72 2024-11-06T02:53:36Z- AHRS_M2 msg: 1730861616.09 epoch sec; seqNo:589 2024-11-06T02:53:36Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.28 psi 2024-11-06T02:53:36Z- handleDepth()- Depth_Keller msg: 1730861616.10 epoch sec; seqNo:636; depth 42.91 2024-11-06T02:53:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861616.11 epoch sec; seqNo:636; 4061539.61 north; 594443.22 east; 36.69 lat; -121.94 lon; 42.91 depth; utmZone: 10 2024-11-06T02:53:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861612.08 2024-11-06T02:53:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:36Z- waiting for 1730861616.08, time is 1730861616.11 2024-11-06T02:53:36Z- LcmTrn::updateTrn() >>>> pose time 1730861616.11, meas time 1730861615.32 <<<< 2024-11-06T02:53:36Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861616.11 2024-11-06T02:53:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861615.32 sec, ping # 0; waiting for INS data more recent than 1730861612.08... 2024-11-06T02:53:36Z- TerrainNav::motionUpdate() - currEstimate = (4061540, 594443.22, 42.91) 2024-11-06T02:53:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:53:36Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:53:36Z- TNavPF::Projecting Measurements 2024-11-06T02:53:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:53:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:53:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:53:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:53:36Z- TNavFilter::4 good beams. 2024-11-06T02:53:36Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:53:36Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:53:36Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:53:36Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:53:36Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:53:36Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:53:36Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:53:36Z- TerrainMapOctree: (vehN, vehE ) = (4061539.17, 594443.94). 2024-11-06T02:53:36Z- TerrainMapOctree: Min Distance = 286.46. 2024-11-06T02:53:36Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:53:36Z- Forcing Subcloud Comparison 2024-11-06T02:53:36Z- Weighting particles with subcloud comparison 2024-11-06T02:53:36Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:36Z- meanExpectedMeasDiff: 1.571313 delta_rms_squared: 4.234519 alpha: 0.995299 2024-11-06T02:53:36Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:36Z- meanExpectedMeasDiff: 1.615651 delta_rms_squared: 1.801708 alpha: 0.989021 2024-11-06T02:53:36Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:36Z- meanExpectedMeasDiff: 2.314363 delta_rms_squared: 5.244774 alpha: 0.996201 2024-11-06T02:53:36Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:36Z- meanExpectedMeasDiff: -4.120649 delta_rms_squared: 1.934229 alpha: 0.989766 2024-11-06T02:53:36Z- Subcloud NIS: 0.534029 2024-11-06T02:53:36Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:53:36Z- TNavPF:: sumWeights = 0.036633 2024-11-06T02:53:36Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:53:36Z- TNavPF::First Term of Map Covariance Matrix is 0.09 2024-11-06T02:53:36Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T02:53:36Z- TerrainNav::Resampling particle filter... 2024-11-06T02:53:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861615.32 sec. 2024-11-06T02:53:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:53:36Z- variances([0], [2], and psi_berg) = 0.395461 30.363859 0.000000 2024-11-06T02:53:36Z- publishEstimatesDecoder() - at 1730861616.11 sec 2024-11-06T02:53:36Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:53:36Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061577.44 594435.63 45.86 2024-11-06T02:53:36Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061572.22 594430.53 45.97 lat/lon: 36.70 -121.94 2024-11-06T02:53:36Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.40 30.36 0.71 0.00 2024-11-06T02:53:36Z- AHRS_M2 msg: 1730861616.49 epoch sec; seqNo:590 2024-11-06T02:53:36Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.28 psi 2024-11-06T02:53:36Z- handleDepth()- Depth_Keller msg: 1730861616.50 epoch sec; seqNo:637; depth 42.96 2024-11-06T02:53:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861616.51 epoch sec; seqNo:637; 4061539.61 north; 594443.22 east; 36.69 lat; -121.94 lon; 42.96 depth; utmZone: 10 2024-11-06T02:53:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861616.11 2024-11-06T02:53:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:36Z- waiting for 1730861620.11, time is 1730861616.51 2024-11-06T02:53:36Z- AHRS_M2 msg: 1730861616.89 epoch sec; seqNo:591 2024-11-06T02:53:36Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.28 psi 2024-11-06T02:53:36Z- handleDepth()- Depth_Keller msg: 1730861616.90 epoch sec; seqNo:638; depth 43.01 2024-11-06T02:53:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861616.93 epoch sec; seqNo:638; 4061539.99 north; 594443.21 east; 36.69 lat; -121.94 lon; 43.01 depth; utmZone: 10 2024-11-06T02:53:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861616.11 2024-11-06T02:53:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:37Z- waiting for 1730861620.11, time is 1730861616.93 2024-11-06T02:53:37Z- AHRS_M2 msg: 1730861617.30 epoch sec; seqNo:592 2024-11-06T02:53:37Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.28 psi 2024-11-06T02:53:37Z- handleDepth()- Depth_Keller msg: 1730861617.32 epoch sec; seqNo:639; depth 43.05 2024-11-06T02:53:37Z- handleDVL() - DVL msg epochMillisec: 1730861617.34 2024-11-06T02:53:37Z- handleDvl() - RDI_Pathfinder msg: 1730861617.34 sec; seqNo:204; #beams:4 2024-11-06T02:53:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.00 , 40.00 , 43.88 , 47.55 2024-11-06T02:53:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.06 , 0.04 2024-11-06T02:53:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861617.36 epoch sec; seqNo:639; 4061539.97 north; 594441.99 east; 36.69 lat; -121.94 lon; 43.05 depth; utmZone: 10 2024-11-06T02:53:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861616.11 2024-11-06T02:53:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:37Z- waiting for 1730861620.11, time is 1730861617.36 2024-11-06T02:53:37Z- AHRS_M2 msg: 1730861617.70 epoch sec; seqNo:593 2024-11-06T02:53:37Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.28 psi 2024-11-06T02:53:37Z- handleDepth()- Depth_Keller msg: 1730861617.71 epoch sec; seqNo:640; depth 43.11 2024-11-06T02:53:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861617.73 epoch sec; seqNo:640; 4061540.35 north; 594441.99 east; 36.69 lat; -121.94 lon; 43.11 depth; utmZone: 10 2024-11-06T02:53:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861616.11 2024-11-06T02:53:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:37Z- waiting for 1730861620.11, time is 1730861617.73 2024-11-06T02:53:38Z- AHRS_M2 msg: 1730861618.12 epoch sec; seqNo:594 2024-11-06T02:53:38Z- AHRS_M2 msg: 0.13 phi; -0.14 theta; 5.28 psi 2024-11-06T02:53:38Z- handleDepth()- Depth_Keller msg: 1730861618.12 epoch sec; seqNo:641; depth 43.14 2024-11-06T02:53:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861618.13 epoch sec; seqNo:641; 4061540.35 north; 594441.99 east; 36.69 lat; -121.94 lon; 43.14 depth; utmZone: 10 2024-11-06T02:53:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861616.11 2024-11-06T02:53:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:38Z- waiting for 1730861620.11, time is 1730861618.13 2024-11-06T02:53:38Z- AHRS_M2 msg: 1730861618.52 epoch sec; seqNo:595 2024-11-06T02:53:38Z- AHRS_M2 msg: 0.13 phi; -0.14 theta; 5.28 psi 2024-11-06T02:53:38Z- handleDepth()- Depth_Keller msg: 1730861618.52 epoch sec; seqNo:642; depth 43.20 2024-11-06T02:53:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861618.54 epoch sec; seqNo:642; 4061540.73 north; 594441.98 east; 36.69 lat; -121.94 lon; 43.20 depth; utmZone: 10 2024-11-06T02:53:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861616.11 2024-11-06T02:53:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:38Z- waiting for 1730861620.11, time is 1730861618.54 2024-11-06T02:53:38Z- AHRS_M2 msg: 1730861618.91 epoch sec; seqNo:596 2024-11-06T02:53:38Z- AHRS_M2 msg: 0.14 phi; -0.14 theta; 5.28 psi 2024-11-06T02:53:38Z- handleDepth()- Depth_Keller msg: 1730861618.92 epoch sec; seqNo:643; depth 43.24 2024-11-06T02:53:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861618.93 epoch sec; seqNo:643; 4061540.72 north; 594440.76 east; 36.69 lat; -121.94 lon; 43.24 depth; utmZone: 10 2024-11-06T02:53:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861616.11 2024-11-06T02:53:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:39Z- waiting for 1730861620.11, time is 1730861618.93 2024-11-06T02:53:39Z- AHRS_M2 msg: 1730861619.32 epoch sec; seqNo:597 2024-11-06T02:53:39Z- AHRS_M2 msg: 0.14 phi; -0.14 theta; 5.28 psi 2024-11-06T02:53:39Z- handleDepth()- Depth_Keller msg: 1730861619.33 epoch sec; seqNo:644; depth 43.30 2024-11-06T02:53:39Z- handleDVL() - DVL msg epochMillisec: 1730861619.34 2024-11-06T02:53:39Z- handleDvl() - RDI_Pathfinder msg: 1730861619.34 sec; seqNo:205; #beams:4 2024-11-06T02:53:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.96 , 39.79 , 43.33 , 48.19 2024-11-06T02:53:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , 0.00 2024-11-06T02:53:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861619.36 epoch sec; seqNo:644; 4061541.10 north; 594440.76 east; 36.69 lat; -121.94 lon; 43.30 depth; utmZone: 10 2024-11-06T02:53:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861616.11 2024-11-06T02:53:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:39Z- waiting for 1730861620.11, time is 1730861619.36 2024-11-06T02:53:39Z- AHRS_M2 msg: 1730861619.72 epoch sec; seqNo:598 2024-11-06T02:53:39Z- AHRS_M2 msg: 0.14 phi; -0.14 theta; 5.28 psi 2024-11-06T02:53:39Z- handleDepth()- Depth_Keller msg: 1730861619.73 epoch sec; seqNo:645; depth 43.33 2024-11-06T02:53:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861619.74 epoch sec; seqNo:645; 4061541.10 north; 594440.76 east; 36.69 lat; -121.94 lon; 43.33 depth; utmZone: 10 2024-11-06T02:53:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861616.11 2024-11-06T02:53:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:39Z- waiting for 1730861620.11, time is 1730861619.74 2024-11-06T02:53:40Z- AHRS_M2 msg: 1730861620.14 epoch sec; seqNo:599 2024-11-06T02:53:40Z- AHRS_M2 msg: 0.14 phi; -0.14 theta; 5.28 psi 2024-11-06T02:53:40Z- handleDepth()- Depth_Keller msg: 1730861620.14 epoch sec; seqNo:646; depth 43.39 2024-11-06T02:53:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861620.15 epoch sec; seqNo:646; 4061541.47 north; 594440.75 east; 36.69 lat; -121.94 lon; 43.39 depth; utmZone: 10 2024-11-06T02:53:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861616.11 2024-11-06T02:53:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:40Z- waiting for 1730861620.11, time is 1730861620.15 2024-11-06T02:53:40Z- LcmTrn::updateTrn() >>>> pose time 1730861620.15, meas time 1730861619.34 <<<< 2024-11-06T02:53:40Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861620.15 2024-11-06T02:53:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861619.34 sec, ping # 0; waiting for INS data more recent than 1730861616.11... 2024-11-06T02:53:40Z- TerrainNav::motionUpdate() - currEstimate = (4061541, 594440.75, 43.39) 2024-11-06T02:53:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:53:40Z- TerrainNav:: SubcloudNIS is 0.53, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:53:40Z- TNavPF::Projecting Measurements 2024-11-06T02:53:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:53:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:53:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:53:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:53:40Z- TNavFilter::4 good beams. 2024-11-06T02:53:40Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:53:40Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:53:40Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:53:40Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:53:40Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:53:40Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:53:40Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:53:40Z- TerrainMapOctree: (vehN, vehE ) = (4061541.10, 594441.24). 2024-11-06T02:53:40Z- TerrainMapOctree: Min Distance = 283.22. 2024-11-06T02:53:40Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:53:40Z- Forcing Subcloud Comparison 2024-11-06T02:53:40Z- Weighting particles with subcloud comparison 2024-11-06T02:53:40Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:40Z- meanExpectedMeasDiff: 0.656589 delta_rms_squared: 4.514781 alpha: 0.995590 2024-11-06T02:53:40Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:40Z- meanExpectedMeasDiff: 2.084906 delta_rms_squared: 1.310467 alpha: 0.984968 2024-11-06T02:53:40Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:40Z- meanExpectedMeasDiff: 1.249413 delta_rms_squared: 3.310163 alpha: 0.993994 2024-11-06T02:53:40Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:40Z- meanExpectedMeasDiff: -2.576194 delta_rms_squared: 2.001416 alpha: 0.990106 2024-11-06T02:53:40Z- Subcloud NIS: 0.483369 2024-11-06T02:53:40Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:53:40Z- TNavPF:: sumWeights = 0.169332 2024-11-06T02:53:40Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:53:40Z- TNavPF::First Term of Map Covariance Matrix is 0.23 2024-11-06T02:53:40Z- TNavPF::Calculated NIS Value : 0.07 numBeams normalized NIS: 0.02 2024-11-06T02:53:40Z- TerrainNav::Resampling particle filter... 2024-11-06T02:53:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861619.34 sec. 2024-11-06T02:53:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:53:40Z- variances([0], [2], and psi_berg) = 0.174042 6.152538 0.000000 2024-11-06T02:53:40Z- publishEstimatesDecoder() - at 1730861620.15 sec 2024-11-06T02:53:40Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:53:40Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061573.11 594438.11 45.65 2024-11-06T02:53:40Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061574.26 594425.54 46.36 lat/lon: 36.70 -121.94 2024-11-06T02:53:40Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 6.15 0.69 0.00 2024-11-06T02:53:40Z- AHRS_M2 msg: 1730861620.53 epoch sec; seqNo:600 2024-11-06T02:53:40Z- AHRS_M2 msg: 0.14 phi; -0.15 theta; 5.28 psi 2024-11-06T02:53:40Z- handleDepth()- Depth_Keller msg: 1730861620.54 epoch sec; seqNo:647; depth 43.43 2024-11-06T02:53:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861620.57 epoch sec; seqNo:647; 4061541.46 north; 594439.53 east; 36.69 lat; -121.94 lon; 43.43 depth; utmZone: 10 2024-11-06T02:53:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861620.15 2024-11-06T02:53:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:40Z- waiting for 1730861624.15, time is 1730861620.57 2024-11-06T02:53:40Z- AHRS_M2 msg: 1730861620.94 epoch sec; seqNo:601 2024-11-06T02:53:40Z- AHRS_M2 msg: 0.14 phi; -0.15 theta; 5.28 psi 2024-11-06T02:53:40Z- handleDepth()- Depth_Keller msg: 1730861620.94 epoch sec; seqNo:648; depth 43.47 2024-11-06T02:53:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861620.95 epoch sec; seqNo:648; 4061541.84 north; 594439.53 east; 36.69 lat; -121.94 lon; 43.47 depth; utmZone: 10 2024-11-06T02:53:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861620.15 2024-11-06T02:53:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:41Z- waiting for 1730861624.15, time is 1730861620.95 2024-11-06T02:53:41Z- AHRS_M2 msg: 1730861621.34 epoch sec; seqNo:602 2024-11-06T02:53:41Z- AHRS_M2 msg: 0.14 phi; -0.15 theta; 5.28 psi 2024-11-06T02:53:41Z- handleDepth()- Depth_Keller msg: 1730861621.35 epoch sec; seqNo:649; depth 43.52 2024-11-06T02:53:41Z- handleDVL() - DVL msg epochMillisec: 1730861621.37 2024-11-06T02:53:41Z- handleDvl() - RDI_Pathfinder msg: 1730861621.37 sec; seqNo:206; #beams:4 2024-11-06T02:53:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.00 , 39.39 , 42.85 , 48.16 2024-11-06T02:53:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , -0.01 2024-11-06T02:53:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861621.41 epoch sec; seqNo:649; 4061541.84 north; 594439.53 east; 36.69 lat; -121.94 lon; 43.52 depth; utmZone: 10 2024-11-06T02:53:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861620.15 2024-11-06T02:53:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:41Z- waiting for 1730861624.15, time is 1730861621.41 2024-11-06T02:53:41Z- AHRS_M2 msg: 1730861621.74 epoch sec; seqNo:603 2024-11-06T02:53:41Z- AHRS_M2 msg: 0.14 phi; -0.15 theta; 5.28 psi 2024-11-06T02:53:41Z- handleDepth()- Depth_Keller msg: 1730861621.75 epoch sec; seqNo:650; depth 43.57 2024-11-06T02:53:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861621.77 epoch sec; seqNo:650; 4061542.22 north; 594439.53 east; 36.69 lat; -121.94 lon; 43.57 depth; utmZone: 10 2024-11-06T02:53:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861620.15 2024-11-06T02:53:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:41Z- waiting for 1730861624.15, time is 1730861621.77 2024-11-06T02:53:42Z- AHRS_M2 msg: 1730861622.15 epoch sec; seqNo:604 2024-11-06T02:53:42Z- AHRS_M2 msg: 0.14 phi; -0.15 theta; 5.28 psi 2024-11-06T02:53:42Z- handleDepth()- Depth_Keller msg: 1730861622.16 epoch sec; seqNo:651; depth 43.62 2024-11-06T02:53:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861622.18 epoch sec; seqNo:651; 4061542.21 north; 594438.31 east; 36.69 lat; -121.94 lon; 43.62 depth; utmZone: 10 2024-11-06T02:53:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861620.15 2024-11-06T02:53:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:42Z- waiting for 1730861624.15, time is 1730861622.18 2024-11-06T02:53:42Z- AHRS_M2 msg: 1730861622.55 epoch sec; seqNo:605 2024-11-06T02:53:42Z- AHRS_M2 msg: 0.14 phi; -0.15 theta; 5.28 psi 2024-11-06T02:53:42Z- handleDepth()- Depth_Keller msg: 1730861622.56 epoch sec; seqNo:652; depth 43.67 2024-11-06T02:53:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861622.57 epoch sec; seqNo:652; 4061542.58 north; 594438.30 east; 36.69 lat; -121.94 lon; 43.67 depth; utmZone: 10 2024-11-06T02:53:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861620.15 2024-11-06T02:53:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:42Z- waiting for 1730861624.15, time is 1730861622.57 2024-11-06T02:53:42Z- AHRS_M2 msg: 1730861622.95 epoch sec; seqNo:606 2024-11-06T02:53:42Z- AHRS_M2 msg: 0.14 phi; -0.15 theta; 5.28 psi 2024-11-06T02:53:42Z- handleDepth()- Depth_Keller msg: 1730861622.96 epoch sec; seqNo:653; depth 43.72 2024-11-06T02:53:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861622.98 epoch sec; seqNo:653; 4061542.58 north; 594438.30 east; 36.69 lat; -121.94 lon; 43.72 depth; utmZone: 10 2024-11-06T02:53:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861620.15 2024-11-06T02:53:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:43Z- waiting for 1730861624.15, time is 1730861622.98 2024-11-06T02:53:43Z- AHRS_M2 msg: 1730861623.36 epoch sec; seqNo:607 2024-11-06T02:53:43Z- AHRS_M2 msg: 0.14 phi; -0.15 theta; 5.28 psi 2024-11-06T02:53:43Z- handleDepth()- Depth_Keller msg: 1730861623.37 epoch sec; seqNo:654; depth 43.75 2024-11-06T02:53:43Z- handleDVL() - DVL msg epochMillisec: 1730861623.39 2024-11-06T02:53:43Z- handleDvl() - RDI_Pathfinder msg: 1730861623.39 sec; seqNo:207; #beams:4 2024-11-06T02:53:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.00 , 39.18 , 42.85 , 48.37 2024-11-06T02:53:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.02 , -0.01 2024-11-06T02:53:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861623.41 epoch sec; seqNo:654; 4061542.95 north; 594437.08 east; 36.69 lat; -121.94 lon; 43.75 depth; utmZone: 10 2024-11-06T02:53:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861620.15 2024-11-06T02:53:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:43Z- waiting for 1730861624.15, time is 1730861623.41 2024-11-06T02:53:43Z- AHRS_M2 msg: 1730861623.77 epoch sec; seqNo:608 2024-11-06T02:53:43Z- AHRS_M2 msg: 0.14 phi; -0.16 theta; 5.28 psi 2024-11-06T02:53:43Z- handleDepth()- Depth_Keller msg: 1730861623.77 epoch sec; seqNo:655; depth 43.82 2024-11-06T02:53:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861623.78 epoch sec; seqNo:655; 4061542.95 north; 594437.08 east; 36.69 lat; -121.94 lon; 43.82 depth; utmZone: 10 2024-11-06T02:53:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861620.15 2024-11-06T02:53:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:43Z- waiting for 1730861624.15, time is 1730861623.78 2024-11-06T02:53:44Z- AHRS_M2 msg: 1730861624.17 epoch sec; seqNo:609 2024-11-06T02:53:44Z- AHRS_M2 msg: 0.14 phi; -0.16 theta; 5.28 psi 2024-11-06T02:53:44Z- handleDepth()- Depth_Keller msg: 1730861624.17 epoch sec; seqNo:656; depth 43.85 2024-11-06T02:53:44Z- handleDVL() - DVL msg epochMillisec: 1730861624.17 2024-11-06T02:53:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:53:44Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:53:44Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:53:44Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:53:44Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:53:44Z- handleDvl() - RDI_Pathfinder msg: 1730861624.17 sec; seqNo:208; #beams:0 2024-11-06T02:53:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:53:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.02 , -0.01 2024-11-06T02:53:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861624.20 epoch sec; seqNo:656; 4061543.33 north; 594437.07 east; 36.69 lat; -121.94 lon; 43.85 depth; utmZone: 10 2024-11-06T02:53:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861620.15 2024-11-06T02:53:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:44Z- waiting for 1730861624.15, time is 1730861624.20 2024-11-06T02:53:44Z- LcmTrn::updateTrn() >>>> pose time 1730861624.20, meas time 1730861624.17 <<<< 2024-11-06T02:53:44Z- LcmTrn::updateTrn() equating pose 1730861624.17 and meas times 1730861624.17 2024-11-06T02:53:44Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861624.17 2024-11-06T02:53:44Z- TerrainNav::motionUpdate() - currEstimate = (4061543, 594437.07, 43.85) 2024-11-06T02:53:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:53:44Z- TerrainNav:: SubcloudNIS is 0.48, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:53:44Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:53:44Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:53:44Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:53:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:53:44Z- variances([0], [2], and psi_berg) = 0.176308 6.157055 0.000000 2024-11-06T02:53:44Z- publishEstimatesDecoder() - at 1730861624.17 sec 2024-11-06T02:53:44Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:53:44Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061575.05 594434.50 45.34 2024-11-06T02:53:44Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061576.11 594421.86 46.82 lat/lon: 36.70 -121.94 2024-11-06T02:53:44Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 6.16 0.92 0.00 2024-11-06T02:53:44Z- AHRS_M2 msg: 1730861624.57 epoch sec; seqNo:610 2024-11-06T02:53:44Z- AHRS_M2 msg: 0.14 phi; -0.16 theta; 5.27 psi 2024-11-06T02:53:44Z- handleDepth()- Depth_Keller msg: 1730861624.58 epoch sec; seqNo:657; depth 43.90 2024-11-06T02:53:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861624.60 epoch sec; seqNo:657; 4061543.33 north; 594437.07 east; 36.69 lat; -121.94 lon; 43.90 depth; utmZone: 10 2024-11-06T02:53:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861624.17 2024-11-06T02:53:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:44Z- waiting for 1730861628.17, time is 1730861624.60 2024-11-06T02:53:44Z- AHRS_M2 msg: 1730861624.97 epoch sec; seqNo:611 2024-11-06T02:53:44Z- AHRS_M2 msg: 0.14 phi; -0.16 theta; 5.27 psi 2024-11-06T02:53:44Z- handleDepth()- Depth_Keller msg: 1730861624.98 epoch sec; seqNo:658; depth 43.95 2024-11-06T02:53:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861624.99 epoch sec; seqNo:658; 4061543.69 north; 594435.85 east; 36.69 lat; -121.94 lon; 43.95 depth; utmZone: 10 2024-11-06T02:53:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861624.17 2024-11-06T02:53:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:45Z- waiting for 1730861628.17, time is 1730861624.99 2024-11-06T02:53:45Z- AHRS_M2 msg: 1730861625.38 epoch sec; seqNo:612 2024-11-06T02:53:45Z- AHRS_M2 msg: 0.14 phi; -0.16 theta; 5.27 psi 2024-11-06T02:53:45Z- handleDepth()- Depth_Keller msg: 1730861625.39 epoch sec; seqNo:659; depth 43.99 2024-11-06T02:53:45Z- handleDVL() - DVL msg epochMillisec: 1730861625.41 2024-11-06T02:53:45Z- handleDvl() - RDI_Pathfinder msg: 1730861625.41 sec; seqNo:209; #beams:4 2024-11-06T02:53:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.38 , 38.57 , 42.44 , 48.36 2024-11-06T02:53:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.03 , -0.00 2024-11-06T02:53:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861625.43 epoch sec; seqNo:659; 4061543.69 north; 594435.85 east; 36.69 lat; -121.94 lon; 43.99 depth; utmZone: 10 2024-11-06T02:53:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861624.17 2024-11-06T02:53:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:45Z- waiting for 1730861628.17, time is 1730861625.43 2024-11-06T02:53:45Z- AHRS_M2 msg: 1730861625.78 epoch sec; seqNo:613 2024-11-06T02:53:45Z- AHRS_M2 msg: 0.14 phi; -0.16 theta; 5.27 psi 2024-11-06T02:53:45Z- handleDepth()- Depth_Keller msg: 1730861625.79 epoch sec; seqNo:660; depth 44.05 2024-11-06T02:53:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861625.82 epoch sec; seqNo:660; 4061544.07 north; 594435.84 east; 36.69 lat; -121.94 lon; 44.05 depth; utmZone: 10 2024-11-06T02:53:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861624.17 2024-11-06T02:53:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:45Z- waiting for 1730861628.17, time is 1730861625.82 2024-11-06T02:53:46Z- AHRS_M2 msg: 1730861626.19 epoch sec; seqNo:614 2024-11-06T02:53:46Z- AHRS_M2 msg: 0.14 phi; -0.16 theta; 5.27 psi 2024-11-06T02:53:46Z- handleDepth()- Depth_Keller msg: 1730861626.19 epoch sec; seqNo:661; depth 44.11 2024-11-06T02:53:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861626.20 epoch sec; seqNo:661; 4061544.07 north; 594435.84 east; 36.69 lat; -121.94 lon; 44.11 depth; utmZone: 10 2024-11-06T02:53:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861624.17 2024-11-06T02:53:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:46Z- waiting for 1730861628.17, time is 1730861626.20 2024-11-06T02:53:46Z- AHRS_M2 msg: 1730861626.59 epoch sec; seqNo:615 2024-11-06T02:53:46Z- AHRS_M2 msg: 0.14 phi; -0.16 theta; 5.26 psi 2024-11-06T02:53:46Z- handleDepth()- Depth_Keller msg: 1730861626.60 epoch sec; seqNo:662; depth 44.16 2024-11-06T02:53:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861626.61 epoch sec; seqNo:662; 4061544.44 north; 594434.62 east; 36.69 lat; -121.94 lon; 44.16 depth; utmZone: 10 2024-11-06T02:53:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861624.17 2024-11-06T02:53:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:46Z- waiting for 1730861628.17, time is 1730861626.61 2024-11-06T02:53:47Z- AHRS_M2 msg: 1730861626.99 epoch sec; seqNo:616 2024-11-06T02:53:47Z- AHRS_M2 msg: 0.14 phi; -0.16 theta; 5.26 psi 2024-11-06T02:53:47Z- handleDepth()- Depth_Keller msg: 1730861627.00 epoch sec; seqNo:663; depth 44.20 2024-11-06T02:53:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861627.02 epoch sec; seqNo:663; 4061544.44 north; 594434.62 east; 36.69 lat; -121.94 lon; 44.20 depth; utmZone: 10 2024-11-06T02:53:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861624.17 2024-11-06T02:53:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:47Z- waiting for 1730861628.17, time is 1730861627.02 2024-11-06T02:53:47Z- handleDepth()- Depth_Keller msg: 1730861627.42 epoch sec; seqNo:664; depth 44.25 2024-11-06T02:53:47Z- handleDVL() - DVL msg epochMillisec: 1730861627.45 2024-11-06T02:53:47Z- handleDvl() - RDI_Pathfinder msg: 1730861627.45 sec; seqNo:210; #beams:4 2024-11-06T02:53:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.38 , 37.54 , 42.03 , 48.56 2024-11-06T02:53:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.05 , 0.01 2024-11-06T02:53:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861627.46 epoch sec; seqNo:664; 4061544.82 north; 594434.62 east; 36.69 lat; -121.94 lon; 44.25 depth; utmZone: 10 2024-11-06T02:53:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861624.17 2024-11-06T02:53:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:47Z- waiting for 1730861628.17, time is 1730861627.46 2024-11-06T02:53:47Z- AHRS_M2 msg: 1730861627.81 epoch sec; seqNo:617 2024-11-06T02:53:47Z- AHRS_M2 msg: 0.14 phi; -0.16 theta; 5.26 psi 2024-11-06T02:53:47Z- handleDepth()- Depth_Keller msg: 1730861627.81 epoch sec; seqNo:665; depth 44.33 2024-11-06T02:53:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861627.83 epoch sec; seqNo:665; 4061544.82 north; 594434.62 east; 36.69 lat; -121.94 lon; 44.33 depth; utmZone: 10 2024-11-06T02:53:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861624.17 2024-11-06T02:53:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:47Z- waiting for 1730861628.17, time is 1730861627.83 2024-11-06T02:53:48Z- AHRS_M2 msg: 1730861628.21 epoch sec; seqNo:618 2024-11-06T02:53:48Z- AHRS_M2 msg: 0.14 phi; -0.16 theta; 5.26 psi 2024-11-06T02:53:48Z- handleDepth()- Depth_Keller msg: 1730861628.22 epoch sec; seqNo:666; depth 44.37 2024-11-06T02:53:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861628.23 epoch sec; seqNo:666; 4061545.18 north; 594433.39 east; 36.69 lat; -121.94 lon; 44.37 depth; utmZone: 10 2024-11-06T02:53:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861624.17 2024-11-06T02:53:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:48Z- waiting for 1730861628.17, time is 1730861628.23 2024-11-06T02:53:48Z- LcmTrn::updateTrn() >>>> pose time 1730861628.23, meas time 1730861627.45 <<<< 2024-11-06T02:53:48Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861628.23 2024-11-06T02:53:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861627.45 sec, ping # 0; waiting for INS data more recent than 1730861624.17... 2024-11-06T02:53:48Z- TerrainNav::motionUpdate() - currEstimate = (4061545, 594433.39, 44.37) 2024-11-06T02:53:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:53:48Z- TerrainNav:: SubcloudNIS is 0.48, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:53:48Z- TNavPF::Projecting Measurements 2024-11-06T02:53:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:53:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:53:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:53:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:53:48Z- TNavFilter::4 good beams. 2024-11-06T02:53:48Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:53:48Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:53:48Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:53:48Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:53:48Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:53:48Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:53:48Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:53:48Z- TerrainMapOctree: (vehN, vehE ) = (4061544.83, 594434.10). 2024-11-06T02:53:48Z- TerrainMapOctree: Min Distance = 275.66. 2024-11-06T02:53:48Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:53:48Z- Forcing Subcloud Comparison 2024-11-06T02:53:48Z- Weighting particles with subcloud comparison 2024-11-06T02:53:48Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:48Z- meanExpectedMeasDiff: -1.334819 delta_rms_squared: 3.891976 alpha: 0.994887 2024-11-06T02:53:48Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:48Z- meanExpectedMeasDiff: 2.079343 delta_rms_squared: 1.237392 alpha: 0.984094 2024-11-06T02:53:48Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:48Z- meanExpectedMeasDiff: -0.455197 delta_rms_squared: 1.622023 alpha: 0.987820 2024-11-06T02:53:48Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:48Z- meanExpectedMeasDiff: -0.642483 delta_rms_squared: 3.478734 alpha: 0.994284 2024-11-06T02:53:48Z- Subcloud NIS: 0.446579 2024-11-06T02:53:48Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:53:48Z- TNavPF:: sumWeights = 0.261293 2024-11-06T02:53:48Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:53:48Z- TNavPF::First Term of Map Covariance Matrix is 0.56 2024-11-06T02:53:48Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T02:53:48Z- TerrainNav::Resampling particle filter... 2024-11-06T02:53:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861627.45 sec. 2024-11-06T02:53:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:53:48Z- variances([0], [2], and psi_berg) = 0.161831 1.348419 0.000000 2024-11-06T02:53:48Z- publishEstimatesDecoder() - at 1730861628.23 sec 2024-11-06T02:53:48Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:53:48Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061576.86 594430.89 46.59 2024-11-06T02:53:48Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061577.99 594417.77 47.43 lat/lon: 36.70 -121.94 2024-11-06T02:53:48Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 1.35 0.72 0.00 2024-11-06T02:53:48Z- handleDepth()- Depth_Keller msg: 1730861628.65 epoch sec; seqNo:667; depth 44.42 2024-11-06T02:53:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861628.66 epoch sec; seqNo:667; 4061545.18 north; 594433.39 east; 36.69 lat; -121.94 lon; 44.42 depth; utmZone: 10 2024-11-06T02:53:48Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:53:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861628.23 2024-11-06T02:53:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:48Z- waiting for 1730861632.23, time is 1730861628.66 2024-11-06T02:53:49Z- AHRS_M2 msg: 1730861629.02 epoch sec; seqNo:619 2024-11-06T02:53:49Z- AHRS_M2 msg: 0.15 phi; -0.16 theta; 5.26 psi 2024-11-06T02:53:49Z- handleDepth()- Depth_Keller msg: 1730861629.02 epoch sec; seqNo:668; depth 44.49 2024-11-06T02:53:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861629.03 epoch sec; seqNo:668; 4061545.56 north; 594433.39 east; 36.69 lat; -121.94 lon; 44.49 depth; utmZone: 10 2024-11-06T02:53:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861628.23 2024-11-06T02:53:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:49Z- waiting for 1730861632.23, time is 1730861629.03 2024-11-06T02:53:49Z- AHRS_M2 msg: 1730861629.42 epoch sec; seqNo:620 2024-11-06T02:53:49Z- AHRS_M2 msg: 0.15 phi; -0.16 theta; 5.26 psi 2024-11-06T02:53:49Z- handleDepth()- Depth_Keller msg: 1730861629.43 epoch sec; seqNo:669; depth 44.53 2024-11-06T02:53:49Z- handleDVL() - DVL msg epochMillisec: 1730861629.47 2024-11-06T02:53:49Z- handleDvl() - RDI_Pathfinder msg: 1730861629.47 sec; seqNo:211; #beams:4 2024-11-06T02:53:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.38 , 37.54 , 42.03 , 48.56 2024-11-06T02:53:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.06 , 0.01 2024-11-06T02:53:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861629.49 epoch sec; seqNo:669; 4061545.55 north; 594432.17 east; 36.69 lat; -121.94 lon; 44.53 depth; utmZone: 10 2024-11-06T02:53:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861628.23 2024-11-06T02:53:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:49Z- waiting for 1730861632.23, time is 1730861629.49 2024-11-06T02:53:49Z- AHRS_M2 msg: 1730861629.83 epoch sec; seqNo:621 2024-11-06T02:53:49Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.26 psi 2024-11-06T02:53:49Z- handleDepth()- Depth_Keller msg: 1730861629.83 epoch sec; seqNo:670; depth 44.59 2024-11-06T02:53:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861629.84 epoch sec; seqNo:670; 4061545.93 north; 594432.16 east; 36.69 lat; -121.94 lon; 44.59 depth; utmZone: 10 2024-11-06T02:53:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861628.23 2024-11-06T02:53:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:49Z- waiting for 1730861632.23, time is 1730861629.84 2024-11-06T02:53:50Z- AHRS_M2 msg: 1730861630.23 epoch sec; seqNo:622 2024-11-06T02:53:50Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.26 psi 2024-11-06T02:53:50Z- handleDepth()- Depth_Keller msg: 1730861630.25 epoch sec; seqNo:671; depth 44.66 2024-11-06T02:53:50Z- handleDVL() - DVL msg epochMillisec: 1730861630.25 2024-11-06T02:53:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:53:50Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:53:50Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:53:50Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:53:50Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:53:50Z- handleDvl() - RDI_Pathfinder msg: 1730861630.25 sec; seqNo:212; #beams:0 2024-11-06T02:53:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:53:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.06 , 0.01 2024-11-06T02:53:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861630.26 epoch sec; seqNo:671; 4061545.93 north; 594432.16 east; 36.69 lat; -121.94 lon; 44.66 depth; utmZone: 10 2024-11-06T02:53:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861628.23 2024-11-06T02:53:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:50Z- waiting for 1730861632.23, time is 1730861630.26 2024-11-06T02:53:50Z- AHRS_M2 msg: 1730861630.63 epoch sec; seqNo:623 2024-11-06T02:53:50Z- AHRS_M2 msg: 0.16 phi; -0.17 theta; 5.27 psi 2024-11-06T02:53:50Z- handleDepth()- Depth_Keller msg: 1730861630.64 epoch sec; seqNo:672; depth 44.72 2024-11-06T02:53:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861630.66 epoch sec; seqNo:672; 4061546.31 north; 594432.16 east; 36.69 lat; -121.94 lon; 44.72 depth; utmZone: 10 2024-11-06T02:53:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861628.23 2024-11-06T02:53:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:50Z- waiting for 1730861632.23, time is 1730861630.66 2024-11-06T02:53:51Z- AHRS_M2 msg: 1730861631.04 epoch sec; seqNo:624 2024-11-06T02:53:51Z- AHRS_M2 msg: 0.16 phi; -0.17 theta; 5.27 psi 2024-11-06T02:53:51Z- handleDepth()- Depth_Keller msg: 1730861631.05 epoch sec; seqNo:673; depth 44.77 2024-11-06T02:53:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861631.06 epoch sec; seqNo:673; 4061546.29 north; 594430.94 east; 36.69 lat; -121.94 lon; 44.77 depth; utmZone: 10 2024-11-06T02:53:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861628.23 2024-11-06T02:53:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:51Z- waiting for 1730861632.23, time is 1730861631.06 2024-11-06T02:53:51Z- AHRS_M2 msg: 1730861631.44 epoch sec; seqNo:625 2024-11-06T02:53:51Z- AHRS_M2 msg: 0.16 phi; -0.17 theta; 5.27 psi 2024-11-06T02:53:51Z- handleDepth()- Depth_Keller msg: 1730861631.45 epoch sec; seqNo:674; depth 44.82 2024-11-06T02:53:51Z- handleDVL() - DVL msg epochMillisec: 1730861631.50 2024-11-06T02:53:51Z- handleDvl() - RDI_Pathfinder msg: 1730861631.50 sec; seqNo:213; #beams:4 2024-11-06T02:53:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.79 , 37.54 , 41.63 , 48.77 2024-11-06T02:53:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.05 , -0.00 2024-11-06T02:53:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861631.51 epoch sec; seqNo:674; 4061546.67 north; 594430.93 east; 36.69 lat; -121.94 lon; 44.82 depth; utmZone: 10 2024-11-06T02:53:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861628.23 2024-11-06T02:53:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:51Z- waiting for 1730861632.23, time is 1730861631.51 2024-11-06T02:53:51Z- AHRS_M2 msg: 1730861631.85 epoch sec; seqNo:626 2024-11-06T02:53:51Z- AHRS_M2 msg: 0.16 phi; -0.17 theta; 5.27 psi 2024-11-06T02:53:51Z- handleDepth()- Depth_Keller msg: 1730861631.85 epoch sec; seqNo:675; depth 44.88 2024-11-06T02:53:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861631.87 epoch sec; seqNo:675; 4061546.67 north; 594430.93 east; 36.69 lat; -121.94 lon; 44.88 depth; utmZone: 10 2024-11-06T02:53:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861628.23 2024-11-06T02:53:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:52Z- waiting for 1730861632.23, time is 1730861631.87 2024-11-06T02:53:52Z- handleDepth()- Depth_Keller msg: 1730861632.27 epoch sec; seqNo:676; depth 44.92 2024-11-06T02:53:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861632.28 epoch sec; seqNo:676; 4061546.66 north; 594429.71 east; 36.69 lat; -121.94 lon; 44.92 depth; utmZone: 10 2024-11-06T02:53:52Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:53:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861628.23 2024-11-06T02:53:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:52Z- waiting for 1730861632.23, time is 1730861632.28 2024-11-06T02:53:52Z- LcmTrn::updateTrn() >>>> pose time 1730861632.28, meas time 1730861631.50 <<<< 2024-11-06T02:53:52Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861632.28 2024-11-06T02:53:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861631.50 sec, ping # 0; waiting for INS data more recent than 1730861628.23... 2024-11-06T02:53:52Z- TerrainNav::motionUpdate() - currEstimate = (4061547, 594429.71, 44.92) 2024-11-06T02:53:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:53:52Z- TerrainNav:: SubcloudNIS is 0.45, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:53:52Z- TNavPF::Projecting Measurements 2024-11-06T02:53:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:53:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:53:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:53:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:53:52Z- TNavFilter::4 good beams. 2024-11-06T02:53:52Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:53:52Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:53:52Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:53:52Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:53:52Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:53:52Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:53:52Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:53:52Z- TerrainMapOctree: (vehN, vehE ) = (4061546.37, 594430.42). 2024-11-06T02:53:52Z- TerrainMapOctree: Min Distance = 272.06. 2024-11-06T02:53:52Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:53:52Z- Forcing Subcloud Comparison 2024-11-06T02:53:52Z- Weighting particles with subcloud comparison 2024-11-06T02:53:52Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:52Z- meanExpectedMeasDiff: -2.103501 delta_rms_squared: 2.145458 alpha: 0.990764 2024-11-06T02:53:52Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:52Z- meanExpectedMeasDiff: 1.913518 delta_rms_squared: 0.991594 alpha: 0.980229 2024-11-06T02:53:52Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:52Z- meanExpectedMeasDiff: -0.249174 delta_rms_squared: 1.105746 alpha: 0.982234 2024-11-06T02:53:52Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:52Z- meanExpectedMeasDiff: 2.820374 delta_rms_squared: 2.651191 alpha: 0.992513 2024-11-06T02:53:52Z- Subcloud NIS: 0.448433 2024-11-06T02:53:52Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:53:52Z- TNavPF:: sumWeights = 0.149451 2024-11-06T02:53:52Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:53:52Z- TNavPF::First Term of Map Covariance Matrix is 0.80 2024-11-06T02:53:52Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.02 2024-11-06T02:53:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861631.50 sec. 2024-11-06T02:53:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:53:52Z- variances([0], [2], and psi_berg) = 0.184894 0.994491 0.000000 2024-11-06T02:53:52Z- publishEstimatesDecoder() - at 1730861632.28 sec 2024-11-06T02:53:52Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:53:52Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061582.50 594418.53 47.16 2024-11-06T02:53:52Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061579.48 594414.10 47.67 lat/lon: 36.70 -121.94 2024-11-06T02:53:52Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.99 0.67 0.00 2024-11-06T02:53:52Z- AHRS_M2 msg: 1730861632.66 epoch sec; seqNo:627 2024-11-06T02:53:52Z- AHRS_M2 msg: 0.16 phi; -0.17 theta; 5.26 psi 2024-11-06T02:53:52Z- handleDepth()- Depth_Keller msg: 1730861632.67 epoch sec; seqNo:677; depth 45.01 2024-11-06T02:53:52Z- handleDVL() - DVL msg epochMillisec: 1730861632.68 2024-11-06T02:53:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:53:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:53:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:53:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:53:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:53:52Z- handleDvl() - RDI_Pathfinder msg: 1730861632.68 sec; seqNo:214; #beams:0 2024-11-06T02:53:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:53:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.05 , -0.00 2024-11-06T02:53:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861632.72 epoch sec; seqNo:677; 4061547.04 north; 594429.71 east; 36.69 lat; -121.94 lon; 45.01 depth; utmZone: 10 2024-11-06T02:53:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861632.28 2024-11-06T02:53:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:52Z- waiting for 1730861636.28, time is 1730861632.72 2024-11-06T02:53:53Z- AHRS_M2 msg: 1730861633.06 epoch sec; seqNo:628 2024-11-06T02:53:53Z- AHRS_M2 msg: 0.15 phi; -0.17 theta; 5.26 psi 2024-11-06T02:53:53Z- handleDepth()- Depth_Keller msg: 1730861633.07 epoch sec; seqNo:678; depth 45.06 2024-11-06T02:53:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861633.08 epoch sec; seqNo:678; 4061547.04 north; 594429.71 east; 36.69 lat; -121.94 lon; 45.06 depth; utmZone: 10 2024-11-06T02:53:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861632.28 2024-11-06T02:53:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:53Z- waiting for 1730861636.28, time is 1730861633.08 2024-11-06T02:53:53Z- AHRS_M2 msg: 1730861633.47 epoch sec; seqNo:629 2024-11-06T02:53:53Z- AHRS_M2 msg: 0.15 phi; -0.17 theta; 5.26 psi 2024-11-06T02:53:53Z- handleDepth()- Depth_Keller msg: 1730861633.48 epoch sec; seqNo:679; depth 45.12 2024-11-06T02:53:53Z- handleDVL() - DVL msg epochMillisec: 1730861633.50 2024-11-06T02:53:53Z- handleDvl() - RDI_Pathfinder msg: 1730861633.50 sec; seqNo:215; #beams:4 2024-11-06T02:53:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.18 , 37.34 , 41.83 , 48.77 2024-11-06T02:53:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.05 , 0.01 2024-11-06T02:53:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861633.54 epoch sec; seqNo:679; 4061547.42 north; 594429.70 east; 36.69 lat; -121.94 lon; 45.12 depth; utmZone: 10 2024-11-06T02:53:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861632.28 2024-11-06T02:53:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:53Z- waiting for 1730861636.28, time is 1730861633.54 2024-11-06T02:53:53Z- AHRS_M2 msg: 1730861633.86 epoch sec; seqNo:630 2024-11-06T02:53:53Z- AHRS_M2 msg: 0.15 phi; -0.17 theta; 5.26 psi 2024-11-06T02:53:53Z- handleDepth()- Depth_Keller msg: 1730861633.87 epoch sec; seqNo:680; depth 45.19 2024-11-06T02:53:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861633.88 epoch sec; seqNo:680; 4061547.40 north; 594428.48 east; 36.69 lat; -121.94 lon; 45.19 depth; utmZone: 10 2024-11-06T02:53:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861632.28 2024-11-06T02:53:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:54Z- waiting for 1730861636.28, time is 1730861633.88 2024-11-06T02:53:54Z- AHRS_M2 msg: 1730861634.27 epoch sec; seqNo:631 2024-11-06T02:53:54Z- AHRS_M2 msg: 0.15 phi; -0.17 theta; 5.26 psi 2024-11-06T02:53:54Z- handleDepth()- Depth_Keller msg: 1730861634.27 epoch sec; seqNo:681; depth 45.23 2024-11-06T02:53:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861634.28 epoch sec; seqNo:681; 4061547.78 north; 594428.48 east; 36.69 lat; -121.94 lon; 45.23 depth; utmZone: 10 2024-11-06T02:53:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861632.28 2024-11-06T02:53:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:54Z- waiting for 1730861636.28, time is 1730861634.28 2024-11-06T02:53:54Z- AHRS_M2 msg: 1730861634.67 epoch sec; seqNo:632 2024-11-06T02:53:54Z- AHRS_M2 msg: 0.16 phi; -0.17 theta; 5.26 psi 2024-11-06T02:53:54Z- handleDepth()- Depth_Keller msg: 1730861634.69 epoch sec; seqNo:682; depth 45.29 2024-11-06T02:53:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861634.70 epoch sec; seqNo:682; 4061547.78 north; 594428.48 east; 36.69 lat; -121.94 lon; 45.29 depth; utmZone: 10 2024-11-06T02:53:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861632.28 2024-11-06T02:53:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:54Z- waiting for 1730861636.28, time is 1730861634.70 2024-11-06T02:53:55Z- AHRS_M2 msg: 1730861635.08 epoch sec; seqNo:633 2024-11-06T02:53:55Z- AHRS_M2 msg: 0.16 phi; -0.17 theta; 5.26 psi 2024-11-06T02:53:55Z- handleDepth()- Depth_Keller msg: 1730861635.09 epoch sec; seqNo:683; depth 45.35 2024-11-06T02:53:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861635.11 epoch sec; seqNo:683; 4061548.15 north; 594427.26 east; 36.69 lat; -121.94 lon; 45.35 depth; utmZone: 10 2024-11-06T02:53:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861632.28 2024-11-06T02:53:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:55Z- waiting for 1730861636.28, time is 1730861635.11 2024-11-06T02:53:55Z- AHRS_M2 msg: 1730861635.49 epoch sec; seqNo:634 2024-11-06T02:53:55Z- AHRS_M2 msg: 0.16 phi; -0.17 theta; 5.27 psi 2024-11-06T02:53:55Z- handleDepth()- Depth_Keller msg: 1730861635.49 epoch sec; seqNo:684; depth 45.40 2024-11-06T02:53:55Z- handleDVL() - DVL msg epochMillisec: 1730861635.52 2024-11-06T02:53:55Z- handleDvl() - RDI_Pathfinder msg: 1730861635.52 sec; seqNo:216; #beams:4 2024-11-06T02:53:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.97 , 37.13 , 41.42 , 48.56 2024-11-06T02:53:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.05 , 0.04 2024-11-06T02:53:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861635.54 epoch sec; seqNo:684; 4061548.15 north; 594427.26 east; 36.69 lat; -121.94 lon; 45.40 depth; utmZone: 10 2024-11-06T02:53:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861632.28 2024-11-06T02:53:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:55Z- waiting for 1730861636.28, time is 1730861635.54 2024-11-06T02:53:55Z- AHRS_M2 msg: 1730861635.89 epoch sec; seqNo:635 2024-11-06T02:53:55Z- AHRS_M2 msg: 0.16 phi; -0.17 theta; 5.27 psi 2024-11-06T02:53:55Z- handleDepth()- Depth_Keller msg: 1730861635.89 epoch sec; seqNo:685; depth 45.47 2024-11-06T02:53:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861635.91 epoch sec; seqNo:685; 4061548.53 north; 594427.25 east; 36.69 lat; -121.94 lon; 45.47 depth; utmZone: 10 2024-11-06T02:53:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861632.28 2024-11-06T02:53:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:56Z- waiting for 1730861636.28, time is 1730861635.91 2024-11-06T02:53:56Z- AHRS_M2 msg: 1730861636.29 epoch sec; seqNo:636 2024-11-06T02:53:56Z- AHRS_M2 msg: 0.16 phi; -0.17 theta; 5.27 psi 2024-11-06T02:53:56Z- handleDepth()- Depth_Keller msg: 1730861636.30 epoch sec; seqNo:686; depth 45.52 2024-11-06T02:53:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861636.31 epoch sec; seqNo:686; 4061548.53 north; 594427.25 east; 36.69 lat; -121.94 lon; 45.52 depth; utmZone: 10 2024-11-06T02:53:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861632.28 2024-11-06T02:53:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:56Z- waiting for 1730861636.28, time is 1730861636.31 2024-11-06T02:53:56Z- LcmTrn::updateTrn() >>>> pose time 1730861636.31, meas time 1730861635.52 <<<< 2024-11-06T02:53:56Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861636.31 2024-11-06T02:53:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861635.52 sec, ping # 0; waiting for INS data more recent than 1730861632.28... 2024-11-06T02:53:56Z- TerrainNav::motionUpdate() - currEstimate = (4061549, 594427.25, 45.52) 2024-11-06T02:53:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:53:56Z- TerrainNav:: SubcloudNIS is 0.45, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:53:56Z- TNavPF::Projecting Measurements 2024-11-06T02:53:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:53:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:53:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:53:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:53:56Z- TNavFilter::4 good beams. 2024-11-06T02:53:56Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:53:56Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:53:56Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:53:56Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:53:56Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:53:56Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:53:56Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:53:56Z- TerrainMapOctree: (vehN, vehE ) = (4061548.16, 594427.73). 2024-11-06T02:53:56Z- TerrainMapOctree: Min Distance = 268.94. 2024-11-06T02:53:56Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:53:56Z- Forcing Subcloud Comparison 2024-11-06T02:53:56Z- Weighting particles with subcloud comparison 2024-11-06T02:53:56Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:56Z- meanExpectedMeasDiff: -3.704966 delta_rms_squared: 2.872170 alpha: 0.993085 2024-11-06T02:53:56Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:56Z- meanExpectedMeasDiff: 1.611647 delta_rms_squared: 0.767538 alpha: 0.974604 2024-11-06T02:53:56Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:56Z- meanExpectedMeasDiff: -0.262275 delta_rms_squared: 1.353067 alpha: 0.985434 2024-11-06T02:53:56Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:53:56Z- meanExpectedMeasDiff: 3.084222 delta_rms_squared: 1.772347 alpha: 0.988841 2024-11-06T02:53:56Z- Subcloud NIS: 0.470324 2024-11-06T02:53:56Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:53:56Z- TNavPF:: sumWeights = 0.057405 2024-11-06T02:53:56Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:53:56Z- TNavPF::First Term of Map Covariance Matrix is 0.74 2024-11-06T02:53:56Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T02:53:56Z- TerrainNav::Resampling particle filter... 2024-11-06T02:53:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861635.52 sec. 2024-11-06T02:53:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:53:56Z- variances([0], [2], and psi_berg) = 0.269168 4.553489 0.000000 2024-11-06T02:53:56Z- publishEstimatesDecoder() - at 1730861636.31 sec 2024-11-06T02:53:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:53:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061580.22 594424.71 47.96 2024-11-06T02:53:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061581.34 594411.97 48.28 lat/lon: 36.70 -121.94 2024-11-06T02:53:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 4.55 0.73 0.00 2024-11-06T02:53:56Z- AHRS_M2 msg: 1730861636.70 epoch sec; seqNo:637 2024-11-06T02:53:56Z- AHRS_M2 msg: 0.16 phi; -0.17 theta; 5.27 psi 2024-11-06T02:53:56Z- handleDepth()- Depth_Keller msg: 1730861636.71 epoch sec; seqNo:687; depth 45.59 2024-11-06T02:53:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861636.72 epoch sec; seqNo:687; 4061548.89 north; 594426.03 east; 36.69 lat; -121.94 lon; 45.59 depth; utmZone: 10 2024-11-06T02:53:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861636.31 2024-11-06T02:53:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:56Z- waiting for 1730861640.31, time is 1730861636.72 2024-11-06T02:53:57Z- AHRS_M2 msg: 1730861637.11 epoch sec; seqNo:638 2024-11-06T02:53:57Z- AHRS_M2 msg: 0.16 phi; -0.17 theta; 5.27 psi 2024-11-06T02:53:57Z- handleDepth()- Depth_Keller msg: 1730861637.12 epoch sec; seqNo:688; depth 45.64 2024-11-06T02:53:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861637.13 epoch sec; seqNo:688; 4061548.89 north; 594426.03 east; 36.69 lat; -121.94 lon; 45.64 depth; utmZone: 10 2024-11-06T02:53:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861636.31 2024-11-06T02:53:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:57Z- waiting for 1730861640.31, time is 1730861637.13 2024-11-06T02:53:57Z- AHRS_M2 msg: 1730861637.50 epoch sec; seqNo:639 2024-11-06T02:53:57Z- AHRS_M2 msg: 0.17 phi; -0.16 theta; 5.28 psi 2024-11-06T02:53:57Z- handleDepth()- Depth_Keller msg: 1730861637.50 epoch sec; seqNo:689; depth 45.68 2024-11-06T02:53:57Z- handleDVL() - DVL msg epochMillisec: 1730861637.55 2024-11-06T02:53:57Z- handleDvl() - RDI_Pathfinder msg: 1730861637.55 sec; seqNo:217; #beams:4 2024-11-06T02:53:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.17 , 36.32 , 41.21 , 48.15 2024-11-06T02:53:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , 0.03 2024-11-06T02:53:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861637.57 epoch sec; seqNo:689; 4061549.27 north; 594426.02 east; 36.69 lat; -121.94 lon; 45.68 depth; utmZone: 10 2024-11-06T02:53:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861636.31 2024-11-06T02:53:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:57Z- waiting for 1730861640.31, time is 1730861637.57 2024-11-06T02:53:57Z- AHRS_M2 msg: 1730861637.90 epoch sec; seqNo:640 2024-11-06T02:53:57Z- AHRS_M2 msg: 0.17 phi; -0.16 theta; 5.28 psi 2024-11-06T02:53:57Z- handleDepth()- Depth_Keller msg: 1730861637.91 epoch sec; seqNo:690; depth 45.75 2024-11-06T02:53:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861637.93 epoch sec; seqNo:690; 4061549.27 north; 594426.02 east; 36.69 lat; -121.94 lon; 45.75 depth; utmZone: 10 2024-11-06T02:53:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861636.31 2024-11-06T02:53:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:58Z- waiting for 1730861640.31, time is 1730861637.93 2024-11-06T02:53:58Z- AHRS_M2 msg: 1730861638.31 epoch sec; seqNo:641 2024-11-06T02:53:58Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.28 psi 2024-11-06T02:53:58Z- handleDepth()- Depth_Keller msg: 1730861638.32 epoch sec; seqNo:691; depth 45.80 2024-11-06T02:53:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861638.33 epoch sec; seqNo:691; 4061549.63 north; 594424.80 east; 36.69 lat; -121.94 lon; 45.80 depth; utmZone: 10 2024-11-06T02:53:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861636.31 2024-11-06T02:53:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:58Z- waiting for 1730861640.31, time is 1730861638.33 2024-11-06T02:53:58Z- AHRS_M2 msg: 1730861638.72 epoch sec; seqNo:642 2024-11-06T02:53:58Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.28 psi 2024-11-06T02:53:58Z- handleDepth()- Depth_Keller msg: 1730861638.72 epoch sec; seqNo:692; depth 45.85 2024-11-06T02:53:58Z- handleDVL() - DVL msg epochMillisec: 1730861638.72 2024-11-06T02:53:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:53:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:53:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:53:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:53:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:53:58Z- handleDvl() - RDI_Pathfinder msg: 1730861638.72 sec; seqNo:218; #beams:0 2024-11-06T02:53:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:53:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , 0.03 2024-11-06T02:53:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861638.73 epoch sec; seqNo:692; 4061549.63 north; 594424.80 east; 36.69 lat; -121.94 lon; 45.85 depth; utmZone: 10 2024-11-06T02:53:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861636.31 2024-11-06T02:53:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:58Z- waiting for 1730861640.31, time is 1730861638.73 2024-11-06T02:53:59Z- AHRS_M2 msg: 1730861639.12 epoch sec; seqNo:643 2024-11-06T02:53:59Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.29 psi 2024-11-06T02:53:59Z- handleDepth()- Depth_Keller msg: 1730861639.12 epoch sec; seqNo:693; depth 45.89 2024-11-06T02:53:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861639.14 epoch sec; seqNo:693; 4061550.01 north; 594424.79 east; 36.69 lat; -121.94 lon; 45.89 depth; utmZone: 10 2024-11-06T02:53:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:53:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861636.31 2024-11-06T02:53:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:59Z- waiting for 1730861640.31, time is 1730861639.14 2024-11-06T02:53:59Z- AHRS_M2 msg: 1730861639.52 epoch sec; seqNo:644 2024-11-06T02:53:59Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.29 psi 2024-11-06T02:53:59Z- handleDepth()- Depth_Keller msg: 1730861639.54 epoch sec; seqNo:694; depth 45.93 2024-11-06T02:53:59Z- handleDVL() - DVL msg epochMillisec: 1730861639.57 2024-11-06T02:53:59Z- handleDvl() - RDI_Pathfinder msg: 1730861639.57 sec; seqNo:219; #beams:4 2024-11-06T02:53:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.97 , 37.34 , 41.42 , 47.74 2024-11-06T02:53:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , -0.01 2024-11-06T02:53:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861639.59 epoch sec; seqNo:694; 4061550.00 north; 594423.57 east; 36.69 lat; -121.94 lon; 45.93 depth; utmZone: 10 2024-11-06T02:53:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:53:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861636.31 2024-11-06T02:53:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:53:59Z- waiting for 1730861640.31, time is 1730861639.59 2024-11-06T02:53:59Z- AHRS_M2 msg: 1730861639.92 epoch sec; seqNo:645 2024-11-06T02:53:59Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.29 psi 2024-11-06T02:53:59Z- handleDepth()- Depth_Keller msg: 1730861639.93 epoch sec; seqNo:695; depth 45.98 2024-11-06T02:53:59Z- handleDVL() - DVL msg epochMillisec: 1730861639.93 2024-11-06T02:53:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:53:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:53:59Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:53:59Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:53:59Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:53:59Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:53:59Z- handleDvl() - RDI_Pathfinder msg: 1730861639.93 sec; seqNo:220; #beams:0 2024-11-06T02:53:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:53:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , -0.01 2024-11-06T02:53:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861639.95 epoch sec; seqNo:695; 4061550.38 north; 594423.57 east; 36.69 lat; -121.94 lon; 45.98 depth; utmZone: 10 2024-11-06T02:54:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861636.31 2024-11-06T02:54:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:00Z- waiting for 1730861640.31, time is 1730861639.95 2024-11-06T02:54:00Z- AHRS_M2 msg: 1730861640.33 epoch sec; seqNo:646 2024-11-06T02:54:00Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.29 psi 2024-11-06T02:54:00Z- handleDepth()- Depth_Keller msg: 1730861640.33 epoch sec; seqNo:696; depth 46.04 2024-11-06T02:54:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861640.35 epoch sec; seqNo:696; 4061550.38 north; 594423.57 east; 36.69 lat; -121.94 lon; 46.04 depth; utmZone: 10 2024-11-06T02:54:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861636.31 2024-11-06T02:54:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:00Z- waiting for 1730861640.31, time is 1730861640.35 2024-11-06T02:54:00Z- LcmTrn::updateTrn() >>>> pose time 1730861640.35, meas time 1730861639.93 <<<< 2024-11-06T02:54:00Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861640.35 2024-11-06T02:54:00Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:54:00Z- TerrainNav::motionUpdate() - currEstimate = (4061550, 594423.57, 46.04) 2024-11-06T02:54:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:54:00Z- TerrainNav:: SubcloudNIS is 0.47, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:54:00Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:54:00Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:54:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:54:00Z- variances([0], [2], and psi_berg) = 0.271702 4.555650 0.000000 2024-11-06T02:54:00Z- publishEstimatesDecoder() - at 1730861640.35 sec 2024-11-06T02:54:00Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:54:00Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061582.01 594420.92 49.35 2024-11-06T02:54:00Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061583.20 594408.29 48.80 lat/lon: 36.70 -121.94 2024-11-06T02:54:00Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 4.56 0.99 0.00 2024-11-06T02:54:00Z- AHRS_M2 msg: 1730861640.73 epoch sec; seqNo:647 2024-11-06T02:54:00Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.30 psi 2024-11-06T02:54:00Z- handleDepth()- Depth_Keller msg: 1730861640.74 epoch sec; seqNo:697; depth 46.08 2024-11-06T02:54:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861640.76 epoch sec; seqNo:697; 4061550.76 north; 594423.57 east; 36.69 lat; -121.94 lon; 46.08 depth; utmZone: 10 2024-11-06T02:54:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861640.35 2024-11-06T02:54:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:00Z- waiting for 1730861644.35, time is 1730861640.76 2024-11-06T02:54:01Z- AHRS_M2 msg: 1730861641.13 epoch sec; seqNo:648 2024-11-06T02:54:01Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.30 psi 2024-11-06T02:54:01Z- handleDepth()- Depth_Keller msg: 1730861641.15 epoch sec; seqNo:698; depth 46.12 2024-11-06T02:54:01Z- handleDVL() - DVL msg epochMillisec: 1730861641.15 2024-11-06T02:54:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:01Z- handleDvl() - RDI_Pathfinder msg: 1730861641.15 sec; seqNo:221; #beams:0 2024-11-06T02:54:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , -0.01 2024-11-06T02:54:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861641.16 epoch sec; seqNo:698; 4061550.74 north; 594422.34 east; 36.69 lat; -121.94 lon; 46.12 depth; utmZone: 10 2024-11-06T02:54:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861640.35 2024-11-06T02:54:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:01Z- waiting for 1730861644.35, time is 1730861641.16 2024-11-06T02:54:01Z- AHRS_M2 msg: 1730861641.55 epoch sec; seqNo:649 2024-11-06T02:54:01Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.30 psi 2024-11-06T02:54:01Z- handleDepth()- Depth_Keller msg: 1730861641.56 epoch sec; seqNo:699; depth 46.18 2024-11-06T02:54:01Z- handleDVL() - DVL msg epochMillisec: 1730861641.58 2024-11-06T02:54:01Z- handleDvl() - RDI_Pathfinder msg: 1730861641.58 sec; seqNo:222; #beams:4 2024-11-06T02:54:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.35 , 37.75 , 40.81 , 47.74 2024-11-06T02:54:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.06 , -0.03 2024-11-06T02:54:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861641.63 epoch sec; seqNo:699; 4061551.12 north; 594422.34 east; 36.69 lat; -121.94 lon; 46.18 depth; utmZone: 10 2024-11-06T02:54:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861640.35 2024-11-06T02:54:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:01Z- waiting for 1730861644.35, time is 1730861641.63 2024-11-06T02:54:01Z- AHRS_M2 msg: 1730861641.94 epoch sec; seqNo:650 2024-11-06T02:54:01Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.30 psi 2024-11-06T02:54:01Z- handleDepth()- Depth_Keller msg: 1730861641.95 epoch sec; seqNo:700; depth 46.24 2024-11-06T02:54:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861642.01 epoch sec; seqNo:700; 4061551.12 north; 594422.34 east; 36.69 lat; -121.94 lon; 46.24 depth; utmZone: 10 2024-11-06T02:54:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861640.35 2024-11-06T02:54:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:02Z- waiting for 1730861644.35, time is 1730861642.01 2024-11-06T02:54:02Z- AHRS_M2 msg: 1730861642.35 epoch sec; seqNo:651 2024-11-06T02:54:02Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.31 psi 2024-11-06T02:54:02Z- handleDepth()- Depth_Keller msg: 1730861642.35 epoch sec; seqNo:701; depth 46.28 2024-11-06T02:54:02Z- handleDVL() - DVL msg epochMillisec: 1730861642.35 2024-11-06T02:54:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:02Z- handleDvl() - RDI_Pathfinder msg: 1730861642.35 sec; seqNo:223; #beams:0 2024-11-06T02:54:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.06 , -0.03 2024-11-06T02:54:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861642.36 epoch sec; seqNo:701; 4061551.50 north; 594422.34 east; 36.69 lat; -121.94 lon; 46.28 depth; utmZone: 10 2024-11-06T02:54:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861640.35 2024-11-06T02:54:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:02Z- waiting for 1730861644.35, time is 1730861642.36 2024-11-06T02:54:02Z- AHRS_M2 msg: 1730861642.77 epoch sec; seqNo:652 2024-11-06T02:54:02Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.31 psi 2024-11-06T02:54:02Z- handleDepth()- Depth_Keller msg: 1730861642.77 epoch sec; seqNo:702; depth 46.32 2024-11-06T02:54:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861642.78 epoch sec; seqNo:702; 4061551.49 north; 594421.12 east; 36.69 lat; -121.94 lon; 46.32 depth; utmZone: 10 2024-11-06T02:54:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861640.35 2024-11-06T02:54:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:02Z- waiting for 1730861644.35, time is 1730861642.78 2024-11-06T02:54:03Z- AHRS_M2 msg: 1730861643.15 epoch sec; seqNo:653 2024-11-06T02:54:03Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.31 psi 2024-11-06T02:54:03Z- handleDepth()- Depth_Keller msg: 1730861643.16 epoch sec; seqNo:703; depth 46.38 2024-11-06T02:54:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861643.17 epoch sec; seqNo:703; 4061551.87 north; 594421.11 east; 36.69 lat; -121.94 lon; 46.38 depth; utmZone: 10 2024-11-06T02:54:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861640.35 2024-11-06T02:54:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:03Z- waiting for 1730861644.35, time is 1730861643.17 2024-11-06T02:54:03Z- AHRS_M2 msg: 1730861643.56 epoch sec; seqNo:654 2024-11-06T02:54:03Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.31 psi 2024-11-06T02:54:03Z- handleDepth()- Depth_Keller msg: 1730861643.57 epoch sec; seqNo:704; depth 46.44 2024-11-06T02:54:03Z- handleDVL() - DVL msg epochMillisec: 1730861643.59 2024-11-06T02:54:03Z- handleDvl() - RDI_Pathfinder msg: 1730861643.59 sec; seqNo:224; #beams:4 2024-11-06T02:54:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.94 , 37.74 , 40.39 , 46.92 2024-11-06T02:54:03Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.05 , -0.01 2024-11-06T02:54:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861643.61 epoch sec; seqNo:704; 4061552.25 north; 594421.11 east; 36.69 lat; -121.94 lon; 46.44 depth; utmZone: 10 2024-11-06T02:54:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861640.35 2024-11-06T02:54:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:03Z- waiting for 1730861644.35, time is 1730861643.61 2024-11-06T02:54:03Z- AHRS_M2 msg: 1730861643.96 epoch sec; seqNo:655 2024-11-06T02:54:03Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.31 psi 2024-11-06T02:54:03Z- handleDepth()- Depth_Keller msg: 1730861643.97 epoch sec; seqNo:705; depth 46.47 2024-11-06T02:54:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861643.99 epoch sec; seqNo:705; 4061552.25 north; 594421.11 east; 36.69 lat; -121.94 lon; 46.47 depth; utmZone: 10 2024-11-06T02:54:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861640.35 2024-11-06T02:54:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:04Z- waiting for 1730861644.35, time is 1730861643.99 2024-11-06T02:54:04Z- AHRS_M2 msg: 1730861644.37 epoch sec; seqNo:656 2024-11-06T02:54:04Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.31 psi 2024-11-06T02:54:04Z- handleDepth()- Depth_Keller msg: 1730861644.38 epoch sec; seqNo:706; depth 46.53 2024-11-06T02:54:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861644.39 epoch sec; seqNo:706; 4061552.61 north; 594419.88 east; 36.69 lat; -121.94 lon; 46.53 depth; utmZone: 10 2024-11-06T02:54:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861640.35 2024-11-06T02:54:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:04Z- waiting for 1730861644.35, time is 1730861644.39 2024-11-06T02:54:04Z- LcmTrn::updateTrn() >>>> pose time 1730861644.39, meas time 1730861643.59 <<<< 2024-11-06T02:54:04Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861644.39 2024-11-06T02:54:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861643.59 sec, ping # 0; waiting for INS data more recent than 1730861640.35... 2024-11-06T02:54:04Z- TerrainNav::motionUpdate() - currEstimate = (4061553, 594419.88, 46.53) 2024-11-06T02:54:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:54:04Z- TerrainNav:: SubcloudNIS is 0.47, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:54:04Z- TNavPF::Projecting Measurements 2024-11-06T02:54:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:54:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:54:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:54:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:54:04Z- TNavFilter::4 good beams. 2024-11-06T02:54:04Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:54:04Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:54:04Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:54:04Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:54:04Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:54:04Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:54:04Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:54:04Z- TerrainMapOctree: (vehN, vehE ) = (4061552.17, 594420.61). 2024-11-06T02:54:04Z- TerrainMapOctree: Min Distance = 261.23. 2024-11-06T02:54:04Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:54:04Z- Forcing Subcloud Comparison 2024-11-06T02:54:04Z- Weighting particles with subcloud comparison 2024-11-06T02:54:04Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:04Z- meanExpectedMeasDiff: -4.811249 delta_rms_squared: 2.273381 alpha: 0.991279 2024-11-06T02:54:04Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:04Z- meanExpectedMeasDiff: 3.078353 delta_rms_squared: 1.058134 alpha: 0.981449 2024-11-06T02:54:04Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:04Z- meanExpectedMeasDiff: -0.133927 delta_rms_squared: 1.413052 alpha: 0.986044 2024-11-06T02:54:04Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:04Z- meanExpectedMeasDiff: 3.285311 delta_rms_squared: 1.573934 alpha: 0.987452 2024-11-06T02:54:04Z- Subcloud NIS: 0.511708 2024-11-06T02:54:04Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:54:04Z- TNavPF:: sumWeights = 0.010748 2024-11-06T02:54:04Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:54:04Z- TNavPF::First Term of Map Covariance Matrix is 0.24 2024-11-06T02:54:04Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T02:54:04Z- TerrainNav::Resampling particle filter... 2024-11-06T02:54:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861643.59 sec. 2024-11-06T02:54:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:54:04Z- variances([0], [2], and psi_berg) = 0.315152 21.910705 0.000000 2024-11-06T02:54:04Z- publishEstimatesDecoder() - at 1730861644.39 sec 2024-11-06T02:54:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:54:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061584.20 594417.25 49.60 2024-11-06T02:54:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061585.22 594406.17 49.09 lat/lon: 36.70 -121.94 2024-11-06T02:54:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.32 21.91 0.97 0.00 2024-11-06T02:54:04Z- AHRS_M2 msg: 1730861644.78 epoch sec; seqNo:657 2024-11-06T02:54:04Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.32 psi 2024-11-06T02:54:04Z- handleDepth()- Depth_Keller msg: 1730861644.79 epoch sec; seqNo:707; depth 46.59 2024-11-06T02:54:04Z- handleDVL() - DVL msg epochMillisec: 1730861644.80 2024-11-06T02:54:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:04Z- handleDvl() - RDI_Pathfinder msg: 1730861644.80 sec; seqNo:225; #beams:0 2024-11-06T02:54:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:04Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.05 , -0.01 2024-11-06T02:54:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861644.82 epoch sec; seqNo:707; 4061552.61 north; 594419.88 east; 36.69 lat; -121.94 lon; 46.59 depth; utmZone: 10 2024-11-06T02:54:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861644.39 2024-11-06T02:54:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:04Z- waiting for 1730861648.39, time is 1730861644.82 2024-11-06T02:54:05Z- AHRS_M2 msg: 1730861645.18 epoch sec; seqNo:658 2024-11-06T02:54:05Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.32 psi 2024-11-06T02:54:05Z- handleDepth()- Depth_Keller msg: 1730861645.19 epoch sec; seqNo:708; depth 46.65 2024-11-06T02:54:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861645.19 epoch sec; seqNo:708; 4061552.99 north; 594419.88 east; 36.69 lat; -121.94 lon; 46.65 depth; utmZone: 10 2024-11-06T02:54:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861644.39 2024-11-06T02:54:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:05Z- waiting for 1730861648.39, time is 1730861645.19 2024-11-06T02:54:05Z- AHRS_M2 msg: 1730861645.58 epoch sec; seqNo:659 2024-11-06T02:54:05Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.32 psi 2024-11-06T02:54:05Z- handleDepth()- Depth_Keller msg: 1730861645.59 epoch sec; seqNo:709; depth 46.70 2024-11-06T02:54:05Z- handleDVL() - DVL msg epochMillisec: 1730861645.62 2024-11-06T02:54:05Z- handleDvl() - RDI_Pathfinder msg: 1730861645.62 sec; seqNo:226; #beams:4 2024-11-06T02:54:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.15 , 37.34 , 40.60 , 47.33 2024-11-06T02:54:05Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.06 , 0.04 2024-11-06T02:54:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861645.65 epoch sec; seqNo:709; 4061552.98 north; 594418.66 east; 36.69 lat; -121.94 lon; 46.70 depth; utmZone: 10 2024-11-06T02:54:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861644.39 2024-11-06T02:54:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:05Z- waiting for 1730861648.39, time is 1730861645.65 2024-11-06T02:54:05Z- AHRS_M2 msg: 1730861645.98 epoch sec; seqNo:660 2024-11-06T02:54:05Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.32 psi 2024-11-06T02:54:05Z- handleDepth()- Depth_Keller msg: 1730861645.99 epoch sec; seqNo:710; depth 46.76 2024-11-06T02:54:06Z- handleDVL() - DVL msg epochMillisec: 1730861646.00 2024-11-06T02:54:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:06Z- handleDvl() - RDI_Pathfinder msg: 1730861646.00 sec; seqNo:227; #beams:0 2024-11-06T02:54:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:06Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.06 , 0.04 2024-11-06T02:54:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861646.02 epoch sec; seqNo:710; 4061553.36 north; 594418.65 east; 36.69 lat; -121.94 lon; 46.76 depth; utmZone: 10 2024-11-06T02:54:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861644.39 2024-11-06T02:54:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:06Z- waiting for 1730861648.39, time is 1730861646.02 2024-11-06T02:54:06Z- AHRS_M2 msg: 1730861646.39 epoch sec; seqNo:661 2024-11-06T02:54:06Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.32 psi 2024-11-06T02:54:06Z- handleDepth()- Depth_Keller msg: 1730861646.39 epoch sec; seqNo:711; depth 46.82 2024-11-06T02:54:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861646.40 epoch sec; seqNo:711; 4061553.36 north; 594418.65 east; 36.69 lat; -121.94 lon; 46.82 depth; utmZone: 10 2024-11-06T02:54:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861644.39 2024-11-06T02:54:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:06Z- waiting for 1730861648.39, time is 1730861646.40 2024-11-06T02:54:06Z- AHRS_M2 msg: 1730861646.79 epoch sec; seqNo:662 2024-11-06T02:54:06Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.32 psi 2024-11-06T02:54:06Z- handleDepth()- Depth_Keller msg: 1730861646.80 epoch sec; seqNo:712; depth 46.88 2024-11-06T02:54:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861646.82 epoch sec; seqNo:712; 4061553.73 north; 594418.65 east; 36.69 lat; -121.94 lon; 46.88 depth; utmZone: 10 2024-11-06T02:54:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861644.39 2024-11-06T02:54:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:06Z- waiting for 1730861648.39, time is 1730861646.82 2024-11-06T02:54:07Z- AHRS_M2 msg: 1730861647.19 epoch sec; seqNo:663 2024-11-06T02:54:07Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.32 psi 2024-11-06T02:54:07Z- handleDepth()- Depth_Keller msg: 1730861647.20 epoch sec; seqNo:713; depth 46.92 2024-11-06T02:54:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861647.24 epoch sec; seqNo:713; 4061554.10 north; 594417.43 east; 36.69 lat; -121.94 lon; 46.92 depth; utmZone: 10 2024-11-06T02:54:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861644.39 2024-11-06T02:54:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:07Z- waiting for 1730861648.39, time is 1730861647.24 2024-11-06T02:54:07Z- handleDepth()- Depth_Keller msg: 1730861647.62 epoch sec; seqNo:714; depth 46.98 2024-11-06T02:54:07Z- handleDVL() - DVL msg epochMillisec: 1730861647.66 2024-11-06T02:54:07Z- handleDvl() - RDI_Pathfinder msg: 1730861647.66 sec; seqNo:228; #beams:4 2024-11-06T02:54:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.15 , 36.93 , 40.39 , 46.72 2024-11-06T02:54:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.07 , 0.06 2024-11-06T02:54:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861647.67 epoch sec; seqNo:714; 4061554.10 north; 594417.43 east; 36.69 lat; -121.94 lon; 46.98 depth; utmZone: 10 2024-11-06T02:54:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861644.39 2024-11-06T02:54:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:07Z- waiting for 1730861648.39, time is 1730861647.67 2024-11-06T02:54:08Z- AHRS_M2 msg: 1730861648.05 epoch sec; seqNo:664 2024-11-06T02:54:08Z- AHRS_M2 msg: 0.16 phi; -0.16 theta; 5.33 psi 2024-11-06T02:54:08Z- handleDepth()- Depth_Keller msg: 1730861648.08 epoch sec; seqNo:715; depth 47.05 2024-11-06T02:54:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861648.09 epoch sec; seqNo:715; 4061554.48 north; 594417.42 east; 36.69 lat; -121.94 lon; 47.05 depth; utmZone: 10 2024-11-06T02:54:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861644.39 2024-11-06T02:54:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:08Z- waiting for 1730861648.39, time is 1730861648.09 2024-11-06T02:54:08Z- AHRS_M2 msg: 1730861648.41 epoch sec; seqNo:665 2024-11-06T02:54:08Z- AHRS_M2 msg: 0.16 phi; -0.15 theta; 5.33 psi 2024-11-06T02:54:08Z- handleDepth()- Depth_Keller msg: 1730861648.41 epoch sec; seqNo:716; depth 47.12 2024-11-06T02:54:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861648.43 epoch sec; seqNo:716; 4061554.47 north; 594416.20 east; 36.69 lat; -121.94 lon; 47.12 depth; utmZone: 10 2024-11-06T02:54:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861644.39 2024-11-06T02:54:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:08Z- waiting for 1730861648.39, time is 1730861648.43 2024-11-06T02:54:08Z- LcmTrn::updateTrn() >>>> pose time 1730861648.43, meas time 1730861647.66 <<<< 2024-11-06T02:54:08Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861648.43 2024-11-06T02:54:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861647.66 sec, ping # 0; waiting for INS data more recent than 1730861644.39... 2024-11-06T02:54:08Z- TerrainNav::motionUpdate() - currEstimate = (4061554, 594416.20, 47.12) 2024-11-06T02:54:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:54:08Z- TerrainNav:: SubcloudNIS is 0.51, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:54:08Z- TNavPF::Projecting Measurements 2024-11-06T02:54:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:54:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:54:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:54:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:54:08Z- TNavFilter::4 good beams. 2024-11-06T02:54:08Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:54:08Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:54:08Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:54:08Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:54:08Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:54:08Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:54:08Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:54:08Z- TerrainMapOctree: (vehN, vehE ) = (4061554.11, 594416.90). 2024-11-06T02:54:08Z- TerrainMapOctree: Min Distance = 257.34. 2024-11-06T02:54:08Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:54:08Z- Forcing Subcloud Comparison 2024-11-06T02:54:08Z- Weighting particles with subcloud comparison 2024-11-06T02:54:08Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:08Z- meanExpectedMeasDiff: -3.858675 delta_rms_squared: 1.921838 alpha: 0.989700 2024-11-06T02:54:08Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:08Z- meanExpectedMeasDiff: 2.498419 delta_rms_squared: 1.167337 alpha: 0.983156 2024-11-06T02:54:08Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:08Z- meanExpectedMeasDiff: 0.004851 delta_rms_squared: 2.007284 alpha: 0.990135 2024-11-06T02:54:08Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:08Z- meanExpectedMeasDiff: 3.423621 delta_rms_squared: 1.907957 alpha: 0.989626 2024-11-06T02:54:08Z- Subcloud NIS: 0.555804 2024-11-06T02:54:08Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:54:08Z- TNavPF:: sumWeights = 0.027034 2024-11-06T02:54:08Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:54:08Z- TNavPF::First Term of Map Covariance Matrix is 0.44 2024-11-06T02:54:08Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T02:54:08Z- TerrainNav::Resampling particle filter... 2024-11-06T02:54:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861647.66 sec. 2024-11-06T02:54:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:54:08Z- variances([0], [2], and psi_berg) = 0.336085 36.571545 0.000000 2024-11-06T02:54:08Z- publishEstimatesDecoder() - at 1730861648.43 sec 2024-11-06T02:54:08Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:54:08Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061586.34 594413.69 49.24 2024-11-06T02:54:08Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061586.90 594404.43 49.28 lat/lon: 36.70 -121.94 2024-11-06T02:54:08Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.34 36.57 1.03 0.00 2024-11-06T02:54:08Z- AHRS_M2 msg: 1730861648.82 epoch sec; seqNo:666 2024-11-06T02:54:08Z- AHRS_M2 msg: 0.16 phi; -0.15 theta; 5.33 psi 2024-11-06T02:54:08Z- handleDepth()- Depth_Keller msg: 1730861648.83 epoch sec; seqNo:717; depth 47.14 2024-11-06T02:54:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861648.84 epoch sec; seqNo:717; 4061554.84 north; 594416.20 east; 36.69 lat; -121.94 lon; 47.14 depth; utmZone: 10 2024-11-06T02:54:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861648.43 2024-11-06T02:54:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:08Z- waiting for 1730861652.43, time is 1730861648.84 2024-11-06T02:54:09Z- AHRS_M2 msg: 1730861649.23 epoch sec; seqNo:667 2024-11-06T02:54:09Z- AHRS_M2 msg: 0.16 phi; -0.15 theta; 5.33 psi 2024-11-06T02:54:09Z- handleDepth()- Depth_Keller msg: 1730861649.24 epoch sec; seqNo:718; depth 47.19 2024-11-06T02:54:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861649.30 epoch sec; seqNo:718; 4061554.84 north; 594416.20 east; 36.69 lat; -121.94 lon; 47.19 depth; utmZone: 10 2024-11-06T02:54:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861648.43 2024-11-06T02:54:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:09Z- waiting for 1730861652.43, time is 1730861649.30 2024-11-06T02:54:09Z- AHRS_M2 msg: 1730861649.62 epoch sec; seqNo:668 2024-11-06T02:54:09Z- AHRS_M2 msg: 0.16 phi; -0.15 theta; 5.33 psi 2024-11-06T02:54:09Z- handleDepth()- Depth_Keller msg: 1730861649.63 epoch sec; seqNo:719; depth 47.26 2024-11-06T02:54:09Z- handleDVL() - DVL msg epochMillisec: 1730861649.67 2024-11-06T02:54:09Z- handleDvl() - RDI_Pathfinder msg: 1730861649.67 sec; seqNo:229; #beams:4 2024-11-06T02:54:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.74 , 36.93 , 40.40 , 46.31 2024-11-06T02:54:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.07 , 0.05 2024-11-06T02:54:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861649.69 epoch sec; seqNo:719; 4061555.22 north; 594416.19 east; 36.69 lat; -121.94 lon; 47.26 depth; utmZone: 10 2024-11-06T02:54:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861648.43 2024-11-06T02:54:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:09Z- waiting for 1730861652.43, time is 1730861649.69 2024-11-06T02:54:10Z- AHRS_M2 msg: 1730861650.03 epoch sec; seqNo:669 2024-11-06T02:54:10Z- AHRS_M2 msg: 0.16 phi; -0.15 theta; 5.33 psi 2024-11-06T02:54:10Z- handleDepth()- Depth_Keller msg: 1730861650.03 epoch sec; seqNo:720; depth 47.28 2024-11-06T02:54:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861650.05 epoch sec; seqNo:720; 4061555.21 north; 594414.97 east; 36.69 lat; -121.94 lon; 47.28 depth; utmZone: 10 2024-11-06T02:54:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861648.43 2024-11-06T02:54:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:10Z- waiting for 1730861652.43, time is 1730861650.05 2024-11-06T02:54:10Z- AHRS_M2 msg: 1730861650.43 epoch sec; seqNo:670 2024-11-06T02:54:10Z- AHRS_M2 msg: 0.16 phi; -0.15 theta; 5.33 psi 2024-11-06T02:54:10Z- handleDepth()- Depth_Keller msg: 1730861650.43 epoch sec; seqNo:721; depth 47.35 2024-11-06T02:54:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861650.45 epoch sec; seqNo:721; 4061555.59 north; 594414.97 east; 36.69 lat; -121.94 lon; 47.35 depth; utmZone: 10 2024-11-06T02:54:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861648.43 2024-11-06T02:54:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:10Z- waiting for 1730861652.43, time is 1730861650.45 2024-11-06T02:54:10Z- AHRS_M2 msg: 1730861650.83 epoch sec; seqNo:671 2024-11-06T02:54:10Z- AHRS_M2 msg: 0.15 phi; -0.15 theta; 5.33 psi 2024-11-06T02:54:10Z- handleDepth()- Depth_Keller msg: 1730861650.84 epoch sec; seqNo:722; depth 47.39 2024-11-06T02:54:10Z- handleDVL() - DVL msg epochMillisec: 1730861650.84 2024-11-06T02:54:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:10Z- handleDvl() - RDI_Pathfinder msg: 1730861650.84 sec; seqNo:230; #beams:0 2024-11-06T02:54:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.07 , 0.05 2024-11-06T02:54:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861650.85 epoch sec; seqNo:722; 4061555.59 north; 594414.97 east; 36.69 lat; -121.94 lon; 47.39 depth; utmZone: 10 2024-11-06T02:54:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861648.43 2024-11-06T02:54:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:10Z- waiting for 1730861652.43, time is 1730861650.85 2024-11-06T02:54:11Z- AHRS_M2 msg: 1730861651.29 epoch sec; seqNo:672 2024-11-06T02:54:11Z- AHRS_M2 msg: 0.15 phi; -0.15 theta; 5.33 psi 2024-11-06T02:54:11Z- handleDepth()- Depth_Keller msg: 1730861651.29 epoch sec; seqNo:723; depth 47.43 2024-11-06T02:54:11Z- handleDVL() - DVL msg epochMillisec: 1730861651.31 2024-11-06T02:54:11Z- handleDvl() - RDI_Pathfinder msg: 1730861651.31 sec; seqNo:231; #beams:4 2024-11-06T02:54:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.33 , 37.13 , 40.19 , 46.11 2024-11-06T02:54:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , 0.02 2024-11-06T02:54:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861651.34 epoch sec; seqNo:723; 4061555.95 north; 594413.74 east; 36.69 lat; -121.94 lon; 47.43 depth; utmZone: 10 2024-11-06T02:54:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861648.43 2024-11-06T02:54:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:11Z- waiting for 1730861652.43, time is 1730861651.34 2024-11-06T02:54:11Z- handleDepth()- Depth_Keller msg: 1730861651.67 epoch sec; seqNo:724; depth 47.49 2024-11-06T02:54:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861651.68 epoch sec; seqNo:724; 4061555.95 north; 594413.74 east; 36.69 lat; -121.94 lon; 47.49 depth; utmZone: 10 2024-11-06T02:54:11Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:54:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861648.43 2024-11-06T02:54:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:11Z- waiting for 1730861652.43, time is 1730861651.68 2024-11-06T02:54:12Z- AHRS_M2 msg: 1730861652.04 epoch sec; seqNo:673 2024-11-06T02:54:12Z- AHRS_M2 msg: 0.15 phi; -0.16 theta; 5.33 psi 2024-11-06T02:54:12Z- handleDepth()- Depth_Keller msg: 1730861652.06 epoch sec; seqNo:725; depth 47.51 2024-11-06T02:54:12Z- handleDVL() - DVL msg epochMillisec: 1730861652.07 2024-11-06T02:54:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:12Z- handleDvl() - RDI_Pathfinder msg: 1730861652.07 sec; seqNo:232; #beams:0 2024-11-06T02:54:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , 0.02 2024-11-06T02:54:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861652.08 epoch sec; seqNo:725; 4061556.33 north; 594413.74 east; 36.69 lat; -121.94 lon; 47.51 depth; utmZone: 10 2024-11-06T02:54:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861648.43 2024-11-06T02:54:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:12Z- waiting for 1730861652.43, time is 1730861652.08 2024-11-06T02:54:12Z- AHRS_M2 msg: 1730861652.45 epoch sec; seqNo:674 2024-11-06T02:54:12Z- AHRS_M2 msg: 0.15 phi; -0.16 theta; 5.33 psi 2024-11-06T02:54:12Z- handleDepth()- Depth_Keller msg: 1730861652.46 epoch sec; seqNo:726; depth 47.56 2024-11-06T02:54:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861652.47 epoch sec; seqNo:726; 4061556.33 north; 594413.74 east; 36.69 lat; -121.94 lon; 47.56 depth; utmZone: 10 2024-11-06T02:54:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861648.43 2024-11-06T02:54:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:12Z- waiting for 1730861652.43, time is 1730861652.47 2024-11-06T02:54:12Z- LcmTrn::updateTrn() >>>> pose time 1730861652.47, meas time 1730861652.07 <<<< 2024-11-06T02:54:12Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861652.47 2024-11-06T02:54:12Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:54:12Z- TerrainNav::motionUpdate() - currEstimate = (4061556, 594413.74, 47.56) 2024-11-06T02:54:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:54:12Z- TerrainNav:: SubcloudNIS is 0.56, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:54:12Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:54:12Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:54:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:54:12Z- variances([0], [2], and psi_berg) = 0.337602 36.572712 0.000000 2024-11-06T02:54:12Z- publishEstimatesDecoder() - at 1730861652.47 sec 2024-11-06T02:54:12Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:54:12Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061588.23 594411.30 49.98 2024-11-06T02:54:12Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061588.77 594401.97 49.72 lat/lon: 36.70 -121.94 2024-11-06T02:54:12Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.34 36.57 1.29 0.00 2024-11-06T02:54:12Z- AHRS_M2 msg: 1730861652.86 epoch sec; seqNo:675 2024-11-06T02:54:12Z- AHRS_M2 msg: 0.15 phi; -0.15 theta; 5.33 psi 2024-11-06T02:54:12Z- handleDepth()- Depth_Keller msg: 1730861652.86 epoch sec; seqNo:727; depth 47.58 2024-11-06T02:54:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861652.88 epoch sec; seqNo:727; 4061556.71 north; 594413.74 east; 36.69 lat; -121.94 lon; 47.58 depth; utmZone: 10 2024-11-06T02:54:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861652.47 2024-11-06T02:54:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:13Z- waiting for 1730861656.47, time is 1730861652.88 2024-11-06T02:54:13Z- handleDepth()- Depth_Keller msg: 1730861653.31 epoch sec; seqNo:728; depth 47.65 2024-11-06T02:54:13Z- handleDVL() - DVL msg epochMillisec: 1730861653.33 2024-11-06T02:54:13Z- handleDvl() - RDI_Pathfinder msg: 1730861653.33 sec; seqNo:233; #beams:4 2024-11-06T02:54:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.33 , 37.33 , 39.99 , 46.11 2024-11-06T02:54:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , -0.01 2024-11-06T02:54:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861653.34 epoch sec; seqNo:728; 4061556.70 north; 594412.52 east; 36.69 lat; -121.94 lon; 47.65 depth; utmZone: 10 2024-11-06T02:54:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861652.47 2024-11-06T02:54:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:13Z- waiting for 1730861656.47, time is 1730861653.34 2024-11-06T02:54:13Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T02:54:13Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T02:54:13Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T02:54:13Z- AHRS_M2 msg: 1730861653.65 epoch sec; seqNo:676 2024-11-06T02:54:13Z- AHRS_M2 msg: 0.15 phi; -0.15 theta; 5.33 psi 2024-11-06T02:54:13Z- handleDepth()- Depth_Keller msg: 1730861653.69 epoch sec; seqNo:729; depth 47.67 2024-11-06T02:54:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861653.70 epoch sec; seqNo:729; 4061557.08 north; 594412.51 east; 36.69 lat; -121.94 lon; 47.67 depth; utmZone: 10 2024-11-06T02:54:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861652.47 2024-11-06T02:54:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:13Z- waiting for 1730861656.47, time is 1730861653.70 2024-11-06T02:54:14Z- AHRS_M2 msg: 1730861654.06 epoch sec; seqNo:677 2024-11-06T02:54:14Z- AHRS_M2 msg: 0.14 phi; -0.15 theta; 5.33 psi 2024-11-06T02:54:14Z- handleDepth()- Depth_Keller msg: 1730861654.07 epoch sec; seqNo:730; depth 47.71 2024-11-06T02:54:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861654.08 epoch sec; seqNo:730; 4061557.08 north; 594412.51 east; 36.69 lat; -121.94 lon; 47.71 depth; utmZone: 10 2024-11-06T02:54:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861652.47 2024-11-06T02:54:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:14Z- waiting for 1730861656.47, time is 1730861654.08 2024-11-06T02:54:14Z- AHRS_M2 msg: 1730861654.47 epoch sec; seqNo:678 2024-11-06T02:54:14Z- AHRS_M2 msg: 0.14 phi; -0.15 theta; 5.33 psi 2024-11-06T02:54:14Z- handleDepth()- Depth_Keller msg: 1730861654.47 epoch sec; seqNo:731; depth 47.76 2024-11-06T02:54:14Z- handleDVL() - DVL msg epochMillisec: 1730861654.47 2024-11-06T02:54:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:14Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:14Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:14Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:14Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:14Z- handleDvl() - RDI_Pathfinder msg: 1730861654.47 sec; seqNo:234; #beams:0 2024-11-06T02:54:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , -0.01 2024-11-06T02:54:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861654.48 epoch sec; seqNo:731; 4061557.44 north; 594411.29 east; 36.69 lat; -121.94 lon; 47.76 depth; utmZone: 10 2024-11-06T02:54:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861652.47 2024-11-06T02:54:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:14Z- waiting for 1730861656.47, time is 1730861654.48 2024-11-06T02:54:14Z- AHRS_M2 msg: 1730861654.89 epoch sec; seqNo:679 2024-11-06T02:54:14Z- AHRS_M2 msg: 0.14 phi; -0.15 theta; 5.33 psi 2024-11-06T02:54:14Z- handleDepth()- Depth_Keller msg: 1730861654.89 epoch sec; seqNo:732; depth 47.80 2024-11-06T02:54:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861654.90 epoch sec; seqNo:732; 4061557.44 north; 594411.29 east; 36.69 lat; -121.94 lon; 47.80 depth; utmZone: 10 2024-11-06T02:54:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861652.47 2024-11-06T02:54:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:15Z- waiting for 1730861656.47, time is 1730861654.90 2024-11-06T02:54:15Z- AHRS_M2 msg: 1730861655.30 epoch sec; seqNo:680 2024-11-06T02:54:15Z- AHRS_M2 msg: 0.14 phi; -0.14 theta; 5.33 psi 2024-11-06T02:54:15Z- handleDepth()- Depth_Keller msg: 1730861655.30 epoch sec; seqNo:733; depth 47.83 2024-11-06T02:54:15Z- handleDVL() - DVL msg epochMillisec: 1730861655.32 2024-11-06T02:54:15Z- handleDvl() - RDI_Pathfinder msg: 1730861655.32 sec; seqNo:235; #beams:4 2024-11-06T02:54:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.92 , 36.93 , 39.78 , 45.49 2024-11-06T02:54:15Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.07 , -0.01 2024-11-06T02:54:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861655.34 epoch sec; seqNo:733; 4061557.82 north; 594411.28 east; 36.69 lat; -121.94 lon; 47.83 depth; utmZone: 10 2024-11-06T02:54:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861652.47 2024-11-06T02:54:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:15Z- waiting for 1730861656.47, time is 1730861655.34 2024-11-06T02:54:15Z- AHRS_M2 msg: 1730861655.68 epoch sec; seqNo:681 2024-11-06T02:54:15Z- AHRS_M2 msg: 0.14 phi; -0.14 theta; 5.33 psi 2024-11-06T02:54:15Z- handleDepth()- Depth_Keller msg: 1730861655.68 epoch sec; seqNo:734; depth 47.87 2024-11-06T02:54:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861655.70 epoch sec; seqNo:734; 4061557.82 north; 594411.28 east; 36.69 lat; -121.94 lon; 47.87 depth; utmZone: 10 2024-11-06T02:54:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861652.47 2024-11-06T02:54:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:15Z- waiting for 1730861656.47, time is 1730861655.70 2024-11-06T02:54:16Z- AHRS_M2 msg: 1730861656.08 epoch sec; seqNo:682 2024-11-06T02:54:16Z- AHRS_M2 msg: 0.14 phi; -0.14 theta; 5.33 psi 2024-11-06T02:54:16Z- handleDepth()- Depth_Keller msg: 1730861656.09 epoch sec; seqNo:735; depth 47.91 2024-11-06T02:54:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861656.10 epoch sec; seqNo:735; 4061558.19 north; 594410.06 east; 36.69 lat; -121.94 lon; 47.91 depth; utmZone: 10 2024-11-06T02:54:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861652.47 2024-11-06T02:54:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:16Z- waiting for 1730861656.47, time is 1730861656.10 2024-11-06T02:54:16Z- AHRS_M2 msg: 1730861656.49 epoch sec; seqNo:683 2024-11-06T02:54:16Z- AHRS_M2 msg: 0.14 phi; -0.14 theta; 5.33 psi 2024-11-06T02:54:16Z- handleDepth()- Depth_Keller msg: 1730861656.49 epoch sec; seqNo:736; depth 47.96 2024-11-06T02:54:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861656.50 epoch sec; seqNo:736; 4061558.57 north; 594410.05 east; 36.69 lat; -121.94 lon; 47.96 depth; utmZone: 10 2024-11-06T02:54:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861652.47 2024-11-06T02:54:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:16Z- waiting for 1730861656.47, time is 1730861656.50 2024-11-06T02:54:16Z- LcmTrn::updateTrn() >>>> pose time 1730861656.50, meas time 1730861655.32 <<<< 2024-11-06T02:54:16Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861656.50 2024-11-06T02:54:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861655.32 sec, ping # 0; waiting for INS data more recent than 1730861652.47... 2024-11-06T02:54:16Z- TerrainNav::motionUpdate() - currEstimate = (4061559, 594410.05, 47.96) 2024-11-06T02:54:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:54:16Z- TerrainNav:: SubcloudNIS is 0.56, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:54:16Z- TNavPF::Projecting Measurements 2024-11-06T02:54:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:54:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:54:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:54:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:54:16Z- TNavFilter::4 good beams. 2024-11-06T02:54:16Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:54:16Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:54:16Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:54:16Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:54:16Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:54:16Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:54:16Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:54:16Z- TerrainMapOctree: (vehN, vehE ) = (4061557.91, 594411.13). 2024-11-06T02:54:16Z- TerrainMapOctree: Min Distance = 250.72. 2024-11-06T02:54:16Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:54:16Z- Forcing Subcloud Comparison 2024-11-06T02:54:16Z- Weighting particles with subcloud comparison 2024-11-06T02:54:16Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:16Z- meanExpectedMeasDiff: -3.706297 delta_rms_squared: 2.427097 alpha: 0.991827 2024-11-06T02:54:16Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:16Z- meanExpectedMeasDiff: 2.361649 delta_rms_squared: 2.096934 alpha: 0.990552 2024-11-06T02:54:16Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:16Z- meanExpectedMeasDiff: -0.482106 delta_rms_squared: 1.647677 alpha: 0.988007 2024-11-06T02:54:16Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:16Z- meanExpectedMeasDiff: 1.804200 delta_rms_squared: 1.606452 alpha: 0.987703 2024-11-06T02:54:16Z- Subcloud NIS: 0.599848 2024-11-06T02:54:16Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:54:16Z- TNavPF:: sumWeights = 0.072230 2024-11-06T02:54:16Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:54:16Z- TNavPF::First Term of Map Covariance Matrix is 0.83 2024-11-06T02:54:16Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.01 2024-11-06T02:54:16Z- TerrainNav::Resampling particle filter... 2024-11-06T02:54:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861655.32 sec. 2024-11-06T02:54:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:54:16Z- variances([0], [2], and psi_berg) = 0.211718 12.106320 0.000000 2024-11-06T02:54:16Z- publishEstimatesDecoder() - at 1730861656.50 sec 2024-11-06T02:54:16Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:54:16Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061590.44 594407.55 48.52 2024-11-06T02:54:16Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061591.18 594395.29 50.50 lat/lon: 36.70 -121.94 2024-11-06T02:54:16Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 12.11 0.92 0.00 2024-11-06T02:54:16Z- AHRS_M2 msg: 1730861656.89 epoch sec; seqNo:684 2024-11-06T02:54:16Z- AHRS_M2 msg: 0.14 phi; -0.14 theta; 5.33 psi 2024-11-06T02:54:16Z- handleDepth()- Depth_Keller msg: 1730861656.91 epoch sec; seqNo:737; depth 47.98 2024-11-06T02:54:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861656.93 epoch sec; seqNo:737; 4061558.57 north; 594410.05 east; 36.69 lat; -121.94 lon; 47.98 depth; utmZone: 10 2024-11-06T02:54:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861656.50 2024-11-06T02:54:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:17Z- waiting for 1730861660.50, time is 1730861656.93 2024-11-06T02:54:17Z- AHRS_M2 msg: 1730861657.30 epoch sec; seqNo:685 2024-11-06T02:54:17Z- AHRS_M2 msg: 0.14 phi; -0.13 theta; 5.33 psi 2024-11-06T02:54:17Z- handleDepth()- Depth_Keller msg: 1730861657.31 epoch sec; seqNo:738; depth 48.05 2024-11-06T02:54:17Z- handleDVL() - DVL msg epochMillisec: 1730861657.34 2024-11-06T02:54:17Z- handleDvl() - RDI_Pathfinder msg: 1730861657.34 sec; seqNo:236; #beams:4 2024-11-06T02:54:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.11 , 36.93 , 39.78 , 44.88 2024-11-06T02:54:17Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , -0.08 , 0.01 2024-11-06T02:54:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861657.35 epoch sec; seqNo:738; 4061558.93 north; 594408.83 east; 36.69 lat; -121.94 lon; 48.05 depth; utmZone: 10 2024-11-06T02:54:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861656.50 2024-11-06T02:54:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:17Z- waiting for 1730861660.50, time is 1730861657.35 2024-11-06T02:54:17Z- AHRS_M2 msg: 1730861657.70 epoch sec; seqNo:686 2024-11-06T02:54:17Z- AHRS_M2 msg: 0.15 phi; -0.13 theta; 5.33 psi 2024-11-06T02:54:17Z- handleDepth()- Depth_Keller msg: 1730861657.71 epoch sec; seqNo:739; depth 48.07 2024-11-06T02:54:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861657.73 epoch sec; seqNo:739; 4061558.93 north; 594408.83 east; 36.70 lat; -121.94 lon; 48.07 depth; utmZone: 10 2024-11-06T02:54:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861656.50 2024-11-06T02:54:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:17Z- waiting for 1730861660.50, time is 1730861657.73 2024-11-06T02:54:18Z- AHRS_M2 msg: 1730861658.10 epoch sec; seqNo:687 2024-11-06T02:54:18Z- AHRS_M2 msg: 0.15 phi; -0.13 theta; 5.33 psi 2024-11-06T02:54:18Z- handleDepth()- Depth_Keller msg: 1730861658.11 epoch sec; seqNo:740; depth 48.12 2024-11-06T02:54:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861658.13 epoch sec; seqNo:740; 4061559.31 north; 594408.82 east; 36.70 lat; -121.94 lon; 48.12 depth; utmZone: 10 2024-11-06T02:54:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861656.50 2024-11-06T02:54:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:18Z- waiting for 1730861660.50, time is 1730861658.13 2024-11-06T02:54:18Z- AHRS_M2 msg: 1730861658.51 epoch sec; seqNo:688 2024-11-06T02:54:18Z- AHRS_M2 msg: 0.15 phi; -0.13 theta; 5.33 psi 2024-11-06T02:54:18Z- handleDepth()- Depth_Keller msg: 1730861658.52 epoch sec; seqNo:741; depth 48.18 2024-11-06T02:54:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861658.55 epoch sec; seqNo:741; 4061559.31 north; 594408.82 east; 36.70 lat; -121.94 lon; 48.18 depth; utmZone: 10 2024-11-06T02:54:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861656.50 2024-11-06T02:54:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:18Z- waiting for 1730861660.50, time is 1730861658.55 2024-11-06T02:54:18Z- AHRS_M2 msg: 1730861658.91 epoch sec; seqNo:689 2024-11-06T02:54:18Z- AHRS_M2 msg: 0.15 phi; -0.13 theta; 5.33 psi 2024-11-06T02:54:18Z- handleDepth()- Depth_Keller msg: 1730861658.92 epoch sec; seqNo:742; depth 48.21 2024-11-06T02:54:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861658.94 epoch sec; seqNo:742; 4061559.68 north; 594407.60 east; 36.70 lat; -121.94 lon; 48.21 depth; utmZone: 10 2024-11-06T02:54:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861656.50 2024-11-06T02:54:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:19Z- waiting for 1730861660.50, time is 1730861658.94 2024-11-06T02:54:19Z- AHRS_M2 msg: 1730861659.32 epoch sec; seqNo:690 2024-11-06T02:54:19Z- AHRS_M2 msg: 0.15 phi; -0.13 theta; 5.33 psi 2024-11-06T02:54:19Z- handleDepth()- Depth_Keller msg: 1730861659.32 epoch sec; seqNo:743; depth 48.24 2024-11-06T02:54:19Z- handleDVL() - DVL msg epochMillisec: 1730861659.34 2024-11-06T02:54:19Z- handleDvl() - RDI_Pathfinder msg: 1730861659.34 sec; seqNo:237; #beams:4 2024-11-06T02:54:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.31 , 37.13 , 39.78 , 44.27 2024-11-06T02:54:19Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , -0.09 , 0.04 2024-11-06T02:54:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861659.36 epoch sec; seqNo:743; 4061559.68 north; 594407.60 east; 36.70 lat; -121.94 lon; 48.24 depth; utmZone: 10 2024-11-06T02:54:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861656.50 2024-11-06T02:54:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:19Z- waiting for 1730861660.50, time is 1730861659.36 2024-11-06T02:54:19Z- AHRS_M2 msg: 1730861659.72 epoch sec; seqNo:691 2024-11-06T02:54:19Z- AHRS_M2 msg: 0.16 phi; -0.12 theta; 5.34 psi 2024-11-06T02:54:19Z- handleDepth()- Depth_Keller msg: 1730861659.72 epoch sec; seqNo:744; depth 48.29 2024-11-06T02:54:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861659.75 epoch sec; seqNo:744; 4061560.05 north; 594407.60 east; 36.70 lat; -121.94 lon; 48.29 depth; utmZone: 10 2024-11-06T02:54:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861656.50 2024-11-06T02:54:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:19Z- waiting for 1730861660.50, time is 1730861659.75 2024-11-06T02:54:20Z- AHRS_M2 msg: 1730861660.12 epoch sec; seqNo:692 2024-11-06T02:54:20Z- AHRS_M2 msg: 0.16 phi; -0.12 theta; 5.34 psi 2024-11-06T02:54:20Z- handleDepth()- Depth_Keller msg: 1730861660.13 epoch sec; seqNo:745; depth 48.34 2024-11-06T02:54:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861660.14 epoch sec; seqNo:745; 4061560.42 north; 594406.37 east; 36.70 lat; -121.94 lon; 48.34 depth; utmZone: 10 2024-11-06T02:54:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861656.50 2024-11-06T02:54:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:20Z- waiting for 1730861660.50, time is 1730861660.14 2024-11-06T02:54:20Z- AHRS_M2 msg: 1730861660.53 epoch sec; seqNo:693 2024-11-06T02:54:20Z- AHRS_M2 msg: 0.16 phi; -0.12 theta; 5.34 psi 2024-11-06T02:54:20Z- handleDepth()- Depth_Keller msg: 1730861660.53 epoch sec; seqNo:746; depth 48.39 2024-11-06T02:54:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861660.54 epoch sec; seqNo:746; 4061560.42 north; 594406.37 east; 36.70 lat; -121.94 lon; 48.39 depth; utmZone: 10 2024-11-06T02:54:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861656.50 2024-11-06T02:54:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:20Z- waiting for 1730861660.50, time is 1730861660.54 2024-11-06T02:54:20Z- LcmTrn::updateTrn() >>>> pose time 1730861660.54, meas time 1730861659.34 <<<< 2024-11-06T02:54:20Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861660.54 2024-11-06T02:54:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861659.34 sec, ping # 0; waiting for INS data more recent than 1730861656.50... 2024-11-06T02:54:20Z- TerrainNav::motionUpdate() - currEstimate = (4061560, 594406.37, 48.39) 2024-11-06T02:54:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:54:20Z- TerrainNav:: SubcloudNIS is 0.60, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:54:20Z- TNavPF::Projecting Measurements 2024-11-06T02:54:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:54:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:54:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:54:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:54:20Z- TNavFilter::4 good beams. 2024-11-06T02:54:20Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:54:20Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:54:20Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:54:20Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:54:20Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:54:20Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:54:20Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:54:20Z- TerrainMapOctree: (vehN, vehE ) = (4061559.87, 594407.47). 2024-11-06T02:54:20Z- TerrainMapOctree: Min Distance = 246.87. 2024-11-06T02:54:20Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:54:20Z- Forcing Subcloud Comparison 2024-11-06T02:54:20Z- Weighting particles with subcloud comparison 2024-11-06T02:54:20Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:20Z- meanExpectedMeasDiff: -3.963108 delta_rms_squared: 2.108314 alpha: 0.990603 2024-11-06T02:54:20Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:20Z- meanExpectedMeasDiff: 2.880370 delta_rms_squared: 1.239617 alpha: 0.984122 2024-11-06T02:54:20Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:20Z- meanExpectedMeasDiff: -0.471967 delta_rms_squared: 1.164397 alpha: 0.983114 2024-11-06T02:54:20Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:20Z- meanExpectedMeasDiff: -0.161428 delta_rms_squared: 1.325027 alpha: 0.985130 2024-11-06T02:54:20Z- Subcloud NIS: 0.574255 2024-11-06T02:54:20Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:54:20Z- TNavPF:: sumWeights = 0.060443 2024-11-06T02:54:20Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:54:20Z- TNavPF::First Term of Map Covariance Matrix is 0.73 2024-11-06T02:54:20Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.01 2024-11-06T02:54:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861659.34 sec. 2024-11-06T02:54:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:54:20Z- variances([0], [2], and psi_berg) = 0.168580 1.608989 0.000000 2024-11-06T02:54:20Z- publishEstimatesDecoder() - at 1730861660.54 sec 2024-11-06T02:54:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:54:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061592.15 594390.17 51.40 2024-11-06T02:54:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061593.08 594390.74 51.24 lat/lon: 36.70 -121.94 2024-11-06T02:54:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 1.61 0.74 0.00 2024-11-06T02:54:20Z- handleDepth()- Depth_Keller msg: 1730861660.96 epoch sec; seqNo:747; depth 48.42 2024-11-06T02:54:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861660.98 epoch sec; seqNo:747; 4061560.80 north; 594406.37 east; 36.70 lat; -121.94 lon; 48.42 depth; utmZone: 10 2024-11-06T02:54:21Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:54:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861660.54 2024-11-06T02:54:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:21Z- waiting for 1730861664.54, time is 1730861660.98 2024-11-06T02:54:21Z- AHRS_M2 msg: 1730861661.34 epoch sec; seqNo:694 2024-11-06T02:54:21Z- AHRS_M2 msg: 0.15 phi; -0.12 theta; 5.34 psi 2024-11-06T02:54:21Z- handleDepth()- Depth_Keller msg: 1730861661.35 epoch sec; seqNo:748; depth 48.47 2024-11-06T02:54:21Z- handleDVL() - DVL msg epochMillisec: 1730861661.37 2024-11-06T02:54:21Z- handleDvl() - RDI_Pathfinder msg: 1730861661.37 sec; seqNo:238; #beams:4 2024-11-06T02:54:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.72 , 37.33 , 39.99 , 44.07 2024-11-06T02:54:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.10 , 0.06 2024-11-06T02:54:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861661.39 epoch sec; seqNo:748; 4061560.80 north; 594406.37 east; 36.70 lat; -121.94 lon; 48.47 depth; utmZone: 10 2024-11-06T02:54:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861660.54 2024-11-06T02:54:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:21Z- waiting for 1730861664.54, time is 1730861661.39 2024-11-06T02:54:21Z- AHRS_M2 msg: 1730861661.74 epoch sec; seqNo:695 2024-11-06T02:54:21Z- AHRS_M2 msg: 0.15 phi; -0.12 theta; 5.34 psi 2024-11-06T02:54:21Z- handleDepth()- Depth_Keller msg: 1730861661.75 epoch sec; seqNo:749; depth 48.51 2024-11-06T02:54:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861661.76 epoch sec; seqNo:749; 4061561.16 north; 594405.14 east; 36.70 lat; -121.94 lon; 48.51 depth; utmZone: 10 2024-11-06T02:54:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861660.54 2024-11-06T02:54:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:21Z- waiting for 1730861664.54, time is 1730861661.76 2024-11-06T02:54:22Z- handleDepth()- Depth_Keller msg: 1730861662.16 epoch sec; seqNo:750; depth 48.55 2024-11-06T02:54:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861662.17 epoch sec; seqNo:750; 4061561.16 north; 594405.14 east; 36.70 lat; -121.94 lon; 48.55 depth; utmZone: 10 2024-11-06T02:54:22Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:54:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861660.54 2024-11-06T02:54:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:22Z- waiting for 1730861664.54, time is 1730861662.17 2024-11-06T02:54:22Z- AHRS_M2 msg: 1730861662.55 epoch sec; seqNo:696 2024-11-06T02:54:22Z- AHRS_M2 msg: 0.15 phi; -0.12 theta; 5.34 psi 2024-11-06T02:54:22Z- handleDepth()- Depth_Keller msg: 1730861662.56 epoch sec; seqNo:751; depth 48.59 2024-11-06T02:54:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861662.58 epoch sec; seqNo:751; 4061561.54 north; 594405.14 east; 36.70 lat; -121.94 lon; 48.59 depth; utmZone: 10 2024-11-06T02:54:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861660.54 2024-11-06T02:54:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:22Z- waiting for 1730861664.54, time is 1730861662.58 2024-11-06T02:54:22Z- AHRS_M2 msg: 1730861662.95 epoch sec; seqNo:697 2024-11-06T02:54:22Z- AHRS_M2 msg: 0.15 phi; -0.12 theta; 5.34 psi 2024-11-06T02:54:22Z- handleDepth()- Depth_Keller msg: 1730861662.96 epoch sec; seqNo:752; depth 48.64 2024-11-06T02:54:22Z- handleDVL() - DVL msg epochMillisec: 1730861662.96 2024-11-06T02:54:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:22Z- handleDvl() - RDI_Pathfinder msg: 1730861662.96 sec; seqNo:239; #beams:0 2024-11-06T02:54:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.10 , 0.06 2024-11-06T02:54:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861662.97 epoch sec; seqNo:752; 4061561.53 north; 594403.92 east; 36.70 lat; -121.94 lon; 48.64 depth; utmZone: 10 2024-11-06T02:54:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861660.54 2024-11-06T02:54:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:23Z- waiting for 1730861664.54, time is 1730861662.97 2024-11-06T02:54:23Z- AHRS_M2 msg: 1730861663.36 epoch sec; seqNo:698 2024-11-06T02:54:23Z- AHRS_M2 msg: 0.15 phi; -0.11 theta; 5.34 psi 2024-11-06T02:54:23Z- handleDepth()- Depth_Keller msg: 1730861663.36 epoch sec; seqNo:753; depth 48.67 2024-11-06T02:54:23Z- handleDVL() - DVL msg epochMillisec: 1730861663.38 2024-11-06T02:54:23Z- handleDvl() - RDI_Pathfinder msg: 1730861663.38 sec; seqNo:240; #beams:4 2024-11-06T02:54:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.71 , 36.92 , 39.98 , 43.86 2024-11-06T02:54:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.08 , 0.06 2024-11-06T02:54:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861663.40 epoch sec; seqNo:753; 4061561.91 north; 594403.91 east; 36.70 lat; -121.94 lon; 48.67 depth; utmZone: 10 2024-11-06T02:54:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861660.54 2024-11-06T02:54:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:23Z- waiting for 1730861664.54, time is 1730861663.40 2024-11-06T02:54:23Z- AHRS_M2 msg: 1730861663.76 epoch sec; seqNo:699 2024-11-06T02:54:23Z- AHRS_M2 msg: 0.15 phi; -0.11 theta; 5.34 psi 2024-11-06T02:54:23Z- handleDepth()- Depth_Keller msg: 1730861663.77 epoch sec; seqNo:754; depth 48.70 2024-11-06T02:54:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861663.79 epoch sec; seqNo:754; 4061561.91 north; 594403.91 east; 36.70 lat; -121.94 lon; 48.70 depth; utmZone: 10 2024-11-06T02:54:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861660.54 2024-11-06T02:54:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:23Z- waiting for 1730861664.54, time is 1730861663.79 2024-11-06T02:54:24Z- AHRS_M2 msg: 1730861664.17 epoch sec; seqNo:700 2024-11-06T02:54:24Z- AHRS_M2 msg: 0.15 phi; -0.11 theta; 5.35 psi 2024-11-06T02:54:24Z- handleDepth()- Depth_Keller msg: 1730861664.17 epoch sec; seqNo:755; depth 48.73 2024-11-06T02:54:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861664.19 epoch sec; seqNo:755; 4061562.29 north; 594403.91 east; 36.70 lat; -121.94 lon; 48.73 depth; utmZone: 10 2024-11-06T02:54:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861660.54 2024-11-06T02:54:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:24Z- waiting for 1730861664.54, time is 1730861664.19 2024-11-06T02:54:24Z- AHRS_M2 msg: 1730861664.57 epoch sec; seqNo:701 2024-11-06T02:54:24Z- AHRS_M2 msg: 0.15 phi; -0.11 theta; 5.35 psi 2024-11-06T02:54:24Z- handleDepth()- Depth_Keller msg: 1730861664.57 epoch sec; seqNo:756; depth 48.75 2024-11-06T02:54:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861664.58 epoch sec; seqNo:756; 4061562.27 north; 594402.69 east; 36.70 lat; -121.94 lon; 48.75 depth; utmZone: 10 2024-11-06T02:54:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861660.54 2024-11-06T02:54:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:24Z- waiting for 1730861664.54, time is 1730861664.58 2024-11-06T02:54:24Z- LcmTrn::updateTrn() >>>> pose time 1730861664.58, meas time 1730861663.38 <<<< 2024-11-06T02:54:24Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861664.58 2024-11-06T02:54:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861663.38 sec, ping # 0; waiting for INS data more recent than 1730861660.54... 2024-11-06T02:54:24Z- TerrainNav::motionUpdate() - currEstimate = (4061562, 594402.69, 48.75) 2024-11-06T02:54:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:54:24Z- TerrainNav:: SubcloudNIS is 0.57, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:54:24Z- TNavPF::Projecting Measurements 2024-11-06T02:54:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:54:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:54:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:54:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:54:24Z- TNavFilter::4 good beams. 2024-11-06T02:54:24Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:54:24Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:54:24Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:54:24Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:54:24Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:54:24Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:54:24Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:54:24Z- TerrainMapOctree: (vehN, vehE ) = (4061561.72, 594403.78). 2024-11-06T02:54:24Z- TerrainMapOctree: Min Distance = 243.10. 2024-11-06T02:54:24Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:54:24Z- Forcing Subcloud Comparison 2024-11-06T02:54:24Z- Weighting particles with subcloud comparison 2024-11-06T02:54:24Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:24Z- meanExpectedMeasDiff: -3.930288 delta_rms_squared: 1.624526 alpha: 0.987838 2024-11-06T02:54:24Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:24Z- meanExpectedMeasDiff: 3.253003 delta_rms_squared: 1.011949 alpha: 0.980619 2024-11-06T02:54:24Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:24Z- meanExpectedMeasDiff: 0.111832 delta_rms_squared: 1.153283 alpha: 0.982954 2024-11-06T02:54:24Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:24Z- meanExpectedMeasDiff: -0.458746 delta_rms_squared: 1.544572 alpha: 0.987217 2024-11-06T02:54:24Z- Subcloud NIS: 0.571365 2024-11-06T02:54:24Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:54:24Z- TNavPF:: sumWeights = 0.041541 2024-11-06T02:54:24Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:54:24Z- TNavPF::First Term of Map Covariance Matrix is 0.58 2024-11-06T02:54:24Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T02:54:24Z- TerrainNav::Resampling particle filter... 2024-11-06T02:54:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861663.38 sec. 2024-11-06T02:54:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:54:24Z- variances([0], [2], and psi_berg) = 0.187990 0.589828 0.000000 2024-11-06T02:54:24Z- publishEstimatesDecoder() - at 1730861664.58 sec 2024-11-06T02:54:24Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:54:24Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061594.10 594400.24 48.65 2024-11-06T02:54:24Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061594.91 594386.97 51.73 lat/lon: 36.70 -121.94 2024-11-06T02:54:24Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.19 0.59 0.71 0.00 2024-11-06T02:54:24Z- AHRS_M2 msg: 1730861664.97 epoch sec; seqNo:702 2024-11-06T02:54:24Z- AHRS_M2 msg: 0.15 phi; -0.11 theta; 5.35 psi 2024-11-06T02:54:24Z- handleDepth()- Depth_Keller msg: 1730861664.98 epoch sec; seqNo:757; depth 48.78 2024-11-06T02:54:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861665.00 epoch sec; seqNo:757; 4061562.65 north; 594402.69 east; 36.70 lat; -121.94 lon; 48.78 depth; utmZone: 10 2024-11-06T02:54:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861664.58 2024-11-06T02:54:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:25Z- waiting for 1730861668.58, time is 1730861665.00 2024-11-06T02:54:25Z- AHRS_M2 msg: 1730861665.38 epoch sec; seqNo:703 2024-11-06T02:54:25Z- AHRS_M2 msg: 0.16 phi; -0.11 theta; 5.36 psi 2024-11-06T02:54:25Z- handleDepth()- Depth_Keller msg: 1730861665.38 epoch sec; seqNo:758; depth 48.81 2024-11-06T02:54:25Z- handleDVL() - DVL msg epochMillisec: 1730861665.40 2024-11-06T02:54:25Z- handleDvl() - RDI_Pathfinder msg: 1730861665.40 sec; seqNo:241; #beams:4 2024-11-06T02:54:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.51 , 36.72 , 40.19 , 43.65 2024-11-06T02:54:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , 0.00 2024-11-06T02:54:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861665.43 epoch sec; seqNo:758; 4061562.65 north; 594402.69 east; 36.70 lat; -121.94 lon; 48.81 depth; utmZone: 10 2024-11-06T02:54:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861664.58 2024-11-06T02:54:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:25Z- waiting for 1730861668.58, time is 1730861665.43 2024-11-06T02:54:25Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T02:54:25Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T02:54:25Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T02:54:25Z- AHRS_M2 msg: 1730861665.77 epoch sec; seqNo:704 2024-11-06T02:54:25Z- AHRS_M2 msg: 0.16 phi; -0.11 theta; 5.36 psi 2024-11-06T02:54:25Z- handleDepth()- Depth_Keller msg: 1730861665.80 epoch sec; seqNo:759; depth 48.82 2024-11-06T02:54:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861665.81 epoch sec; seqNo:759; 4061563.03 north; 594402.68 east; 36.70 lat; -121.94 lon; 48.82 depth; utmZone: 10 2024-11-06T02:54:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861664.58 2024-11-06T02:54:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:25Z- waiting for 1730861668.58, time is 1730861665.81 2024-11-06T02:54:26Z- AHRS_M2 msg: 1730861666.18 epoch sec; seqNo:705 2024-11-06T02:54:26Z- AHRS_M2 msg: 0.15 phi; -0.11 theta; 5.36 psi 2024-11-06T02:54:26Z- handleDepth()- Depth_Keller msg: 1730861666.19 epoch sec; seqNo:760; depth 48.84 2024-11-06T02:54:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861666.20 epoch sec; seqNo:760; 4061563.02 north; 594401.46 east; 36.70 lat; -121.94 lon; 48.84 depth; utmZone: 10 2024-11-06T02:54:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861664.58 2024-11-06T02:54:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:26Z- waiting for 1730861668.58, time is 1730861666.20 2024-11-06T02:54:26Z- AHRS_M2 msg: 1730861666.59 epoch sec; seqNo:706 2024-11-06T02:54:26Z- AHRS_M2 msg: 0.15 phi; -0.11 theta; 5.37 psi 2024-11-06T02:54:26Z- handleDepth()- Depth_Keller msg: 1730861666.59 epoch sec; seqNo:761; depth 48.87 2024-11-06T02:54:26Z- handleDVL() - DVL msg epochMillisec: 1730861666.60 2024-11-06T02:54:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:26Z- handleDvl() - RDI_Pathfinder msg: 1730861666.60 sec; seqNo:242; #beams:0 2024-11-06T02:54:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , 0.00 2024-11-06T02:54:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861666.61 epoch sec; seqNo:761; 4061563.40 north; 594401.46 east; 36.70 lat; -121.94 lon; 48.87 depth; utmZone: 10 2024-11-06T02:54:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861664.58 2024-11-06T02:54:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:26Z- waiting for 1730861668.58, time is 1730861666.61 2024-11-06T02:54:27Z- AHRS_M2 msg: 1730861667.01 epoch sec; seqNo:707 2024-11-06T02:54:27Z- AHRS_M2 msg: 0.15 phi; -0.10 theta; 5.37 psi 2024-11-06T02:54:27Z- handleDepth()- Depth_Keller msg: 1730861667.02 epoch sec; seqNo:762; depth 48.90 2024-11-06T02:54:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861667.03 epoch sec; seqNo:762; 4061563.40 north; 594401.46 east; 36.70 lat; -121.94 lon; 48.90 depth; utmZone: 10 2024-11-06T02:54:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861664.58 2024-11-06T02:54:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:27Z- waiting for 1730861668.58, time is 1730861667.03 2024-11-06T02:54:27Z- AHRS_M2 msg: 1730861667.39 epoch sec; seqNo:708 2024-11-06T02:54:27Z- AHRS_M2 msg: 0.15 phi; -0.10 theta; 5.37 psi 2024-11-06T02:54:27Z- handleDepth()- Depth_Keller msg: 1730861667.40 epoch sec; seqNo:763; depth 48.91 2024-11-06T02:54:27Z- handleDVL() - DVL msg epochMillisec: 1730861667.43 2024-11-06T02:54:27Z- handleDvl() - RDI_Pathfinder msg: 1730861667.43 sec; seqNo:243; #beams:4 2024-11-06T02:54:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.31 , 37.33 , 39.78 , 43.25 2024-11-06T02:54:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.06 , -0.05 2024-11-06T02:54:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861667.45 epoch sec; seqNo:763; 4061563.78 north; 594401.45 east; 36.70 lat; -121.94 lon; 48.91 depth; utmZone: 10 2024-11-06T02:54:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861664.58 2024-11-06T02:54:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:27Z- waiting for 1730861668.58, time is 1730861667.45 2024-11-06T02:54:27Z- AHRS_M2 msg: 1730861667.80 epoch sec; seqNo:709 2024-11-06T02:54:27Z- AHRS_M2 msg: 0.15 phi; -0.10 theta; 5.37 psi 2024-11-06T02:54:27Z- handleDepth()- Depth_Keller msg: 1730861667.81 epoch sec; seqNo:764; depth 48.95 2024-11-06T02:54:27Z- handleDVL() - DVL msg epochMillisec: 1730861667.81 2024-11-06T02:54:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:27Z- handleDvl() - RDI_Pathfinder msg: 1730861667.81 sec; seqNo:244; #beams:0 2024-11-06T02:54:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.06 , -0.05 2024-11-06T02:54:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861667.82 epoch sec; seqNo:764; 4061564.14 north; 594400.23 east; 36.70 lat; -121.94 lon; 48.95 depth; utmZone: 10 2024-11-06T02:54:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861664.58 2024-11-06T02:54:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:27Z- waiting for 1730861668.58, time is 1730861667.82 2024-11-06T02:54:28Z- AHRS_M2 msg: 1730861668.20 epoch sec; seqNo:710 2024-11-06T02:54:28Z- AHRS_M2 msg: 0.15 phi; -0.10 theta; 5.37 psi 2024-11-06T02:54:28Z- handleDepth()- Depth_Keller msg: 1730861668.21 epoch sec; seqNo:765; depth 48.95 2024-11-06T02:54:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861668.22 epoch sec; seqNo:765; 4061564.14 north; 594400.23 east; 36.70 lat; -121.94 lon; 48.95 depth; utmZone: 10 2024-11-06T02:54:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861664.58 2024-11-06T02:54:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:28Z- waiting for 1730861668.58, time is 1730861668.22 2024-11-06T02:54:28Z- AHRS_M2 msg: 1730861668.61 epoch sec; seqNo:711 2024-11-06T02:54:28Z- AHRS_M2 msg: 0.15 phi; -0.10 theta; 5.37 psi 2024-11-06T02:54:28Z- handleDepth()- Depth_Keller msg: 1730861668.62 epoch sec; seqNo:766; depth 48.96 2024-11-06T02:54:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861668.64 epoch sec; seqNo:766; 4061564.52 north; 594400.22 east; 36.70 lat; -121.94 lon; 48.96 depth; utmZone: 10 2024-11-06T02:54:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861664.58 2024-11-06T02:54:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:28Z- waiting for 1730861668.58, time is 1730861668.64 2024-11-06T02:54:28Z- LcmTrn::updateTrn() >>>> pose time 1730861668.64, meas time 1730861667.81 <<<< 2024-11-06T02:54:28Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861668.64 2024-11-06T02:54:28Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:54:28Z- TerrainNav::motionUpdate() - currEstimate = (4061565, 594400.22, 48.96) 2024-11-06T02:54:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:54:28Z- TerrainNav:: SubcloudNIS is 0.57, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:54:28Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:54:28Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:54:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:54:28Z- variances([0], [2], and psi_berg) = 0.189741 0.591496 0.000000 2024-11-06T02:54:28Z- publishEstimatesDecoder() - at 1730861668.64 sec 2024-11-06T02:54:28Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:54:28Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061596.37 594397.76 48.93 2024-11-06T02:54:28Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061597.16 594384.50 51.93 lat/lon: 36.70 -121.94 2024-11-06T02:54:28Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.19 0.59 0.97 0.00 2024-11-06T02:54:29Z- AHRS_M2 msg: 1730861669.01 epoch sec; seqNo:712 2024-11-06T02:54:29Z- AHRS_M2 msg: 0.15 phi; -0.10 theta; 5.37 psi 2024-11-06T02:54:29Z- handleDepth()- Depth_Keller msg: 1730861669.02 epoch sec; seqNo:767; depth 49.00 2024-11-06T02:54:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861669.04 epoch sec; seqNo:767; 4061564.52 north; 594400.22 east; 36.70 lat; -121.94 lon; 49.00 depth; utmZone: 10 2024-11-06T02:54:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861668.64 2024-11-06T02:54:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:29Z- waiting for 1730861672.64, time is 1730861669.04 2024-11-06T02:54:29Z- handleDepth()- Depth_Keller msg: 1730861669.45 epoch sec; seqNo:768; depth 49.03 2024-11-06T02:54:29Z- handleDVL() - DVL msg epochMillisec: 1730861669.47 2024-11-06T02:54:29Z- handleDvl() - RDI_Pathfinder msg: 1730861669.47 sec; seqNo:245; #beams:4 2024-11-06T02:54:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.63 , 37.02 , 40.02 , 43.01 2024-11-06T02:54:29Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , -0.08 , -0.02 2024-11-06T02:54:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861669.48 epoch sec; seqNo:768; 4061564.88 north; 594399.00 east; 36.70 lat; -121.94 lon; 49.03 depth; utmZone: 10 2024-11-06T02:54:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861668.64 2024-11-06T02:54:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:29Z- waiting for 1730861672.64, time is 1730861669.48 2024-11-06T02:54:29Z- AHRS_M2 msg: 1730861669.84 epoch sec; seqNo:713 2024-11-06T02:54:29Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 5.36 psi 2024-11-06T02:54:29Z- handleDepth()- Depth_Keller msg: 1730861669.84 epoch sec; seqNo:769; depth 49.06 2024-11-06T02:54:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861669.86 epoch sec; seqNo:769; 4061565.26 north; 594399.00 east; 36.70 lat; -121.94 lon; 49.06 depth; utmZone: 10 2024-11-06T02:54:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861668.64 2024-11-06T02:54:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:29Z- waiting for 1730861672.64, time is 1730861669.86 2024-11-06T02:54:30Z- AHRS_M2 msg: 1730861670.22 epoch sec; seqNo:714 2024-11-06T02:54:30Z- AHRS_M2 msg: 0.13 phi; -0.10 theta; 5.36 psi 2024-11-06T02:54:30Z- handleDepth()- Depth_Keller msg: 1730861670.23 epoch sec; seqNo:770; depth 49.07 2024-11-06T02:54:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861670.24 epoch sec; seqNo:770; 4061565.26 north; 594399.00 east; 36.70 lat; -121.94 lon; 49.07 depth; utmZone: 10 2024-11-06T02:54:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861668.64 2024-11-06T02:54:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:30Z- waiting for 1730861672.64, time is 1730861670.24 2024-11-06T02:54:30Z- AHRS_M2 msg: 1730861670.63 epoch sec; seqNo:715 2024-11-06T02:54:30Z- AHRS_M2 msg: 0.13 phi; -0.10 theta; 5.35 psi 2024-11-06T02:54:30Z- handleDepth()- Depth_Keller msg: 1730861670.64 epoch sec; seqNo:771; depth 49.12 2024-11-06T02:54:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861670.65 epoch sec; seqNo:771; 4061565.63 north; 594397.77 east; 36.70 lat; -121.94 lon; 49.12 depth; utmZone: 10 2024-11-06T02:54:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861668.64 2024-11-06T02:54:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:30Z- waiting for 1730861672.64, time is 1730861670.65 2024-11-06T02:54:31Z- AHRS_M2 msg: 1730861671.04 epoch sec; seqNo:716 2024-11-06T02:54:31Z- AHRS_M2 msg: 0.13 phi; -0.10 theta; 5.35 psi 2024-11-06T02:54:31Z- handleDepth()- Depth_Keller msg: 1730861671.04 epoch sec; seqNo:772; depth 49.14 2024-11-06T02:54:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861671.05 epoch sec; seqNo:772; 4061565.63 north; 594397.77 east; 36.70 lat; -121.94 lon; 49.14 depth; utmZone: 10 2024-11-06T02:54:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861668.64 2024-11-06T02:54:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:31Z- waiting for 1730861672.64, time is 1730861671.05 2024-11-06T02:54:31Z- AHRS_M2 msg: 1730861671.44 epoch sec; seqNo:717 2024-11-06T02:54:31Z- AHRS_M2 msg: 0.13 phi; -0.10 theta; 5.35 psi 2024-11-06T02:54:31Z- handleDepth()- Depth_Keller msg: 1730861671.45 epoch sec; seqNo:773; depth 49.17 2024-11-06T02:54:31Z- handleDVL() - DVL msg epochMillisec: 1730861671.47 2024-11-06T02:54:31Z- handleDvl() - RDI_Pathfinder msg: 1730861671.47 sec; seqNo:246; #beams:4 2024-11-06T02:54:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.44 , 37.58 , 39.83 , 42.82 2024-11-06T02:54:31Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.07 , 0.07 2024-11-06T02:54:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861671.48 epoch sec; seqNo:773; 4061566.01 north; 594397.77 east; 36.70 lat; -121.94 lon; 49.17 depth; utmZone: 10 2024-11-06T02:54:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861668.64 2024-11-06T02:54:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:31Z- waiting for 1730861672.64, time is 1730861671.48 2024-11-06T02:54:31Z- AHRS_M2 msg: 1730861671.84 epoch sec; seqNo:718 2024-11-06T02:54:31Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 5.35 psi 2024-11-06T02:54:31Z- handleDepth()- Depth_Keller msg: 1730861671.85 epoch sec; seqNo:774; depth 49.20 2024-11-06T02:54:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861671.87 epoch sec; seqNo:774; 4061566.39 north; 594397.76 east; 36.70 lat; -121.94 lon; 49.20 depth; utmZone: 10 2024-11-06T02:54:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861668.64 2024-11-06T02:54:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:32Z- waiting for 1730861672.64, time is 1730861671.87 2024-11-06T02:54:32Z- AHRS_M2 msg: 1730861672.24 epoch sec; seqNo:719 2024-11-06T02:54:32Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 5.34 psi 2024-11-06T02:54:32Z- handleDepth()- Depth_Keller msg: 1730861672.25 epoch sec; seqNo:775; depth 49.23 2024-11-06T02:54:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861672.26 epoch sec; seqNo:775; 4061566.37 north; 594396.54 east; 36.70 lat; -121.94 lon; 49.23 depth; utmZone: 10 2024-11-06T02:54:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861668.64 2024-11-06T02:54:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:32Z- waiting for 1730861672.64, time is 1730861672.26 2024-11-06T02:54:32Z- AHRS_M2 msg: 1730861672.65 epoch sec; seqNo:720 2024-11-06T02:54:32Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 5.34 psi 2024-11-06T02:54:32Z- handleDepth()- Depth_Keller msg: 1730861672.66 epoch sec; seqNo:776; depth 49.27 2024-11-06T02:54:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861672.68 epoch sec; seqNo:776; 4061566.75 north; 594396.54 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T02:54:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861668.64 2024-11-06T02:54:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:32Z- waiting for 1730861672.64, time is 1730861672.68 2024-11-06T02:54:32Z- LcmTrn::updateTrn() >>>> pose time 1730861672.68, meas time 1730861671.47 <<<< 2024-11-06T02:54:32Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861672.68 2024-11-06T02:54:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861671.47 sec, ping # 0; waiting for INS data more recent than 1730861668.64... 2024-11-06T02:54:32Z- TerrainNav::motionUpdate() - currEstimate = (4061567, 594396.54, 49.27) 2024-11-06T02:54:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:54:32Z- TerrainNav:: SubcloudNIS is 0.57, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:54:32Z- TNavPF::Projecting Measurements 2024-11-06T02:54:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:54:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:54:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:54:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:54:32Z- TNavFilter::4 good beams. 2024-11-06T02:54:32Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:54:32Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:54:32Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:54:32Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:54:32Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:54:32Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:54:32Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:54:32Z- TerrainMapOctree: (vehN, vehE ) = (4061566.08, 594397.64). 2024-11-06T02:54:32Z- TerrainMapOctree: Min Distance = 235.85. 2024-11-06T02:54:32Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:54:32Z- Forcing Subcloud Comparison 2024-11-06T02:54:32Z- Weighting particles with subcloud comparison 2024-11-06T02:54:32Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:32Z- meanExpectedMeasDiff: -3.505557 delta_rms_squared: 1.967285 alpha: 0.989936 2024-11-06T02:54:32Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:32Z- meanExpectedMeasDiff: 3.359652 delta_rms_squared: 1.418127 alpha: 0.986093 2024-11-06T02:54:32Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:32Z- meanExpectedMeasDiff: 0.436044 delta_rms_squared: 1.496339 alpha: 0.986810 2024-11-06T02:54:32Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:32Z- meanExpectedMeasDiff: -0.410264 delta_rms_squared: 2.169609 alpha: 0.990866 2024-11-06T02:54:32Z- Subcloud NIS: 0.587288 2024-11-06T02:54:32Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:54:32Z- TNavPF:: sumWeights = 0.044358 2024-11-06T02:54:32Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:54:32Z- TNavPF::First Term of Map Covariance Matrix is 0.54 2024-11-06T02:54:32Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T02:54:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861671.47 sec. 2024-11-06T02:54:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:54:32Z- variances([0], [2], and psi_berg) = 0.180686 0.337447 0.000000 2024-11-06T02:54:32Z- publishEstimatesDecoder() - at 1730861672.68 sec 2024-11-06T02:54:32Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:54:32Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061599.91 594380.48 51.09 2024-11-06T02:54:32Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061599.41 594380.81 52.23 lat/lon: 36.70 -121.94 2024-11-06T02:54:32Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.34 0.75 0.00 2024-11-06T02:54:33Z- AHRS_M2 msg: 1730861673.06 epoch sec; seqNo:721 2024-11-06T02:54:33Z- AHRS_M2 msg: 0.12 phi; -0.09 theta; 5.34 psi 2024-11-06T02:54:33Z- handleDepth()- Depth_Keller msg: 1730861673.06 epoch sec; seqNo:777; depth 49.28 2024-11-06T02:54:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861673.09 epoch sec; seqNo:777; 4061566.75 north; 594396.54 east; 36.70 lat; -121.94 lon; 49.28 depth; utmZone: 10 2024-11-06T02:54:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861672.68 2024-11-06T02:54:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:33Z- waiting for 1730861676.68, time is 1730861673.09 2024-11-06T02:54:33Z- AHRS_M2 msg: 1730861673.46 epoch sec; seqNo:722 2024-11-06T02:54:33Z- AHRS_M2 msg: 0.12 phi; -0.09 theta; 5.34 psi 2024-11-06T02:54:33Z- handleDepth()- Depth_Keller msg: 1730861673.46 epoch sec; seqNo:778; depth 49.32 2024-11-06T02:54:33Z- handleDVL() - DVL msg epochMillisec: 1730861673.48 2024-11-06T02:54:33Z- handleDvl() - RDI_Pathfinder msg: 1730861673.48 sec; seqNo:247; #beams:4 2024-11-06T02:54:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.25 , 37.40 , 40.20 , 42.63 2024-11-06T02:54:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.07 , 0.08 2024-11-06T02:54:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861673.49 epoch sec; seqNo:778; 4061567.12 north; 594395.31 east; 36.70 lat; -121.94 lon; 49.32 depth; utmZone: 10 2024-11-06T02:54:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861672.68 2024-11-06T02:54:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:33Z- waiting for 1730861676.68, time is 1730861673.49 2024-11-06T02:54:33Z- AHRS_M2 msg: 1730861673.86 epoch sec; seqNo:723 2024-11-06T02:54:33Z- AHRS_M2 msg: 0.11 phi; -0.09 theta; 5.34 psi 2024-11-06T02:54:33Z- handleDepth()- Depth_Keller msg: 1730861673.87 epoch sec; seqNo:779; depth 49.35 2024-11-06T02:54:33Z- handleDVL() - DVL msg epochMillisec: 1730861673.87 2024-11-06T02:54:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:33Z- handleDvl() - RDI_Pathfinder msg: 1730861673.87 sec; seqNo:248; #beams:0 2024-11-06T02:54:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.07 , 0.08 2024-11-06T02:54:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861673.89 epoch sec; seqNo:779; 4061567.12 north; 594395.31 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T02:54:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861672.68 2024-11-06T02:54:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:34Z- waiting for 1730861676.68, time is 1730861673.89 2024-11-06T02:54:34Z- AHRS_M2 msg: 1730861674.26 epoch sec; seqNo:724 2024-11-06T02:54:34Z- AHRS_M2 msg: 0.11 phi; -0.09 theta; 5.34 psi 2024-11-06T02:54:34Z- handleDepth()- Depth_Keller msg: 1730861674.27 epoch sec; seqNo:780; depth 49.36 2024-11-06T02:54:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861674.28 epoch sec; seqNo:780; 4061567.50 north; 594395.31 east; 36.70 lat; -121.94 lon; 49.36 depth; utmZone: 10 2024-11-06T02:54:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861672.68 2024-11-06T02:54:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:34Z- waiting for 1730861676.68, time is 1730861674.28 2024-11-06T02:54:34Z- AHRS_M2 msg: 1730861674.67 epoch sec; seqNo:725 2024-11-06T02:54:34Z- AHRS_M2 msg: 0.11 phi; -0.09 theta; 5.34 psi 2024-11-06T02:54:34Z- handleDepth()- Depth_Keller msg: 1730861674.68 epoch sec; seqNo:781; depth 49.40 2024-11-06T02:54:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861674.69 epoch sec; seqNo:781; 4061567.50 north; 594395.31 east; 36.70 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T02:54:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861672.68 2024-11-06T02:54:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:34Z- waiting for 1730861676.68, time is 1730861674.69 2024-11-06T02:54:35Z- AHRS_M2 msg: 1730861675.07 epoch sec; seqNo:726 2024-11-06T02:54:35Z- AHRS_M2 msg: 0.12 phi; -0.09 theta; 5.34 psi 2024-11-06T02:54:35Z- handleDepth()- Depth_Keller msg: 1730861675.07 epoch sec; seqNo:782; depth 49.41 2024-11-06T02:54:35Z- handleDVL() - DVL msg epochMillisec: 1730861675.08 2024-11-06T02:54:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:35Z- handleDvl() - RDI_Pathfinder msg: 1730861675.08 sec; seqNo:249; #beams:0 2024-11-06T02:54:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.07 , 0.08 2024-11-06T02:54:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861675.11 epoch sec; seqNo:782; 4061567.86 north; 594394.08 east; 36.70 lat; -121.94 lon; 49.41 depth; utmZone: 10 2024-11-06T02:54:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861672.68 2024-11-06T02:54:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:35Z- waiting for 1730861676.68, time is 1730861675.11 2024-11-06T02:54:35Z- AHRS_M2 msg: 1730861675.47 epoch sec; seqNo:727 2024-11-06T02:54:35Z- AHRS_M2 msg: 0.12 phi; -0.09 theta; 5.34 psi 2024-11-06T02:54:35Z- handleDepth()- Depth_Keller msg: 1730861675.49 epoch sec; seqNo:783; depth 49.44 2024-11-06T02:54:35Z- handleDVL() - DVL msg epochMillisec: 1730861675.51 2024-11-06T02:54:35Z- handleDvl() - RDI_Pathfinder msg: 1730861675.51 sec; seqNo:250; #beams:4 2024-11-06T02:54:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.50 , 37.96 , 40.01 , 42.44 2024-11-06T02:54:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , 0.02 2024-11-06T02:54:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861675.53 epoch sec; seqNo:783; 4061567.86 north; 594394.08 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T02:54:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861672.68 2024-11-06T02:54:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:35Z- waiting for 1730861676.68, time is 1730861675.53 2024-11-06T02:54:35Z- AHRS_M2 msg: 1730861675.89 epoch sec; seqNo:728 2024-11-06T02:54:35Z- AHRS_M2 msg: 0.12 phi; -0.09 theta; 5.33 psi 2024-11-06T02:54:35Z- handleDepth()- Depth_Keller msg: 1730861675.89 epoch sec; seqNo:784; depth 49.45 2024-11-06T02:54:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861675.91 epoch sec; seqNo:784; 4061568.24 north; 594394.08 east; 36.70 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T02:54:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861672.68 2024-11-06T02:54:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:36Z- waiting for 1730861676.68, time is 1730861675.91 2024-11-06T02:54:36Z- AHRS_M2 msg: 1730861676.28 epoch sec; seqNo:729 2024-11-06T02:54:36Z- AHRS_M2 msg: 0.12 phi; -0.09 theta; 5.33 psi 2024-11-06T02:54:36Z- handleDepth()- Depth_Keller msg: 1730861676.30 epoch sec; seqNo:785; depth 49.48 2024-11-06T02:54:36Z- handleDVL() - DVL msg epochMillisec: 1730861676.30 2024-11-06T02:54:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:36Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:36Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:36Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:36Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:36Z- handleDvl() - RDI_Pathfinder msg: 1730861676.30 sec; seqNo:251; #beams:0 2024-11-06T02:54:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , 0.02 2024-11-06T02:54:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861676.31 epoch sec; seqNo:785; 4061568.24 north; 594394.08 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T02:54:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861672.68 2024-11-06T02:54:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:36Z- waiting for 1730861676.68, time is 1730861676.31 2024-11-06T02:54:36Z- AHRS_M2 msg: 1730861676.69 epoch sec; seqNo:730 2024-11-06T02:54:36Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 5.33 psi 2024-11-06T02:54:36Z- handleDepth()- Depth_Keller msg: 1730861676.69 epoch sec; seqNo:786; depth 49.48 2024-11-06T02:54:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861676.70 epoch sec; seqNo:786; 4061568.61 north; 594392.86 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T02:54:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861672.68 2024-11-06T02:54:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:36Z- waiting for 1730861676.68, time is 1730861676.70 2024-11-06T02:54:36Z- LcmTrn::updateTrn() >>>> pose time 1730861676.70, meas time 1730861676.30 <<<< 2024-11-06T02:54:36Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861676.70 2024-11-06T02:54:36Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:54:36Z- TerrainNav::motionUpdate() - currEstimate = (4061569, 594392.86, 49.48) 2024-11-06T02:54:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:54:36Z- TerrainNav:: SubcloudNIS is 0.59, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:54:36Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:54:36Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:54:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:54:36Z- variances([0], [2], and psi_berg) = 0.183527 0.340402 0.000000 2024-11-06T02:54:36Z- publishEstimatesDecoder() - at 1730861676.70 sec 2024-11-06T02:54:36Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:54:36Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061601.84 594376.82 51.67 2024-11-06T02:54:36Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061601.27 594377.13 52.44 lat/lon: 36.70 -121.94 2024-11-06T02:54:36Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.34 1.00 0.00 2024-11-06T02:54:37Z- AHRS_M2 msg: 1730861677.09 epoch sec; seqNo:731 2024-11-06T02:54:37Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 5.33 psi 2024-11-06T02:54:37Z- handleDepth()- Depth_Keller msg: 1730861677.10 epoch sec; seqNo:787; depth 49.50 2024-11-06T02:54:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861677.11 epoch sec; seqNo:787; 4061568.61 north; 594392.86 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T02:54:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861676.70 2024-11-06T02:54:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:37Z- waiting for 1730861680.70, time is 1730861677.11 2024-11-06T02:54:37Z- AHRS_M2 msg: 1730861677.50 epoch sec; seqNo:732 2024-11-06T02:54:37Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 5.33 psi 2024-11-06T02:54:37Z- handleDepth()- Depth_Keller msg: 1730861677.51 epoch sec; seqNo:788; depth 49.51 2024-11-06T02:54:37Z- handleDVL() - DVL msg epochMillisec: 1730861677.53 2024-11-06T02:54:37Z- handleDvl() - RDI_Pathfinder msg: 1730861677.53 sec; seqNo:252; #beams:4 2024-11-06T02:54:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.50 , 37.77 , 39.64 , 42.44 2024-11-06T02:54:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.07 , -0.01 2024-11-06T02:54:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861677.54 epoch sec; seqNo:788; 4061568.98 north; 594392.85 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T02:54:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861676.70 2024-11-06T02:54:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:37Z- waiting for 1730861680.70, time is 1730861677.54 2024-11-06T02:54:37Z- AHRS_M2 msg: 1730861677.90 epoch sec; seqNo:733 2024-11-06T02:54:37Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 5.33 psi 2024-11-06T02:54:37Z- handleDepth()- Depth_Keller msg: 1730861677.92 epoch sec; seqNo:789; depth 49.53 2024-11-06T02:54:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861677.94 epoch sec; seqNo:789; 4061569.35 north; 594391.63 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T02:54:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861676.70 2024-11-06T02:54:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:38Z- waiting for 1730861680.70, time is 1730861677.94 2024-11-06T02:54:38Z- AHRS_M2 msg: 1730861678.30 epoch sec; seqNo:734 2024-11-06T02:54:38Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 5.33 psi 2024-11-06T02:54:38Z- handleDepth()- Depth_Keller msg: 1730861678.31 epoch sec; seqNo:790; depth 49.55 2024-11-06T02:54:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861678.33 epoch sec; seqNo:790; 4061569.35 north; 594391.63 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T02:54:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861676.70 2024-11-06T02:54:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:38Z- waiting for 1730861680.70, time is 1730861678.33 2024-11-06T02:54:38Z- AHRS_M2 msg: 1730861678.71 epoch sec; seqNo:735 2024-11-06T02:54:38Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.33 psi 2024-11-06T02:54:38Z- handleDepth()- Depth_Keller msg: 1730861678.71 epoch sec; seqNo:791; depth 49.57 2024-11-06T02:54:38Z- handleDVL() - DVL msg epochMillisec: 1730861678.72 2024-11-06T02:54:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:38Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:38Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:38Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:38Z- handleDvl() - RDI_Pathfinder msg: 1730861678.72 sec; seqNo:253; #beams:0 2024-11-06T02:54:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.07 , -0.01 2024-11-06T02:54:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861678.72 epoch sec; seqNo:791; 4061569.73 north; 594391.62 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T02:54:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861676.70 2024-11-06T02:54:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:38Z- waiting for 1730861680.70, time is 1730861678.72 2024-11-06T02:54:39Z- AHRS_M2 msg: 1730861679.11 epoch sec; seqNo:736 2024-11-06T02:54:39Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.33 psi 2024-11-06T02:54:39Z- handleDepth()- Depth_Keller msg: 1730861679.13 epoch sec; seqNo:792; depth 49.58 2024-11-06T02:54:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861679.13 epoch sec; seqNo:792; 4061569.73 north; 594391.62 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T02:54:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861676.70 2024-11-06T02:54:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:39Z- waiting for 1730861680.70, time is 1730861679.13 2024-11-06T02:54:39Z- AHRS_M2 msg: 1730861679.52 epoch sec; seqNo:737 2024-11-06T02:54:39Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.33 psi 2024-11-06T02:54:39Z- handleDepth()- Depth_Keller msg: 1730861679.52 epoch sec; seqNo:793; depth 49.59 2024-11-06T02:54:39Z- handleDVL() - DVL msg epochMillisec: 1730861679.55 2024-11-06T02:54:39Z- handleDvl() - RDI_Pathfinder msg: 1730861679.55 sec; seqNo:254; #beams:4 2024-11-06T02:54:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.50 , 37.58 , 39.26 , 42.44 2024-11-06T02:54:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.07 , -0.00 2024-11-06T02:54:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861679.56 epoch sec; seqNo:793; 4061570.09 north; 594390.40 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T02:54:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861676.70 2024-11-06T02:54:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:39Z- waiting for 1730861680.70, time is 1730861679.56 2024-11-06T02:54:39Z- AHRS_M2 msg: 1730861679.92 epoch sec; seqNo:738 2024-11-06T02:54:39Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.33 psi 2024-11-06T02:54:39Z- handleDepth()- Depth_Keller msg: 1730861679.93 epoch sec; seqNo:794; depth 49.60 2024-11-06T02:54:39Z- handleDVL() - DVL msg epochMillisec: 1730861679.94 2024-11-06T02:54:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:39Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:39Z- handleDvl() - RDI_Pathfinder msg: 1730861679.94 sec; seqNo:255; #beams:0 2024-11-06T02:54:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.07 , -0.00 2024-11-06T02:54:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861679.97 epoch sec; seqNo:794; 4061570.09 north; 594390.40 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T02:54:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861676.70 2024-11-06T02:54:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:40Z- waiting for 1730861680.70, time is 1730861679.97 2024-11-06T02:54:40Z- AHRS_M2 msg: 1730861680.32 epoch sec; seqNo:739 2024-11-06T02:54:40Z- AHRS_M2 msg: 0.14 phi; -0.08 theta; 5.33 psi 2024-11-06T02:54:40Z- handleDepth()- Depth_Keller msg: 1730861680.34 epoch sec; seqNo:795; depth 49.62 2024-11-06T02:54:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861680.35 epoch sec; seqNo:795; 4061570.47 north; 594390.39 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T02:54:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861676.70 2024-11-06T02:54:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:40Z- waiting for 1730861680.70, time is 1730861680.35 2024-11-06T02:54:40Z- AHRS_M2 msg: 1730861680.73 epoch sec; seqNo:740 2024-11-06T02:54:40Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.33 psi 2024-11-06T02:54:40Z- handleDepth()- Depth_Keller msg: 1730861680.74 epoch sec; seqNo:796; depth 49.64 2024-11-06T02:54:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861680.76 epoch sec; seqNo:796; 4061570.47 north; 594390.39 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T02:54:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861676.70 2024-11-06T02:54:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:40Z- waiting for 1730861680.70, time is 1730861680.76 2024-11-06T02:54:40Z- LcmTrn::updateTrn() >>>> pose time 1730861680.76, meas time 1730861679.94 <<<< 2024-11-06T02:54:40Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861680.76 2024-11-06T02:54:40Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:54:40Z- TerrainNav::motionUpdate() - currEstimate = (4061570, 594390.39, 49.64) 2024-11-06T02:54:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:54:40Z- TerrainNav:: SubcloudNIS is 0.59, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:54:40Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:54:40Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:54:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:54:40Z- variances([0], [2], and psi_berg) = 0.184574 0.341621 0.000000 2024-11-06T02:54:40Z- publishEstimatesDecoder() - at 1730861680.76 sec 2024-11-06T02:54:40Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:54:40Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061603.72 594374.36 51.51 2024-11-06T02:54:40Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061603.13 594374.67 52.60 lat/lon: 36.70 -121.94 2024-11-06T02:54:40Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.34 1.21 0.00 2024-11-06T02:54:41Z- AHRS_M2 msg: 1730861681.13 epoch sec; seqNo:741 2024-11-06T02:54:41Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.33 psi 2024-11-06T02:54:41Z- handleDepth()- Depth_Keller msg: 1730861681.14 epoch sec; seqNo:797; depth 49.65 2024-11-06T02:54:41Z- handleDVL() - DVL msg epochMillisec: 1730861681.14 2024-11-06T02:54:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:41Z- handleDvl() - RDI_Pathfinder msg: 1730861681.14 sec; seqNo:256; #beams:0 2024-11-06T02:54:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.07 , -0.00 2024-11-06T02:54:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861681.15 epoch sec; seqNo:797; 4061570.84 north; 594389.17 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T02:54:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861680.76 2024-11-06T02:54:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:41Z- waiting for 1730861684.76, time is 1730861681.15 2024-11-06T02:54:41Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T02:54:41Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T02:54:41Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T02:54:41Z- AHRS_M2 msg: 1730861681.53 epoch sec; seqNo:742 2024-11-06T02:54:41Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.33 psi 2024-11-06T02:54:41Z- handleDepth()- Depth_Keller msg: 1730861681.56 epoch sec; seqNo:798; depth 49.67 2024-11-06T02:54:41Z- handleDVL() - DVL msg epochMillisec: 1730861681.58 2024-11-06T02:54:41Z- handleDvl() - RDI_Pathfinder msg: 1730861681.58 sec; seqNo:257; #beams:4 2024-11-06T02:54:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.87 , 37.39 , 40.01 , 42.26 2024-11-06T02:54:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.06 , 0.01 2024-11-06T02:54:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861680.76 2024-11-06T02:54:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:41Z- waiting for 1730861684.76, time is 1730861681.58 2024-11-06T02:54:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861681.71 epoch sec; seqNo:798; 4061571.22 north; 594389.17 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T02:54:41Z- 1 msgs in 200 ms and 2 handle calls 2024-11-06T02:54:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861680.76 2024-11-06T02:54:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:41Z- waiting for 1730861684.76, time is 1730861681.71 2024-11-06T02:54:41Z- AHRS_M2 msg: 1730861681.94 epoch sec; seqNo:743 2024-11-06T02:54:41Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.33 psi 2024-11-06T02:54:41Z- handleDepth()- Depth_Keller msg: 1730861681.95 epoch sec; seqNo:799; depth 49.69 2024-11-06T02:54:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861681.96 epoch sec; seqNo:799; 4061571.22 north; 594389.17 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T02:54:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861680.76 2024-11-06T02:54:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:42Z- waiting for 1730861684.76, time is 1730861681.96 2024-11-06T02:54:42Z- AHRS_M2 msg: 1730861682.35 epoch sec; seqNo:744 2024-11-06T02:54:42Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.33 psi 2024-11-06T02:54:42Z- handleDepth()- Depth_Keller msg: 1730861682.35 epoch sec; seqNo:800; depth 49.69 2024-11-06T02:54:42Z- handleDVL() - DVL msg epochMillisec: 1730861682.35 2024-11-06T02:54:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:42Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:42Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:42Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:42Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:42Z- handleDvl() - RDI_Pathfinder msg: 1730861682.35 sec; seqNo:258; #beams:0 2024-11-06T02:54:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.06 , 0.01 2024-11-06T02:54:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861682.36 epoch sec; seqNo:800; 4061571.58 north; 594387.94 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T02:54:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861680.76 2024-11-06T02:54:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:42Z- waiting for 1730861684.76, time is 1730861682.36 2024-11-06T02:54:42Z- AHRS_M2 msg: 1730861682.75 epoch sec; seqNo:745 2024-11-06T02:54:42Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.32 psi 2024-11-06T02:54:42Z- handleDepth()- Depth_Keller msg: 1730861682.76 epoch sec; seqNo:801; depth 49.73 2024-11-06T02:54:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861682.77 epoch sec; seqNo:801; 4061571.58 north; 594387.94 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T02:54:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861680.76 2024-11-06T02:54:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:42Z- waiting for 1730861684.76, time is 1730861682.77 2024-11-06T02:54:43Z- AHRS_M2 msg: 1730861683.15 epoch sec; seqNo:746 2024-11-06T02:54:43Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 5.32 psi 2024-11-06T02:54:43Z- handleDepth()- Depth_Keller msg: 1730861683.16 epoch sec; seqNo:802; depth 49.74 2024-11-06T02:54:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861683.17 epoch sec; seqNo:802; 4061571.96 north; 594387.94 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T02:54:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861680.76 2024-11-06T02:54:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:43Z- waiting for 1730861684.76, time is 1730861683.17 2024-11-06T02:54:43Z- AHRS_M2 msg: 1730861683.56 epoch sec; seqNo:747 2024-11-06T02:54:43Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 5.32 psi 2024-11-06T02:54:43Z- handleDepth()- Depth_Keller msg: 1730861683.57 epoch sec; seqNo:803; depth 49.77 2024-11-06T02:54:43Z- handleDVL() - DVL msg epochMillisec: 1730861683.59 2024-11-06T02:54:43Z- handleDvl() - RDI_Pathfinder msg: 1730861683.59 sec; seqNo:259; #beams:4 2024-11-06T02:54:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.06 , 37.39 , 39.83 , 41.88 2024-11-06T02:54:43Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.04 , 0.03 2024-11-06T02:54:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861683.61 epoch sec; seqNo:803; 4061571.96 north; 594387.94 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T02:54:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861680.76 2024-11-06T02:54:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:43Z- waiting for 1730861684.76, time is 1730861683.61 2024-11-06T02:54:43Z- AHRS_M2 msg: 1730861683.96 epoch sec; seqNo:748 2024-11-06T02:54:43Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 5.32 psi 2024-11-06T02:54:43Z- handleDepth()- Depth_Keller msg: 1730861683.97 epoch sec; seqNo:804; depth 49.78 2024-11-06T02:54:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861684.03 epoch sec; seqNo:804; 4061572.33 north; 594386.71 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T02:54:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861680.76 2024-11-06T02:54:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:44Z- waiting for 1730861684.76, time is 1730861684.03 2024-11-06T02:54:44Z- AHRS_M2 msg: 1730861684.37 epoch sec; seqNo:749 2024-11-06T02:54:44Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 5.32 psi 2024-11-06T02:54:44Z- handleDepth()- Depth_Keller msg: 1730861684.38 epoch sec; seqNo:805; depth 49.81 2024-11-06T02:54:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861684.39 epoch sec; seqNo:805; 4061572.33 north; 594386.71 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T02:54:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861680.76 2024-11-06T02:54:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:44Z- waiting for 1730861684.76, time is 1730861684.39 2024-11-06T02:54:44Z- AHRS_M2 msg: 1730861684.77 epoch sec; seqNo:750 2024-11-06T02:54:44Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 5.32 psi 2024-11-06T02:54:44Z- handleDepth()- Depth_Keller msg: 1730861684.78 epoch sec; seqNo:806; depth 49.83 2024-11-06T02:54:44Z- handleDVL() - DVL msg epochMillisec: 1730861684.78 2024-11-06T02:54:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:44Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:44Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:44Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:44Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:44Z- handleDvl() - RDI_Pathfinder msg: 1730861684.78 sec; seqNo:260; #beams:0 2024-11-06T02:54:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:44Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.04 , 0.03 2024-11-06T02:54:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861684.80 epoch sec; seqNo:806; 4061572.71 north; 594386.71 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T02:54:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861680.76 2024-11-06T02:54:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:44Z- waiting for 1730861684.76, time is 1730861684.80 2024-11-06T02:54:44Z- LcmTrn::updateTrn() >>>> pose time 1730861684.80, meas time 1730861684.78 <<<< 2024-11-06T02:54:44Z- LcmTrn::updateTrn() equating pose 1730861684.78 and meas times 1730861684.78 2024-11-06T02:54:44Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861684.78 2024-11-06T02:54:44Z- TerrainNav::motionUpdate() - currEstimate = (4061573, 594386.71, 49.83) 2024-11-06T02:54:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:54:44Z- TerrainNav:: SubcloudNIS is 0.59, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:54:44Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:54:44Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:54:44Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:54:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:54:44Z- variances([0], [2], and psi_berg) = 0.187590 0.344813 0.000000 2024-11-06T02:54:44Z- publishEstimatesDecoder() - at 1730861684.78 sec 2024-11-06T02:54:44Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:54:44Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061605.91 594370.82 52.63 2024-11-06T02:54:44Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061605.37 594370.98 52.79 lat/lon: 36.70 -121.94 2024-11-06T02:54:44Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.19 0.34 1.46 0.00 2024-11-06T02:54:45Z- AHRS_M2 msg: 1730861685.17 epoch sec; seqNo:751 2024-11-06T02:54:45Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 5.31 psi 2024-11-06T02:54:45Z- handleDepth()- Depth_Keller msg: 1730861685.18 epoch sec; seqNo:807; depth 49.85 2024-11-06T02:54:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861685.19 epoch sec; seqNo:807; 4061572.69 north; 594385.49 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T02:54:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861684.78 2024-11-06T02:54:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:45Z- waiting for 1730861688.78, time is 1730861685.19 2024-11-06T02:54:45Z- AHRS_M2 msg: 1730861685.59 epoch sec; seqNo:752 2024-11-06T02:54:45Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 5.31 psi 2024-11-06T02:54:45Z- handleDepth()- Depth_Keller msg: 1730861685.60 epoch sec; seqNo:808; depth 49.87 2024-11-06T02:54:45Z- handleDVL() - DVL msg epochMillisec: 1730861685.62 2024-11-06T02:54:45Z- handleDvl() - RDI_Pathfinder msg: 1730861685.62 sec; seqNo:261; #beams:4 2024-11-06T02:54:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.31 , 37.58 , 40.01 , 41.88 2024-11-06T02:54:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.02 , 0.07 2024-11-06T02:54:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861685.65 epoch sec; seqNo:808; 4061573.07 north; 594385.48 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T02:54:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861684.78 2024-11-06T02:54:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:45Z- waiting for 1730861688.78, time is 1730861685.65 2024-11-06T02:54:45Z- AHRS_M2 msg: 1730861685.98 epoch sec; seqNo:753 2024-11-06T02:54:45Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 5.31 psi 2024-11-06T02:54:45Z- handleDepth()- Depth_Keller msg: 1730861685.98 epoch sec; seqNo:809; depth 49.90 2024-11-06T02:54:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861686.02 epoch sec; seqNo:809; 4061573.45 north; 594385.48 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T02:54:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861684.78 2024-11-06T02:54:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:46Z- waiting for 1730861688.78, time is 1730861686.02 2024-11-06T02:54:46Z- AHRS_M2 msg: 1730861686.40 epoch sec; seqNo:754 2024-11-06T02:54:46Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.31 psi 2024-11-06T02:54:46Z- handleDepth()- Depth_Keller msg: 1730861686.40 epoch sec; seqNo:810; depth 49.91 2024-11-06T02:54:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861686.42 epoch sec; seqNo:810; 4061573.45 north; 594385.48 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T02:54:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861684.78 2024-11-06T02:54:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:46Z- waiting for 1730861688.78, time is 1730861686.42 2024-11-06T02:54:46Z- AHRS_M2 msg: 1730861686.79 epoch sec; seqNo:755 2024-11-06T02:54:46Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.31 psi 2024-11-06T02:54:46Z- handleDepth()- Depth_Keller msg: 1730861686.80 epoch sec; seqNo:811; depth 49.93 2024-11-06T02:54:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861686.81 epoch sec; seqNo:811; 4061573.82 north; 594384.26 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T02:54:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861684.78 2024-11-06T02:54:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:46Z- waiting for 1730861688.78, time is 1730861686.81 2024-11-06T02:54:47Z- AHRS_M2 msg: 1730861687.19 epoch sec; seqNo:756 2024-11-06T02:54:47Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.31 psi 2024-11-06T02:54:47Z- handleDepth()- Depth_Keller msg: 1730861687.21 epoch sec; seqNo:812; depth 49.95 2024-11-06T02:54:47Z- handleDVL() - DVL msg epochMillisec: 1730861687.21 2024-11-06T02:54:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:47Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:47Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:47Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:47Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:47Z- handleDvl() - RDI_Pathfinder msg: 1730861687.21 sec; seqNo:262; #beams:0 2024-11-06T02:54:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.02 , 0.07 2024-11-06T02:54:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861687.24 epoch sec; seqNo:812; 4061573.82 north; 594384.26 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T02:54:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861684.78 2024-11-06T02:54:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:47Z- waiting for 1730861688.78, time is 1730861687.24 2024-11-06T02:54:47Z- AHRS_M2 msg: 1730861687.60 epoch sec; seqNo:757 2024-11-06T02:54:47Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.31 psi 2024-11-06T02:54:47Z- handleDepth()- Depth_Keller msg: 1730861687.60 epoch sec; seqNo:813; depth 49.96 2024-11-06T02:54:47Z- handleDVL() - DVL msg epochMillisec: 1730861687.63 2024-11-06T02:54:47Z- handleDvl() - RDI_Pathfinder msg: 1730861687.63 sec; seqNo:263; #beams:4 2024-11-06T02:54:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.69 , 37.39 , 39.83 , 42.07 2024-11-06T02:54:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.05 2024-11-06T02:54:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861687.66 epoch sec; seqNo:813; 4061574.19 north; 594384.25 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T02:54:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861684.78 2024-11-06T02:54:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:47Z- waiting for 1730861688.78, time is 1730861687.66 2024-11-06T02:54:48Z- AHRS_M2 msg: 1730861688.01 epoch sec; seqNo:758 2024-11-06T02:54:48Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.32 psi 2024-11-06T02:54:48Z- handleDepth()- Depth_Keller msg: 1730861688.01 epoch sec; seqNo:814; depth 49.99 2024-11-06T02:54:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861688.02 epoch sec; seqNo:814; 4061574.19 north; 594384.25 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T02:54:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861684.78 2024-11-06T02:54:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:48Z- waiting for 1730861688.78, time is 1730861688.02 2024-11-06T02:54:48Z- AHRS_M2 msg: 1730861688.40 epoch sec; seqNo:759 2024-11-06T02:54:48Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.32 psi 2024-11-06T02:54:48Z- handleDepth()- Depth_Keller msg: 1730861688.42 epoch sec; seqNo:815; depth 50.00 2024-11-06T02:54:48Z- handleDVL() - DVL msg epochMillisec: 1730861688.42 2024-11-06T02:54:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:48Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:48Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:48Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:48Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:48Z- handleDvl() - RDI_Pathfinder msg: 1730861688.42 sec; seqNo:264; #beams:0 2024-11-06T02:54:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.05 2024-11-06T02:54:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861688.43 epoch sec; seqNo:815; 4061574.56 north; 594383.03 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T02:54:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861684.78 2024-11-06T02:54:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:48Z- waiting for 1730861688.78, time is 1730861688.43 2024-11-06T02:54:48Z- AHRS_M2 msg: 1730861688.81 epoch sec; seqNo:760 2024-11-06T02:54:48Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.32 psi 2024-11-06T02:54:48Z- handleDepth()- Depth_Keller msg: 1730861688.81 epoch sec; seqNo:816; depth 50.00 2024-11-06T02:54:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861688.83 epoch sec; seqNo:816; 4061574.56 north; 594383.03 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T02:54:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861684.78 2024-11-06T02:54:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:48Z- waiting for 1730861688.78, time is 1730861688.83 2024-11-06T02:54:48Z- LcmTrn::updateTrn() >>>> pose time 1730861688.83, meas time 1730861688.42 <<<< 2024-11-06T02:54:48Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861688.83 2024-11-06T02:54:48Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:54:48Z- TerrainNav::motionUpdate() - currEstimate = (4061575, 594383.03, 50.00) 2024-11-06T02:54:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:54:48Z- TerrainNav:: SubcloudNIS is 0.59, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:54:48Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:54:48Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:54:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:54:48Z- variances([0], [2], and psi_berg) = 0.190528 0.348089 0.000000 2024-11-06T02:54:48Z- publishEstimatesDecoder() - at 1730861688.83 sec 2024-11-06T02:54:48Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:54:48Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061607.76 594367.05 52.11 2024-11-06T02:54:48Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061607.22 594367.30 52.96 lat/lon: 36.70 -121.94 2024-11-06T02:54:48Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.19 0.35 1.71 0.00 2024-11-06T02:54:49Z- AHRS_M2 msg: 1730861689.22 epoch sec; seqNo:761 2024-11-06T02:54:49Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.32 psi 2024-11-06T02:54:49Z- handleDepth()- Depth_Keller msg: 1730861689.23 epoch sec; seqNo:817; depth 50.02 2024-11-06T02:54:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861689.30 epoch sec; seqNo:817; 4061574.94 north; 594383.02 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T02:54:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861688.83 2024-11-06T02:54:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:49Z- waiting for 1730861692.83, time is 1730861689.30 2024-11-06T02:54:49Z- AHRS_M2 msg: 1730861689.62 epoch sec; seqNo:762 2024-11-06T02:54:49Z- AHRS_M2 msg: 0.14 phi; -0.08 theta; 5.32 psi 2024-11-06T02:54:49Z- handleDepth()- Depth_Keller msg: 1730861689.62 epoch sec; seqNo:818; depth 50.03 2024-11-06T02:54:49Z- handleDVL() - DVL msg epochMillisec: 1730861689.65 2024-11-06T02:54:49Z- handleDvl() - RDI_Pathfinder msg: 1730861689.65 sec; seqNo:265; #beams:4 2024-11-06T02:54:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.87 , 37.58 , 39.63 , 42.25 2024-11-06T02:54:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.03 2024-11-06T02:54:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861689.68 epoch sec; seqNo:818; 4061574.94 north; 594383.02 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T02:54:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861688.83 2024-11-06T02:54:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:49Z- waiting for 1730861692.83, time is 1730861689.68 2024-11-06T02:54:50Z- AHRS_M2 msg: 1730861690.02 epoch sec; seqNo:763 2024-11-06T02:54:50Z- AHRS_M2 msg: 0.14 phi; -0.08 theta; 5.32 psi 2024-11-06T02:54:50Z- handleDepth()- Depth_Keller msg: 1730861690.03 epoch sec; seqNo:819; depth 50.04 2024-11-06T02:54:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861690.05 epoch sec; seqNo:819; 4061575.30 north; 594381.80 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T02:54:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861688.83 2024-11-06T02:54:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:50Z- waiting for 1730861692.83, time is 1730861690.05 2024-11-06T02:54:50Z- AHRS_M2 msg: 1730861690.42 epoch sec; seqNo:764 2024-11-06T02:54:50Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.32 psi 2024-11-06T02:54:50Z- handleDepth()- Depth_Keller msg: 1730861690.43 epoch sec; seqNo:820; depth 50.06 2024-11-06T02:54:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861690.44 epoch sec; seqNo:820; 4061575.30 north; 594381.80 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T02:54:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861688.83 2024-11-06T02:54:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:50Z- waiting for 1730861692.83, time is 1730861690.44 2024-11-06T02:54:50Z- AHRS_M2 msg: 1730861690.83 epoch sec; seqNo:765 2024-11-06T02:54:50Z- AHRS_M2 msg: 0.14 phi; -0.08 theta; 5.32 psi 2024-11-06T02:54:50Z- handleDepth()- Depth_Keller msg: 1730861690.84 epoch sec; seqNo:821; depth 50.07 2024-11-06T02:54:50Z- handleDVL() - DVL msg epochMillisec: 1730861690.84 2024-11-06T02:54:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:54:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:54:50Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:54:50Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:54:50Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:54:50Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:54:50Z- handleDvl() - RDI_Pathfinder msg: 1730861690.84 sec; seqNo:266; #beams:0 2024-11-06T02:54:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:54:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.03 2024-11-06T02:54:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861690.85 epoch sec; seqNo:821; 4061575.68 north; 594381.79 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T02:54:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861688.83 2024-11-06T02:54:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:50Z- waiting for 1730861692.83, time is 1730861690.85 2024-11-06T02:54:51Z- AHRS_M2 msg: 1730861691.27 epoch sec; seqNo:766 2024-11-06T02:54:51Z- AHRS_M2 msg: 0.14 phi; -0.08 theta; 5.32 psi 2024-11-06T02:54:51Z- handleDepth()- Depth_Keller msg: 1730861691.30 epoch sec; seqNo:822; depth 50.08 2024-11-06T02:54:51Z- handleDVL() - DVL msg epochMillisec: 1730861691.31 2024-11-06T02:54:51Z- handleDvl() - RDI_Pathfinder msg: 1730861691.31 sec; seqNo:267; #beams:4 2024-11-06T02:54:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.49 , 37.20 , 39.45 , 42.25 2024-11-06T02:54:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.06 , -0.06 2024-11-06T02:54:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861691.33 epoch sec; seqNo:822; 4061576.06 north; 594381.79 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T02:54:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861688.83 2024-11-06T02:54:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:51Z- waiting for 1730861692.83, time is 1730861691.33 2024-11-06T02:54:51Z- AHRS_M2 msg: 1730861691.64 epoch sec; seqNo:767 2024-11-06T02:54:51Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.32 psi 2024-11-06T02:54:51Z- handleDepth()- Depth_Keller msg: 1730861691.64 epoch sec; seqNo:823; depth 50.10 2024-11-06T02:54:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861691.67 epoch sec; seqNo:823; 4061576.05 north; 594380.57 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T02:54:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861688.83 2024-11-06T02:54:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:51Z- waiting for 1730861692.83, time is 1730861691.67 2024-11-06T02:54:52Z- AHRS_M2 msg: 1730861692.04 epoch sec; seqNo:768 2024-11-06T02:54:52Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.31 psi 2024-11-06T02:54:52Z- handleDepth()- Depth_Keller msg: 1730861692.05 epoch sec; seqNo:824; depth 50.12 2024-11-06T02:54:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861692.06 epoch sec; seqNo:824; 4061576.43 north; 594380.56 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T02:54:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861688.83 2024-11-06T02:54:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:52Z- waiting for 1730861692.83, time is 1730861692.06 2024-11-06T02:54:52Z- handleDepth()- Depth_Keller msg: 1730861692.47 epoch sec; seqNo:825; depth 50.14 2024-11-06T02:54:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861692.48 epoch sec; seqNo:825; 4061576.43 north; 594380.56 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T02:54:52Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:54:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861688.83 2024-11-06T02:54:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:52Z- waiting for 1730861692.83, time is 1730861692.48 2024-11-06T02:54:52Z- AHRS_M2 msg: 1730861692.85 epoch sec; seqNo:769 2024-11-06T02:54:52Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.31 psi 2024-11-06T02:54:52Z- handleDepth()- Depth_Keller msg: 1730861692.86 epoch sec; seqNo:826; depth 50.16 2024-11-06T02:54:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861692.87 epoch sec; seqNo:826; 4061576.79 north; 594379.34 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T02:54:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861688.83 2024-11-06T02:54:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:53Z- waiting for 1730861692.83, time is 1730861692.87 2024-11-06T02:54:53Z- LcmTrn::updateTrn() >>>> pose time 1730861692.87, meas time 1730861691.31 <<<< 2024-11-06T02:54:53Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861692.87 2024-11-06T02:54:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861691.31 sec, ping # 0; waiting for INS data more recent than 1730861688.83... 2024-11-06T02:54:53Z- TerrainNav::motionUpdate() - currEstimate = (4061577, 594379.34, 50.16) 2024-11-06T02:54:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:54:53Z- TerrainNav:: SubcloudNIS is 0.59, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:54:53Z- TNavPF::Projecting Measurements 2024-11-06T02:54:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:54:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:54:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:54:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:54:53Z- TNavFilter::4 good beams. 2024-11-06T02:54:53Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:54:53Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:54:53Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:54:53Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:54:53Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:54:53Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:54:53Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:54:53Z- TerrainMapOctree: (vehN, vehE ) = (4061575.93, 594380.76). 2024-11-06T02:54:53Z- TerrainMapOctree: Min Distance = 217.71. 2024-11-06T02:54:53Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:54:53Z- Forcing Subcloud Comparison 2024-11-06T02:54:53Z- Weighting particles with subcloud comparison 2024-11-06T02:54:53Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:53Z- meanExpectedMeasDiff: -3.094013 delta_rms_squared: 3.164559 alpha: 0.993720 2024-11-06T02:54:53Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:53Z- meanExpectedMeasDiff: 4.021180 delta_rms_squared: 1.915446 alpha: 0.989666 2024-11-06T02:54:53Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:53Z- meanExpectedMeasDiff: -0.293230 delta_rms_squared: 2.410961 alpha: 0.991773 2024-11-06T02:54:53Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:53Z- meanExpectedMeasDiff: -0.232529 delta_rms_squared: 2.906819 alpha: 0.993167 2024-11-06T02:54:53Z- Subcloud NIS: 0.607878 2024-11-06T02:54:53Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:54:53Z- TNavPF:: sumWeights = 0.028221 2024-11-06T02:54:53Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:54:53Z- TNavPF::First Term of Map Covariance Matrix is 0.29 2024-11-06T02:54:53Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T02:54:53Z- TerrainNav::Resampling particle filter... 2024-11-06T02:54:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861691.31 sec. 2024-11-06T02:54:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:54:53Z- variances([0], [2], and psi_berg) = 0.177820 0.269481 0.000000 2024-11-06T02:54:53Z- publishEstimatesDecoder() - at 1730861692.87 sec 2024-11-06T02:54:53Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:54:53Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061608.68 594376.87 52.30 2024-11-06T02:54:53Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061609.43 594363.61 53.03 lat/lon: 36.70 -121.94 2024-11-06T02:54:53Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.27 0.82 0.00 2024-11-06T02:54:53Z- AHRS_M2 msg: 1730861693.29 epoch sec; seqNo:770 2024-11-06T02:54:53Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.31 psi 2024-11-06T02:54:53Z- handleDepth()- Depth_Keller msg: 1730861693.32 epoch sec; seqNo:827; depth 50.18 2024-11-06T02:54:53Z- handleDVL() - DVL msg epochMillisec: 1730861693.36 2024-11-06T02:54:53Z- handleDvl() - RDI_Pathfinder msg: 1730861693.36 sec; seqNo:268; #beams:4 2024-11-06T02:54:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.49 , 37.02 , 39.26 , 41.88 2024-11-06T02:54:53Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , -0.07 , 0.01 2024-11-06T02:54:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861693.37 epoch sec; seqNo:827; 4061576.79 north; 594379.34 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T02:54:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861692.87 2024-11-06T02:54:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:53Z- waiting for 1730861696.87, time is 1730861693.37 2024-11-06T02:54:53Z- handleDepth()- Depth_Keller msg: 1730861693.68 epoch sec; seqNo:828; depth 50.21 2024-11-06T02:54:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861693.70 epoch sec; seqNo:828; 4061577.17 north; 594379.34 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T02:54:53Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:54:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861692.87 2024-11-06T02:54:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:53Z- waiting for 1730861696.87, time is 1730861693.70 2024-11-06T02:54:54Z- AHRS_M2 msg: 1730861694.06 epoch sec; seqNo:771 2024-11-06T02:54:54Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.30 psi 2024-11-06T02:54:54Z- handleDepth()- Depth_Keller msg: 1730861694.07 epoch sec; seqNo:829; depth 50.22 2024-11-06T02:54:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861694.08 epoch sec; seqNo:829; 4061577.17 north; 594379.34 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T02:54:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861692.87 2024-11-06T02:54:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:54Z- waiting for 1730861696.87, time is 1730861694.08 2024-11-06T02:54:54Z- AHRS_M2 msg: 1730861694.47 epoch sec; seqNo:772 2024-11-06T02:54:54Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.30 psi 2024-11-06T02:54:54Z- handleDepth()- Depth_Keller msg: 1730861694.48 epoch sec; seqNo:830; depth 50.26 2024-11-06T02:54:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861694.50 epoch sec; seqNo:830; 4061577.54 north; 594378.11 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T02:54:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861692.87 2024-11-06T02:54:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:54Z- waiting for 1730861696.87, time is 1730861694.50 2024-11-06T02:54:54Z- AHRS_M2 msg: 1730861694.87 epoch sec; seqNo:773 2024-11-06T02:54:54Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.30 psi 2024-11-06T02:54:54Z- handleDepth()- Depth_Keller msg: 1730861694.88 epoch sec; seqNo:831; depth 50.28 2024-11-06T02:54:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861694.89 epoch sec; seqNo:831; 4061577.54 north; 594378.11 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T02:54:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861692.87 2024-11-06T02:54:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:55Z- waiting for 1730861696.87, time is 1730861694.89 2024-11-06T02:54:55Z- AHRS_M2 msg: 1730861695.29 epoch sec; seqNo:774 2024-11-06T02:54:55Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.30 psi 2024-11-06T02:54:55Z- handleDepth()- Depth_Keller msg: 1730861695.30 epoch sec; seqNo:832; depth 50.29 2024-11-06T02:54:55Z- handleDVL() - DVL msg epochMillisec: 1730861695.32 2024-11-06T02:54:55Z- handleDvl() - RDI_Pathfinder msg: 1730861695.32 sec; seqNo:269; #beams:4 2024-11-06T02:54:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.12 , 37.02 , 39.45 , 41.50 2024-11-06T02:54:55Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.06 , 0.07 2024-11-06T02:54:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861695.33 epoch sec; seqNo:832; 4061577.92 north; 594378.11 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T02:54:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861692.87 2024-11-06T02:54:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:55Z- waiting for 1730861696.87, time is 1730861695.33 2024-11-06T02:54:55Z- AHRS_M2 msg: 1730861695.67 epoch sec; seqNo:775 2024-11-06T02:54:55Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.30 psi 2024-11-06T02:54:55Z- handleDepth()- Depth_Keller msg: 1730861695.68 epoch sec; seqNo:833; depth 50.33 2024-11-06T02:54:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861695.69 epoch sec; seqNo:833; 4061577.90 north; 594376.89 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T02:54:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861692.87 2024-11-06T02:54:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:55Z- waiting for 1730861696.87, time is 1730861695.69 2024-11-06T02:54:56Z- AHRS_M2 msg: 1730861696.09 epoch sec; seqNo:776 2024-11-06T02:54:56Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.30 psi 2024-11-06T02:54:56Z- handleDepth()- Depth_Keller msg: 1730861696.09 epoch sec; seqNo:834; depth 50.36 2024-11-06T02:54:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861696.10 epoch sec; seqNo:834; 4061578.28 north; 594376.88 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T02:54:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861692.87 2024-11-06T02:54:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:56Z- waiting for 1730861696.87, time is 1730861696.10 2024-11-06T02:54:56Z- AHRS_M2 msg: 1730861696.49 epoch sec; seqNo:777 2024-11-06T02:54:56Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.30 psi 2024-11-06T02:54:56Z- handleDepth()- Depth_Keller msg: 1730861696.50 epoch sec; seqNo:835; depth 50.39 2024-11-06T02:54:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861696.51 epoch sec; seqNo:835; 4061578.66 north; 594376.88 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T02:54:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861692.87 2024-11-06T02:54:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:56Z- waiting for 1730861696.87, time is 1730861696.51 2024-11-06T02:54:56Z- AHRS_M2 msg: 1730861696.89 epoch sec; seqNo:778 2024-11-06T02:54:56Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.30 psi 2024-11-06T02:54:56Z- handleDepth()- Depth_Keller msg: 1730861696.89 epoch sec; seqNo:836; depth 50.40 2024-11-06T02:54:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861696.91 epoch sec; seqNo:836; 4061578.65 north; 594375.66 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T02:54:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861692.87 2024-11-06T02:54:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:57Z- waiting for 1730861696.87, time is 1730861696.91 2024-11-06T02:54:57Z- LcmTrn::updateTrn() >>>> pose time 1730861696.91, meas time 1730861695.32 <<<< 2024-11-06T02:54:57Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861696.91 2024-11-06T02:54:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861695.32 sec, ping # 0; waiting for INS data more recent than 1730861692.87... 2024-11-06T02:54:57Z- TerrainNav::motionUpdate() - currEstimate = (4061579, 594375.66, 50.40) 2024-11-06T02:54:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:54:57Z- TerrainNav:: SubcloudNIS is 0.61, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:54:57Z- TNavPF::Projecting Measurements 2024-11-06T02:54:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:54:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:54:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:54:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:54:57Z- TNavFilter::4 good beams. 2024-11-06T02:54:57Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:54:57Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:54:57Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:54:57Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:54:57Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:54:57Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:54:57Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:54:57Z- TerrainMapOctree: (vehN, vehE ) = (4061577.92, 594377.11). 2024-11-06T02:54:57Z- TerrainMapOctree: Min Distance = 213.94. 2024-11-06T02:54:57Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:54:57Z- Forcing Subcloud Comparison 2024-11-06T02:54:57Z- Weighting particles with subcloud comparison 2024-11-06T02:54:57Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:57Z- meanExpectedMeasDiff: -3.354868 delta_rms_squared: 2.027195 alpha: 0.990231 2024-11-06T02:54:57Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:57Z- meanExpectedMeasDiff: 3.663981 delta_rms_squared: 0.989541 alpha: 0.980189 2024-11-06T02:54:57Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:57Z- meanExpectedMeasDiff: -0.150796 delta_rms_squared: 1.359760 alpha: 0.985505 2024-11-06T02:54:57Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:54:57Z- meanExpectedMeasDiff: -1.128582 delta_rms_squared: 2.038190 alpha: 0.990283 2024-11-06T02:54:57Z- Subcloud NIS: 0.617863 2024-11-06T02:54:57Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:54:57Z- TNavPF:: sumWeights = 0.035731 2024-11-06T02:54:57Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:54:57Z- TNavPF::First Term of Map Covariance Matrix is 0.33 2024-11-06T02:54:57Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T02:54:57Z- TerrainNav::Resampling particle filter... 2024-11-06T02:54:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861695.32 sec. 2024-11-06T02:54:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:54:57Z- variances([0], [2], and psi_berg) = 0.207486 0.244579 0.000000 2024-11-06T02:54:57Z- publishEstimatesDecoder() - at 1730861696.91 sec 2024-11-06T02:54:57Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:54:57Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061610.51 594373.14 52.79 2024-11-06T02:54:57Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061611.22 594359.89 53.51 lat/lon: 36.70 -121.94 2024-11-06T02:54:57Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.24 0.78 0.00 2024-11-06T02:54:57Z- handleDepth()- Depth_Keller msg: 1730861697.33 epoch sec; seqNo:837; depth 50.44 2024-11-06T02:54:57Z- handleDVL() - DVL msg epochMillisec: 1730861697.36 2024-11-06T02:54:57Z- handleDvl() - RDI_Pathfinder msg: 1730861697.36 sec; seqNo:270; #beams:4 2024-11-06T02:54:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.49 , 37.20 , 39.82 , 41.50 2024-11-06T02:54:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , 0.07 2024-11-06T02:54:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861697.38 epoch sec; seqNo:837; 4061579.03 north; 594375.65 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T02:54:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861696.91 2024-11-06T02:54:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:57Z- waiting for 1730861700.91, time is 1730861697.38 2024-11-06T02:54:57Z- AHRS_M2 msg: 1730861697.69 epoch sec; seqNo:779 2024-11-06T02:54:57Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.30 psi 2024-11-06T02:54:57Z- handleDepth()- Depth_Keller msg: 1730861697.70 epoch sec; seqNo:838; depth 50.48 2024-11-06T02:54:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861697.72 epoch sec; seqNo:838; 4061579.03 north; 594375.65 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T02:54:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861696.91 2024-11-06T02:54:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:57Z- waiting for 1730861700.91, time is 1730861697.72 2024-11-06T02:54:58Z- AHRS_M2 msg: 1730861698.11 epoch sec; seqNo:780 2024-11-06T02:54:58Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.30 psi 2024-11-06T02:54:58Z- handleDepth()- Depth_Keller msg: 1730861698.11 epoch sec; seqNo:839; depth 50.50 2024-11-06T02:54:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861698.12 epoch sec; seqNo:839; 4061579.03 north; 594375.65 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T02:54:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861696.91 2024-11-06T02:54:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:58Z- waiting for 1730861700.91, time is 1730861698.12 2024-11-06T02:54:58Z- AHRS_M2 msg: 1730861698.51 epoch sec; seqNo:781 2024-11-06T02:54:58Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.30 psi 2024-11-06T02:54:58Z- handleDepth()- Depth_Keller msg: 1730861698.51 epoch sec; seqNo:840; depth 50.53 2024-11-06T02:54:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861698.53 epoch sec; seqNo:840; 4061579.39 north; 594374.43 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T02:54:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861696.91 2024-11-06T02:54:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:58Z- waiting for 1730861700.91, time is 1730861698.53 2024-11-06T02:54:58Z- AHRS_M2 msg: 1730861698.91 epoch sec; seqNo:782 2024-11-06T02:54:58Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 5.31 psi 2024-11-06T02:54:58Z- handleDepth()- Depth_Keller msg: 1730861698.92 epoch sec; seqNo:841; depth 50.55 2024-11-06T02:54:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861698.93 epoch sec; seqNo:841; 4061579.39 north; 594374.43 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T02:54:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861696.91 2024-11-06T02:54:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:59Z- waiting for 1730861700.91, time is 1730861698.93 2024-11-06T02:54:59Z- AHRS_M2 msg: 1730861699.31 epoch sec; seqNo:783 2024-11-06T02:54:59Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 5.31 psi 2024-11-06T02:54:59Z- handleDepth()- Depth_Keller msg: 1730861699.32 epoch sec; seqNo:842; depth 50.58 2024-11-06T02:54:59Z- handleDVL() - DVL msg epochMillisec: 1730861699.34 2024-11-06T02:54:59Z- handleDvl() - RDI_Pathfinder msg: 1730861699.34 sec; seqNo:271; #beams:4 2024-11-06T02:54:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.87 , 37.20 , 39.45 , 41.69 2024-11-06T02:54:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.02 2024-11-06T02:54:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861699.37 epoch sec; seqNo:842; 4061579.77 north; 594374.43 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T02:54:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:54:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861696.91 2024-11-06T02:54:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:59Z- waiting for 1730861700.91, time is 1730861699.37 2024-11-06T02:54:59Z- AHRS_M2 msg: 1730861699.71 epoch sec; seqNo:784 2024-11-06T02:54:59Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 5.31 psi 2024-11-06T02:54:59Z- handleDepth()- Depth_Keller msg: 1730861699.73 epoch sec; seqNo:843; depth 50.60 2024-11-06T02:54:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861699.76 epoch sec; seqNo:843; 4061580.15 north; 594374.42 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T02:54:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:54:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861696.91 2024-11-06T02:54:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:54:59Z- waiting for 1730861700.91, time is 1730861699.76 2024-11-06T02:55:00Z- AHRS_M2 msg: 1730861700.12 epoch sec; seqNo:785 2024-11-06T02:55:00Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 5.32 psi 2024-11-06T02:55:00Z- handleDepth()- Depth_Keller msg: 1730861700.13 epoch sec; seqNo:844; depth 50.63 2024-11-06T02:55:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861700.14 epoch sec; seqNo:844; 4061580.14 north; 594373.20 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T02:55:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861696.91 2024-11-06T02:55:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:00Z- waiting for 1730861700.91, time is 1730861700.14 2024-11-06T02:55:00Z- AHRS_M2 msg: 1730861700.53 epoch sec; seqNo:786 2024-11-06T02:55:00Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 5.32 psi 2024-11-06T02:55:00Z- handleDepth()- Depth_Keller msg: 1730861700.54 epoch sec; seqNo:845; depth 50.62 2024-11-06T02:55:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861700.55 epoch sec; seqNo:845; 4061580.51 north; 594373.20 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T02:55:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861696.91 2024-11-06T02:55:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:00Z- waiting for 1730861700.91, time is 1730861700.55 2024-11-06T02:55:00Z- AHRS_M2 msg: 1730861700.93 epoch sec; seqNo:787 2024-11-06T02:55:00Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 5.33 psi 2024-11-06T02:55:00Z- handleDepth()- Depth_Keller msg: 1730861700.94 epoch sec; seqNo:846; depth 50.64 2024-11-06T02:55:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861700.95 epoch sec; seqNo:846; 4061580.51 north; 594373.20 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T02:55:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861696.91 2024-11-06T02:55:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:01Z- waiting for 1730861700.91, time is 1730861700.95 2024-11-06T02:55:01Z- LcmTrn::updateTrn() >>>> pose time 1730861700.95, meas time 1730861699.34 <<<< 2024-11-06T02:55:01Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861700.95 2024-11-06T02:55:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861699.34 sec, ping # 0; waiting for INS data more recent than 1730861696.91... 2024-11-06T02:55:01Z- TerrainNav::motionUpdate() - currEstimate = (4061581, 594373.20, 50.64) 2024-11-06T02:55:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:55:01Z- TerrainNav:: SubcloudNIS is 0.62, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:55:01Z- TNavPF::Projecting Measurements 2024-11-06T02:55:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:55:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:55:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:55:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:55:01Z- TNavFilter::4 good beams. 2024-11-06T02:55:01Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:55:01Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:55:01Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:55:01Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:55:01Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:55:01Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:55:01Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:55:01Z- TerrainMapOctree: (vehN, vehE ) = (4061579.77, 594374.18). 2024-11-06T02:55:01Z- TerrainMapOctree: Min Distance = 210.71. 2024-11-06T02:55:01Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:55:01Z- Forcing Subcloud Comparison 2024-11-06T02:55:01Z- Weighting particles with subcloud comparison 2024-11-06T02:55:01Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:01Z- meanExpectedMeasDiff: -2.989914 delta_rms_squared: 2.984312 alpha: 0.993343 2024-11-06T02:55:01Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:01Z- meanExpectedMeasDiff: 3.622788 delta_rms_squared: 1.190423 alpha: 0.983477 2024-11-06T02:55:01Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:01Z- meanExpectedMeasDiff: -0.514507 delta_rms_squared: 2.170555 alpha: 0.990870 2024-11-06T02:55:01Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:01Z- meanExpectedMeasDiff: -0.253105 delta_rms_squared: 1.663544 alpha: 0.988120 2024-11-06T02:55:01Z- Subcloud NIS: 0.630242 2024-11-06T02:55:01Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:55:01Z- TNavPF:: sumWeights = 0.043662 2024-11-06T02:55:01Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:55:01Z- TNavPF::First Term of Map Covariance Matrix is 0.37 2024-11-06T02:55:01Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T02:55:01Z- TerrainNav::Resampling particle filter... 2024-11-06T02:55:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861699.34 sec. 2024-11-06T02:55:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:55:01Z- variances([0], [2], and psi_berg) = 0.226702 0.243675 0.000000 2024-11-06T02:55:01Z- publishEstimatesDecoder() - at 1730861700.95 sec 2024-11-06T02:55:01Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:55:01Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061612.76 594370.88 52.92 2024-11-06T02:55:01Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061613.18 594357.43 53.87 lat/lon: 36.70 -121.94 2024-11-06T02:55:01Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.24 0.67 0.00 2024-11-06T02:55:01Z- AHRS_M2 msg: 1730861701.33 epoch sec; seqNo:788 2024-11-06T02:55:01Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 5.33 psi 2024-11-06T02:55:01Z- handleDepth()- Depth_Keller msg: 1730861701.35 epoch sec; seqNo:847; depth 50.65 2024-11-06T02:55:01Z- handleDVL() - DVL msg epochMillisec: 1730861701.41 2024-11-06T02:55:01Z- handleDvl() - RDI_Pathfinder msg: 1730861701.41 sec; seqNo:272; #beams:4 2024-11-06T02:55:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.68 , 36.83 , 39.63 , 41.50 2024-11-06T02:55:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , -0.02 2024-11-06T02:55:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861701.43 epoch sec; seqNo:847; 4061580.89 north; 594373.19 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T02:55:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861700.95 2024-11-06T02:55:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:01Z- waiting for 1730861704.95, time is 1730861701.43 2024-11-06T02:55:01Z- AHRS_M2 msg: 1730861701.74 epoch sec; seqNo:789 2024-11-06T02:55:01Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 5.33 psi 2024-11-06T02:55:01Z- handleDepth()- Depth_Keller msg: 1730861701.75 epoch sec; seqNo:848; depth 50.68 2024-11-06T02:55:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861701.77 epoch sec; seqNo:848; 4061580.88 north; 594371.97 east; 36.70 lat; -121.94 lon; 50.68 depth; utmZone: 10 2024-11-06T02:55:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861700.95 2024-11-06T02:55:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:01Z- waiting for 1730861704.95, time is 1730861701.77 2024-11-06T02:55:02Z- AHRS_M2 msg: 1730861702.14 epoch sec; seqNo:790 2024-11-06T02:55:02Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 5.33 psi 2024-11-06T02:55:02Z- handleDepth()- Depth_Keller msg: 1730861702.15 epoch sec; seqNo:849; depth 50.69 2024-11-06T02:55:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861702.16 epoch sec; seqNo:849; 4061581.26 north; 594371.97 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T02:55:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861700.95 2024-11-06T02:55:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:02Z- waiting for 1730861704.95, time is 1730861702.16 2024-11-06T02:55:02Z- AHRS_M2 msg: 1730861702.54 epoch sec; seqNo:791 2024-11-06T02:55:02Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 5.33 psi 2024-11-06T02:55:02Z- handleDepth()- Depth_Keller msg: 1730861702.55 epoch sec; seqNo:850; depth 50.69 2024-11-06T02:55:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861702.58 epoch sec; seqNo:850; 4061581.26 north; 594371.97 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T02:55:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861700.95 2024-11-06T02:55:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:02Z- waiting for 1730861704.95, time is 1730861702.58 2024-11-06T02:55:02Z- AHRS_M2 msg: 1730861702.95 epoch sec; seqNo:792 2024-11-06T02:55:02Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 5.33 psi 2024-11-06T02:55:02Z- handleDepth()- Depth_Keller msg: 1730861702.96 epoch sec; seqNo:851; depth 50.70 2024-11-06T02:55:02Z- handleDVL() - DVL msg epochMillisec: 1730861702.96 2024-11-06T02:55:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:55:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:55:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:55:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:55:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:55:02Z- handleDvl() - RDI_Pathfinder msg: 1730861702.96 sec; seqNo:273; #beams:0 2024-11-06T02:55:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:55:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , -0.02 2024-11-06T02:55:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861702.97 epoch sec; seqNo:851; 4061581.64 north; 594371.96 east; 36.70 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T02:55:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861700.95 2024-11-06T02:55:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:03Z- waiting for 1730861704.95, time is 1730861702.97 2024-11-06T02:55:03Z- AHRS_M2 msg: 1730861703.35 epoch sec; seqNo:793 2024-11-06T02:55:03Z- AHRS_M2 msg: 0.13 phi; -0.05 theta; 5.33 psi 2024-11-06T02:55:03Z- handleDepth()- Depth_Keller msg: 1730861703.36 epoch sec; seqNo:852; depth 50.71 2024-11-06T02:55:03Z- handleDVL() - DVL msg epochMillisec: 1730861703.39 2024-11-06T02:55:03Z- handleDvl() - RDI_Pathfinder msg: 1730861703.39 sec; seqNo:274; #beams:4 2024-11-06T02:55:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.49 , 36.83 , 40.19 , 40.94 2024-11-06T02:55:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.03 2024-11-06T02:55:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861703.41 epoch sec; seqNo:852; 4061581.62 north; 594370.74 east; 36.70 lat; -121.94 lon; 50.71 depth; utmZone: 10 2024-11-06T02:55:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861700.95 2024-11-06T02:55:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:03Z- waiting for 1730861704.95, time is 1730861703.41 2024-11-06T02:55:03Z- AHRS_M2 msg: 1730861703.76 epoch sec; seqNo:794 2024-11-06T02:55:03Z- AHRS_M2 msg: 0.13 phi; -0.05 theta; 5.32 psi 2024-11-06T02:55:03Z- handleDepth()- Depth_Keller msg: 1730861703.79 epoch sec; seqNo:853; depth 50.72 2024-11-06T02:55:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861703.81 epoch sec; seqNo:853; 4061582.00 north; 594370.74 east; 36.70 lat; -121.94 lon; 50.72 depth; utmZone: 10 2024-11-06T02:55:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861700.95 2024-11-06T02:55:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:03Z- waiting for 1730861704.95, time is 1730861703.81 2024-11-06T02:55:04Z- AHRS_M2 msg: 1730861704.17 epoch sec; seqNo:795 2024-11-06T02:55:04Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 5.32 psi 2024-11-06T02:55:04Z- handleDepth()- Depth_Keller msg: 1730861704.17 epoch sec; seqNo:854; depth 50.70 2024-11-06T02:55:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861704.18 epoch sec; seqNo:854; 4061582.00 north; 594370.74 east; 36.70 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T02:55:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861700.95 2024-11-06T02:55:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:04Z- waiting for 1730861704.95, time is 1730861704.18 2024-11-06T02:55:04Z- AHRS_M2 msg: 1730861704.57 epoch sec; seqNo:796 2024-11-06T02:55:04Z- AHRS_M2 msg: 0.11 phi; -0.04 theta; 5.32 psi 2024-11-06T02:55:04Z- handleDepth()- Depth_Keller msg: 1730861704.57 epoch sec; seqNo:855; depth 50.72 2024-11-06T02:55:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861704.59 epoch sec; seqNo:855; 4061582.38 north; 594370.74 east; 36.70 lat; -121.94 lon; 50.72 depth; utmZone: 10 2024-11-06T02:55:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861700.95 2024-11-06T02:55:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:04Z- waiting for 1730861704.95, time is 1730861704.59 2024-11-06T02:55:04Z- AHRS_M2 msg: 1730861704.97 epoch sec; seqNo:797 2024-11-06T02:55:04Z- AHRS_M2 msg: 0.11 phi; -0.04 theta; 5.32 psi 2024-11-06T02:55:04Z- handleDepth()- Depth_Keller msg: 1730861704.97 epoch sec; seqNo:856; depth 50.73 2024-11-06T02:55:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861704.98 epoch sec; seqNo:856; 4061582.37 north; 594369.52 east; 36.70 lat; -121.94 lon; 50.73 depth; utmZone: 10 2024-11-06T02:55:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861700.95 2024-11-06T02:55:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:05Z- waiting for 1730861704.95, time is 1730861704.98 2024-11-06T02:55:05Z- LcmTrn::updateTrn() >>>> pose time 1730861704.98, meas time 1730861703.39 <<<< 2024-11-06T02:55:05Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861704.98 2024-11-06T02:55:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861703.39 sec, ping # 0; waiting for INS data more recent than 1730861700.95... 2024-11-06T02:55:05Z- TerrainNav::motionUpdate() - currEstimate = (4061582, 594369.52, 50.73) 2024-11-06T02:55:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:55:05Z- TerrainNav:: SubcloudNIS is 0.63, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:55:05Z- TNavPF::Projecting Measurements 2024-11-06T02:55:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:55:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:55:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:55:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:55:05Z- TNavFilter::4 good beams. 2024-11-06T02:55:05Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:55:05Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:55:05Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:55:05Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:55:05Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:55:05Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:55:05Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:55:05Z- TerrainMapOctree: (vehN, vehE ) = (4061581.63, 594370.97). 2024-11-06T02:55:05Z- TerrainMapOctree: Min Distance = 207.32. 2024-11-06T02:55:05Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:55:05Z- Forcing Subcloud Comparison 2024-11-06T02:55:05Z- Weighting particles with subcloud comparison 2024-11-06T02:55:05Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:05Z- meanExpectedMeasDiff: -2.549441 delta_rms_squared: 2.584843 alpha: 0.992322 2024-11-06T02:55:05Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:05Z- meanExpectedMeasDiff: 3.396544 delta_rms_squared: 0.989596 alpha: 0.980190 2024-11-06T02:55:05Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:05Z- meanExpectedMeasDiff: -0.258306 delta_rms_squared: 1.148642 alpha: 0.982886 2024-11-06T02:55:05Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:05Z- meanExpectedMeasDiff: 0.346909 delta_rms_squared: 1.325350 alpha: 0.985134 2024-11-06T02:55:05Z- Subcloud NIS: 0.624035 2024-11-06T02:55:05Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:55:05Z- TNavPF:: sumWeights = 0.076636 2024-11-06T02:55:05Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:55:05Z- TNavPF::First Term of Map Covariance Matrix is 0.54 2024-11-06T02:55:05Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T02:55:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861703.39 sec. 2024-11-06T02:55:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:55:05Z- variances([0], [2], and psi_berg) = 0.228784 0.235222 0.000000 2024-11-06T02:55:05Z- publishEstimatesDecoder() - at 1730861704.98 sec 2024-11-06T02:55:05Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:55:05Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061615.68 594354.29 53.74 2024-11-06T02:55:05Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061615.09 594353.79 53.91 lat/lon: 36.70 -121.94 2024-11-06T02:55:05Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.24 0.65 0.00 2024-11-06T02:55:05Z- AHRS_M2 msg: 1730861705.38 epoch sec; seqNo:798 2024-11-06T02:55:05Z- AHRS_M2 msg: 0.11 phi; -0.04 theta; 5.32 psi 2024-11-06T02:55:05Z- handleDepth()- Depth_Keller msg: 1730861705.39 epoch sec; seqNo:857; depth 50.72 2024-11-06T02:55:05Z- handleDVL() - DVL msg epochMillisec: 1730861705.43 2024-11-06T02:55:05Z- handleDvl() - RDI_Pathfinder msg: 1730861705.43 sec; seqNo:275; #beams:4 2024-11-06T02:55:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.56 , 37.01 , 39.82 , 40.19 2024-11-06T02:55:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.06 , -0.02 2024-11-06T02:55:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861705.45 epoch sec; seqNo:857; 4061582.75 north; 594369.51 east; 36.70 lat; -121.94 lon; 50.72 depth; utmZone: 10 2024-11-06T02:55:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861704.98 2024-11-06T02:55:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:05Z- waiting for 1730861708.98, time is 1730861705.45 2024-11-06T02:55:05Z- AHRS_M2 msg: 1730861705.78 epoch sec; seqNo:799 2024-11-06T02:55:05Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 5.32 psi 2024-11-06T02:55:05Z- handleDepth()- Depth_Keller msg: 1730861705.79 epoch sec; seqNo:858; depth 50.73 2024-11-06T02:55:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861705.80 epoch sec; seqNo:858; 4061582.75 north; 594369.51 east; 36.70 lat; -121.94 lon; 50.73 depth; utmZone: 10 2024-11-06T02:55:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861704.98 2024-11-06T02:55:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:05Z- waiting for 1730861708.98, time is 1730861705.80 2024-11-06T02:55:06Z- AHRS_M2 msg: 1730861706.18 epoch sec; seqNo:800 2024-11-06T02:55:06Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 5.32 psi 2024-11-06T02:55:06Z- handleDepth()- Depth_Keller msg: 1730861706.19 epoch sec; seqNo:859; depth 50.74 2024-11-06T02:55:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861706.20 epoch sec; seqNo:859; 4061583.13 north; 594369.51 east; 36.70 lat; -121.94 lon; 50.74 depth; utmZone: 10 2024-11-06T02:55:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861704.98 2024-11-06T02:55:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:06Z- waiting for 1730861708.98, time is 1730861706.20 2024-11-06T02:55:06Z- AHRS_M2 msg: 1730861706.58 epoch sec; seqNo:801 2024-11-06T02:55:06Z- AHRS_M2 msg: 0.11 phi; -0.04 theta; 5.31 psi 2024-11-06T02:55:06Z- handleDepth()- Depth_Keller msg: 1730861706.59 epoch sec; seqNo:860; depth 50.75 2024-11-06T02:55:06Z- handleDVL() - DVL msg epochMillisec: 1730861706.60 2024-11-06T02:55:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:55:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:55:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:55:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:55:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:55:06Z- handleDvl() - RDI_Pathfinder msg: 1730861706.60 sec; seqNo:276; #beams:0 2024-11-06T02:55:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:55:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.06 , -0.02 2024-11-06T02:55:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861706.61 epoch sec; seqNo:860; 4061583.11 north; 594368.29 east; 36.70 lat; -121.94 lon; 50.75 depth; utmZone: 10 2024-11-06T02:55:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861704.98 2024-11-06T02:55:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:06Z- waiting for 1730861708.98, time is 1730861706.61 2024-11-06T02:55:07Z- AHRS_M2 msg: 1730861707.00 epoch sec; seqNo:802 2024-11-06T02:55:07Z- AHRS_M2 msg: 0.11 phi; -0.04 theta; 5.32 psi 2024-11-06T02:55:07Z- handleDepth()- Depth_Keller msg: 1730861707.03 epoch sec; seqNo:861; depth 50.76 2024-11-06T02:55:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861707.05 epoch sec; seqNo:861; 4061583.49 north; 594368.28 east; 36.70 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T02:55:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861704.98 2024-11-06T02:55:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:07Z- waiting for 1730861708.98, time is 1730861707.05 2024-11-06T02:55:07Z- AHRS_M2 msg: 1730861707.39 epoch sec; seqNo:803 2024-11-06T02:55:07Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 5.32 psi 2024-11-06T02:55:07Z- handleDepth()- Depth_Keller msg: 1730861707.40 epoch sec; seqNo:862; depth 50.76 2024-11-06T02:55:07Z- handleDVL() - DVL msg epochMillisec: 1730861707.42 2024-11-06T02:55:07Z- handleDvl() - RDI_Pathfinder msg: 1730861707.42 sec; seqNo:277; #beams:4 2024-11-06T02:55:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.56 , 37.01 , 40.00 , 40.38 2024-11-06T02:55:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.08 , 0.01 2024-11-06T02:55:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861707.45 epoch sec; seqNo:862; 4061583.87 north; 594368.28 east; 36.70 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T02:55:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861704.98 2024-11-06T02:55:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:07Z- waiting for 1730861708.98, time is 1730861707.45 2024-11-06T02:55:07Z- AHRS_M2 msg: 1730861707.81 epoch sec; seqNo:804 2024-11-06T02:55:07Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 5.32 psi 2024-11-06T02:55:07Z- handleDepth()- Depth_Keller msg: 1730861707.82 epoch sec; seqNo:863; depth 50.77 2024-11-06T02:55:07Z- handleDVL() - DVL msg epochMillisec: 1730861707.82 2024-11-06T02:55:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:55:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:55:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:55:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:55:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:55:07Z- handleDvl() - RDI_Pathfinder msg: 1730861707.82 sec; seqNo:278; #beams:0 2024-11-06T02:55:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:55:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.08 , 0.01 2024-11-06T02:55:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861707.84 epoch sec; seqNo:863; 4061583.86 north; 594367.06 east; 36.70 lat; -121.94 lon; 50.77 depth; utmZone: 10 2024-11-06T02:55:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861704.98 2024-11-06T02:55:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:07Z- waiting for 1730861708.98, time is 1730861707.84 2024-11-06T02:55:08Z- AHRS_M2 msg: 1730861708.20 epoch sec; seqNo:805 2024-11-06T02:55:08Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 5.32 psi 2024-11-06T02:55:08Z- handleDepth()- Depth_Keller msg: 1730861708.20 epoch sec; seqNo:864; depth 50.79 2024-11-06T02:55:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861708.22 epoch sec; seqNo:864; 4061584.24 north; 594367.05 east; 36.70 lat; -121.94 lon; 50.79 depth; utmZone: 10 2024-11-06T02:55:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861704.98 2024-11-06T02:55:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:08Z- waiting for 1730861708.98, time is 1730861708.22 2024-11-06T02:55:08Z- AHRS_M2 msg: 1730861708.61 epoch sec; seqNo:806 2024-11-06T02:55:08Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 5.32 psi 2024-11-06T02:55:08Z- handleDepth()- Depth_Keller msg: 1730861708.61 epoch sec; seqNo:865; depth 50.80 2024-11-06T02:55:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861708.62 epoch sec; seqNo:865; 4061584.24 north; 594367.05 east; 36.70 lat; -121.94 lon; 50.80 depth; utmZone: 10 2024-11-06T02:55:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861704.98 2024-11-06T02:55:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:08Z- waiting for 1730861708.98, time is 1730861708.62 2024-11-06T02:55:09Z- AHRS_M2 msg: 1730861709.01 epoch sec; seqNo:807 2024-11-06T02:55:09Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.32 psi 2024-11-06T02:55:09Z- handleDepth()- Depth_Keller msg: 1730861709.02 epoch sec; seqNo:866; depth 50.82 2024-11-06T02:55:09Z- handleDVL() - DVL msg epochMillisec: 1730861709.02 2024-11-06T02:55:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:55:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:55:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:55:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:55:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:55:09Z- handleDvl() - RDI_Pathfinder msg: 1730861709.02 sec; seqNo:279; #beams:0 2024-11-06T02:55:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:55:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.08 , 0.01 2024-11-06T02:55:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861709.03 epoch sec; seqNo:866; 4061584.61 north; 594367.05 east; 36.70 lat; -121.94 lon; 50.82 depth; utmZone: 10 2024-11-06T02:55:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861704.98 2024-11-06T02:55:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:09Z- waiting for 1730861708.98, time is 1730861709.03 2024-11-06T02:55:09Z- LcmTrn::updateTrn() >>>> pose time 1730861709.03, meas time 1730861709.02 <<<< 2024-11-06T02:55:09Z- LcmTrn::updateTrn() equating pose 1730861709.02 and meas times 1730861709.02 2024-11-06T02:55:09Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861709.02 2024-11-06T02:55:09Z- TerrainNav::motionUpdate() - currEstimate = (4061585, 594367.05, 50.82) 2024-11-06T02:55:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:55:09Z- TerrainNav:: SubcloudNIS is 0.62, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:55:09Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:55:09Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:55:09Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:55:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:55:09Z- variances([0], [2], and psi_berg) = 0.230397 0.237354 0.000000 2024-11-06T02:55:09Z- publishEstimatesDecoder() - at 1730861709.02 sec 2024-11-06T02:55:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:55:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061617.93 594351.82 53.05 2024-11-06T02:55:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061617.33 594351.33 54.00 lat/lon: 36.70 -121.94 2024-11-06T02:55:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.24 0.90 0.00 2024-11-06T02:55:09Z- AHRS_M2 msg: 1730861709.41 epoch sec; seqNo:808 2024-11-06T02:55:09Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.32 psi 2024-11-06T02:55:09Z- handleDepth()- Depth_Keller msg: 1730861709.42 epoch sec; seqNo:867; depth 50.84 2024-11-06T02:55:09Z- handleDVL() - DVL msg epochMillisec: 1730861709.44 2024-11-06T02:55:09Z- handleDvl() - RDI_Pathfinder msg: 1730861709.44 sec; seqNo:280; #beams:4 2024-11-06T02:55:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.30 , 36.83 , 40.19 , 40.38 2024-11-06T02:55:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.06 , 0.03 2024-11-06T02:55:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861709.45 epoch sec; seqNo:867; 4061584.60 north; 594365.83 east; 36.70 lat; -121.94 lon; 50.84 depth; utmZone: 10 2024-11-06T02:55:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861709.02 2024-11-06T02:55:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:09Z- waiting for 1730861713.02, time is 1730861709.45 2024-11-06T02:55:09Z- AHRS_M2 msg: 1730861709.82 epoch sec; seqNo:809 2024-11-06T02:55:09Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.32 psi 2024-11-06T02:55:09Z- handleDepth()- Depth_Keller msg: 1730861709.84 epoch sec; seqNo:868; depth 50.85 2024-11-06T02:55:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861709.85 epoch sec; seqNo:868; 4061584.98 north; 594365.82 east; 36.70 lat; -121.94 lon; 50.85 depth; utmZone: 10 2024-11-06T02:55:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861709.02 2024-11-06T02:55:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:09Z- waiting for 1730861713.02, time is 1730861709.85 2024-11-06T02:55:10Z- AHRS_M2 msg: 1730861710.22 epoch sec; seqNo:810 2024-11-06T02:55:10Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.32 psi 2024-11-06T02:55:10Z- handleDepth()- Depth_Keller msg: 1730861710.23 epoch sec; seqNo:869; depth 50.86 2024-11-06T02:55:10Z- handleDVL() - DVL msg epochMillisec: 1730861710.23 2024-11-06T02:55:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:55:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:55:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:55:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:55:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:55:10Z- handleDvl() - RDI_Pathfinder msg: 1730861710.23 sec; seqNo:281; #beams:0 2024-11-06T02:55:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:55:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.06 , 0.03 2024-11-06T02:55:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861710.24 epoch sec; seqNo:869; 4061584.98 north; 594365.82 east; 36.70 lat; -121.94 lon; 50.86 depth; utmZone: 10 2024-11-06T02:55:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861709.02 2024-11-06T02:55:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:10Z- waiting for 1730861713.02, time is 1730861710.24 2024-11-06T02:55:10Z- AHRS_M2 msg: 1730861710.63 epoch sec; seqNo:811 2024-11-06T02:55:10Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.33 psi 2024-11-06T02:55:10Z- handleDepth()- Depth_Keller msg: 1730861710.63 epoch sec; seqNo:870; depth 50.87 2024-11-06T02:55:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861710.65 epoch sec; seqNo:870; 4061585.36 north; 594365.82 east; 36.70 lat; -121.94 lon; 50.87 depth; utmZone: 10 2024-11-06T02:55:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861709.02 2024-11-06T02:55:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:10Z- waiting for 1730861713.02, time is 1730861710.65 2024-11-06T02:55:11Z- AHRS_M2 msg: 1730861711.03 epoch sec; seqNo:812 2024-11-06T02:55:11Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 5.33 psi 2024-11-06T02:55:11Z- handleDepth()- Depth_Keller msg: 1730861711.04 epoch sec; seqNo:871; depth 50.87 2024-11-06T02:55:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861711.05 epoch sec; seqNo:871; 4061585.34 north; 594364.60 east; 36.70 lat; -121.94 lon; 50.87 depth; utmZone: 10 2024-11-06T02:55:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861709.02 2024-11-06T02:55:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:11Z- waiting for 1730861713.02, time is 1730861711.05 2024-11-06T02:55:11Z- handleDepth()- Depth_Keller msg: 1730861711.46 epoch sec; seqNo:872; depth 50.90 2024-11-06T02:55:11Z- handleDVL() - DVL msg epochMillisec: 1730861711.48 2024-11-06T02:55:11Z- handleDvl() - RDI_Pathfinder msg: 1730861711.48 sec; seqNo:282; #beams:4 2024-11-06T02:55:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.30 , 37.20 , 40.56 , 40.38 2024-11-06T02:55:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , 0.05 2024-11-06T02:55:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861711.49 epoch sec; seqNo:872; 4061585.72 north; 594364.60 east; 36.70 lat; -121.94 lon; 50.90 depth; utmZone: 10 2024-11-06T02:55:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861709.02 2024-11-06T02:55:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:11Z- waiting for 1730861713.02, time is 1730861711.49 2024-11-06T02:55:11Z- AHRS_M2 msg: 1730861711.84 epoch sec; seqNo:813 2024-11-06T02:55:11Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 5.33 psi 2024-11-06T02:55:11Z- handleDepth()- Depth_Keller msg: 1730861711.84 epoch sec; seqNo:873; depth 50.90 2024-11-06T02:55:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861711.86 epoch sec; seqNo:873; 4061585.72 north; 594364.60 east; 36.70 lat; -121.94 lon; 50.90 depth; utmZone: 10 2024-11-06T02:55:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861709.02 2024-11-06T02:55:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:12Z- waiting for 1730861713.02, time is 1730861711.86 2024-11-06T02:55:12Z- AHRS_M2 msg: 1730861712.24 epoch sec; seqNo:814 2024-11-06T02:55:12Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 5.33 psi 2024-11-06T02:55:12Z- handleDepth()- Depth_Keller msg: 1730861712.24 epoch sec; seqNo:874; depth 50.92 2024-11-06T02:55:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861712.25 epoch sec; seqNo:874; 4061586.10 north; 594364.59 east; 36.70 lat; -121.94 lon; 50.92 depth; utmZone: 10 2024-11-06T02:55:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861709.02 2024-11-06T02:55:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:12Z- waiting for 1730861713.02, time is 1730861712.25 2024-11-06T02:55:12Z- AHRS_M2 msg: 1730861712.66 epoch sec; seqNo:815 2024-11-06T02:55:12Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 5.33 psi 2024-11-06T02:55:12Z- handleDepth()- Depth_Keller msg: 1730861712.66 epoch sec; seqNo:875; depth 50.91 2024-11-06T02:55:12Z- handleDVL() - DVL msg epochMillisec: 1730861712.67 2024-11-06T02:55:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:55:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:55:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:55:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:55:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:55:12Z- handleDvl() - RDI_Pathfinder msg: 1730861712.67 sec; seqNo:283; #beams:0 2024-11-06T02:55:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:55:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , 0.05 2024-11-06T02:55:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861712.67 epoch sec; seqNo:875; 4061586.09 north; 594363.37 east; 36.70 lat; -121.94 lon; 50.91 depth; utmZone: 10 2024-11-06T02:55:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861709.02 2024-11-06T02:55:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:12Z- waiting for 1730861713.02, time is 1730861712.67 2024-11-06T02:55:13Z- AHRS_M2 msg: 1730861713.05 epoch sec; seqNo:816 2024-11-06T02:55:13Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 5.33 psi 2024-11-06T02:55:13Z- handleDepth()- Depth_Keller msg: 1730861713.06 epoch sec; seqNo:876; depth 50.93 2024-11-06T02:55:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861713.08 epoch sec; seqNo:876; 4061586.47 north; 594363.37 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T02:55:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861709.02 2024-11-06T02:55:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:13Z- waiting for 1730861713.02, time is 1730861713.08 2024-11-06T02:55:13Z- LcmTrn::updateTrn() >>>> pose time 1730861713.08, meas time 1730861712.67 <<<< 2024-11-06T02:55:13Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861713.08 2024-11-06T02:55:13Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:55:13Z- TerrainNav::motionUpdate() - currEstimate = (4061586, 594363.37, 50.93) 2024-11-06T02:55:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:55:13Z- TerrainNav:: SubcloudNIS is 0.62, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:55:13Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:55:13Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:55:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:55:13Z- variances([0], [2], and psi_berg) = 0.232868 0.240035 0.000000 2024-11-06T02:55:13Z- publishEstimatesDecoder() - at 1730861713.08 sec 2024-11-06T02:55:13Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:55:13Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061619.70 594348.07 53.25 2024-11-06T02:55:13Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061619.18 594347.65 54.10 lat/lon: 36.70 -121.94 2024-11-06T02:55:13Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.24 1.16 0.00 2024-11-06T02:55:13Z- AHRS_M2 msg: 1730861713.45 epoch sec; seqNo:817 2024-11-06T02:55:13Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.33 psi 2024-11-06T02:55:13Z- handleDepth()- Depth_Keller msg: 1730861713.46 epoch sec; seqNo:877; depth 50.93 2024-11-06T02:55:13Z- handleDVL() - DVL msg epochMillisec: 1730861713.49 2024-11-06T02:55:13Z- handleDvl() - RDI_Pathfinder msg: 1730861713.49 sec; seqNo:284; #beams:4 2024-11-06T02:55:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.68 , 37.20 , 40.75 , 40.38 2024-11-06T02:55:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , 0.03 2024-11-06T02:55:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861713.50 epoch sec; seqNo:877; 4061586.47 north; 594363.37 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T02:55:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861713.08 2024-11-06T02:55:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:13Z- waiting for 1730861717.08, time is 1730861713.50 2024-11-06T02:55:13Z- handleDepth()- Depth_Keller msg: 1730861713.89 epoch sec; seqNo:878; depth 50.94 2024-11-06T02:55:13Z- handleDVL() - DVL msg epochMillisec: 1730861713.90 2024-11-06T02:55:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:55:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:55:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:55:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:55:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:55:13Z- handleDvl() - RDI_Pathfinder msg: 1730861713.90 sec; seqNo:285; #beams:0 2024-11-06T02:55:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:55:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , 0.03 2024-11-06T02:55:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861713.91 epoch sec; seqNo:878; 4061586.85 north; 594363.36 east; 36.70 lat; -121.94 lon; 50.94 depth; utmZone: 10 2024-11-06T02:55:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861713.08 2024-11-06T02:55:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:14Z- waiting for 1730861717.08, time is 1730861713.91 2024-11-06T02:55:14Z- AHRS_M2 msg: 1730861714.26 epoch sec; seqNo:818 2024-11-06T02:55:14Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.33 psi 2024-11-06T02:55:14Z- handleDepth()- Depth_Keller msg: 1730861714.27 epoch sec; seqNo:879; depth 50.93 2024-11-06T02:55:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861714.28 epoch sec; seqNo:879; 4061586.83 north; 594362.14 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T02:55:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861713.08 2024-11-06T02:55:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:14Z- waiting for 1730861717.08, time is 1730861714.28 2024-11-06T02:55:14Z- AHRS_M2 msg: 1730861714.67 epoch sec; seqNo:819 2024-11-06T02:55:14Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.33 psi 2024-11-06T02:55:14Z- handleDepth()- Depth_Keller msg: 1730861714.67 epoch sec; seqNo:880; depth 50.93 2024-11-06T02:55:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861714.68 epoch sec; seqNo:880; 4061587.21 north; 594362.14 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T02:55:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861713.08 2024-11-06T02:55:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:14Z- waiting for 1730861717.08, time is 1730861714.68 2024-11-06T02:55:15Z- AHRS_M2 msg: 1730861715.07 epoch sec; seqNo:820 2024-11-06T02:55:15Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.33 psi 2024-11-06T02:55:15Z- handleDepth()- Depth_Keller msg: 1730861715.08 epoch sec; seqNo:881; depth 50.94 2024-11-06T02:55:15Z- handleDVL() - DVL msg epochMillisec: 1730861715.08 2024-11-06T02:55:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:55:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:55:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:55:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:55:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:55:15Z- handleDvl() - RDI_Pathfinder msg: 1730861715.08 sec; seqNo:286; #beams:0 2024-11-06T02:55:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:55:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , 0.03 2024-11-06T02:55:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861715.09 epoch sec; seqNo:881; 4061587.59 north; 594362.13 east; 36.70 lat; -121.94 lon; 50.94 depth; utmZone: 10 2024-11-06T02:55:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861713.08 2024-11-06T02:55:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:15Z- waiting for 1730861717.08, time is 1730861715.09 2024-11-06T02:55:15Z- AHRS_M2 msg: 1730861715.47 epoch sec; seqNo:821 2024-11-06T02:55:15Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.33 psi 2024-11-06T02:55:15Z- handleDepth()- Depth_Keller msg: 1730861715.49 epoch sec; seqNo:882; depth 50.94 2024-11-06T02:55:15Z- handleDVL() - DVL msg epochMillisec: 1730861715.51 2024-11-06T02:55:15Z- handleDvl() - RDI_Pathfinder msg: 1730861715.51 sec; seqNo:287; #beams:4 2024-11-06T02:55:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.30 , 37.20 , 40.56 , 40.56 2024-11-06T02:55:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , -0.01 2024-11-06T02:55:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861715.52 epoch sec; seqNo:882; 4061587.59 north; 594362.13 east; 36.70 lat; -121.94 lon; 50.94 depth; utmZone: 10 2024-11-06T02:55:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861713.08 2024-11-06T02:55:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:15Z- waiting for 1730861717.08, time is 1730861715.52 2024-11-06T02:55:15Z- AHRS_M2 msg: 1730861715.88 epoch sec; seqNo:822 2024-11-06T02:55:15Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.33 psi 2024-11-06T02:55:15Z- handleDepth()- Depth_Keller msg: 1730861715.89 epoch sec; seqNo:883; depth 50.92 2024-11-06T02:55:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861715.91 epoch sec; seqNo:883; 4061587.96 north; 594360.91 east; 36.70 lat; -121.94 lon; 50.92 depth; utmZone: 10 2024-11-06T02:55:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861713.08 2024-11-06T02:55:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:16Z- waiting for 1730861717.08, time is 1730861715.91 2024-11-06T02:55:16Z- AHRS_M2 msg: 1730861716.28 epoch sec; seqNo:823 2024-11-06T02:55:16Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.33 psi 2024-11-06T02:55:16Z- handleDepth()- Depth_Keller msg: 1730861716.29 epoch sec; seqNo:884; depth 50.92 2024-11-06T02:55:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861716.30 epoch sec; seqNo:884; 4061587.96 north; 594360.91 east; 36.70 lat; -121.94 lon; 50.92 depth; utmZone: 10 2024-11-06T02:55:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861713.08 2024-11-06T02:55:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:16Z- waiting for 1730861717.08, time is 1730861716.30 2024-11-06T02:55:16Z- AHRS_M2 msg: 1730861716.68 epoch sec; seqNo:824 2024-11-06T02:55:16Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.33 psi 2024-11-06T02:55:16Z- handleDepth()- Depth_Keller msg: 1730861716.69 epoch sec; seqNo:885; depth 50.92 2024-11-06T02:55:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861716.70 epoch sec; seqNo:885; 4061588.34 north; 594360.91 east; 36.70 lat; -121.94 lon; 50.92 depth; utmZone: 10 2024-11-06T02:55:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861713.08 2024-11-06T02:55:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:16Z- waiting for 1730861717.08, time is 1730861716.70 2024-11-06T02:55:17Z- AHRS_M2 msg: 1730861717.09 epoch sec; seqNo:825 2024-11-06T02:55:17Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 5.33 psi 2024-11-06T02:55:17Z- handleDepth()- Depth_Keller msg: 1730861717.10 epoch sec; seqNo:886; depth 50.93 2024-11-06T02:55:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861717.12 epoch sec; seqNo:886; 4061588.34 north; 594360.91 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T02:55:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861713.08 2024-11-06T02:55:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:17Z- waiting for 1730861717.08, time is 1730861717.12 2024-11-06T02:55:17Z- LcmTrn::updateTrn() >>>> pose time 1730861717.12, meas time 1730861715.51 <<<< 2024-11-06T02:55:17Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861717.12 2024-11-06T02:55:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861715.51 sec, ping # 0; waiting for INS data more recent than 1730861713.08... 2024-11-06T02:55:17Z- TerrainNav::motionUpdate() - currEstimate = (4061588, 594360.91, 50.93) 2024-11-06T02:55:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:55:17Z- TerrainNav:: SubcloudNIS is 0.62, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:55:17Z- TNavPF::Projecting Measurements 2024-11-06T02:55:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:55:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:55:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:55:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:55:17Z- TNavFilter::4 good beams. 2024-11-06T02:55:17Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:55:17Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:55:17Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:55:17Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:55:17Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:55:17Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:55:17Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:55:17Z- TerrainMapOctree: (vehN, vehE ) = (4061587.59, 594361.89). 2024-11-06T02:55:17Z- TerrainMapOctree: Min Distance = 197.22. 2024-11-06T02:55:17Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:55:17Z- Forcing Subcloud Comparison 2024-11-06T02:55:17Z- Weighting particles with subcloud comparison 2024-11-06T02:55:17Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:17Z- meanExpectedMeasDiff: -3.010546 delta_rms_squared: 2.267291 alpha: 0.991256 2024-11-06T02:55:17Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:17Z- meanExpectedMeasDiff: 6.240046 delta_rms_squared: 1.661434 alpha: 0.988105 2024-11-06T02:55:17Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:17Z- meanExpectedMeasDiff: -1.517442 delta_rms_squared: 1.881790 alpha: 0.989484 2024-11-06T02:55:17Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:17Z- meanExpectedMeasDiff: 0.951635 delta_rms_squared: 2.006509 alpha: 0.990131 2024-11-06T02:55:17Z- Subcloud NIS: 0.651830 2024-11-06T02:55:17Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:55:17Z- TNavPF:: sumWeights = 0.001489 2024-11-06T02:55:17Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:55:17Z- TNavPF::First Term of Map Covariance Matrix is 0.02 2024-11-06T02:55:17Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T02:55:17Z- TerrainNav::Resampling particle filter... 2024-11-06T02:55:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861715.51 sec. 2024-11-06T02:55:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:55:17Z- variances([0], [2], and psi_berg) = 0.229292 0.285317 0.000000 2024-11-06T02:55:17Z- publishEstimatesDecoder() - at 1730861717.12 sec 2024-11-06T02:55:17Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:55:17Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061620.56 594358.56 55.05 2024-11-06T02:55:17Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061621.06 594345.20 53.70 lat/lon: 36.70 -121.94 2024-11-06T02:55:17Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.29 0.76 0.00 2024-11-06T02:55:17Z- AHRS_M2 msg: 1730861717.49 epoch sec; seqNo:826 2024-11-06T02:55:17Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 5.32 psi 2024-11-06T02:55:17Z- handleDepth()- Depth_Keller msg: 1730861717.51 epoch sec; seqNo:887; depth 50.93 2024-11-06T02:55:17Z- handleDVL() - DVL msg epochMillisec: 1730861717.55 2024-11-06T02:55:17Z- handleDvl() - RDI_Pathfinder msg: 1730861717.55 sec; seqNo:288; #beams:4 2024-11-06T02:55:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.30 , 37.39 , 40.75 , 40.38 2024-11-06T02:55:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.04 2024-11-06T02:55:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861717.57 epoch sec; seqNo:887; 4061588.70 north; 594359.68 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T02:55:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861717.12 2024-11-06T02:55:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:17Z- waiting for 1730861721.12, time is 1730861717.57 2024-11-06T02:55:17Z- AHRS_M2 msg: 1730861717.89 epoch sec; seqNo:827 2024-11-06T02:55:17Z- AHRS_M2 msg: 0.11 phi; -0.02 theta; 5.32 psi 2024-11-06T02:55:17Z- handleDepth()- Depth_Keller msg: 1730861717.91 epoch sec; seqNo:888; depth 50.93 2024-11-06T02:55:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861717.93 epoch sec; seqNo:888; 4061588.70 north; 594359.68 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T02:55:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861717.12 2024-11-06T02:55:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:18Z- waiting for 1730861721.12, time is 1730861717.93 2024-11-06T02:55:18Z- AHRS_M2 msg: 1730861718.30 epoch sec; seqNo:828 2024-11-06T02:55:18Z- AHRS_M2 msg: 0.11 phi; -0.02 theta; 5.32 psi 2024-11-06T02:55:18Z- handleDepth()- Depth_Keller msg: 1730861718.31 epoch sec; seqNo:889; depth 50.91 2024-11-06T02:55:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861718.32 epoch sec; seqNo:889; 4061589.08 north; 594359.68 east; 36.70 lat; -121.94 lon; 50.91 depth; utmZone: 10 2024-11-06T02:55:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861717.12 2024-11-06T02:55:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:18Z- waiting for 1730861721.12, time is 1730861718.32 2024-11-06T02:55:18Z- AHRS_M2 msg: 1730861718.70 epoch sec; seqNo:829 2024-11-06T02:55:18Z- AHRS_M2 msg: 0.11 phi; -0.01 theta; 5.32 psi 2024-11-06T02:55:18Z- handleDepth()- Depth_Keller msg: 1730861718.71 epoch sec; seqNo:890; depth 50.92 2024-11-06T02:55:18Z- handleDVL() - DVL msg epochMillisec: 1730861718.71 2024-11-06T02:55:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:55:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:55:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:55:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:55:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:55:18Z- handleDvl() - RDI_Pathfinder msg: 1730861718.71 sec; seqNo:289; #beams:0 2024-11-06T02:55:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:55:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.04 2024-11-06T02:55:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861718.72 epoch sec; seqNo:890; 4061589.08 north; 594359.68 east; 36.70 lat; -121.94 lon; 50.92 depth; utmZone: 10 2024-11-06T02:55:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861717.12 2024-11-06T02:55:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:18Z- waiting for 1730861721.12, time is 1730861718.72 2024-11-06T02:55:19Z- AHRS_M2 msg: 1730861719.11 epoch sec; seqNo:830 2024-11-06T02:55:19Z- AHRS_M2 msg: 0.11 phi; -0.01 theta; 5.32 psi 2024-11-06T02:55:19Z- handleDepth()- Depth_Keller msg: 1730861719.12 epoch sec; seqNo:891; depth 50.92 2024-11-06T02:55:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861719.13 epoch sec; seqNo:891; 4061589.44 north; 594358.45 east; 36.70 lat; -121.94 lon; 50.92 depth; utmZone: 10 2024-11-06T02:55:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861717.12 2024-11-06T02:55:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:19Z- waiting for 1730861721.12, time is 1730861719.13 2024-11-06T02:55:19Z- AHRS_M2 msg: 1730861719.51 epoch sec; seqNo:831 2024-11-06T02:55:19Z- AHRS_M2 msg: 0.11 phi; -0.01 theta; 5.32 psi 2024-11-06T02:55:19Z- handleDepth()- Depth_Keller msg: 1730861719.52 epoch sec; seqNo:892; depth 50.92 2024-11-06T02:55:19Z- handleDVL() - DVL msg epochMillisec: 1730861719.55 2024-11-06T02:55:19Z- handleDvl() - RDI_Pathfinder msg: 1730861719.55 sec; seqNo:290; #beams:4 2024-11-06T02:55:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.56 , 37.39 , 40.75 , 39.63 2024-11-06T02:55:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.05 , -0.02 2024-11-06T02:55:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861719.57 epoch sec; seqNo:892; 4061589.44 north; 594358.45 east; 36.70 lat; -121.94 lon; 50.92 depth; utmZone: 10 2024-11-06T02:55:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861717.12 2024-11-06T02:55:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:19Z- waiting for 1730861721.12, time is 1730861719.57 2024-11-06T02:55:19Z- AHRS_M2 msg: 1730861719.92 epoch sec; seqNo:832 2024-11-06T02:55:19Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 5.32 psi 2024-11-06T02:55:19Z- handleDepth()- Depth_Keller msg: 1730861719.93 epoch sec; seqNo:893; depth 50.93 2024-11-06T02:55:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861719.96 epoch sec; seqNo:893; 4061589.82 north; 594358.45 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T02:55:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861717.12 2024-11-06T02:55:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:20Z- waiting for 1730861721.12, time is 1730861719.96 2024-11-06T02:55:20Z- AHRS_M2 msg: 1730861720.32 epoch sec; seqNo:833 2024-11-06T02:55:20Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 5.32 psi 2024-11-06T02:55:20Z- handleDepth()- Depth_Keller msg: 1730861720.33 epoch sec; seqNo:894; depth 50.93 2024-11-06T02:55:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861720.34 epoch sec; seqNo:894; 4061589.82 north; 594358.45 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T02:55:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861717.12 2024-11-06T02:55:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:20Z- waiting for 1730861721.12, time is 1730861720.34 2024-11-06T02:55:20Z- AHRS_M2 msg: 1730861720.72 epoch sec; seqNo:834 2024-11-06T02:55:20Z- AHRS_M2 msg: 0.11 phi; -0.00 theta; 5.32 psi 2024-11-06T02:55:20Z- handleDepth()- Depth_Keller msg: 1730861720.73 epoch sec; seqNo:895; depth 50.93 2024-11-06T02:55:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861720.75 epoch sec; seqNo:895; 4061590.19 north; 594357.22 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T02:55:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861717.12 2024-11-06T02:55:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:20Z- waiting for 1730861721.12, time is 1730861720.75 2024-11-06T02:55:21Z- AHRS_M2 msg: 1730861721.13 epoch sec; seqNo:835 2024-11-06T02:55:21Z- AHRS_M2 msg: 0.11 phi; 0.01 theta; 5.32 psi 2024-11-06T02:55:21Z- handleDepth()- Depth_Keller msg: 1730861721.14 epoch sec; seqNo:896; depth 50.93 2024-11-06T02:55:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861721.16 epoch sec; seqNo:896; 4061590.19 north; 594357.22 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T02:55:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861717.12 2024-11-06T02:55:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:21Z- waiting for 1730861721.12, time is 1730861721.16 2024-11-06T02:55:21Z- LcmTrn::updateTrn() >>>> pose time 1730861721.16, meas time 1730861719.55 <<<< 2024-11-06T02:55:21Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861721.16 2024-11-06T02:55:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861719.55 sec, ping # 0; waiting for INS data more recent than 1730861717.12... 2024-11-06T02:55:21Z- TerrainNav::motionUpdate() - currEstimate = (4061590, 594357.22, 50.93) 2024-11-06T02:55:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:55:21Z- TerrainNav:: SubcloudNIS is 0.65, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:55:21Z- TNavPF::Projecting Measurements 2024-11-06T02:55:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:55:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:55:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:55:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:55:21Z- TNavFilter::4 good beams. 2024-11-06T02:55:21Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:55:21Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:55:21Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:55:21Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:55:21Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:55:21Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:55:21Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:55:21Z- TerrainMapOctree: (vehN, vehE ) = (4061589.45, 594358.69). 2024-11-06T02:55:21Z- TerrainMapOctree: Min Distance = 193.88. 2024-11-06T02:55:21Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:55:21Z- Forcing Subcloud Comparison 2024-11-06T02:55:21Z- Weighting particles with subcloud comparison 2024-11-06T02:55:21Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:21Z- meanExpectedMeasDiff: -3.806704 delta_rms_squared: 1.559578 alpha: 0.987338 2024-11-06T02:55:21Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:21Z- meanExpectedMeasDiff: 5.840075 delta_rms_squared: 1.183198 alpha: 0.983378 2024-11-06T02:55:21Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:21Z- meanExpectedMeasDiff: -2.217729 delta_rms_squared: 1.356949 alpha: 0.985475 2024-11-06T02:55:21Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:21Z- meanExpectedMeasDiff: 0.088453 delta_rms_squared: 1.314049 alpha: 0.985008 2024-11-06T02:55:21Z- Subcloud NIS: 0.681579 2024-11-06T02:55:21Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:55:21Z- TNavPF:: sumWeights = 0.001258 2024-11-06T02:55:21Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:55:21Z- TNavPF::First Term of Map Covariance Matrix is 0.02 2024-11-06T02:55:21Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T02:55:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861719.55 sec. 2024-11-06T02:55:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:55:21Z- variances([0], [2], and psi_berg) = 0.225042 0.271386 0.000000 2024-11-06T02:55:21Z- publishEstimatesDecoder() - at 1730861721.16 sec 2024-11-06T02:55:21Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:55:21Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061623.39 594340.19 53.41 2024-11-06T02:55:21Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061622.91 594341.51 53.70 lat/lon: 36.70 -121.94 2024-11-06T02:55:21Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.27 0.71 0.00 2024-11-06T02:55:21Z- AHRS_M2 msg: 1730861721.54 epoch sec; seqNo:836 2024-11-06T02:55:21Z- AHRS_M2 msg: 0.11 phi; 0.02 theta; 5.32 psi 2024-11-06T02:55:21Z- handleDepth()- Depth_Keller msg: 1730861721.55 epoch sec; seqNo:897; depth 50.94 2024-11-06T02:55:21Z- handleDVL() - DVL msg epochMillisec: 1730861721.57 2024-11-06T02:55:21Z- handleDvl() - RDI_Pathfinder msg: 1730861721.57 sec; seqNo:291; #beams:4 2024-11-06T02:55:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.93 , 37.57 , 41.31 , 39.07 2024-11-06T02:55:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.05 , 0.02 2024-11-06T02:55:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861721.58 epoch sec; seqNo:897; 4061590.57 north; 594357.22 east; 36.70 lat; -121.94 lon; 50.94 depth; utmZone: 10 2024-11-06T02:55:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861721.16 2024-11-06T02:55:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:21Z- waiting for 1730861725.16, time is 1730861721.58 2024-11-06T02:55:21Z- AHRS_M2 msg: 1730861721.93 epoch sec; seqNo:837 2024-11-06T02:55:21Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 5.32 psi 2024-11-06T02:55:21Z- handleDepth()- Depth_Keller msg: 1730861721.94 epoch sec; seqNo:898; depth 50.93 2024-11-06T02:55:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861721.96 epoch sec; seqNo:898; 4061590.93 north; 594356.00 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T02:55:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861721.16 2024-11-06T02:55:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:22Z- waiting for 1730861725.16, time is 1730861721.96 2024-11-06T02:55:22Z- AHRS_M2 msg: 1730861722.34 epoch sec; seqNo:838 2024-11-06T02:55:22Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 5.32 psi 2024-11-06T02:55:22Z- handleDepth()- Depth_Keller msg: 1730861722.35 epoch sec; seqNo:899; depth 50.92 2024-11-06T02:55:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861722.36 epoch sec; seqNo:899; 4061590.93 north; 594356.00 east; 36.70 lat; -121.94 lon; 50.92 depth; utmZone: 10 2024-11-06T02:55:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861721.16 2024-11-06T02:55:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:22Z- waiting for 1730861725.16, time is 1730861722.36 2024-11-06T02:55:22Z- AHRS_M2 msg: 1730861722.74 epoch sec; seqNo:839 2024-11-06T02:55:22Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 5.32 psi 2024-11-06T02:55:22Z- handleDepth()- Depth_Keller msg: 1730861722.75 epoch sec; seqNo:900; depth 50.93 2024-11-06T02:55:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861722.76 epoch sec; seqNo:900; 4061591.31 north; 594355.99 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T02:55:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861721.16 2024-11-06T02:55:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:22Z- waiting for 1730861725.16, time is 1730861722.76 2024-11-06T02:55:23Z- AHRS_M2 msg: 1730861723.15 epoch sec; seqNo:840 2024-11-06T02:55:23Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 5.32 psi 2024-11-06T02:55:23Z- handleDepth()- Depth_Keller msg: 1730861723.16 epoch sec; seqNo:901; depth 50.91 2024-11-06T02:55:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861723.17 epoch sec; seqNo:901; 4061591.31 north; 594355.99 east; 36.70 lat; -121.94 lon; 50.91 depth; utmZone: 10 2024-11-06T02:55:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861721.16 2024-11-06T02:55:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:23Z- waiting for 1730861725.16, time is 1730861723.17 2024-11-06T02:55:23Z- AHRS_M2 msg: 1730861723.55 epoch sec; seqNo:841 2024-11-06T02:55:23Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 5.32 psi 2024-11-06T02:55:23Z- handleDepth()- Depth_Keller msg: 1730861723.56 epoch sec; seqNo:902; depth 50.90 2024-11-06T02:55:23Z- handleDVL() - DVL msg epochMillisec: 1730861723.58 2024-11-06T02:55:23Z- handleDvl() - RDI_Pathfinder msg: 1730861723.58 sec; seqNo:292; #beams:4 2024-11-06T02:55:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.67 , 37.76 , 42.06 , 39.44 2024-11-06T02:55:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , 0.02 2024-11-06T02:55:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861723.59 epoch sec; seqNo:902; 4061591.68 north; 594354.77 east; 36.70 lat; -121.94 lon; 50.90 depth; utmZone: 10 2024-11-06T02:55:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861721.16 2024-11-06T02:55:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:23Z- waiting for 1730861725.16, time is 1730861723.59 2024-11-06T02:55:23Z- AHRS_M2 msg: 1730861723.96 epoch sec; seqNo:842 2024-11-06T02:55:23Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 5.32 psi 2024-11-06T02:55:23Z- handleDepth()- Depth_Keller msg: 1730861723.96 epoch sec; seqNo:903; depth 50.88 2024-11-06T02:55:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861723.99 epoch sec; seqNo:903; 4061591.68 north; 594354.77 east; 36.70 lat; -121.94 lon; 50.88 depth; utmZone: 10 2024-11-06T02:55:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861721.16 2024-11-06T02:55:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:24Z- waiting for 1730861725.16, time is 1730861723.99 2024-11-06T02:55:24Z- AHRS_M2 msg: 1730861724.36 epoch sec; seqNo:843 2024-11-06T02:55:24Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 5.32 psi 2024-11-06T02:55:24Z- handleDepth()- Depth_Keller msg: 1730861724.36 epoch sec; seqNo:904; depth 50.87 2024-11-06T02:55:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861724.37 epoch sec; seqNo:904; 4061592.06 north; 594354.76 east; 36.70 lat; -121.94 lon; 50.87 depth; utmZone: 10 2024-11-06T02:55:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861721.16 2024-11-06T02:55:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:24Z- waiting for 1730861725.16, time is 1730861724.37 2024-11-06T02:55:24Z- AHRS_M2 msg: 1730861724.76 epoch sec; seqNo:844 2024-11-06T02:55:24Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 5.32 psi 2024-11-06T02:55:24Z- handleDepth()- Depth_Keller msg: 1730861724.77 epoch sec; seqNo:905; depth 50.85 2024-11-06T02:55:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861724.78 epoch sec; seqNo:905; 4061592.06 north; 594354.76 east; 36.70 lat; -121.94 lon; 50.85 depth; utmZone: 10 2024-11-06T02:55:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861721.16 2024-11-06T02:55:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:24Z- waiting for 1730861725.16, time is 1730861724.78 2024-11-06T02:55:25Z- AHRS_M2 msg: 1730861725.17 epoch sec; seqNo:845 2024-11-06T02:55:25Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 5.32 psi 2024-11-06T02:55:25Z- handleDepth()- Depth_Keller msg: 1730861725.18 epoch sec; seqNo:906; depth 50.84 2024-11-06T02:55:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861725.19 epoch sec; seqNo:906; 4061592.42 north; 594353.54 east; 36.70 lat; -121.94 lon; 50.84 depth; utmZone: 10 2024-11-06T02:55:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861721.16 2024-11-06T02:55:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:25Z- waiting for 1730861725.16, time is 1730861725.19 2024-11-06T02:55:25Z- LcmTrn::updateTrn() >>>> pose time 1730861725.19, meas time 1730861723.58 <<<< 2024-11-06T02:55:25Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861725.19 2024-11-06T02:55:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861723.58 sec, ping # 0; waiting for INS data more recent than 1730861721.16... 2024-11-06T02:55:25Z- TerrainNav::motionUpdate() - currEstimate = (4061592, 594353.54, 50.84) 2024-11-06T02:55:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:55:25Z- TerrainNav:: SubcloudNIS is 0.68, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:55:25Z- TNavPF::Projecting Measurements 2024-11-06T02:55:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:55:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:55:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:55:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:55:25Z- TNavFilter::4 good beams. 2024-11-06T02:55:25Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:55:25Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:55:25Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:55:25Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:55:25Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:55:25Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:55:25Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:55:25Z- TerrainMapOctree: (vehN, vehE ) = (4061591.53, 594355.01). 2024-11-06T02:55:25Z- TerrainMapOctree: Min Distance = 190.11. 2024-11-06T02:55:25Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:55:25Z- Forcing Subcloud Comparison 2024-11-06T02:55:25Z- Weighting particles with subcloud comparison 2024-11-06T02:55:25Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:25Z- meanExpectedMeasDiff: -2.908993 delta_rms_squared: 1.513763 alpha: 0.986960 2024-11-06T02:55:25Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:25Z- meanExpectedMeasDiff: 4.079768 delta_rms_squared: 0.967216 alpha: 0.979741 2024-11-06T02:55:25Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:25Z- meanExpectedMeasDiff: -1.754862 delta_rms_squared: 1.302036 alpha: 0.984872 2024-11-06T02:55:25Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:25Z- meanExpectedMeasDiff: -0.651127 delta_rms_squared: 1.266382 alpha: 0.984453 2024-11-06T02:55:25Z- Subcloud NIS: 0.695906 2024-11-06T02:55:25Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:55:25Z- TNavPF:: sumWeights = 0.027824 2024-11-06T02:55:25Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:55:25Z- TNavPF::First Term of Map Covariance Matrix is 0.21 2024-11-06T02:55:25Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T02:55:25Z- TerrainNav::Resampling particle filter... 2024-11-06T02:55:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861723.58 sec. 2024-11-06T02:55:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:55:25Z- variances([0], [2], and psi_berg) = 0.229732 0.231223 0.000000 2024-11-06T02:55:25Z- publishEstimatesDecoder() - at 1730861725.19 sec 2024-11-06T02:55:25Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:55:25Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061624.43 594338.24 53.42 2024-11-06T02:55:25Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061625.12 594337.79 53.78 lat/lon: 36.70 -121.94 2024-11-06T02:55:25Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.23 0.71 0.00 2024-11-06T02:55:25Z- AHRS_M2 msg: 1730861725.57 epoch sec; seqNo:846 2024-11-06T02:55:25Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 5.32 psi 2024-11-06T02:55:25Z- handleDepth()- Depth_Keller msg: 1730861725.60 epoch sec; seqNo:907; depth 50.83 2024-11-06T02:55:25Z- handleDVL() - DVL msg epochMillisec: 1730861725.63 2024-11-06T02:55:25Z- handleDvl() - RDI_Pathfinder msg: 1730861725.63 sec; seqNo:293; #beams:4 2024-11-06T02:55:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.05 , 37.94 , 42.43 , 39.44 2024-11-06T02:55:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , -0.01 2024-11-06T02:55:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861725.65 epoch sec; seqNo:907; 4061592.42 north; 594353.54 east; 36.70 lat; -121.94 lon; 50.83 depth; utmZone: 10 2024-11-06T02:55:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861725.19 2024-11-06T02:55:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:25Z- waiting for 1730861729.19, time is 1730861725.65 2024-11-06T02:55:25Z- AHRS_M2 msg: 1730861725.97 epoch sec; seqNo:847 2024-11-06T02:55:25Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 5.32 psi 2024-11-06T02:55:25Z- handleDepth()- Depth_Keller msg: 1730861725.99 epoch sec; seqNo:908; depth 50.81 2024-11-06T02:55:25Z- handleDVL() - DVL msg epochMillisec: 1730861725.99 2024-11-06T02:55:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:55:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:55:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:55:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:55:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:55:26Z- handleDvl() - RDI_Pathfinder msg: 1730861725.99 sec; seqNo:294; #beams:0 2024-11-06T02:55:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:55:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , -0.01 2024-11-06T02:55:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861726.00 epoch sec; seqNo:908; 4061592.80 north; 594353.53 east; 36.70 lat; -121.94 lon; 50.81 depth; utmZone: 10 2024-11-06T02:55:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861725.19 2024-11-06T02:55:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:26Z- waiting for 1730861729.19, time is 1730861726.00 2024-11-06T02:55:26Z- AHRS_M2 msg: 1730861726.38 epoch sec; seqNo:848 2024-11-06T02:55:26Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 5.31 psi 2024-11-06T02:55:26Z- handleDepth()- Depth_Keller msg: 1730861726.38 epoch sec; seqNo:909; depth 50.79 2024-11-06T02:55:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861726.40 epoch sec; seqNo:909; 4061592.80 north; 594353.53 east; 36.70 lat; -121.94 lon; 50.79 depth; utmZone: 10 2024-11-06T02:55:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861725.19 2024-11-06T02:55:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:26Z- waiting for 1730861729.19, time is 1730861726.40 2024-11-06T02:55:26Z- AHRS_M2 msg: 1730861726.78 epoch sec; seqNo:849 2024-11-06T02:55:26Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 5.31 psi 2024-11-06T02:55:26Z- handleDepth()- Depth_Keller msg: 1730861726.79 epoch sec; seqNo:910; depth 50.78 2024-11-06T02:55:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861726.80 epoch sec; seqNo:910; 4061593.17 north; 594352.31 east; 36.70 lat; -121.94 lon; 50.78 depth; utmZone: 10 2024-11-06T02:55:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861725.19 2024-11-06T02:55:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:26Z- waiting for 1730861729.19, time is 1730861726.80 2024-11-06T02:55:27Z- handleDepth()- Depth_Keller msg: 1730861727.21 epoch sec; seqNo:911; depth 50.74 2024-11-06T02:55:27Z- handleDVL() - DVL msg epochMillisec: 1730861727.21 2024-11-06T02:55:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:55:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:55:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:55:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:55:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:55:27Z- handleDvl() - RDI_Pathfinder msg: 1730861727.21 sec; seqNo:295; #beams:0 2024-11-06T02:55:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:55:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , -0.01 2024-11-06T02:55:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861727.22 epoch sec; seqNo:911; 4061593.17 north; 594352.31 east; 36.70 lat; -121.94 lon; 50.74 depth; utmZone: 10 2024-11-06T02:55:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861725.19 2024-11-06T02:55:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:27Z- waiting for 1730861729.19, time is 1730861727.22 2024-11-06T02:55:27Z- AHRS_M2 msg: 1730861727.59 epoch sec; seqNo:850 2024-11-06T02:55:27Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 5.31 psi 2024-11-06T02:55:27Z- handleDepth()- Depth_Keller msg: 1730861727.60 epoch sec; seqNo:912; depth 50.73 2024-11-06T02:55:27Z- handleDVL() - DVL msg epochMillisec: 1730861727.62 2024-11-06T02:55:27Z- handleDvl() - RDI_Pathfinder msg: 1730861727.62 sec; seqNo:296; #beams:4 2024-11-06T02:55:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.86 , 37.94 , 42.80 , 39.25 2024-11-06T02:55:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , -0.00 2024-11-06T02:55:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861727.64 epoch sec; seqNo:912; 4061593.55 north; 594352.31 east; 36.70 lat; -121.94 lon; 50.73 depth; utmZone: 10 2024-11-06T02:55:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861725.19 2024-11-06T02:55:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:27Z- waiting for 1730861729.19, time is 1730861727.64 2024-11-06T02:55:28Z- AHRS_M2 msg: 1730861728.00 epoch sec; seqNo:851 2024-11-06T02:55:28Z- AHRS_M2 msg: 0.11 phi; 0.05 theta; 5.31 psi 2024-11-06T02:55:28Z- handleDepth()- Depth_Keller msg: 1730861728.00 epoch sec; seqNo:913; depth 50.71 2024-11-06T02:55:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861728.02 epoch sec; seqNo:913; 4061593.55 north; 594352.31 east; 36.70 lat; -121.94 lon; 50.71 depth; utmZone: 10 2024-11-06T02:55:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861725.19 2024-11-06T02:55:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:28Z- waiting for 1730861729.19, time is 1730861728.02 2024-11-06T02:55:28Z- AHRS_M2 msg: 1730861728.40 epoch sec; seqNo:852 2024-11-06T02:55:28Z- AHRS_M2 msg: 0.11 phi; 0.05 theta; 5.30 psi 2024-11-06T02:55:28Z- handleDepth()- Depth_Keller msg: 1730861728.41 epoch sec; seqNo:914; depth 50.69 2024-11-06T02:55:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861728.43 epoch sec; seqNo:914; 4061593.91 north; 594351.08 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T02:55:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861725.19 2024-11-06T02:55:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:28Z- waiting for 1730861729.19, time is 1730861728.43 2024-11-06T02:55:28Z- AHRS_M2 msg: 1730861728.80 epoch sec; seqNo:853 2024-11-06T02:55:28Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 5.30 psi 2024-11-06T02:55:28Z- handleDepth()- Depth_Keller msg: 1730861728.81 epoch sec; seqNo:915; depth 50.67 2024-11-06T02:55:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861728.82 epoch sec; seqNo:915; 4061594.29 north; 594351.08 east; 36.70 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T02:55:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861725.19 2024-11-06T02:55:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:28Z- waiting for 1730861729.19, time is 1730861728.82 2024-11-06T02:55:29Z- AHRS_M2 msg: 1730861729.21 epoch sec; seqNo:854 2024-11-06T02:55:29Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 5.30 psi 2024-11-06T02:55:29Z- handleDepth()- Depth_Keller msg: 1730861729.22 epoch sec; seqNo:916; depth 50.66 2024-11-06T02:55:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861729.28 epoch sec; seqNo:916; 4061594.29 north; 594351.08 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T02:55:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861725.19 2024-11-06T02:55:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:29Z- waiting for 1730861729.19, time is 1730861729.28 2024-11-06T02:55:29Z- LcmTrn::updateTrn() >>>> pose time 1730861729.28, meas time 1730861727.62 <<<< 2024-11-06T02:55:29Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861729.28 2024-11-06T02:55:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861727.62 sec, ping # 0; waiting for INS data more recent than 1730861725.19... 2024-11-06T02:55:29Z- TerrainNav::motionUpdate() - currEstimate = (4061594, 594351.08, 50.66) 2024-11-06T02:55:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:55:29Z- TerrainNav:: SubcloudNIS is 0.70, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:55:29Z- TNavPF::Projecting Measurements 2024-11-06T02:55:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:55:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:55:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:55:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:55:29Z- TNavFilter::4 good beams. 2024-11-06T02:55:29Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:55:29Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:55:29Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:55:29Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:55:29Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:55:29Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:55:29Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:55:29Z- TerrainMapOctree: (vehN, vehE ) = (4061593.53, 594352.07). 2024-11-06T02:55:29Z- TerrainMapOctree: Min Distance = 186.82. 2024-11-06T02:55:29Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:55:29Z- Forcing Subcloud Comparison 2024-11-06T02:55:29Z- Weighting particles with subcloud comparison 2024-11-06T02:55:29Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:29Z- meanExpectedMeasDiff: -2.869883 delta_rms_squared: 1.497919 alpha: 0.986824 2024-11-06T02:55:29Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:29Z- meanExpectedMeasDiff: 3.668429 delta_rms_squared: 0.845703 alpha: 0.976897 2024-11-06T02:55:29Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:29Z- meanExpectedMeasDiff: -0.329231 delta_rms_squared: 1.473532 alpha: 0.986609 2024-11-06T02:55:29Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:29Z- meanExpectedMeasDiff: -0.473617 delta_rms_squared: 1.221229 alpha: 0.983887 2024-11-06T02:55:29Z- Subcloud NIS: 0.690213 2024-11-06T02:55:29Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:55:29Z- TNavPF:: sumWeights = 0.062149 2024-11-06T02:55:29Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:55:29Z- TNavPF::First Term of Map Covariance Matrix is 0.46 2024-11-06T02:55:29Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.01 2024-11-06T02:55:29Z- TerrainNav::Resampling particle filter... 2024-11-06T02:55:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861727.62 sec. 2024-11-06T02:55:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:55:29Z- variances([0], [2], and psi_berg) = 0.221190 0.209325 0.000000 2024-11-06T02:55:29Z- publishEstimatesDecoder() - at 1730861729.28 sec 2024-11-06T02:55:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:55:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061626.29 594335.74 52.98 2024-11-06T02:55:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061626.90 594335.22 53.69 lat/lon: 36.70 -121.94 2024-11-06T02:55:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.21 0.73 0.00 2024-11-06T02:55:29Z- AHRS_M2 msg: 1730861729.62 epoch sec; seqNo:855 2024-11-06T02:55:29Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 5.30 psi 2024-11-06T02:55:29Z- handleDepth()- Depth_Keller msg: 1730861729.64 epoch sec; seqNo:917; depth 50.63 2024-11-06T02:55:29Z- handleDVL() - DVL msg epochMillisec: 1730861729.68 2024-11-06T02:55:29Z- handleDvl() - RDI_Pathfinder msg: 1730861729.68 sec; seqNo:297; #beams:4 2024-11-06T02:55:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.67 , 38.13 , 43.18 , 39.44 2024-11-06T02:55:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , 0.01 2024-11-06T02:55:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861729.70 epoch sec; seqNo:917; 4061594.67 north; 594351.07 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T02:55:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861729.28 2024-11-06T02:55:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:29Z- waiting for 1730861733.28, time is 1730861729.70 2024-11-06T02:55:30Z- AHRS_M2 msg: 1730861730.02 epoch sec; seqNo:856 2024-11-06T02:55:30Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 5.30 psi 2024-11-06T02:55:30Z- handleDepth()- Depth_Keller msg: 1730861730.02 epoch sec; seqNo:918; depth 50.62 2024-11-06T02:55:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861730.04 epoch sec; seqNo:918; 4061594.65 north; 594349.85 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T02:55:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861729.28 2024-11-06T02:55:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:30Z- waiting for 1730861733.28, time is 1730861730.04 2024-11-06T02:55:30Z- AHRS_M2 msg: 1730861730.42 epoch sec; seqNo:857 2024-11-06T02:55:30Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 5.31 psi 2024-11-06T02:55:30Z- handleDepth()- Depth_Keller msg: 1730861730.42 epoch sec; seqNo:919; depth 50.59 2024-11-06T02:55:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861730.43 epoch sec; seqNo:919; 4061595.03 north; 594349.85 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T02:55:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861729.28 2024-11-06T02:55:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:30Z- waiting for 1730861733.28, time is 1730861730.43 2024-11-06T02:55:30Z- AHRS_M2 msg: 1730861730.83 epoch sec; seqNo:858 2024-11-06T02:55:30Z- AHRS_M2 msg: 0.12 phi; 0.06 theta; 5.31 psi 2024-11-06T02:55:30Z- handleDepth()- Depth_Keller msg: 1730861730.83 epoch sec; seqNo:920; depth 50.57 2024-11-06T02:55:30Z- handleDVL() - DVL msg epochMillisec: 1730861730.84 2024-11-06T02:55:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:55:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:55:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:55:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:55:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:55:30Z- handleDvl() - RDI_Pathfinder msg: 1730861730.84 sec; seqNo:298; #beams:0 2024-11-06T02:55:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:55:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , 0.01 2024-11-06T02:55:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861730.84 epoch sec; seqNo:920; 4061595.03 north; 594349.85 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T02:55:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861729.28 2024-11-06T02:55:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:30Z- waiting for 1730861733.28, time is 1730861730.84 2024-11-06T02:55:31Z- AHRS_M2 msg: 1730861731.26 epoch sec; seqNo:859 2024-11-06T02:55:31Z- AHRS_M2 msg: 0.12 phi; 0.06 theta; 5.31 psi 2024-11-06T02:55:31Z- handleDepth()- Depth_Keller msg: 1730861731.27 epoch sec; seqNo:921; depth 50.53 2024-11-06T02:55:31Z- handleDVL() - DVL msg epochMillisec: 1730861731.31 2024-11-06T02:55:31Z- handleDvl() - RDI_Pathfinder msg: 1730861731.31 sec; seqNo:299; #beams:4 2024-11-06T02:55:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.86 , 37.76 , 43.55 , 39.44 2024-11-06T02:55:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.00 2024-11-06T02:55:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861731.32 epoch sec; seqNo:921; 4061595.41 north; 594349.84 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T02:55:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861729.28 2024-11-06T02:55:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:31Z- waiting for 1730861733.28, time is 1730861731.32 2024-11-06T02:55:31Z- AHRS_M2 msg: 1730861731.63 epoch sec; seqNo:860 2024-11-06T02:55:31Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.31 psi 2024-11-06T02:55:31Z- handleDepth()- Depth_Keller msg: 1730861731.64 epoch sec; seqNo:922; depth 50.51 2024-11-06T02:55:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861731.67 epoch sec; seqNo:922; 4061595.40 north; 594348.62 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T02:55:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861729.28 2024-11-06T02:55:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:31Z- waiting for 1730861733.28, time is 1730861731.67 2024-11-06T02:55:32Z- AHRS_M2 msg: 1730861732.07 epoch sec; seqNo:861 2024-11-06T02:55:32Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.31 psi 2024-11-06T02:55:32Z- handleDepth()- Depth_Keller msg: 1730861732.08 epoch sec; seqNo:923; depth 50.49 2024-11-06T02:55:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861732.09 epoch sec; seqNo:923; 4061595.78 north; 594348.62 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T02:55:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861729.28 2024-11-06T02:55:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:32Z- waiting for 1730861733.28, time is 1730861732.09 2024-11-06T02:55:32Z- AHRS_M2 msg: 1730861732.44 epoch sec; seqNo:862 2024-11-06T02:55:32Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.30 psi 2024-11-06T02:55:32Z- handleDepth()- Depth_Keller msg: 1730861732.45 epoch sec; seqNo:924; depth 50.46 2024-11-06T02:55:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861732.46 epoch sec; seqNo:924; 4061595.78 north; 594348.62 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T02:55:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861729.28 2024-11-06T02:55:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:32Z- waiting for 1730861733.28, time is 1730861732.46 2024-11-06T02:55:32Z- AHRS_M2 msg: 1730861732.84 epoch sec; seqNo:863 2024-11-06T02:55:32Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.30 psi 2024-11-06T02:55:32Z- handleDepth()- Depth_Keller msg: 1730861732.85 epoch sec; seqNo:925; depth 50.43 2024-11-06T02:55:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861732.86 epoch sec; seqNo:925; 4061596.16 north; 594348.61 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T02:55:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861729.28 2024-11-06T02:55:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:33Z- waiting for 1730861733.28, time is 1730861732.86 2024-11-06T02:55:33Z- AHRS_M2 msg: 1730861733.28 epoch sec; seqNo:864 2024-11-06T02:55:33Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.30 psi 2024-11-06T02:55:33Z- handleDepth()- Depth_Keller msg: 1730861733.29 epoch sec; seqNo:926; depth 50.40 2024-11-06T02:55:33Z- handleDVL() - DVL msg epochMillisec: 1730861733.31 2024-11-06T02:55:33Z- handleDvl() - RDI_Pathfinder msg: 1730861733.31 sec; seqNo:300; #beams:4 2024-11-06T02:55:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.42 , 38.50 , 44.11 , 39.44 2024-11-06T02:55:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.00 2024-11-06T02:55:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861733.34 epoch sec; seqNo:926; 4061596.14 north; 594347.39 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T02:55:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861729.28 2024-11-06T02:55:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:33Z- waiting for 1730861733.28, time is 1730861733.34 2024-11-06T02:55:33Z- LcmTrn::updateTrn() >>>> pose time 1730861733.34, meas time 1730861733.31 <<<< 2024-11-06T02:55:33Z- LcmTrn::updateTrn() equating pose 1730861733.31 and meas times 1730861733.31 2024-11-06T02:55:33Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861733.31 2024-11-06T02:55:33Z- TerrainNav::motionUpdate() - currEstimate = (4061596, 594347.39, 50.40) 2024-11-06T02:55:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:55:33Z- TerrainNav:: SubcloudNIS is 0.69, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:55:33Z- TNavPF::Projecting Measurements 2024-11-06T02:55:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:55:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:55:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:55:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:55:33Z- TNavFilter::4 good beams. 2024-11-06T02:55:33Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:55:33Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:55:33Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:55:33Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:55:33Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:55:33Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:55:33Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:55:33Z- TerrainMapOctree: (vehN, vehE ) = (4061596.14, 594347.39). 2024-11-06T02:55:33Z- TerrainMapOctree: Min Distance = 182.09. 2024-11-06T02:55:33Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:55:33Z- Forcing Subcloud Comparison 2024-11-06T02:55:33Z- Weighting particles with subcloud comparison 2024-11-06T02:55:33Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:33Z- meanExpectedMeasDiff: -2.988449 delta_rms_squared: 1.555608 alpha: 0.987306 2024-11-06T02:55:33Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:33Z- meanExpectedMeasDiff: 2.839374 delta_rms_squared: 0.844554 alpha: 0.976867 2024-11-06T02:55:33Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:33Z- meanExpectedMeasDiff: -2.121558 delta_rms_squared: 1.884187 alpha: 0.989497 2024-11-06T02:55:33Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:33Z- meanExpectedMeasDiff: -0.350249 delta_rms_squared: 1.220769 alpha: 0.983881 2024-11-06T02:55:33Z- Subcloud NIS: 0.684816 2024-11-06T02:55:33Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:55:33Z- TNavPF:: sumWeights = 0.079799 2024-11-06T02:55:33Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:55:33Z- TNavPF::First Term of Map Covariance Matrix is 0.48 2024-11-06T02:55:33Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T02:55:33Z- TerrainNav::Resampling particle filter... 2024-11-06T02:55:33Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861733.31 sec, ping # 0. 2024-11-06T02:55:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:55:33Z- variances([0], [2], and psi_berg) = 0.208028 0.204108 0.000000 2024-11-06T02:55:33Z- publishEstimatesDecoder() - at 1730861733.31 sec 2024-11-06T02:55:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:55:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061628.21 594332.05 53.80 2024-11-06T02:55:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061628.71 594331.49 53.82 lat/lon: 36.70 -121.94 2024-11-06T02:55:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.20 0.44 0.00 2024-11-06T02:55:33Z- AHRS_M2 msg: 1730861733.68 epoch sec; seqNo:865 2024-11-06T02:55:33Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.29 psi 2024-11-06T02:55:33Z- handleDepth()- Depth_Keller msg: 1730861733.70 epoch sec; seqNo:927; depth 50.37 2024-11-06T02:55:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861733.72 epoch sec; seqNo:927; 4061596.52 north; 594347.39 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T02:55:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861733.31 2024-11-06T02:55:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:33Z- waiting for 1730861737.31, time is 1730861733.72 2024-11-06T02:55:34Z- AHRS_M2 msg: 1730861734.07 epoch sec; seqNo:866 2024-11-06T02:55:34Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.29 psi 2024-11-06T02:55:34Z- handleDepth()- Depth_Keller msg: 1730861734.08 epoch sec; seqNo:928; depth 50.35 2024-11-06T02:55:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861734.09 epoch sec; seqNo:928; 4061596.52 north; 594347.39 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T02:55:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861733.31 2024-11-06T02:55:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:34Z- waiting for 1730861737.31, time is 1730861734.09 2024-11-06T02:55:34Z- AHRS_M2 msg: 1730861734.46 epoch sec; seqNo:867 2024-11-06T02:55:34Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.29 psi 2024-11-06T02:55:34Z- handleDepth()- Depth_Keller msg: 1730861734.46 epoch sec; seqNo:929; depth 50.31 2024-11-06T02:55:34Z- handleDVL() - DVL msg epochMillisec: 1730861734.47 2024-11-06T02:55:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:55:34Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:55:34Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:55:34Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:55:34Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:55:34Z- handleDvl() - RDI_Pathfinder msg: 1730861734.47 sec; seqNo:301; #beams:0 2024-11-06T02:55:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:55:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.00 2024-11-06T02:55:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861734.47 epoch sec; seqNo:929; 4061596.90 north; 594347.39 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T02:55:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861733.31 2024-11-06T02:55:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:34Z- waiting for 1730861737.31, time is 1730861734.47 2024-11-06T02:55:34Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T02:55:34Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T02:55:34Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T02:55:34Z- AHRS_M2 msg: 1730861734.86 epoch sec; seqNo:868 2024-11-06T02:55:34Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.29 psi 2024-11-06T02:55:34Z- handleDepth()- Depth_Keller msg: 1730861734.89 epoch sec; seqNo:930; depth 50.29 2024-11-06T02:55:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861734.91 epoch sec; seqNo:930; 4061596.89 north; 594346.17 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T02:55:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861733.31 2024-11-06T02:55:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:35Z- waiting for 1730861737.31, time is 1730861734.91 2024-11-06T02:55:35Z- AHRS_M2 msg: 1730861735.29 epoch sec; seqNo:869 2024-11-06T02:55:35Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 5.28 psi 2024-11-06T02:55:35Z- handleDepth()- Depth_Keller msg: 1730861735.30 epoch sec; seqNo:931; depth 50.25 2024-11-06T02:55:35Z- handleDVL() - DVL msg epochMillisec: 1730861735.32 2024-11-06T02:55:35Z- handleDvl() - RDI_Pathfinder msg: 1730861735.32 sec; seqNo:302; #beams:4 2024-11-06T02:55:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 37.93 , 44.25 , 39.76 2024-11-06T02:55:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , 0.00 2024-11-06T02:55:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861735.34 epoch sec; seqNo:931; 4061597.27 north; 594346.16 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T02:55:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861733.31 2024-11-06T02:55:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:35Z- waiting for 1730861737.31, time is 1730861735.34 2024-11-06T02:55:35Z- AHRS_M2 msg: 1730861735.67 epoch sec; seqNo:870 2024-11-06T02:55:35Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 5.28 psi 2024-11-06T02:55:35Z- handleDepth()- Depth_Keller msg: 1730861735.68 epoch sec; seqNo:932; depth 50.21 2024-11-06T02:55:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861735.70 epoch sec; seqNo:932; 4061597.27 north; 594346.16 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T02:55:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861733.31 2024-11-06T02:55:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:35Z- waiting for 1730861737.31, time is 1730861735.70 2024-11-06T02:55:36Z- AHRS_M2 msg: 1730861736.07 epoch sec; seqNo:871 2024-11-06T02:55:36Z- AHRS_M2 msg: 0.11 phi; 0.05 theta; 5.28 psi 2024-11-06T02:55:36Z- handleDepth()- Depth_Keller msg: 1730861736.08 epoch sec; seqNo:933; depth 50.20 2024-11-06T02:55:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861736.09 epoch sec; seqNo:933; 4061597.65 north; 594346.16 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T02:55:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861733.31 2024-11-06T02:55:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:36Z- waiting for 1730861737.31, time is 1730861736.09 2024-11-06T02:55:36Z- AHRS_M2 msg: 1730861736.48 epoch sec; seqNo:872 2024-11-06T02:55:36Z- AHRS_M2 msg: 0.11 phi; 0.05 theta; 5.28 psi 2024-11-06T02:55:36Z- handleDepth()- Depth_Keller msg: 1730861736.49 epoch sec; seqNo:934; depth 50.16 2024-11-06T02:55:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861736.49 epoch sec; seqNo:934; 4061597.63 north; 594344.94 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T02:55:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861733.31 2024-11-06T02:55:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:36Z- waiting for 1730861737.31, time is 1730861736.49 2024-11-06T02:55:36Z- AHRS_M2 msg: 1730861736.89 epoch sec; seqNo:873 2024-11-06T02:55:36Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 5.28 psi 2024-11-06T02:55:36Z- handleDepth()- Depth_Keller msg: 1730861736.89 epoch sec; seqNo:935; depth 50.13 2024-11-06T02:55:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861736.90 epoch sec; seqNo:935; 4061598.01 north; 594344.93 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T02:55:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861733.31 2024-11-06T02:55:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:37Z- waiting for 1730861737.31, time is 1730861736.90 2024-11-06T02:55:37Z- AHRS_M2 msg: 1730861737.30 epoch sec; seqNo:874 2024-11-06T02:55:37Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 5.28 psi 2024-11-06T02:55:37Z- handleDepth()- Depth_Keller msg: 1730861737.30 epoch sec; seqNo:936; depth 50.11 2024-11-06T02:55:37Z- handleDVL() - DVL msg epochMillisec: 1730861737.33 2024-11-06T02:55:37Z- handleDvl() - RDI_Pathfinder msg: 1730861737.33 sec; seqNo:303; #beams:4 2024-11-06T02:55:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.06 , 38.54 , 43.64 , 40.17 2024-11-06T02:55:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , -0.02 2024-11-06T02:55:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861737.35 epoch sec; seqNo:936; 4061598.01 north; 594344.93 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T02:55:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861733.31 2024-11-06T02:55:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:37Z- waiting for 1730861737.31, time is 1730861737.35 2024-11-06T02:55:37Z- LcmTrn::updateTrn() >>>> pose time 1730861737.35, meas time 1730861737.33 <<<< 2024-11-06T02:55:37Z- LcmTrn::updateTrn() equating pose 1730861737.33 and meas times 1730861737.33 2024-11-06T02:55:37Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861737.33 2024-11-06T02:55:37Z- TerrainNav::motionUpdate() - currEstimate = (4061598, 594344.93, 50.11) 2024-11-06T02:55:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:55:37Z- TerrainNav:: SubcloudNIS is 0.68, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:55:37Z- TNavPF::Projecting Measurements 2024-11-06T02:55:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:55:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:55:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:55:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:55:37Z- TNavFilter::4 good beams. 2024-11-06T02:55:37Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:55:37Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:55:37Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:55:37Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:55:37Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:55:37Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:55:37Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:55:37Z- TerrainMapOctree: (vehN, vehE ) = (4061598.01, 594344.93). 2024-11-06T02:55:37Z- TerrainMapOctree: Min Distance = 179.20. 2024-11-06T02:55:37Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:55:37Z- Forcing Subcloud Comparison 2024-11-06T02:55:37Z- Weighting particles with subcloud comparison 2024-11-06T02:55:37Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:37Z- meanExpectedMeasDiff: -2.375942 delta_rms_squared: 1.101493 alpha: 0.982167 2024-11-06T02:55:37Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:37Z- meanExpectedMeasDiff: 2.530046 delta_rms_squared: 0.695502 alpha: 0.972048 2024-11-06T02:55:37Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:37Z- meanExpectedMeasDiff: -1.764230 delta_rms_squared: 0.967071 alpha: 0.979738 2024-11-06T02:55:37Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:37Z- meanExpectedMeasDiff: -0.061744 delta_rms_squared: 0.979290 alpha: 0.979986 2024-11-06T02:55:37Z- Subcloud NIS: 0.675770 2024-11-06T02:55:37Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:55:37Z- TNavPF:: sumWeights = 0.153934 2024-11-06T02:55:37Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:55:37Z- TNavPF::First Term of Map Covariance Matrix is 0.59 2024-11-06T02:55:37Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T02:55:37Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861737.33 sec, ping # 0. 2024-11-06T02:55:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:55:37Z- variances([0], [2], and psi_berg) = 0.211082 0.187641 0.000000 2024-11-06T02:55:37Z- publishEstimatesDecoder() - at 1730861737.33 sec 2024-11-06T02:55:37Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:55:37Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061631.67 594328.18 54.00 2024-11-06T02:55:37Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061630.55 594329.00 53.73 lat/lon: 36.70 -121.94 2024-11-06T02:55:37Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.19 0.38 0.00 2024-11-06T02:55:37Z- AHRS_M2 msg: 1730861737.69 epoch sec; seqNo:875 2024-11-06T02:55:37Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 5.28 psi 2024-11-06T02:55:37Z- handleDepth()- Depth_Keller msg: 1730861737.70 epoch sec; seqNo:937; depth 50.09 2024-11-06T02:55:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861737.72 epoch sec; seqNo:937; 4061598.00 north; 594343.71 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T02:55:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861737.33 2024-11-06T02:55:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:37Z- waiting for 1730861741.33, time is 1730861737.72 2024-11-06T02:55:38Z- AHRS_M2 msg: 1730861738.10 epoch sec; seqNo:876 2024-11-06T02:55:38Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 5.28 psi 2024-11-06T02:55:38Z- handleDepth()- Depth_Keller msg: 1730861738.11 epoch sec; seqNo:938; depth 50.07 2024-11-06T02:55:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861738.12 epoch sec; seqNo:938; 4061598.38 north; 594343.71 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T02:55:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861737.33 2024-11-06T02:55:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:38Z- waiting for 1730861741.33, time is 1730861738.12 2024-11-06T02:55:38Z- AHRS_M2 msg: 1730861738.51 epoch sec; seqNo:877 2024-11-06T02:55:38Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 5.27 psi 2024-11-06T02:55:38Z- handleDepth()- Depth_Keller msg: 1730861738.51 epoch sec; seqNo:939; depth 50.04 2024-11-06T02:55:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861738.52 epoch sec; seqNo:939; 4061598.38 north; 594343.71 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T02:55:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861737.33 2024-11-06T02:55:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:38Z- waiting for 1730861741.33, time is 1730861738.52 2024-11-06T02:55:38Z- AHRS_M2 msg: 1730861738.91 epoch sec; seqNo:878 2024-11-06T02:55:38Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 5.27 psi 2024-11-06T02:55:38Z- handleDepth()- Depth_Keller msg: 1730861738.94 epoch sec; seqNo:940; depth 50.03 2024-11-06T02:55:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861738.95 epoch sec; seqNo:940; 4061598.75 north; 594343.70 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T02:55:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861737.33 2024-11-06T02:55:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:39Z- waiting for 1730861741.33, time is 1730861738.95 2024-11-06T02:55:39Z- AHRS_M2 msg: 1730861739.31 epoch sec; seqNo:879 2024-11-06T02:55:39Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 5.27 psi 2024-11-06T02:55:39Z- handleDepth()- Depth_Keller msg: 1730861739.31 epoch sec; seqNo:941; depth 50.00 2024-11-06T02:55:39Z- handleDVL() - DVL msg epochMillisec: 1730861739.33 2024-11-06T02:55:39Z- handleDvl() - RDI_Pathfinder msg: 1730861739.33 sec; seqNo:304; #beams:4 2024-11-06T02:55:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.47 , 38.33 , 43.23 , 40.58 2024-11-06T02:55:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , -0.04 2024-11-06T02:55:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861739.38 epoch sec; seqNo:941; 4061598.74 north; 594342.48 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T02:55:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861737.33 2024-11-06T02:55:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:39Z- waiting for 1730861741.33, time is 1730861739.38 2024-11-06T02:55:39Z- AHRS_M2 msg: 1730861739.72 epoch sec; seqNo:880 2024-11-06T02:55:39Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 5.27 psi 2024-11-06T02:55:39Z- handleDepth()- Depth_Keller msg: 1730861739.72 epoch sec; seqNo:942; depth 50.00 2024-11-06T02:55:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861739.74 epoch sec; seqNo:942; 4061599.12 north; 594342.48 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T02:55:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861737.33 2024-11-06T02:55:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:39Z- waiting for 1730861741.33, time is 1730861739.74 2024-11-06T02:55:40Z- AHRS_M2 msg: 1730861740.12 epoch sec; seqNo:881 2024-11-06T02:55:40Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 5.27 psi 2024-11-06T02:55:40Z- handleDepth()- Depth_Keller msg: 1730861740.12 epoch sec; seqNo:943; depth 49.99 2024-11-06T02:55:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861740.13 epoch sec; seqNo:943; 4061599.12 north; 594342.48 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T02:55:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861737.33 2024-11-06T02:55:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:40Z- waiting for 1730861741.33, time is 1730861740.13 2024-11-06T02:55:40Z- AHRS_M2 msg: 1730861740.52 epoch sec; seqNo:882 2024-11-06T02:55:40Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 5.27 psi 2024-11-06T02:55:40Z- handleDepth()- Depth_Keller msg: 1730861740.53 epoch sec; seqNo:944; depth 49.97 2024-11-06T02:55:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861740.55 epoch sec; seqNo:944; 4061599.50 north; 594342.48 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T02:55:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861737.33 2024-11-06T02:55:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:40Z- waiting for 1730861741.33, time is 1730861740.55 2024-11-06T02:55:40Z- AHRS_M2 msg: 1730861740.93 epoch sec; seqNo:883 2024-11-06T02:55:40Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.27 psi 2024-11-06T02:55:40Z- handleDepth()- Depth_Keller msg: 1730861740.94 epoch sec; seqNo:945; depth 49.97 2024-11-06T02:55:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861740.95 epoch sec; seqNo:945; 4061599.49 north; 594341.26 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T02:55:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861737.33 2024-11-06T02:55:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:41Z- waiting for 1730861741.33, time is 1730861740.95 2024-11-06T02:55:41Z- handleDepth()- Depth_Keller msg: 1730861741.36 epoch sec; seqNo:946; depth 49.96 2024-11-06T02:55:41Z- handleDVL() - DVL msg epochMillisec: 1730861741.38 2024-11-06T02:55:41Z- handleDvl() - RDI_Pathfinder msg: 1730861741.38 sec; seqNo:305; #beams:4 2024-11-06T02:55:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 38.54 , 43.02 , 40.78 2024-11-06T02:55:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.02 , 0.00 2024-11-06T02:55:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861741.39 epoch sec; seqNo:946; 4061599.86 north; 594341.25 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T02:55:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861737.33 2024-11-06T02:55:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:41Z- waiting for 1730861741.33, time is 1730861741.39 2024-11-06T02:55:41Z- LcmTrn::updateTrn() >>>> pose time 1730861741.39, meas time 1730861741.38 <<<< 2024-11-06T02:55:41Z- LcmTrn::updateTrn() equating pose 1730861741.38 and meas times 1730861741.38 2024-11-06T02:55:41Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861741.38 2024-11-06T02:55:41Z- TerrainNav::motionUpdate() - currEstimate = (4061600, 594341.25, 49.96) 2024-11-06T02:55:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:55:41Z- TerrainNav:: SubcloudNIS is 0.68, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:55:41Z- TNavPF::Projecting Measurements 2024-11-06T02:55:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:55:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:55:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:55:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:55:41Z- TNavFilter::4 good beams. 2024-11-06T02:55:41Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:55:41Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:55:41Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:55:41Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:55:41Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:55:41Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:55:41Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:55:41Z- TerrainMapOctree: (vehN, vehE ) = (4061599.86, 594341.25). 2024-11-06T02:55:41Z- TerrainMapOctree: Min Distance = 175.69. 2024-11-06T02:55:41Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:55:41Z- Forcing Subcloud Comparison 2024-11-06T02:55:41Z- Weighting particles with subcloud comparison 2024-11-06T02:55:41Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:41Z- meanExpectedMeasDiff: -3.178115 delta_rms_squared: 0.998970 alpha: 0.980372 2024-11-06T02:55:41Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:41Z- meanExpectedMeasDiff: 2.248937 delta_rms_squared: 0.615259 alpha: 0.968517 2024-11-06T02:55:41Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:41Z- meanExpectedMeasDiff: -1.667778 delta_rms_squared: 0.854554 alpha: 0.977131 2024-11-06T02:55:41Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:41Z- meanExpectedMeasDiff: -0.861293 delta_rms_squared: 0.823795 alpha: 0.976298 2024-11-06T02:55:41Z- Subcloud NIS: 0.684740 2024-11-06T02:55:41Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:55:41Z- TNavPF:: sumWeights = 0.132098 2024-11-06T02:55:41Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:55:41Z- TNavPF::First Term of Map Covariance Matrix is 0.82 2024-11-06T02:55:41Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.02 2024-11-06T02:55:41Z- TerrainNav::Resampling particle filter... 2024-11-06T02:55:41Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861741.38 sec, ping # 0. 2024-11-06T02:55:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:55:41Z- variances([0], [2], and psi_berg) = 0.236859 0.204784 0.000000 2024-11-06T02:55:41Z- publishEstimatesDecoder() - at 1730861741.38 sec 2024-11-06T02:55:41Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:55:41Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061631.91 594325.89 53.21 2024-11-06T02:55:41Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061632.35 594325.26 53.96 lat/lon: 36.70 -121.94 2024-11-06T02:55:41Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.20 0.38 0.00 2024-11-06T02:55:41Z- AHRS_M2 msg: 1730861741.73 epoch sec; seqNo:884 2024-11-06T02:55:41Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.26 psi 2024-11-06T02:55:41Z- handleDepth()- Depth_Keller msg: 1730861741.77 epoch sec; seqNo:947; depth 49.96 2024-11-06T02:55:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861741.79 epoch sec; seqNo:947; 4061600.24 north; 594341.25 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T02:55:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861741.38 2024-11-06T02:55:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:41Z- waiting for 1730861745.38, time is 1730861741.79 2024-11-06T02:55:42Z- handleDepth()- Depth_Keller msg: 1730861742.16 epoch sec; seqNo:948; depth 49.95 2024-11-06T02:55:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861742.17 epoch sec; seqNo:948; 4061600.23 north; 594340.03 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T02:55:42Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:55:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861741.38 2024-11-06T02:55:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:42Z- waiting for 1730861745.38, time is 1730861742.17 2024-11-06T02:55:42Z- AHRS_M2 msg: 1730861742.54 epoch sec; seqNo:885 2024-11-06T02:55:42Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.26 psi 2024-11-06T02:55:42Z- handleDepth()- Depth_Keller msg: 1730861742.55 epoch sec; seqNo:949; depth 49.95 2024-11-06T02:55:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861742.56 epoch sec; seqNo:949; 4061600.61 north; 594340.02 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T02:55:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861741.38 2024-11-06T02:55:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:42Z- waiting for 1730861745.38, time is 1730861742.56 2024-11-06T02:55:42Z- AHRS_M2 msg: 1730861742.96 epoch sec; seqNo:886 2024-11-06T02:55:42Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.26 psi 2024-11-06T02:55:42Z- handleDepth()- Depth_Keller msg: 1730861742.96 epoch sec; seqNo:950; depth 49.95 2024-11-06T02:55:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861742.97 epoch sec; seqNo:950; 4061600.61 north; 594340.02 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T02:55:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861741.38 2024-11-06T02:55:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:43Z- waiting for 1730861745.38, time is 1730861742.97 2024-11-06T02:55:43Z- AHRS_M2 msg: 1730861743.35 epoch sec; seqNo:887 2024-11-06T02:55:43Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.26 psi 2024-11-06T02:55:43Z- handleDepth()- Depth_Keller msg: 1730861743.37 epoch sec; seqNo:951; depth 49.95 2024-11-06T02:55:43Z- handleDVL() - DVL msg epochMillisec: 1730861743.38 2024-11-06T02:55:43Z- handleDvl() - RDI_Pathfinder msg: 1730861743.38 sec; seqNo:306; #beams:4 2024-11-06T02:55:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.88 , 38.95 , 43.23 , 41.19 2024-11-06T02:55:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.02 , 0.05 2024-11-06T02:55:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861743.40 epoch sec; seqNo:951; 4061600.99 north; 594340.02 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T02:55:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861741.38 2024-11-06T02:55:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:43Z- waiting for 1730861745.38, time is 1730861743.40 2024-11-06T02:55:43Z- AHRS_M2 msg: 1730861743.76 epoch sec; seqNo:888 2024-11-06T02:55:43Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.26 psi 2024-11-06T02:55:43Z- handleDepth()- Depth_Keller msg: 1730861743.76 epoch sec; seqNo:952; depth 49.95 2024-11-06T02:55:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861743.78 epoch sec; seqNo:952; 4061600.97 north; 594338.80 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T02:55:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861741.38 2024-11-06T02:55:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:43Z- waiting for 1730861745.38, time is 1730861743.78 2024-11-06T02:55:44Z- AHRS_M2 msg: 1730861744.16 epoch sec; seqNo:889 2024-11-06T02:55:44Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.26 psi 2024-11-06T02:55:44Z- handleDepth()- Depth_Keller msg: 1730861744.17 epoch sec; seqNo:953; depth 49.94 2024-11-06T02:55:44Z- handleDVL() - DVL msg epochMillisec: 1730861744.17 2024-11-06T02:55:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:55:44Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:55:44Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:55:44Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:55:44Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:55:44Z- handleDvl() - RDI_Pathfinder msg: 1730861744.17 sec; seqNo:307; #beams:0 2024-11-06T02:55:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:55:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.02 , 0.05 2024-11-06T02:55:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861744.18 epoch sec; seqNo:953; 4061601.35 north; 594338.79 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T02:55:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861741.38 2024-11-06T02:55:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:44Z- waiting for 1730861745.38, time is 1730861744.18 2024-11-06T02:55:44Z- AHRS_M2 msg: 1730861744.57 epoch sec; seqNo:890 2024-11-06T02:55:44Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.26 psi 2024-11-06T02:55:44Z- handleDepth()- Depth_Keller msg: 1730861744.57 epoch sec; seqNo:954; depth 49.93 2024-11-06T02:55:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861744.58 epoch sec; seqNo:954; 4061601.35 north; 594338.79 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T02:55:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861741.38 2024-11-06T02:55:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:44Z- waiting for 1730861745.38, time is 1730861744.58 2024-11-06T02:55:44Z- AHRS_M2 msg: 1730861744.97 epoch sec; seqNo:891 2024-11-06T02:55:44Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.26 psi 2024-11-06T02:55:44Z- handleDepth()- Depth_Keller msg: 1730861744.97 epoch sec; seqNo:955; depth 49.93 2024-11-06T02:55:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861744.99 epoch sec; seqNo:955; 4061601.73 north; 594338.79 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T02:55:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861741.38 2024-11-06T02:55:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:45Z- waiting for 1730861745.38, time is 1730861744.99 2024-11-06T02:55:45Z- AHRS_M2 msg: 1730861745.38 epoch sec; seqNo:892 2024-11-06T02:55:45Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 5.26 psi 2024-11-06T02:55:45Z- handleDepth()- Depth_Keller msg: 1730861745.39 epoch sec; seqNo:956; depth 49.93 2024-11-06T02:55:45Z- handleDVL() - DVL msg epochMillisec: 1730861745.41 2024-11-06T02:55:45Z- handleDvl() - RDI_Pathfinder msg: 1730861745.41 sec; seqNo:308; #beams:4 2024-11-06T02:55:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 39.15 , 43.23 , 41.19 2024-11-06T02:55:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.04 2024-11-06T02:55:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861745.43 epoch sec; seqNo:956; 4061601.72 north; 594337.57 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T02:55:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861741.38 2024-11-06T02:55:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:45Z- waiting for 1730861745.38, time is 1730861745.43 2024-11-06T02:55:45Z- LcmTrn::updateTrn() >>>> pose time 1730861745.43, meas time 1730861745.41 <<<< 2024-11-06T02:55:45Z- LcmTrn::updateTrn() equating pose 1730861745.41 and meas times 1730861745.41 2024-11-06T02:55:45Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861745.41 2024-11-06T02:55:45Z- TerrainNav::motionUpdate() - currEstimate = (4061602, 594337.57, 49.93) 2024-11-06T02:55:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:55:45Z- TerrainNav:: SubcloudNIS is 0.68, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:55:45Z- TNavPF::Projecting Measurements 2024-11-06T02:55:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:55:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:55:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:55:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:55:45Z- TNavFilter::4 good beams. 2024-11-06T02:55:45Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:55:45Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:55:45Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:55:45Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:55:45Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:55:45Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:55:45Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:55:45Z- TerrainMapOctree: (vehN, vehE ) = (4061601.72, 594337.57). 2024-11-06T02:55:45Z- TerrainMapOctree: Min Distance = 172.21. 2024-11-06T02:55:45Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:55:45Z- Forcing Subcloud Comparison 2024-11-06T02:55:45Z- Weighting particles with subcloud comparison 2024-11-06T02:55:45Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:45Z- meanExpectedMeasDiff: -2.714097 delta_rms_squared: 0.958059 alpha: 0.979551 2024-11-06T02:55:45Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:45Z- meanExpectedMeasDiff: 2.415123 delta_rms_squared: 0.710552 alpha: 0.972623 2024-11-06T02:55:45Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:45Z- meanExpectedMeasDiff: -0.645642 delta_rms_squared: 0.801810 alpha: 0.975663 2024-11-06T02:55:45Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:45Z- meanExpectedMeasDiff: -0.275664 delta_rms_squared: 0.825115 alpha: 0.976335 2024-11-06T02:55:45Z- Subcloud NIS: 0.680655 2024-11-06T02:55:45Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:55:45Z- TNavPF:: sumWeights = 0.182575 2024-11-06T02:55:45Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:55:45Z- TNavPF::First Term of Map Covariance Matrix is 0.92 2024-11-06T02:55:45Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.02 2024-11-06T02:55:45Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861745.41 sec, ping # 0. 2024-11-06T02:55:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:55:45Z- variances([0], [2], and psi_berg) = 0.246161 0.210216 0.000000 2024-11-06T02:55:45Z- publishEstimatesDecoder() - at 1730861745.41 sec 2024-11-06T02:55:45Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:55:45Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061633.73 594320.37 54.58 2024-11-06T02:55:45Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061634.21 594321.57 54.03 lat/lon: 36.70 -121.94 2024-11-06T02:55:45Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.21 0.38 0.00 2024-11-06T02:55:45Z- AHRS_M2 msg: 1730861745.78 epoch sec; seqNo:893 2024-11-06T02:55:45Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 5.26 psi 2024-11-06T02:55:45Z- handleDepth()- Depth_Keller msg: 1730861745.79 epoch sec; seqNo:957; depth 49.91 2024-11-06T02:55:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861745.80 epoch sec; seqNo:957; 4061601.72 north; 594337.57 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T02:55:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861745.41 2024-11-06T02:55:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:45Z- waiting for 1730861749.41, time is 1730861745.80 2024-11-06T02:55:46Z- AHRS_M2 msg: 1730861746.18 epoch sec; seqNo:894 2024-11-06T02:55:46Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 5.26 psi 2024-11-06T02:55:46Z- handleDepth()- Depth_Keller msg: 1730861746.18 epoch sec; seqNo:958; depth 49.91 2024-11-06T02:55:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861746.20 epoch sec; seqNo:958; 4061602.10 north; 594337.57 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T02:55:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861745.41 2024-11-06T02:55:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:46Z- waiting for 1730861749.41, time is 1730861746.20 2024-11-06T02:55:46Z- AHRS_M2 msg: 1730861746.58 epoch sec; seqNo:895 2024-11-06T02:55:46Z- AHRS_M2 msg: 0.12 phi; -0.00 theta; 5.26 psi 2024-11-06T02:55:46Z- handleDepth()- Depth_Keller msg: 1730861746.59 epoch sec; seqNo:959; depth 49.90 2024-11-06T02:55:46Z- handleDVL() - DVL msg epochMillisec: 1730861746.60 2024-11-06T02:55:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:55:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:55:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:55:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:55:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:55:46Z- handleDvl() - RDI_Pathfinder msg: 1730861746.60 sec; seqNo:309; #beams:0 2024-11-06T02:55:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:55:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.04 2024-11-06T02:55:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861746.62 epoch sec; seqNo:959; 4061602.10 north; 594337.57 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T02:55:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861745.41 2024-11-06T02:55:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:46Z- waiting for 1730861749.41, time is 1730861746.62 2024-11-06T02:55:46Z- AHRS_M2 msg: 1730861746.99 epoch sec; seqNo:896 2024-11-06T02:55:46Z- AHRS_M2 msg: 0.12 phi; -0.00 theta; 5.26 psi 2024-11-06T02:55:46Z- handleDepth()- Depth_Keller msg: 1730861746.99 epoch sec; seqNo:960; depth 49.89 2024-11-06T02:55:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861747.00 epoch sec; seqNo:960; 4061602.46 north; 594336.34 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T02:55:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861745.41 2024-11-06T02:55:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:47Z- waiting for 1730861749.41, time is 1730861747.00 2024-11-06T02:55:47Z- AHRS_M2 msg: 1730861747.39 epoch sec; seqNo:897 2024-11-06T02:55:47Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 5.26 psi 2024-11-06T02:55:47Z- handleDepth()- Depth_Keller msg: 1730861747.41 epoch sec; seqNo:961; depth 49.88 2024-11-06T02:55:47Z- handleDVL() - DVL msg epochMillisec: 1730861747.43 2024-11-06T02:55:47Z- handleDvl() - RDI_Pathfinder msg: 1730861747.43 sec; seqNo:310; #beams:4 2024-11-06T02:55:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.88 , 39.15 , 43.02 , 41.39 2024-11-06T02:55:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.03 2024-11-06T02:55:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861747.44 epoch sec; seqNo:961; 4061602.46 north; 594336.34 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T02:55:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861745.41 2024-11-06T02:55:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:47Z- waiting for 1730861749.41, time is 1730861747.44 2024-11-06T02:55:47Z- AHRS_M2 msg: 1730861747.80 epoch sec; seqNo:898 2024-11-06T02:55:47Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 5.26 psi 2024-11-06T02:55:47Z- handleDepth()- Depth_Keller msg: 1730861747.80 epoch sec; seqNo:962; depth 49.87 2024-11-06T02:55:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861747.82 epoch sec; seqNo:962; 4061602.84 north; 594336.34 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T02:55:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861745.41 2024-11-06T02:55:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:47Z- waiting for 1730861749.41, time is 1730861747.82 2024-11-06T02:55:48Z- AHRS_M2 msg: 1730861748.20 epoch sec; seqNo:899 2024-11-06T02:55:48Z- AHRS_M2 msg: 0.11 phi; -0.01 theta; 5.26 psi 2024-11-06T02:55:48Z- handleDepth()- Depth_Keller msg: 1730861748.21 epoch sec; seqNo:963; depth 49.88 2024-11-06T02:55:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861748.22 epoch sec; seqNo:963; 4061602.84 north; 594336.34 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T02:55:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861745.41 2024-11-06T02:55:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:48Z- waiting for 1730861749.41, time is 1730861748.22 2024-11-06T02:55:48Z- AHRS_M2 msg: 1730861748.60 epoch sec; seqNo:900 2024-11-06T02:55:48Z- AHRS_M2 msg: 0.11 phi; -0.01 theta; 5.26 psi 2024-11-06T02:55:48Z- handleDepth()- Depth_Keller msg: 1730861748.61 epoch sec; seqNo:964; depth 49.86 2024-11-06T02:55:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861748.62 epoch sec; seqNo:964; 4061603.21 north; 594335.11 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T02:55:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861745.41 2024-11-06T02:55:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:48Z- waiting for 1730861749.41, time is 1730861748.62 2024-11-06T02:55:49Z- AHRS_M2 msg: 1730861749.01 epoch sec; seqNo:901 2024-11-06T02:55:49Z- AHRS_M2 msg: 0.10 phi; -0.01 theta; 5.26 psi 2024-11-06T02:55:49Z- handleDepth()- Depth_Keller msg: 1730861749.03 epoch sec; seqNo:965; depth 49.85 2024-11-06T02:55:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861749.04 epoch sec; seqNo:965; 4061603.21 north; 594335.11 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T02:55:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861745.41 2024-11-06T02:55:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:49Z- waiting for 1730861749.41, time is 1730861749.04 2024-11-06T02:55:49Z- AHRS_M2 msg: 1730861749.41 epoch sec; seqNo:902 2024-11-06T02:55:49Z- AHRS_M2 msg: 0.10 phi; -0.01 theta; 5.25 psi 2024-11-06T02:55:49Z- handleDepth()- Depth_Keller msg: 1730861749.42 epoch sec; seqNo:966; depth 49.84 2024-11-06T02:55:49Z- handleDVL() - DVL msg epochMillisec: 1730861749.46 2024-11-06T02:55:49Z- handleDvl() - RDI_Pathfinder msg: 1730861749.46 sec; seqNo:311; #beams:4 2024-11-06T02:55:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.27 , 39.15 , 42.62 , 41.60 2024-11-06T02:55:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , -0.07 2024-11-06T02:55:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861749.47 epoch sec; seqNo:966; 4061603.21 north; 594335.11 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T02:55:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861745.41 2024-11-06T02:55:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:49Z- waiting for 1730861749.41, time is 1730861749.47 2024-11-06T02:55:49Z- LcmTrn::updateTrn() >>>> pose time 1730861749.47, meas time 1730861749.46 <<<< 2024-11-06T02:55:49Z- LcmTrn::updateTrn() equating pose 1730861749.46 and meas times 1730861749.46 2024-11-06T02:55:49Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861749.46 2024-11-06T02:55:49Z- TerrainNav::motionUpdate() - currEstimate = (4061603, 594335.11, 49.84) 2024-11-06T02:55:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:55:49Z- TerrainNav:: SubcloudNIS is 0.68, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:55:49Z- TNavPF::Projecting Measurements 2024-11-06T02:55:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:55:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:55:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:55:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:55:49Z- TNavFilter::4 good beams. 2024-11-06T02:55:49Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:55:49Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:55:49Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:55:49Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:55:49Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:55:49Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:55:49Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:55:49Z- TerrainMapOctree: (vehN, vehE ) = (4061603.21, 594335.11). 2024-11-06T02:55:49Z- TerrainMapOctree: Min Distance = 169.68. 2024-11-06T02:55:49Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:55:49Z- Forcing Subcloud Comparison 2024-11-06T02:55:49Z- Weighting particles with subcloud comparison 2024-11-06T02:55:49Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:49Z- meanExpectedMeasDiff: -2.395306 delta_rms_squared: 0.934693 alpha: 0.979051 2024-11-06T02:55:49Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:49Z- meanExpectedMeasDiff: 2.111976 delta_rms_squared: 0.723003 alpha: 0.973082 2024-11-06T02:55:49Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:49Z- meanExpectedMeasDiff: 0.138820 delta_rms_squared: 0.961151 alpha: 0.979616 2024-11-06T02:55:49Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:49Z- meanExpectedMeasDiff: -0.097355 delta_rms_squared: 0.824322 alpha: 0.976312 2024-11-06T02:55:49Z- Subcloud NIS: 0.670477 2024-11-06T02:55:49Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:55:49Z- TNavPF:: sumWeights = 0.255453 2024-11-06T02:55:49Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:55:49Z- TNavPF::First Term of Map Covariance Matrix is 0.94 2024-11-06T02:55:49Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T02:55:49Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861749.46 sec, ping # 0. 2024-11-06T02:55:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:55:49Z- variances([0], [2], and psi_berg) = 0.247300 0.211293 0.000000 2024-11-06T02:55:49Z- publishEstimatesDecoder() - at 1730861749.46 sec 2024-11-06T02:55:49Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:55:49Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061635.22 594317.88 54.78 2024-11-06T02:55:49Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061635.71 594319.12 53.96 lat/lon: 36.70 -121.94 2024-11-06T02:55:49Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.21 0.36 0.00 2024-11-06T02:55:49Z- handleDepth()- Depth_Keller msg: 1730861749.84 epoch sec; seqNo:967; depth 49.84 2024-11-06T02:55:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861749.87 epoch sec; seqNo:967; 4061603.59 north; 594335.11 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T02:55:49Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:55:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861749.46 2024-11-06T02:55:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:49Z- waiting for 1730861753.46, time is 1730861749.87 2024-11-06T02:55:50Z- AHRS_M2 msg: 1730861750.22 epoch sec; seqNo:903 2024-11-06T02:55:50Z- AHRS_M2 msg: 0.11 phi; -0.01 theta; 5.25 psi 2024-11-06T02:55:50Z- handleDepth()- Depth_Keller msg: 1730861750.23 epoch sec; seqNo:968; depth 49.83 2024-11-06T02:55:50Z- handleDVL() - DVL msg epochMillisec: 1730861750.23 2024-11-06T02:55:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:55:50Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:55:50Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:55:50Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:55:50Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:55:50Z- handleDvl() - RDI_Pathfinder msg: 1730861750.23 sec; seqNo:312; #beams:0 2024-11-06T02:55:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:55:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , -0.07 2024-11-06T02:55:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861750.24 epoch sec; seqNo:968; 4061603.57 north; 594333.89 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T02:55:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861749.46 2024-11-06T02:55:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:50Z- waiting for 1730861753.46, time is 1730861750.24 2024-11-06T02:55:50Z- AHRS_M2 msg: 1730861750.62 epoch sec; seqNo:904 2024-11-06T02:55:50Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 5.25 psi 2024-11-06T02:55:50Z- handleDepth()- Depth_Keller msg: 1730861750.63 epoch sec; seqNo:969; depth 49.85 2024-11-06T02:55:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861750.65 epoch sec; seqNo:969; 4061603.95 north; 594333.88 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T02:55:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861749.46 2024-11-06T02:55:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:50Z- waiting for 1730861753.46, time is 1730861750.65 2024-11-06T02:55:51Z- AHRS_M2 msg: 1730861751.03 epoch sec; seqNo:905 2024-11-06T02:55:51Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 5.25 psi 2024-11-06T02:55:51Z- handleDepth()- Depth_Keller msg: 1730861751.03 epoch sec; seqNo:970; depth 49.84 2024-11-06T02:55:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861751.04 epoch sec; seqNo:970; 4061603.95 north; 594333.88 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T02:55:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861749.46 2024-11-06T02:55:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:51Z- waiting for 1730861753.46, time is 1730861751.04 2024-11-06T02:55:51Z- AHRS_M2 msg: 1730861751.44 epoch sec; seqNo:906 2024-11-06T02:55:51Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 5.25 psi 2024-11-06T02:55:51Z- handleDepth()- Depth_Keller msg: 1730861751.44 epoch sec; seqNo:971; depth 49.84 2024-11-06T02:55:51Z- handleDVL() - DVL msg epochMillisec: 1730861751.46 2024-11-06T02:55:51Z- handleDvl() - RDI_Pathfinder msg: 1730861751.46 sec; seqNo:313; #beams:4 2024-11-06T02:55:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.47 , 39.15 , 42.61 , 41.80 2024-11-06T02:55:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.08 , -0.04 2024-11-06T02:55:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861751.49 epoch sec; seqNo:971; 4061604.32 north; 594332.66 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T02:55:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861749.46 2024-11-06T02:55:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:51Z- waiting for 1730861753.46, time is 1730861751.49 2024-11-06T02:55:51Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T02:55:51Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T02:55:51Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T02:55:51Z- AHRS_M2 msg: 1730861751.83 epoch sec; seqNo:907 2024-11-06T02:55:51Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 5.25 psi 2024-11-06T02:55:51Z- handleDepth()- Depth_Keller msg: 1730861751.86 epoch sec; seqNo:972; depth 49.84 2024-11-06T02:55:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861751.88 epoch sec; seqNo:972; 4061604.32 north; 594332.66 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T02:55:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861749.46 2024-11-06T02:55:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:52Z- waiting for 1730861753.46, time is 1730861751.88 2024-11-06T02:55:52Z- AHRS_M2 msg: 1730861752.24 epoch sec; seqNo:908 2024-11-06T02:55:52Z- AHRS_M2 msg: 0.11 phi; -0.02 theta; 5.24 psi 2024-11-06T02:55:52Z- handleDepth()- Depth_Keller msg: 1730861752.25 epoch sec; seqNo:973; depth 49.85 2024-11-06T02:55:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861752.27 epoch sec; seqNo:973; 4061604.70 north; 594332.65 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T02:55:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861749.46 2024-11-06T02:55:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:52Z- waiting for 1730861753.46, time is 1730861752.27 2024-11-06T02:55:52Z- AHRS_M2 msg: 1730861752.65 epoch sec; seqNo:909 2024-11-06T02:55:52Z- AHRS_M2 msg: 0.11 phi; -0.02 theta; 5.25 psi 2024-11-06T02:55:52Z- handleDepth()- Depth_Keller msg: 1730861752.65 epoch sec; seqNo:974; depth 49.86 2024-11-06T02:55:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861752.67 epoch sec; seqNo:974; 4061604.70 north; 594332.65 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T02:55:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861749.46 2024-11-06T02:55:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:52Z- waiting for 1730861753.46, time is 1730861752.67 2024-11-06T02:55:53Z- AHRS_M2 msg: 1730861753.05 epoch sec; seqNo:910 2024-11-06T02:55:53Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 5.25 psi 2024-11-06T02:55:53Z- handleDepth()- Depth_Keller msg: 1730861753.05 epoch sec; seqNo:975; depth 49.87 2024-11-06T02:55:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861753.06 epoch sec; seqNo:975; 4061605.06 north; 594331.43 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T02:55:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861749.46 2024-11-06T02:55:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:53Z- waiting for 1730861753.46, time is 1730861753.06 2024-11-06T02:55:53Z- AHRS_M2 msg: 1730861753.45 epoch sec; seqNo:911 2024-11-06T02:55:53Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.25 psi 2024-11-06T02:55:53Z- handleDepth()- Depth_Keller msg: 1730861753.46 epoch sec; seqNo:976; depth 49.88 2024-11-06T02:55:53Z- handleDVL() - DVL msg epochMillisec: 1730861753.48 2024-11-06T02:55:53Z- handleDvl() - RDI_Pathfinder msg: 1730861753.48 sec; seqNo:314; #beams:4 2024-11-06T02:55:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.26 , 39.15 , 42.41 , 41.80 2024-11-06T02:55:53Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.05 , 0.03 2024-11-06T02:55:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861753.50 epoch sec; seqNo:976; 4061605.06 north; 594331.43 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T02:55:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861749.46 2024-11-06T02:55:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:53Z- waiting for 1730861753.46, time is 1730861753.50 2024-11-06T02:55:53Z- LcmTrn::updateTrn() >>>> pose time 1730861753.50, meas time 1730861753.48 <<<< 2024-11-06T02:55:53Z- LcmTrn::updateTrn() equating pose 1730861753.48 and meas times 1730861753.48 2024-11-06T02:55:53Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861753.48 2024-11-06T02:55:53Z- TerrainNav::motionUpdate() - currEstimate = (4061605, 594331.43, 49.88) 2024-11-06T02:55:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:55:53Z- TerrainNav:: SubcloudNIS is 0.67, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:55:53Z- TNavPF::Projecting Measurements 2024-11-06T02:55:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:55:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:55:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:55:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:55:53Z- TNavFilter::4 good beams. 2024-11-06T02:55:53Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:55:53Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:55:53Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:55:53Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:55:53Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:55:53Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:55:53Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:55:53Z- TerrainMapOctree: (vehN, vehE ) = (4061605.06, 594331.43). 2024-11-06T02:55:53Z- TerrainMapOctree: Min Distance = 166.25. 2024-11-06T02:55:53Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:55:53Z- Forcing Subcloud Comparison 2024-11-06T02:55:53Z- Weighting particles with subcloud comparison 2024-11-06T02:55:53Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:53Z- meanExpectedMeasDiff: -2.582380 delta_rms_squared: 1.553656 alpha: 0.987291 2024-11-06T02:55:53Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:53Z- meanExpectedMeasDiff: 1.636000 delta_rms_squared: 0.646869 alpha: 0.970009 2024-11-06T02:55:53Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:53Z- meanExpectedMeasDiff: -0.945453 delta_rms_squared: 0.982308 alpha: 0.980046 2024-11-06T02:55:53Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:53Z- meanExpectedMeasDiff: -1.363233 delta_rms_squared: 0.840624 alpha: 0.976761 2024-11-06T02:55:53Z- Subcloud NIS: 0.657380 2024-11-06T02:55:53Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:55:53Z- TNavPF:: sumWeights = 0.247842 2024-11-06T02:55:53Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:55:53Z- TNavPF::First Term of Map Covariance Matrix is 0.78 2024-11-06T02:55:53Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T02:55:53Z- TerrainNav::Resampling particle filter... 2024-11-06T02:55:53Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861753.48 sec, ping # 0. 2024-11-06T02:55:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:55:53Z- variances([0], [2], and psi_berg) = 0.237863 0.195554 0.000000 2024-11-06T02:55:53Z- publishEstimatesDecoder() - at 1730861753.48 sec 2024-11-06T02:55:53Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:55:53Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061637.11 594316.01 53.99 2024-11-06T02:55:53Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061637.57 594315.41 54.31 lat/lon: 36.70 -121.94 2024-11-06T02:55:53Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.20 0.35 0.00 2024-11-06T02:55:53Z- handleDepth()- Depth_Keller msg: 1730861753.87 epoch sec; seqNo:977; depth 49.89 2024-11-06T02:55:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861753.89 epoch sec; seqNo:977; 4061605.44 north; 594331.43 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T02:55:54Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:55:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861753.48 2024-11-06T02:55:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:54Z- waiting for 1730861757.48, time is 1730861753.89 2024-11-06T02:55:54Z- AHRS_M2 msg: 1730861754.26 epoch sec; seqNo:912 2024-11-06T02:55:54Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 5.26 psi 2024-11-06T02:55:54Z- handleDepth()- Depth_Keller msg: 1730861754.28 epoch sec; seqNo:978; depth 49.90 2024-11-06T02:55:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861754.29 epoch sec; seqNo:978; 4061605.43 north; 594330.21 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T02:55:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861753.48 2024-11-06T02:55:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:54Z- waiting for 1730861757.48, time is 1730861754.29 2024-11-06T02:55:54Z- AHRS_M2 msg: 1730861754.67 epoch sec; seqNo:913 2024-11-06T02:55:54Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 5.26 psi 2024-11-06T02:55:54Z- handleDepth()- Depth_Keller msg: 1730861754.67 epoch sec; seqNo:979; depth 49.92 2024-11-06T02:55:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861754.69 epoch sec; seqNo:979; 4061605.81 north; 594330.20 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T02:55:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861753.48 2024-11-06T02:55:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:54Z- waiting for 1730861757.48, time is 1730861754.69 2024-11-06T02:55:55Z- handleDepth()- Depth_Keller msg: 1730861755.08 epoch sec; seqNo:980; depth 49.93 2024-11-06T02:55:55Z- handleDVL() - DVL msg epochMillisec: 1730861755.09 2024-11-06T02:55:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:55:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:55:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:55:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:55:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:55:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:55:55Z- handleDvl() - RDI_Pathfinder msg: 1730861755.09 sec; seqNo:315; #beams:0 2024-11-06T02:55:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:55:55Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.05 , 0.03 2024-11-06T02:55:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861755.10 epoch sec; seqNo:980; 4061605.81 north; 594330.20 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T02:55:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861753.48 2024-11-06T02:55:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:55Z- waiting for 1730861757.48, time is 1730861755.10 2024-11-06T02:55:55Z- AHRS_M2 msg: 1730861755.48 epoch sec; seqNo:914 2024-11-06T02:55:55Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 5.25 psi 2024-11-06T02:55:55Z- handleDepth()- Depth_Keller msg: 1730861755.49 epoch sec; seqNo:981; depth 49.93 2024-11-06T02:55:55Z- handleDVL() - DVL msg epochMillisec: 1730861755.52 2024-11-06T02:55:55Z- handleDvl() - RDI_Pathfinder msg: 1730861755.52 sec; seqNo:316; #beams:4 2024-11-06T02:55:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 38.94 , 42.61 , 42.41 2024-11-06T02:55:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.03 , 0.04 2024-11-06T02:55:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861755.54 epoch sec; seqNo:981; 4061606.17 north; 594328.98 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T02:55:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861753.48 2024-11-06T02:55:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:55Z- waiting for 1730861757.48, time is 1730861755.54 2024-11-06T02:55:55Z- AHRS_M2 msg: 1730861755.88 epoch sec; seqNo:915 2024-11-06T02:55:55Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 5.25 psi 2024-11-06T02:55:55Z- handleDepth()- Depth_Keller msg: 1730861755.88 epoch sec; seqNo:982; depth 49.96 2024-11-06T02:55:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861755.90 epoch sec; seqNo:982; 4061606.17 north; 594328.98 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T02:55:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861753.48 2024-11-06T02:55:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:56Z- waiting for 1730861757.48, time is 1730861755.90 2024-11-06T02:55:56Z- AHRS_M2 msg: 1730861756.28 epoch sec; seqNo:916 2024-11-06T02:55:56Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 5.25 psi 2024-11-06T02:55:56Z- handleDepth()- Depth_Keller msg: 1730861756.28 epoch sec; seqNo:983; depth 49.96 2024-11-06T02:55:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861756.30 epoch sec; seqNo:983; 4061606.55 north; 594328.97 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T02:55:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861753.48 2024-11-06T02:55:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:56Z- waiting for 1730861757.48, time is 1730861756.30 2024-11-06T02:55:56Z- AHRS_M2 msg: 1730861756.68 epoch sec; seqNo:917 2024-11-06T02:55:56Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.25 psi 2024-11-06T02:55:56Z- handleDepth()- Depth_Keller msg: 1730861756.69 epoch sec; seqNo:984; depth 49.99 2024-11-06T02:55:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861756.70 epoch sec; seqNo:984; 4061606.55 north; 594328.97 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T02:55:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861753.48 2024-11-06T02:55:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:56Z- waiting for 1730861757.48, time is 1730861756.70 2024-11-06T02:55:57Z- AHRS_M2 msg: 1730861757.09 epoch sec; seqNo:918 2024-11-06T02:55:57Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.25 psi 2024-11-06T02:55:57Z- handleDepth()- Depth_Keller msg: 1730861757.11 epoch sec; seqNo:985; depth 50.00 2024-11-06T02:55:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861757.12 epoch sec; seqNo:985; 4061606.91 north; 594327.75 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T02:55:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861753.48 2024-11-06T02:55:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:57Z- waiting for 1730861757.48, time is 1730861757.12 2024-11-06T02:55:57Z- AHRS_M2 msg: 1730861757.49 epoch sec; seqNo:919 2024-11-06T02:55:57Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.26 psi 2024-11-06T02:55:57Z- handleDepth()- Depth_Keller msg: 1730861757.51 epoch sec; seqNo:986; depth 50.00 2024-11-06T02:55:57Z- handleDVL() - DVL msg epochMillisec: 1730861757.53 2024-11-06T02:55:57Z- handleDvl() - RDI_Pathfinder msg: 1730861757.53 sec; seqNo:317; #beams:4 2024-11-06T02:55:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.49 , 39.15 , 42.41 , 42.21 2024-11-06T02:55:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.01 2024-11-06T02:55:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861757.55 epoch sec; seqNo:986; 4061606.91 north; 594327.75 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T02:55:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861753.48 2024-11-06T02:55:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:57Z- waiting for 1730861757.48, time is 1730861757.55 2024-11-06T02:55:57Z- LcmTrn::updateTrn() >>>> pose time 1730861757.55, meas time 1730861757.53 <<<< 2024-11-06T02:55:57Z- LcmTrn::updateTrn() equating pose 1730861757.53 and meas times 1730861757.53 2024-11-06T02:55:57Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861757.53 2024-11-06T02:55:57Z- TerrainNav::motionUpdate() - currEstimate = (4061607, 594327.75, 50.00) 2024-11-06T02:55:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:55:57Z- TerrainNav:: SubcloudNIS is 0.66, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:55:57Z- TNavPF::Projecting Measurements 2024-11-06T02:55:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:55:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:55:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:55:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:55:57Z- TNavFilter::4 good beams. 2024-11-06T02:55:57Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:55:57Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:55:57Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:55:57Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:55:57Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:55:57Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:55:57Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:55:57Z- TerrainMapOctree: (vehN, vehE ) = (4061606.91, 594327.75). 2024-11-06T02:55:57Z- TerrainMapOctree: Min Distance = 162.84. 2024-11-06T02:55:57Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:55:57Z- Forcing Subcloud Comparison 2024-11-06T02:55:57Z- Weighting particles with subcloud comparison 2024-11-06T02:55:57Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:57Z- meanExpectedMeasDiff: -0.427171 delta_rms_squared: 0.923003 alpha: 0.978791 2024-11-06T02:55:57Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:57Z- meanExpectedMeasDiff: 2.029237 delta_rms_squared: 0.675059 alpha: 0.971225 2024-11-06T02:55:57Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:57Z- meanExpectedMeasDiff: -0.316381 delta_rms_squared: 0.776713 alpha: 0.974897 2024-11-06T02:55:57Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:55:57Z- meanExpectedMeasDiff: -0.678589 delta_rms_squared: 0.826924 alpha: 0.976385 2024-11-06T02:55:57Z- Subcloud NIS: 0.615330 2024-11-06T02:55:57Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:55:57Z- TNavPF:: sumWeights = 0.443796 2024-11-06T02:55:57Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:55:57Z- TNavPF::First Term of Map Covariance Matrix is 0.28 2024-11-06T02:55:57Z- TNavPF::Calculated NIS Value : 0.30 numBeams normalized NIS: 0.07 2024-11-06T02:55:57Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861757.53 sec, ping # 0. 2024-11-06T02:55:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:55:57Z- variances([0], [2], and psi_berg) = 0.240535 0.200430 0.000000 2024-11-06T02:55:57Z- publishEstimatesDecoder() - at 1730861757.53 sec 2024-11-06T02:55:57Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:55:57Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061639.03 594310.57 54.64 2024-11-06T02:55:57Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061639.43 594311.72 54.39 lat/lon: 36.70 -121.94 2024-11-06T02:55:57Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.20 0.36 0.00 2024-11-06T02:55:57Z- AHRS_M2 msg: 1730861757.90 epoch sec; seqNo:920 2024-11-06T02:55:57Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 5.26 psi 2024-11-06T02:55:57Z- handleDepth()- Depth_Keller msg: 1730861757.91 epoch sec; seqNo:987; depth 50.00 2024-11-06T02:55:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861757.93 epoch sec; seqNo:987; 4061606.91 north; 594327.75 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T02:55:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861757.53 2024-11-06T02:55:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:58Z- waiting for 1730861761.53, time is 1730861757.93 2024-11-06T02:55:58Z- AHRS_M2 msg: 1730861758.30 epoch sec; seqNo:921 2024-11-06T02:55:58Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 5.26 psi 2024-11-06T02:55:58Z- handleDepth()- Depth_Keller msg: 1730861758.31 epoch sec; seqNo:988; depth 50.02 2024-11-06T02:55:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861758.33 epoch sec; seqNo:988; 4061607.29 north; 594327.74 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T02:55:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861757.53 2024-11-06T02:55:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:58Z- waiting for 1730861761.53, time is 1730861758.33 2024-11-06T02:55:58Z- AHRS_M2 msg: 1730861758.70 epoch sec; seqNo:922 2024-11-06T02:55:58Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.26 psi 2024-11-06T02:55:58Z- handleDepth()- Depth_Keller msg: 1730861758.72 epoch sec; seqNo:989; depth 50.02 2024-11-06T02:55:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861758.72 epoch sec; seqNo:989; 4061607.28 north; 594326.52 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T02:55:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861757.53 2024-11-06T02:55:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:58Z- waiting for 1730861761.53, time is 1730861758.72 2024-11-06T02:55:59Z- AHRS_M2 msg: 1730861759.11 epoch sec; seqNo:923 2024-11-06T02:55:59Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.26 psi 2024-11-06T02:55:59Z- handleDepth()- Depth_Keller msg: 1730861759.12 epoch sec; seqNo:990; depth 50.05 2024-11-06T02:55:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861759.13 epoch sec; seqNo:990; 4061607.66 north; 594326.52 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T02:55:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:55:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861757.53 2024-11-06T02:55:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:59Z- waiting for 1730861761.53, time is 1730861759.13 2024-11-06T02:55:59Z- AHRS_M2 msg: 1730861759.51 epoch sec; seqNo:924 2024-11-06T02:55:59Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.26 psi 2024-11-06T02:55:59Z- handleDepth()- Depth_Keller msg: 1730861759.53 epoch sec; seqNo:991; depth 50.05 2024-11-06T02:55:59Z- handleDVL() - DVL msg epochMillisec: 1730861759.55 2024-11-06T02:55:59Z- handleDvl() - RDI_Pathfinder msg: 1730861759.55 sec; seqNo:318; #beams:4 2024-11-06T02:55:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 39.35 , 42.21 , 42.41 2024-11-06T02:55:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , -0.04 2024-11-06T02:55:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861759.57 epoch sec; seqNo:991; 4061607.66 north; 594326.52 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T02:55:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:55:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861757.53 2024-11-06T02:55:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:55:59Z- waiting for 1730861761.53, time is 1730861759.57 2024-11-06T02:55:59Z- AHRS_M2 msg: 1730861759.91 epoch sec; seqNo:925 2024-11-06T02:55:59Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.27 psi 2024-11-06T02:55:59Z- handleDepth()- Depth_Keller msg: 1730861759.92 epoch sec; seqNo:992; depth 50.04 2024-11-06T02:55:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861759.93 epoch sec; seqNo:992; 4061608.04 north; 594326.52 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T02:56:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861757.53 2024-11-06T02:56:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:00Z- waiting for 1730861761.53, time is 1730861759.93 2024-11-06T02:56:00Z- AHRS_M2 msg: 1730861760.32 epoch sec; seqNo:926 2024-11-06T02:56:00Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.27 psi 2024-11-06T02:56:00Z- handleDepth()- Depth_Keller msg: 1730861760.33 epoch sec; seqNo:993; depth 50.06 2024-11-06T02:56:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861760.34 epoch sec; seqNo:993; 4061608.02 north; 594325.29 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T02:56:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861757.53 2024-11-06T02:56:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:00Z- waiting for 1730861761.53, time is 1730861760.34 2024-11-06T02:56:00Z- AHRS_M2 msg: 1730861760.72 epoch sec; seqNo:927 2024-11-06T02:56:00Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.27 psi 2024-11-06T02:56:00Z- handleDepth()- Depth_Keller msg: 1730861760.73 epoch sec; seqNo:994; depth 50.06 2024-11-06T02:56:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861760.74 epoch sec; seqNo:994; 4061608.40 north; 594325.29 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T02:56:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861757.53 2024-11-06T02:56:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:00Z- waiting for 1730861761.53, time is 1730861760.74 2024-11-06T02:56:01Z- handleDepth()- Depth_Keller msg: 1730861761.15 epoch sec; seqNo:995; depth 50.07 2024-11-06T02:56:01Z- handleDVL() - DVL msg epochMillisec: 1730861761.15 2024-11-06T02:56:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:56:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:56:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:56:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:56:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:56:01Z- handleDvl() - RDI_Pathfinder msg: 1730861761.15 sec; seqNo:319; #beams:0 2024-11-06T02:56:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:56:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , -0.04 2024-11-06T02:56:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861761.16 epoch sec; seqNo:995; 4061608.40 north; 594325.29 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T02:56:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861757.53 2024-11-06T02:56:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:01Z- waiting for 1730861761.53, time is 1730861761.16 2024-11-06T02:56:01Z- AHRS_M2 msg: 1730861761.54 epoch sec; seqNo:928 2024-11-06T02:56:01Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 5.28 psi 2024-11-06T02:56:01Z- handleDepth()- Depth_Keller msg: 1730861761.54 epoch sec; seqNo:996; depth 50.08 2024-11-06T02:56:01Z- handleDVL() - DVL msg epochMillisec: 1730861761.57 2024-11-06T02:56:01Z- handleDvl() - RDI_Pathfinder msg: 1730861761.57 sec; seqNo:320; #beams:4 2024-11-06T02:56:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.88 , 39.15 , 42.00 , 42.41 2024-11-06T02:56:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.06 2024-11-06T02:56:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861761.59 epoch sec; seqNo:996; 4061608.78 north; 594325.29 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T02:56:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861757.53 2024-11-06T02:56:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:01Z- waiting for 1730861761.53, time is 1730861761.59 2024-11-06T02:56:01Z- LcmTrn::updateTrn() >>>> pose time 1730861761.59, meas time 1730861761.57 <<<< 2024-11-06T02:56:01Z- LcmTrn::updateTrn() equating pose 1730861761.57 and meas times 1730861761.57 2024-11-06T02:56:01Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861761.57 2024-11-06T02:56:01Z- TerrainNav::motionUpdate() - currEstimate = (4061609, 594325.29, 50.08) 2024-11-06T02:56:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:56:01Z- TerrainNav:: SubcloudNIS is 0.62, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:56:01Z- TNavPF::Projecting Measurements 2024-11-06T02:56:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:56:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:56:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:56:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:56:01Z- TNavFilter::4 good beams. 2024-11-06T02:56:01Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:56:01Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:56:01Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:56:01Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:56:01Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:56:01Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:56:01Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:56:01Z- TerrainMapOctree: (vehN, vehE ) = (4061608.78, 594325.29). 2024-11-06T02:56:01Z- TerrainMapOctree: Min Distance = 160.03. 2024-11-06T02:56:01Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:56:01Z- Forcing Subcloud Comparison 2024-11-06T02:56:01Z- Weighting particles with subcloud comparison 2024-11-06T02:56:01Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:01Z- meanExpectedMeasDiff: -1.604459 delta_rms_squared: 0.734668 alpha: 0.973498 2024-11-06T02:56:01Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:01Z- meanExpectedMeasDiff: 2.143547 delta_rms_squared: 0.667745 alpha: 0.970919 2024-11-06T02:56:01Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:01Z- meanExpectedMeasDiff: -0.568485 delta_rms_squared: 0.793952 alpha: 0.975429 2024-11-06T02:56:01Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:01Z- meanExpectedMeasDiff: -0.657941 delta_rms_squared: 0.858884 alpha: 0.977244 2024-11-06T02:56:01Z- Subcloud NIS: 0.594895 2024-11-06T02:56:01Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:56:01Z- TNavPF:: sumWeights = 0.333766 2024-11-06T02:56:01Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:56:01Z- TNavPF::First Term of Map Covariance Matrix is 0.49 2024-11-06T02:56:01Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.06 2024-11-06T02:56:01Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861761.57 sec, ping # 0. 2024-11-06T02:56:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:56:01Z- variances([0], [2], and psi_berg) = 0.233204 0.204778 0.000000 2024-11-06T02:56:01Z- publishEstimatesDecoder() - at 1730861761.57 sec 2024-11-06T02:56:01Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:56:01Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061641.03 594308.15 55.05 2024-11-06T02:56:01Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061641.24 594309.26 54.50 lat/lon: 36.70 -121.94 2024-11-06T02:56:01Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.20 0.37 0.00 2024-11-06T02:56:01Z- handleDepth()- Depth_Keller msg: 1730861761.96 epoch sec; seqNo:997; depth 50.08 2024-11-06T02:56:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861761.98 epoch sec; seqNo:997; 4061608.77 north; 594324.07 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T02:56:02Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:56:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861761.57 2024-11-06T02:56:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:02Z- waiting for 1730861765.57, time is 1730861761.98 2024-11-06T02:56:02Z- AHRS_M2 msg: 1730861762.34 epoch sec; seqNo:929 2024-11-06T02:56:02Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 5.28 psi 2024-11-06T02:56:02Z- handleDepth()- Depth_Keller msg: 1730861762.35 epoch sec; seqNo:998; depth 50.10 2024-11-06T02:56:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861762.36 epoch sec; seqNo:998; 4061609.15 north; 594324.06 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T02:56:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861761.57 2024-11-06T02:56:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:02Z- waiting for 1730861765.57, time is 1730861762.36 2024-11-06T02:56:02Z- AHRS_M2 msg: 1730861762.74 epoch sec; seqNo:930 2024-11-06T02:56:02Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 5.28 psi 2024-11-06T02:56:02Z- handleDepth()- Depth_Keller msg: 1730861762.76 epoch sec; seqNo:999; depth 50.10 2024-11-06T02:56:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861762.77 epoch sec; seqNo:999; 4061609.15 north; 594324.06 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T02:56:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861761.57 2024-11-06T02:56:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:02Z- waiting for 1730861765.57, time is 1730861762.77 2024-11-06T02:56:03Z- AHRS_M2 msg: 1730861763.15 epoch sec; seqNo:931 2024-11-06T02:56:03Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 5.28 psi 2024-11-06T02:56:03Z- handleDepth()- Depth_Keller msg: 1730861763.15 epoch sec; seqNo:1000; depth 50.11 2024-11-06T02:56:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861763.16 epoch sec; seqNo:1000; 4061609.53 north; 594324.06 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T02:56:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861761.57 2024-11-06T02:56:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:03Z- waiting for 1730861765.57, time is 1730861763.16 2024-11-06T02:56:03Z- AHRS_M2 msg: 1730861763.56 epoch sec; seqNo:932 2024-11-06T02:56:03Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 5.28 psi 2024-11-06T02:56:03Z- handleDepth()- Depth_Keller msg: 1730861763.57 epoch sec; seqNo:1001; depth 50.14 2024-11-06T02:56:03Z- handleDVL() - DVL msg epochMillisec: 1730861763.59 2024-11-06T02:56:03Z- handleDvl() - RDI_Pathfinder msg: 1730861763.59 sec; seqNo:321; #beams:4 2024-11-06T02:56:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 38.74 , 42.00 , 42.41 2024-11-06T02:56:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.07 , -0.05 2024-11-06T02:56:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861763.62 epoch sec; seqNo:1001; 4061609.51 north; 594322.84 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T02:56:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861761.57 2024-11-06T02:56:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:03Z- waiting for 1730861765.57, time is 1730861763.62 2024-11-06T02:56:03Z- AHRS_M2 msg: 1730861763.95 epoch sec; seqNo:933 2024-11-06T02:56:03Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 5.28 psi 2024-11-06T02:56:03Z- handleDepth()- Depth_Keller msg: 1730861763.97 epoch sec; seqNo:1002; depth 50.15 2024-11-06T02:56:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861763.99 epoch sec; seqNo:1002; 4061609.89 north; 594322.83 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T02:56:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861761.57 2024-11-06T02:56:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:04Z- waiting for 1730861765.57, time is 1730861763.99 2024-11-06T02:56:04Z- AHRS_M2 msg: 1730861764.36 epoch sec; seqNo:934 2024-11-06T02:56:04Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 5.28 psi 2024-11-06T02:56:04Z- handleDepth()- Depth_Keller msg: 1730861764.37 epoch sec; seqNo:1003; depth 50.16 2024-11-06T02:56:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861764.38 epoch sec; seqNo:1003; 4061609.89 north; 594322.83 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T02:56:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861761.57 2024-11-06T02:56:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:04Z- waiting for 1730861765.57, time is 1730861764.38 2024-11-06T02:56:04Z- AHRS_M2 msg: 1730861764.76 epoch sec; seqNo:935 2024-11-06T02:56:04Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 5.27 psi 2024-11-06T02:56:04Z- handleDepth()- Depth_Keller msg: 1730861764.77 epoch sec; seqNo:1004; depth 50.19 2024-11-06T02:56:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861764.79 epoch sec; seqNo:1004; 4061610.26 north; 594321.61 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T02:56:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861761.57 2024-11-06T02:56:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:04Z- waiting for 1730861765.57, time is 1730861764.79 2024-11-06T02:56:05Z- AHRS_M2 msg: 1730861765.17 epoch sec; seqNo:936 2024-11-06T02:56:05Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.27 psi 2024-11-06T02:56:05Z- handleDepth()- Depth_Keller msg: 1730861765.17 epoch sec; seqNo:1005; depth 50.21 2024-11-06T02:56:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861765.18 epoch sec; seqNo:1005; 4061610.26 north; 594321.61 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T02:56:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861761.57 2024-11-06T02:56:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:05Z- waiting for 1730861765.57, time is 1730861765.18 2024-11-06T02:56:05Z- AHRS_M2 msg: 1730861765.57 epoch sec; seqNo:937 2024-11-06T02:56:05Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.27 psi 2024-11-06T02:56:05Z- handleDepth()- Depth_Keller msg: 1730861765.58 epoch sec; seqNo:1006; depth 50.22 2024-11-06T02:56:05Z- handleDVL() - DVL msg epochMillisec: 1730861765.62 2024-11-06T02:56:05Z- handleDvl() - RDI_Pathfinder msg: 1730861765.62 sec; seqNo:322; #beams:4 2024-11-06T02:56:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.08 , 38.94 , 42.00 , 42.21 2024-11-06T02:56:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.07 , -0.01 2024-11-06T02:56:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861765.66 epoch sec; seqNo:1006; 4061610.64 north; 594321.60 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T02:56:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861761.57 2024-11-06T02:56:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:05Z- waiting for 1730861765.57, time is 1730861765.66 2024-11-06T02:56:05Z- LcmTrn::updateTrn() >>>> pose time 1730861765.66, meas time 1730861765.62 <<<< 2024-11-06T02:56:05Z- LcmTrn::updateTrn() equating pose 1730861765.62 and meas times 1730861765.62 2024-11-06T02:56:05Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861765.62 2024-11-06T02:56:05Z- TerrainNav::motionUpdate() - currEstimate = (4061611, 594321.60, 50.22) 2024-11-06T02:56:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:56:05Z- TerrainNav:: SubcloudNIS is 0.59, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:56:05Z- TNavPF::Projecting Measurements 2024-11-06T02:56:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:56:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:56:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:56:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:56:05Z- TNavFilter::4 good beams. 2024-11-06T02:56:05Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:56:05Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:56:05Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:56:05Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:56:05Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:56:05Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:56:05Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:56:05Z- TerrainMapOctree: (vehN, vehE ) = (4061610.64, 594321.60). 2024-11-06T02:56:05Z- TerrainMapOctree: Min Distance = 156.68. 2024-11-06T02:56:05Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:56:05Z- Forcing Subcloud Comparison 2024-11-06T02:56:05Z- Weighting particles with subcloud comparison 2024-11-06T02:56:05Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:05Z- meanExpectedMeasDiff: -1.938819 delta_rms_squared: 0.983227 alpha: 0.980064 2024-11-06T02:56:05Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:05Z- meanExpectedMeasDiff: 1.109276 delta_rms_squared: 0.610691 alpha: 0.968289 2024-11-06T02:56:05Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:05Z- meanExpectedMeasDiff: -0.387637 delta_rms_squared: 0.807573 alpha: 0.975833 2024-11-06T02:56:05Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:05Z- meanExpectedMeasDiff: -0.595433 delta_rms_squared: 0.869127 alpha: 0.977506 2024-11-06T02:56:05Z- Subcloud NIS: 0.589852 2024-11-06T02:56:05Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:56:05Z- TNavPF:: sumWeights = 0.459348 2024-11-06T02:56:05Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:56:05Z- TNavPF::First Term of Map Covariance Matrix is 0.67 2024-11-06T02:56:05Z- TNavPF::Calculated NIS Value : 0.27 numBeams normalized NIS: 0.07 2024-11-06T02:56:05Z- TerrainNav::Resampling particle filter... 2024-11-06T02:56:05Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861765.62 sec, ping # 0. 2024-11-06T02:56:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:56:05Z- variances([0], [2], and psi_berg) = 0.231593 0.205120 0.000000 2024-11-06T02:56:05Z- publishEstimatesDecoder() - at 1730861765.62 sec 2024-11-06T02:56:05Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:56:05Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061642.66 594306.16 53.68 2024-11-06T02:56:05Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061643.08 594305.56 54.81 lat/lon: 36.70 -121.94 2024-11-06T02:56:05Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.21 0.37 0.00 2024-11-06T02:56:05Z- AHRS_M2 msg: 1730861765.97 epoch sec; seqNo:938 2024-11-06T02:56:05Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.27 psi 2024-11-06T02:56:05Z- handleDepth()- Depth_Keller msg: 1730861765.99 epoch sec; seqNo:1007; depth 50.24 2024-11-06T02:56:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861766.00 epoch sec; seqNo:1007; 4061610.64 north; 594321.60 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T02:56:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861765.62 2024-11-06T02:56:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:06Z- waiting for 1730861769.62, time is 1730861766.00 2024-11-06T02:56:06Z- AHRS_M2 msg: 1730861766.38 epoch sec; seqNo:939 2024-11-06T02:56:06Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.27 psi 2024-11-06T02:56:06Z- handleDepth()- Depth_Keller msg: 1730861766.38 epoch sec; seqNo:1008; depth 50.26 2024-11-06T02:56:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861766.39 epoch sec; seqNo:1008; 4061611.00 north; 594320.38 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T02:56:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861765.62 2024-11-06T02:56:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:06Z- waiting for 1730861769.62, time is 1730861766.39 2024-11-06T02:56:06Z- AHRS_M2 msg: 1730861766.78 epoch sec; seqNo:940 2024-11-06T02:56:06Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 5.27 psi 2024-11-06T02:56:06Z- handleDepth()- Depth_Keller msg: 1730861766.79 epoch sec; seqNo:1009; depth 50.28 2024-11-06T02:56:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861766.81 epoch sec; seqNo:1009; 4061611.00 north; 594320.38 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T02:56:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861765.62 2024-11-06T02:56:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:06Z- waiting for 1730861769.62, time is 1730861766.81 2024-11-06T02:56:07Z- AHRS_M2 msg: 1730861767.19 epoch sec; seqNo:941 2024-11-06T02:56:07Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.27 psi 2024-11-06T02:56:07Z- handleDepth()- Depth_Keller msg: 1730861767.19 epoch sec; seqNo:1010; depth 50.29 2024-11-06T02:56:07Z- handleDVL() - DVL msg epochMillisec: 1730861767.20 2024-11-06T02:56:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:56:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:56:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:56:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:56:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:56:07Z- handleDvl() - RDI_Pathfinder msg: 1730861767.20 sec; seqNo:323; #beams:0 2024-11-06T02:56:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:56:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.07 , -0.01 2024-11-06T02:56:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861767.23 epoch sec; seqNo:1010; 4061611.38 north; 594320.38 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T02:56:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861765.62 2024-11-06T02:56:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:07Z- waiting for 1730861769.62, time is 1730861767.23 2024-11-06T02:56:07Z- AHRS_M2 msg: 1730861767.59 epoch sec; seqNo:942 2024-11-06T02:56:07Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.27 psi 2024-11-06T02:56:07Z- handleDepth()- Depth_Keller msg: 1730861767.60 epoch sec; seqNo:1011; depth 50.32 2024-11-06T02:56:07Z- handleDVL() - DVL msg epochMillisec: 1730861767.62 2024-11-06T02:56:07Z- handleDvl() - RDI_Pathfinder msg: 1730861767.62 sec; seqNo:324; #beams:4 2024-11-06T02:56:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 38.94 , 42.21 , 41.80 2024-11-06T02:56:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , 0.02 2024-11-06T02:56:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861767.66 epoch sec; seqNo:1011; 4061611.37 north; 594319.16 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T02:56:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861765.62 2024-11-06T02:56:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:07Z- waiting for 1730861769.62, time is 1730861767.66 2024-11-06T02:56:08Z- AHRS_M2 msg: 1730861768.00 epoch sec; seqNo:943 2024-11-06T02:56:08Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.28 psi 2024-11-06T02:56:08Z- handleDepth()- Depth_Keller msg: 1730861768.01 epoch sec; seqNo:1012; depth 50.32 2024-11-06T02:56:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861768.02 epoch sec; seqNo:1012; 4061611.75 north; 594319.15 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T02:56:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861765.62 2024-11-06T02:56:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:08Z- waiting for 1730861769.62, time is 1730861768.02 2024-11-06T02:56:08Z- AHRS_M2 msg: 1730861768.40 epoch sec; seqNo:944 2024-11-06T02:56:08Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 5.28 psi 2024-11-06T02:56:08Z- handleDepth()- Depth_Keller msg: 1730861768.40 epoch sec; seqNo:1013; depth 50.36 2024-11-06T02:56:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861768.42 epoch sec; seqNo:1013; 4061611.75 north; 594319.15 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T02:56:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861765.62 2024-11-06T02:56:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:08Z- waiting for 1730861769.62, time is 1730861768.42 2024-11-06T02:56:08Z- AHRS_M2 msg: 1730861768.80 epoch sec; seqNo:945 2024-11-06T02:56:08Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.28 psi 2024-11-06T02:56:08Z- handleDepth()- Depth_Keller msg: 1730861768.81 epoch sec; seqNo:1014; depth 50.36 2024-11-06T02:56:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861768.82 epoch sec; seqNo:1014; 4061612.12 north; 594319.15 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T02:56:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861765.62 2024-11-06T02:56:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:08Z- waiting for 1730861769.62, time is 1730861768.82 2024-11-06T02:56:09Z- AHRS_M2 msg: 1730861769.22 epoch sec; seqNo:946 2024-11-06T02:56:09Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.28 psi 2024-11-06T02:56:09Z- handleDepth()- Depth_Keller msg: 1730861769.23 epoch sec; seqNo:1015; depth 50.39 2024-11-06T02:56:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861769.29 epoch sec; seqNo:1015; 4061612.11 north; 594317.93 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T02:56:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861765.62 2024-11-06T02:56:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:09Z- waiting for 1730861769.62, time is 1730861769.29 2024-11-06T02:56:09Z- AHRS_M2 msg: 1730861769.62 epoch sec; seqNo:947 2024-11-06T02:56:09Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.28 psi 2024-11-06T02:56:09Z- handleDepth()- Depth_Keller msg: 1730861769.62 epoch sec; seqNo:1016; depth 50.40 2024-11-06T02:56:09Z- handleDVL() - DVL msg epochMillisec: 1730861769.64 2024-11-06T02:56:09Z- handleDvl() - RDI_Pathfinder msg: 1730861769.64 sec; seqNo:325; #beams:4 2024-11-06T02:56:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.08 , 39.15 , 42.41 , 42.21 2024-11-06T02:56:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , 0.00 2024-11-06T02:56:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861769.68 epoch sec; seqNo:1016; 4061612.49 north; 594317.92 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T02:56:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861765.62 2024-11-06T02:56:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:09Z- waiting for 1730861769.62, time is 1730861769.68 2024-11-06T02:56:09Z- LcmTrn::updateTrn() >>>> pose time 1730861769.68, meas time 1730861769.64 <<<< 2024-11-06T02:56:09Z- LcmTrn::updateTrn() equating pose 1730861769.64 and meas times 1730861769.64 2024-11-06T02:56:09Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861769.64 2024-11-06T02:56:09Z- TerrainNav::motionUpdate() - currEstimate = (4061612, 594317.92, 50.40) 2024-11-06T02:56:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:56:09Z- TerrainNav:: SubcloudNIS is 0.59, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:56:09Z- TNavPF::Projecting Measurements 2024-11-06T02:56:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:56:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:56:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:56:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:56:09Z- TNavFilter::4 good beams. 2024-11-06T02:56:09Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:56:09Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:56:09Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:56:09Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:56:09Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:56:09Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:56:09Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:56:09Z- TerrainMapOctree: (vehN, vehE ) = (4061612.49, 594317.92). 2024-11-06T02:56:09Z- TerrainMapOctree: Min Distance = 153.37. 2024-11-06T02:56:09Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:56:09Z- Forcing Subcloud Comparison 2024-11-06T02:56:09Z- Weighting particles with subcloud comparison 2024-11-06T02:56:09Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:09Z- meanExpectedMeasDiff: -1.579552 delta_rms_squared: 0.992481 alpha: 0.980247 2024-11-06T02:56:09Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:09Z- meanExpectedMeasDiff: 1.648239 delta_rms_squared: 0.722602 alpha: 0.973068 2024-11-06T02:56:09Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:09Z- meanExpectedMeasDiff: -0.876901 delta_rms_squared: 1.126274 alpha: 0.982552 2024-11-06T02:56:09Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:09Z- meanExpectedMeasDiff: -0.132206 delta_rms_squared: 1.226837 alpha: 0.983959 2024-11-06T02:56:09Z- Subcloud NIS: 0.577137 2024-11-06T02:56:09Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:56:09Z- TNavPF:: sumWeights = 0.386514 2024-11-06T02:56:09Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:56:09Z- TNavPF::First Term of Map Covariance Matrix is 0.53 2024-11-06T02:56:09Z- TNavPF::Calculated NIS Value : 0.24 numBeams normalized NIS: 0.06 2024-11-06T02:56:09Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861769.64 sec, ping # 0. 2024-11-06T02:56:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:56:09Z- variances([0], [2], and psi_berg) = 0.230406 0.208442 0.000000 2024-11-06T02:56:09Z- publishEstimatesDecoder() - at 1730861769.64 sec 2024-11-06T02:56:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:56:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061645.06 594302.70 54.91 2024-11-06T02:56:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061644.94 594301.88 55.07 lat/lon: 36.70 -121.94 2024-11-06T02:56:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.21 0.35 0.00 2024-11-06T02:56:10Z- AHRS_M2 msg: 1730861770.02 epoch sec; seqNo:948 2024-11-06T02:56:10Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 5.29 psi 2024-11-06T02:56:10Z- handleDepth()- Depth_Keller msg: 1730861770.03 epoch sec; seqNo:1017; depth 50.41 2024-11-06T02:56:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861770.06 epoch sec; seqNo:1017; 4061612.49 north; 594317.92 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T02:56:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861769.64 2024-11-06T02:56:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:10Z- waiting for 1730861773.64, time is 1730861770.06 2024-11-06T02:56:10Z- AHRS_M2 msg: 1730861770.42 epoch sec; seqNo:949 2024-11-06T02:56:10Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.29 psi 2024-11-06T02:56:10Z- handleDepth()- Depth_Keller msg: 1730861770.42 epoch sec; seqNo:1018; depth 50.42 2024-11-06T02:56:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861770.43 epoch sec; seqNo:1018; 4061612.87 north; 594317.92 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T02:56:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861769.64 2024-11-06T02:56:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:10Z- waiting for 1730861773.64, time is 1730861770.43 2024-11-06T02:56:10Z- AHRS_M2 msg: 1730861770.82 epoch sec; seqNo:950 2024-11-06T02:56:10Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.29 psi 2024-11-06T02:56:10Z- handleDepth()- Depth_Keller msg: 1730861770.83 epoch sec; seqNo:1019; depth 50.43 2024-11-06T02:56:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861770.84 epoch sec; seqNo:1019; 4061612.86 north; 594316.70 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T02:56:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861769.64 2024-11-06T02:56:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:10Z- waiting for 1730861773.64, time is 1730861770.84 2024-11-06T02:56:11Z- AHRS_M2 msg: 1730861771.26 epoch sec; seqNo:951 2024-11-06T02:56:11Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.29 psi 2024-11-06T02:56:11Z- handleDepth()- Depth_Keller msg: 1730861771.29 epoch sec; seqNo:1020; depth 50.44 2024-11-06T02:56:11Z- handleDVL() - DVL msg epochMillisec: 1730861771.31 2024-11-06T02:56:11Z- handleDvl() - RDI_Pathfinder msg: 1730861771.31 sec; seqNo:326; #beams:4 2024-11-06T02:56:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.08 , 38.94 , 42.41 , 42.00 2024-11-06T02:56:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , -0.05 2024-11-06T02:56:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861771.32 epoch sec; seqNo:1020; 4061613.23 north; 594316.69 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T02:56:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861769.64 2024-11-06T02:56:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:11Z- waiting for 1730861773.64, time is 1730861771.32 2024-11-06T02:56:11Z- handleDepth()- Depth_Keller msg: 1730861771.66 epoch sec; seqNo:1021; depth 50.45 2024-11-06T02:56:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861771.68 epoch sec; seqNo:1021; 4061613.23 north; 594316.69 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T02:56:11Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:56:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861769.64 2024-11-06T02:56:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:11Z- waiting for 1730861773.64, time is 1730861771.68 2024-11-06T02:56:12Z- AHRS_M2 msg: 1730861772.04 epoch sec; seqNo:952 2024-11-06T02:56:12Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.29 psi 2024-11-06T02:56:12Z- handleDepth()- Depth_Keller msg: 1730861772.04 epoch sec; seqNo:1022; depth 50.46 2024-11-06T02:56:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861772.10 epoch sec; seqNo:1022; 4061613.61 north; 594316.69 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T02:56:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861769.64 2024-11-06T02:56:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:12Z- waiting for 1730861773.64, time is 1730861772.10 2024-11-06T02:56:12Z- AHRS_M2 msg: 1730861772.45 epoch sec; seqNo:953 2024-11-06T02:56:12Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.30 psi 2024-11-06T02:56:12Z- handleDepth()- Depth_Keller msg: 1730861772.45 epoch sec; seqNo:1023; depth 50.48 2024-11-06T02:56:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861772.47 epoch sec; seqNo:1023; 4061613.60 north; 594315.47 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T02:56:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861769.64 2024-11-06T02:56:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:12Z- waiting for 1730861773.64, time is 1730861772.47 2024-11-06T02:56:12Z- AHRS_M2 msg: 1730861772.85 epoch sec; seqNo:954 2024-11-06T02:56:12Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.30 psi 2024-11-06T02:56:12Z- handleDepth()- Depth_Keller msg: 1730861772.86 epoch sec; seqNo:1024; depth 50.50 2024-11-06T02:56:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861772.87 epoch sec; seqNo:1024; 4061613.98 north; 594315.47 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T02:56:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861769.64 2024-11-06T02:56:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:13Z- waiting for 1730861773.64, time is 1730861772.87 2024-11-06T02:56:13Z- AHRS_M2 msg: 1730861773.29 epoch sec; seqNo:955 2024-11-06T02:56:13Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.30 psi 2024-11-06T02:56:13Z- handleDepth()- Depth_Keller msg: 1730861773.29 epoch sec; seqNo:1025; depth 50.51 2024-11-06T02:56:13Z- handleDVL() - DVL msg epochMillisec: 1730861773.31 2024-11-06T02:56:13Z- handleDvl() - RDI_Pathfinder msg: 1730861773.31 sec; seqNo:327; #beams:4 2024-11-06T02:56:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 38.94 , 42.41 , 41.80 2024-11-06T02:56:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , -0.05 2024-11-06T02:56:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861773.34 epoch sec; seqNo:1025; 4061613.98 north; 594315.47 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T02:56:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861769.64 2024-11-06T02:56:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:13Z- waiting for 1730861773.64, time is 1730861773.34 2024-11-06T02:56:13Z- AHRS_M2 msg: 1730861773.66 epoch sec; seqNo:956 2024-11-06T02:56:13Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.30 psi 2024-11-06T02:56:13Z- handleDepth()- Depth_Keller msg: 1730861773.66 epoch sec; seqNo:1026; depth 50.51 2024-11-06T02:56:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861773.68 epoch sec; seqNo:1026; 4061614.36 north; 594315.46 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T02:56:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861769.64 2024-11-06T02:56:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:13Z- waiting for 1730861773.64, time is 1730861773.68 2024-11-06T02:56:13Z- LcmTrn::updateTrn() >>>> pose time 1730861773.68, meas time 1730861773.31 <<<< 2024-11-06T02:56:13Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861773.68 2024-11-06T02:56:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861773.31 sec, ping # 0; waiting for INS data more recent than 1730861769.64... 2024-11-06T02:56:13Z- TerrainNav::motionUpdate() - currEstimate = (4061614, 594315.46, 50.51) 2024-11-06T02:56:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:56:13Z- TerrainNav:: SubcloudNIS is 0.58, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:56:13Z- TNavPF::Projecting Measurements 2024-11-06T02:56:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:56:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:56:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:56:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:56:13Z- TNavFilter::4 good beams. 2024-11-06T02:56:13Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:56:13Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:56:13Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:56:13Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:56:13Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:56:13Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:56:13Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:56:13Z- TerrainMapOctree: (vehN, vehE ) = (4061614.19, 594315.69). 2024-11-06T02:56:13Z- TerrainMapOctree: Min Distance = 150.86. 2024-11-06T02:56:13Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:56:13Z- Forcing Subcloud Comparison 2024-11-06T02:56:13Z- Weighting particles with subcloud comparison 2024-11-06T02:56:13Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:13Z- meanExpectedMeasDiff: -1.648392 delta_rms_squared: 0.952540 alpha: 0.979435 2024-11-06T02:56:13Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:13Z- meanExpectedMeasDiff: 1.604399 delta_rms_squared: 0.698023 alpha: 0.972146 2024-11-06T02:56:13Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:13Z- meanExpectedMeasDiff: -0.851680 delta_rms_squared: 0.794465 alpha: 0.975444 2024-11-06T02:56:13Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:13Z- meanExpectedMeasDiff: 0.009415 delta_rms_squared: 0.846178 alpha: 0.976910 2024-11-06T02:56:13Z- Subcloud NIS: 0.555281 2024-11-06T02:56:13Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:56:13Z- TNavPF:: sumWeights = 0.406248 2024-11-06T02:56:13Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:56:13Z- TNavPF::First Term of Map Covariance Matrix is 0.58 2024-11-06T02:56:13Z- TNavPF::Calculated NIS Value : 0.27 numBeams normalized NIS: 0.07 2024-11-06T02:56:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861773.31 sec. 2024-11-06T02:56:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:56:13Z- variances([0], [2], and psi_berg) = 0.231491 0.208933 0.000000 2024-11-06T02:56:13Z- publishEstimatesDecoder() - at 1730861773.68 sec 2024-11-06T02:56:13Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:56:13Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061647.17 594300.39 55.84 2024-11-06T02:56:13Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061646.81 594299.42 55.26 lat/lon: 36.70 -121.94 2024-11-06T02:56:13Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.21 0.60 0.00 2024-11-06T02:56:14Z- AHRS_M2 msg: 1730861774.07 epoch sec; seqNo:957 2024-11-06T02:56:14Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.29 psi 2024-11-06T02:56:14Z- handleDepth()- Depth_Keller msg: 1730861774.08 epoch sec; seqNo:1027; depth 50.52 2024-11-06T02:56:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861774.09 epoch sec; seqNo:1027; 4061614.34 north; 594314.24 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T02:56:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861773.68 2024-11-06T02:56:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:14Z- waiting for 1730861777.68, time is 1730861774.09 2024-11-06T02:56:14Z- AHRS_M2 msg: 1730861774.46 epoch sec; seqNo:958 2024-11-06T02:56:14Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 5.29 psi 2024-11-06T02:56:14Z- handleDepth()- Depth_Keller msg: 1730861774.47 epoch sec; seqNo:1028; depth 50.53 2024-11-06T02:56:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861774.48 epoch sec; seqNo:1028; 4061614.72 north; 594314.24 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T02:56:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861773.68 2024-11-06T02:56:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:14Z- waiting for 1730861777.68, time is 1730861774.48 2024-11-06T02:56:14Z- AHRS_M2 msg: 1730861774.87 epoch sec; seqNo:959 2024-11-06T02:56:14Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 5.29 psi 2024-11-06T02:56:14Z- handleDepth()- Depth_Keller msg: 1730861774.87 epoch sec; seqNo:1029; depth 50.57 2024-11-06T02:56:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861774.88 epoch sec; seqNo:1029; 4061615.10 north; 594314.23 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T02:56:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861773.68 2024-11-06T02:56:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:15Z- waiting for 1730861777.68, time is 1730861774.88 2024-11-06T02:56:15Z- AHRS_M2 msg: 1730861775.29 epoch sec; seqNo:960 2024-11-06T02:56:15Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 5.29 psi 2024-11-06T02:56:15Z- handleDepth()- Depth_Keller msg: 1730861775.30 epoch sec; seqNo:1030; depth 50.57 2024-11-06T02:56:15Z- handleDVL() - DVL msg epochMillisec: 1730861775.32 2024-11-06T02:56:15Z- handleDvl() - RDI_Pathfinder msg: 1730861775.32 sec; seqNo:328; #beams:4 2024-11-06T02:56:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 38.94 , 42.41 , 41.59 2024-11-06T02:56:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.04 , -0.00 2024-11-06T02:56:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861775.34 epoch sec; seqNo:1030; 4061615.10 north; 594314.23 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T02:56:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861773.68 2024-11-06T02:56:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:15Z- waiting for 1730861777.68, time is 1730861775.34 2024-11-06T02:56:15Z- AHRS_M2 msg: 1730861775.67 epoch sec; seqNo:961 2024-11-06T02:56:15Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 5.29 psi 2024-11-06T02:56:15Z- handleDepth()- Depth_Keller msg: 1730861775.68 epoch sec; seqNo:1031; depth 50.59 2024-11-06T02:56:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861775.69 epoch sec; seqNo:1031; 4061615.47 north; 594313.01 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T02:56:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861773.68 2024-11-06T02:56:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:15Z- waiting for 1730861777.68, time is 1730861775.69 2024-11-06T02:56:16Z- AHRS_M2 msg: 1730861776.08 epoch sec; seqNo:962 2024-11-06T02:56:16Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 5.29 psi 2024-11-06T02:56:16Z- handleDepth()- Depth_Keller msg: 1730861776.08 epoch sec; seqNo:1032; depth 50.59 2024-11-06T02:56:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861776.10 epoch sec; seqNo:1032; 4061615.47 north; 594313.01 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T02:56:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861773.68 2024-11-06T02:56:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:16Z- waiting for 1730861777.68, time is 1730861776.10 2024-11-06T02:56:16Z- AHRS_M2 msg: 1730861776.48 epoch sec; seqNo:963 2024-11-06T02:56:16Z- AHRS_M2 msg: 0.12 phi; -0.00 theta; 5.29 psi 2024-11-06T02:56:16Z- handleDepth()- Depth_Keller msg: 1730861776.48 epoch sec; seqNo:1033; depth 50.60 2024-11-06T02:56:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861776.49 epoch sec; seqNo:1033; 4061615.85 north; 594313.00 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T02:56:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861773.68 2024-11-06T02:56:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:16Z- waiting for 1730861777.68, time is 1730861776.49 2024-11-06T02:56:16Z- AHRS_M2 msg: 1730861776.89 epoch sec; seqNo:964 2024-11-06T02:56:16Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 5.29 psi 2024-11-06T02:56:16Z- handleDepth()- Depth_Keller msg: 1730861776.90 epoch sec; seqNo:1034; depth 50.61 2024-11-06T02:56:16Z- handleDVL() - DVL msg epochMillisec: 1730861776.90 2024-11-06T02:56:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:56:16Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:56:16Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:56:16Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:56:16Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:56:16Z- handleDvl() - RDI_Pathfinder msg: 1730861776.90 sec; seqNo:329; #beams:0 2024-11-06T02:56:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:56:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.04 , -0.00 2024-11-06T02:56:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861776.92 epoch sec; seqNo:1034; 4061615.83 north; 594311.78 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T02:56:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861773.68 2024-11-06T02:56:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:17Z- waiting for 1730861777.68, time is 1730861776.92 2024-11-06T02:56:17Z- AHRS_M2 msg: 1730861777.31 epoch sec; seqNo:965 2024-11-06T02:56:17Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 5.29 psi 2024-11-06T02:56:17Z- handleDepth()- Depth_Keller msg: 1730861777.31 epoch sec; seqNo:1035; depth 50.61 2024-11-06T02:56:17Z- handleDVL() - DVL msg epochMillisec: 1730861777.33 2024-11-06T02:56:17Z- handleDvl() - RDI_Pathfinder msg: 1730861777.33 sec; seqNo:330; #beams:4 2024-11-06T02:56:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 39.15 , 42.82 , 41.59 2024-11-06T02:56:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , 0.01 2024-11-06T02:56:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861777.35 epoch sec; seqNo:1035; 4061616.21 north; 594311.78 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T02:56:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861773.68 2024-11-06T02:56:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:17Z- waiting for 1730861777.68, time is 1730861777.35 2024-11-06T02:56:17Z- AHRS_M2 msg: 1730861777.69 epoch sec; seqNo:966 2024-11-06T02:56:17Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.29 psi 2024-11-06T02:56:17Z- handleDepth()- Depth_Keller msg: 1730861777.71 epoch sec; seqNo:1036; depth 50.63 2024-11-06T02:56:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861777.73 epoch sec; seqNo:1036; 4061616.21 north; 594311.78 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T02:56:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861773.68 2024-11-06T02:56:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:17Z- waiting for 1730861777.68, time is 1730861777.73 2024-11-06T02:56:17Z- LcmTrn::updateTrn() >>>> pose time 1730861777.73, meas time 1730861777.33 <<<< 2024-11-06T02:56:17Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861777.73 2024-11-06T02:56:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861777.33 sec, ping # 0; waiting for INS data more recent than 1730861773.68... 2024-11-06T02:56:17Z- TerrainNav::motionUpdate() - currEstimate = (4061616, 594311.78, 50.63) 2024-11-06T02:56:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:56:17Z- TerrainNav:: SubcloudNIS is 0.56, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:56:17Z- TNavPF::Projecting Measurements 2024-11-06T02:56:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:56:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:56:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:56:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:56:17Z- TNavFilter::4 good beams. 2024-11-06T02:56:17Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:56:17Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:56:17Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:56:17Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:56:17Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:56:17Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:56:17Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:56:17Z- TerrainMapOctree: (vehN, vehE ) = (4061616.03, 594312.14). 2024-11-06T02:56:17Z- TerrainMapOctree: Min Distance = 147.68. 2024-11-06T02:56:17Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:56:17Z- Forcing Subcloud Comparison 2024-11-06T02:56:17Z- Weighting particles with subcloud comparison 2024-11-06T02:56:17Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:17Z- meanExpectedMeasDiff: -1.781561 delta_rms_squared: 2.590331 alpha: 0.992338 2024-11-06T02:56:17Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:17Z- meanExpectedMeasDiff: 1.843863 delta_rms_squared: 0.954092 alpha: 0.979468 2024-11-06T02:56:17Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:17Z- meanExpectedMeasDiff: -0.686009 delta_rms_squared: 1.137315 alpha: 0.982719 2024-11-06T02:56:17Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:17Z- meanExpectedMeasDiff: 0.134167 delta_rms_squared: 0.987587 alpha: 0.980151 2024-11-06T02:56:17Z- Subcloud NIS: 0.539821 2024-11-06T02:56:17Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:56:17Z- TNavPF:: sumWeights = 0.310291 2024-11-06T02:56:17Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:56:17Z- TNavPF::First Term of Map Covariance Matrix is 0.82 2024-11-06T02:56:17Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T02:56:17Z- TerrainNav::Resampling particle filter... 2024-11-06T02:56:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861777.33 sec. 2024-11-06T02:56:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:56:18Z- variances([0], [2], and psi_berg) = 0.241997 0.215317 0.000000 2024-11-06T02:56:18Z- publishEstimatesDecoder() - at 1730861777.73 sec 2024-11-06T02:56:18Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:56:18Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061648.02 594296.71 55.19 2024-11-06T02:56:18Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061648.69 594295.76 55.49 lat/lon: 36.70 -121.94 2024-11-06T02:56:18Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.22 0.66 0.00 2024-11-06T02:56:18Z- AHRS_M2 msg: 1730861778.09 epoch sec; seqNo:967 2024-11-06T02:56:18Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.30 psi 2024-11-06T02:56:18Z- handleDepth()- Depth_Keller msg: 1730861778.11 epoch sec; seqNo:1037; depth 50.62 2024-11-06T02:56:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861778.12 epoch sec; seqNo:1037; 4061616.59 north; 594311.78 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T02:56:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861777.73 2024-11-06T02:56:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:18Z- waiting for 1730861781.73, time is 1730861778.12 2024-11-06T02:56:18Z- AHRS_M2 msg: 1730861778.50 epoch sec; seqNo:968 2024-11-06T02:56:18Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.30 psi 2024-11-06T02:56:18Z- handleDepth()- Depth_Keller msg: 1730861778.50 epoch sec; seqNo:1038; depth 50.64 2024-11-06T02:56:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861778.51 epoch sec; seqNo:1038; 4061616.58 north; 594310.55 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T02:56:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861777.73 2024-11-06T02:56:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:18Z- waiting for 1730861781.73, time is 1730861778.51 2024-11-06T02:56:18Z- AHRS_M2 msg: 1730861778.90 epoch sec; seqNo:969 2024-11-06T02:56:18Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.30 psi 2024-11-06T02:56:18Z- handleDepth()- Depth_Keller msg: 1730861778.92 epoch sec; seqNo:1039; depth 50.64 2024-11-06T02:56:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861778.93 epoch sec; seqNo:1039; 4061616.58 north; 594310.55 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T02:56:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861777.73 2024-11-06T02:56:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:19Z- waiting for 1730861781.73, time is 1730861778.93 2024-11-06T02:56:19Z- AHRS_M2 msg: 1730861779.31 epoch sec; seqNo:970 2024-11-06T02:56:19Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.30 psi 2024-11-06T02:56:19Z- handleDepth()- Depth_Keller msg: 1730861779.31 epoch sec; seqNo:1040; depth 50.64 2024-11-06T02:56:19Z- handleDVL() - DVL msg epochMillisec: 1730861779.34 2024-11-06T02:56:19Z- handleDvl() - RDI_Pathfinder msg: 1730861779.34 sec; seqNo:331; #beams:4 2024-11-06T02:56:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.88 , 38.94 , 43.02 , 41.19 2024-11-06T02:56:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , -0.03 2024-11-06T02:56:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861779.35 epoch sec; seqNo:1040; 4061616.96 north; 594310.55 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T02:56:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861777.73 2024-11-06T02:56:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:19Z- waiting for 1730861781.73, time is 1730861779.35 2024-11-06T02:56:19Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T02:56:19Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T02:56:19Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T02:56:19Z- AHRS_M2 msg: 1730861779.70 epoch sec; seqNo:971 2024-11-06T02:56:19Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.30 psi 2024-11-06T02:56:19Z- handleDepth()- Depth_Keller msg: 1730861779.73 epoch sec; seqNo:1041; depth 50.62 2024-11-06T02:56:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861779.75 epoch sec; seqNo:1041; 4061616.96 north; 594310.55 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T02:56:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861777.73 2024-11-06T02:56:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:19Z- waiting for 1730861781.73, time is 1730861779.75 2024-11-06T02:56:20Z- AHRS_M2 msg: 1730861780.12 epoch sec; seqNo:972 2024-11-06T02:56:20Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 5.30 psi 2024-11-06T02:56:20Z- handleDepth()- Depth_Keller msg: 1730861780.14 epoch sec; seqNo:1042; depth 50.62 2024-11-06T02:56:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861780.15 epoch sec; seqNo:1042; 4061617.32 north; 594309.33 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T02:56:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861777.73 2024-11-06T02:56:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:20Z- waiting for 1730861781.73, time is 1730861780.15 2024-11-06T02:56:20Z- AHRS_M2 msg: 1730861780.52 epoch sec; seqNo:973 2024-11-06T02:56:20Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 5.31 psi 2024-11-06T02:56:20Z- handleDepth()- Depth_Keller msg: 1730861780.53 epoch sec; seqNo:1043; depth 50.63 2024-11-06T02:56:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861780.54 epoch sec; seqNo:1043; 4061617.32 north; 594309.33 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T02:56:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861777.73 2024-11-06T02:56:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:20Z- waiting for 1730861781.73, time is 1730861780.54 2024-11-06T02:56:20Z- AHRS_M2 msg: 1730861780.92 epoch sec; seqNo:974 2024-11-06T02:56:20Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 5.31 psi 2024-11-06T02:56:20Z- handleDepth()- Depth_Keller msg: 1730861780.93 epoch sec; seqNo:1044; depth 50.61 2024-11-06T02:56:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861780.96 epoch sec; seqNo:1044; 4061617.70 north; 594309.32 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T02:56:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861777.73 2024-11-06T02:56:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:21Z- waiting for 1730861781.73, time is 1730861780.96 2024-11-06T02:56:21Z- AHRS_M2 msg: 1730861781.33 epoch sec; seqNo:975 2024-11-06T02:56:21Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 5.31 psi 2024-11-06T02:56:21Z- handleDepth()- Depth_Keller msg: 1730861781.33 epoch sec; seqNo:1045; depth 50.60 2024-11-06T02:56:21Z- handleDVL() - DVL msg epochMillisec: 1730861781.35 2024-11-06T02:56:21Z- handleDvl() - RDI_Pathfinder msg: 1730861781.35 sec; seqNo:332; #beams:4 2024-11-06T02:56:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 39.15 , 43.02 , 41.19 2024-11-06T02:56:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.07 2024-11-06T02:56:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861781.38 epoch sec; seqNo:1045; 4061617.70 north; 594309.32 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T02:56:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861777.73 2024-11-06T02:56:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:21Z- waiting for 1730861781.73, time is 1730861781.38 2024-11-06T02:56:21Z- AHRS_M2 msg: 1730861781.73 epoch sec; seqNo:976 2024-11-06T02:56:21Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 5.31 psi 2024-11-06T02:56:21Z- handleDepth()- Depth_Keller msg: 1730861781.74 epoch sec; seqNo:1046; depth 50.60 2024-11-06T02:56:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861781.76 epoch sec; seqNo:1046; 4061618.07 north; 594308.10 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T02:56:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861777.73 2024-11-06T02:56:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:21Z- waiting for 1730861781.73, time is 1730861781.76 2024-11-06T02:56:21Z- LcmTrn::updateTrn() >>>> pose time 1730861781.76, meas time 1730861781.35 <<<< 2024-11-06T02:56:21Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861781.76 2024-11-06T02:56:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861781.35 sec, ping # 0; waiting for INS data more recent than 1730861777.73... 2024-11-06T02:56:21Z- TerrainNav::motionUpdate() - currEstimate = (4061618, 594308.10, 50.60) 2024-11-06T02:56:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:56:21Z- TerrainNav:: SubcloudNIS is 0.54, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:56:21Z- TNavPF::Projecting Measurements 2024-11-06T02:56:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:56:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:56:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:56:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:56:21Z- TNavFilter::4 good beams. 2024-11-06T02:56:21Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:56:21Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:56:21Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:56:21Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:56:21Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:56:21Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:56:21Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:56:21Z- TerrainMapOctree: (vehN, vehE ) = (4061617.88, 594308.47). 2024-11-06T02:56:21Z- TerrainMapOctree: Min Distance = 144.48. 2024-11-06T02:56:21Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:56:22Z- Forcing Subcloud Comparison 2024-11-06T02:56:22Z- Weighting particles with subcloud comparison 2024-11-06T02:56:22Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:22Z- meanExpectedMeasDiff: -2.848457 delta_rms_squared: 1.490921 alpha: 0.986763 2024-11-06T02:56:22Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:22Z- meanExpectedMeasDiff: 1.158231 delta_rms_squared: 1.070641 alpha: 0.981662 2024-11-06T02:56:22Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:22Z- meanExpectedMeasDiff: -1.056187 delta_rms_squared: 1.406865 alpha: 0.985983 2024-11-06T02:56:22Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:22Z- meanExpectedMeasDiff: -0.447703 delta_rms_squared: 1.218586 alpha: 0.983853 2024-11-06T02:56:22Z- Subcloud NIS: 0.527364 2024-11-06T02:56:22Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:56:22Z- TNavPF:: sumWeights = 0.268466 2024-11-06T02:56:22Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:56:22Z- TNavPF::First Term of Map Covariance Matrix is 1.02 2024-11-06T02:56:22Z- TNavPF::Calculated NIS Value : 0.14 numBeams normalized NIS: 0.04 2024-11-06T02:56:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861781.35 sec. 2024-11-06T02:56:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:56:22Z- variances([0], [2], and psi_berg) = 0.243364 0.217245 0.000000 2024-11-06T02:56:22Z- publishEstimatesDecoder() - at 1730861781.76 sec 2024-11-06T02:56:22Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:56:22Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061651.81 594291.36 55.89 2024-11-06T02:56:22Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061650.54 594292.07 55.82 lat/lon: 36.70 -121.94 2024-11-06T02:56:22Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.22 0.70 0.00 2024-11-06T02:56:22Z- AHRS_M2 msg: 1730861782.14 epoch sec; seqNo:977 2024-11-06T02:56:22Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 5.31 psi 2024-11-06T02:56:22Z- handleDepth()- Depth_Keller msg: 1730861782.17 epoch sec; seqNo:1047; depth 50.58 2024-11-06T02:56:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861782.18 epoch sec; seqNo:1047; 4061618.07 north; 594308.10 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T02:56:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861781.76 2024-11-06T02:56:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:22Z- waiting for 1730861785.76, time is 1730861782.18 2024-11-06T02:56:22Z- AHRS_M2 msg: 1730861782.55 epoch sec; seqNo:978 2024-11-06T02:56:22Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 5.31 psi 2024-11-06T02:56:22Z- handleDepth()- Depth_Keller msg: 1730861782.55 epoch sec; seqNo:1048; depth 50.59 2024-11-06T02:56:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861782.57 epoch sec; seqNo:1048; 4061618.44 north; 594308.09 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T02:56:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861781.76 2024-11-06T02:56:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:22Z- waiting for 1730861785.76, time is 1730861782.57 2024-11-06T02:56:22Z- AHRS_M2 msg: 1730861782.95 epoch sec; seqNo:979 2024-11-06T02:56:22Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 5.31 psi 2024-11-06T02:56:22Z- handleDepth()- Depth_Keller msg: 1730861782.95 epoch sec; seqNo:1049; depth 50.58 2024-11-06T02:56:22Z- handleDVL() - DVL msg epochMillisec: 1730861782.95 2024-11-06T02:56:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:56:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:56:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:56:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:56:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:56:22Z- handleDvl() - RDI_Pathfinder msg: 1730861782.95 sec; seqNo:333; #beams:0 2024-11-06T02:56:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:56:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.07 2024-11-06T02:56:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861782.96 epoch sec; seqNo:1049; 4061618.44 north; 594308.09 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T02:56:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861781.76 2024-11-06T02:56:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:23Z- waiting for 1730861785.76, time is 1730861782.96 2024-11-06T02:56:23Z- AHRS_M2 msg: 1730861783.35 epoch sec; seqNo:980 2024-11-06T02:56:23Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 5.31 psi 2024-11-06T02:56:23Z- handleDepth()- Depth_Keller msg: 1730861783.36 epoch sec; seqNo:1050; depth 50.57 2024-11-06T02:56:23Z- handleDVL() - DVL msg epochMillisec: 1730861783.37 2024-11-06T02:56:23Z- handleDvl() - RDI_Pathfinder msg: 1730861783.37 sec; seqNo:334; #beams:4 2024-11-06T02:56:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.49 , 38.74 , 43.63 , 40.98 2024-11-06T02:56:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.04 , -0.07 2024-11-06T02:56:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861783.39 epoch sec; seqNo:1050; 4061618.81 north; 594306.87 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T02:56:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861781.76 2024-11-06T02:56:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:23Z- waiting for 1730861785.76, time is 1730861783.39 2024-11-06T02:56:23Z- AHRS_M2 msg: 1730861783.75 epoch sec; seqNo:981 2024-11-06T02:56:23Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 5.31 psi 2024-11-06T02:56:23Z- handleDepth()- Depth_Keller msg: 1730861783.76 epoch sec; seqNo:1051; depth 50.57 2024-11-06T02:56:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861783.78 epoch sec; seqNo:1051; 4061618.81 north; 594306.87 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T02:56:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861781.76 2024-11-06T02:56:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:23Z- waiting for 1730861785.76, time is 1730861783.78 2024-11-06T02:56:24Z- AHRS_M2 msg: 1730861784.15 epoch sec; seqNo:982 2024-11-06T02:56:24Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 5.31 psi 2024-11-06T02:56:24Z- handleDepth()- Depth_Keller msg: 1730861784.16 epoch sec; seqNo:1052; depth 50.56 2024-11-06T02:56:24Z- handleDVL() - DVL msg epochMillisec: 1730861784.16 2024-11-06T02:56:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:56:24Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:56:24Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:56:24Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:56:24Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:56:24Z- handleDvl() - RDI_Pathfinder msg: 1730861784.16 sec; seqNo:335; #beams:0 2024-11-06T02:56:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:56:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.04 , -0.07 2024-11-06T02:56:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861784.19 epoch sec; seqNo:1052; 4061619.19 north; 594306.86 east; 36.70 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T02:56:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861781.76 2024-11-06T02:56:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:24Z- waiting for 1730861785.76, time is 1730861784.19 2024-11-06T02:56:24Z- AHRS_M2 msg: 1730861784.56 epoch sec; seqNo:983 2024-11-06T02:56:24Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 5.31 psi 2024-11-06T02:56:24Z- handleDepth()- Depth_Keller msg: 1730861784.57 epoch sec; seqNo:1053; depth 50.58 2024-11-06T02:56:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861784.57 epoch sec; seqNo:1053; 4061619.19 north; 594306.86 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T02:56:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861781.76 2024-11-06T02:56:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:24Z- waiting for 1730861785.76, time is 1730861784.57 2024-11-06T02:56:24Z- AHRS_M2 msg: 1730861784.97 epoch sec; seqNo:984 2024-11-06T02:56:24Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 5.31 psi 2024-11-06T02:56:24Z- handleDepth()- Depth_Keller msg: 1730861784.98 epoch sec; seqNo:1054; depth 50.57 2024-11-06T02:56:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861784.99 epoch sec; seqNo:1054; 4061619.55 north; 594305.64 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T02:56:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861781.76 2024-11-06T02:56:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:25Z- waiting for 1730861785.76, time is 1730861784.99 2024-11-06T02:56:25Z- AHRS_M2 msg: 1730861785.37 epoch sec; seqNo:985 2024-11-06T02:56:25Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 5.31 psi 2024-11-06T02:56:25Z- handleDepth()- Depth_Keller msg: 1730861785.38 epoch sec; seqNo:1055; depth 50.57 2024-11-06T02:56:25Z- handleDVL() - DVL msg epochMillisec: 1730861785.41 2024-11-06T02:56:25Z- handleDvl() - RDI_Pathfinder msg: 1730861785.41 sec; seqNo:336; #beams:4 2024-11-06T02:56:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.08 , 38.94 , 43.84 , 40.57 2024-11-06T02:56:25Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.04 , -0.04 2024-11-06T02:56:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861785.42 epoch sec; seqNo:1055; 4061619.93 north; 594305.64 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T02:56:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861781.76 2024-11-06T02:56:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:25Z- waiting for 1730861785.76, time is 1730861785.42 2024-11-06T02:56:25Z- AHRS_M2 msg: 1730861785.77 epoch sec; seqNo:986 2024-11-06T02:56:25Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 5.31 psi 2024-11-06T02:56:25Z- handleDepth()- Depth_Keller msg: 1730861785.78 epoch sec; seqNo:1056; depth 50.57 2024-11-06T02:56:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861785.79 epoch sec; seqNo:1056; 4061619.93 north; 594305.64 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T02:56:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861781.76 2024-11-06T02:56:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:25Z- waiting for 1730861785.76, time is 1730861785.79 2024-11-06T02:56:25Z- LcmTrn::updateTrn() >>>> pose time 1730861785.79, meas time 1730861785.41 <<<< 2024-11-06T02:56:25Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861785.79 2024-11-06T02:56:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861785.41 sec, ping # 0; waiting for INS data more recent than 1730861781.76... 2024-11-06T02:56:25Z- TerrainNav::motionUpdate() - currEstimate = (4061620, 594305.64, 50.57) 2024-11-06T02:56:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:56:25Z- TerrainNav:: SubcloudNIS is 0.53, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:56:25Z- TNavPF::Projecting Measurements 2024-11-06T02:56:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:56:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:56:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:56:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:56:25Z- TNavFilter::4 good beams. 2024-11-06T02:56:25Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:56:25Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:56:25Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:56:25Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:56:25Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:56:25Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:56:25Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:56:25Z- TerrainMapOctree: (vehN, vehE ) = (4061619.75, 594305.87). 2024-11-06T02:56:25Z- TerrainMapOctree: Min Distance = 141.72. 2024-11-06T02:56:25Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:56:26Z- Forcing Subcloud Comparison 2024-11-06T02:56:26Z- Weighting particles with subcloud comparison 2024-11-06T02:56:26Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:26Z- meanExpectedMeasDiff: -0.872914 delta_rms_squared: 2.738183 alpha: 0.992749 2024-11-06T02:56:26Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:26Z- meanExpectedMeasDiff: 1.161864 delta_rms_squared: 1.110296 alpha: 0.982306 2024-11-06T02:56:26Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:26Z- meanExpectedMeasDiff: -1.321776 delta_rms_squared: 1.398585 alpha: 0.985901 2024-11-06T02:56:26Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:26Z- meanExpectedMeasDiff: -0.340520 delta_rms_squared: 1.233128 alpha: 0.984040 2024-11-06T02:56:26Z- Subcloud NIS: 0.504866 2024-11-06T02:56:26Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:56:26Z- TNavPF:: sumWeights = 0.438964 2024-11-06T02:56:26Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:56:26Z- TNavPF::First Term of Map Covariance Matrix is 0.56 2024-11-06T02:56:26Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T02:56:26Z- TerrainNav::Resampling particle filter... 2024-11-06T02:56:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861785.41 sec. 2024-11-06T02:56:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:56:26Z- variances([0], [2], and psi_berg) = 0.243012 0.217367 0.000000 2024-11-06T02:56:26Z- publishEstimatesDecoder() - at 1730861785.79 sec 2024-11-06T02:56:26Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:56:26Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061651.75 594290.62 56.21 2024-11-06T02:56:26Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061652.44 594289.62 55.96 lat/lon: 36.70 -121.94 2024-11-06T02:56:26Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.22 0.66 0.00 2024-11-06T02:56:26Z- AHRS_M2 msg: 1730861786.18 epoch sec; seqNo:987 2024-11-06T02:56:26Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 5.30 psi 2024-11-06T02:56:26Z- handleDepth()- Depth_Keller msg: 1730861786.19 epoch sec; seqNo:1057; depth 50.57 2024-11-06T02:56:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861786.21 epoch sec; seqNo:1057; 4061620.31 north; 594305.63 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T02:56:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861785.79 2024-11-06T02:56:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:26Z- waiting for 1730861789.79, time is 1730861786.21 2024-11-06T02:56:26Z- AHRS_M2 msg: 1730861786.58 epoch sec; seqNo:988 2024-11-06T02:56:26Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 5.30 psi 2024-11-06T02:56:26Z- handleDepth()- Depth_Keller msg: 1730861786.58 epoch sec; seqNo:1058; depth 50.57 2024-11-06T02:56:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861786.59 epoch sec; seqNo:1058; 4061620.30 north; 594304.41 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T02:56:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861785.79 2024-11-06T02:56:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:26Z- waiting for 1730861789.79, time is 1730861786.59 2024-11-06T02:56:26Z- AHRS_M2 msg: 1730861786.99 epoch sec; seqNo:989 2024-11-06T02:56:26Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 5.30 psi 2024-11-06T02:56:26Z- handleDepth()- Depth_Keller msg: 1730861786.99 epoch sec; seqNo:1059; depth 50.57 2024-11-06T02:56:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861787.00 epoch sec; seqNo:1059; 4061620.68 north; 594304.41 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T02:56:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861785.79 2024-11-06T02:56:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:27Z- waiting for 1730861789.79, time is 1730861787.00 2024-11-06T02:56:27Z- AHRS_M2 msg: 1730861787.39 epoch sec; seqNo:990 2024-11-06T02:56:27Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 5.30 psi 2024-11-06T02:56:27Z- handleDepth()- Depth_Keller msg: 1730861787.40 epoch sec; seqNo:1060; depth 50.59 2024-11-06T02:56:27Z- handleDVL() - DVL msg epochMillisec: 1730861787.42 2024-11-06T02:56:27Z- handleDvl() - RDI_Pathfinder msg: 1730861787.42 sec; seqNo:337; #beams:4 2024-11-06T02:56:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.47 , 39.35 , 43.84 , 40.78 2024-11-06T02:56:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.06 , 0.01 2024-11-06T02:56:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861787.44 epoch sec; seqNo:1060; 4061620.68 north; 594304.41 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T02:56:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861785.79 2024-11-06T02:56:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:27Z- waiting for 1730861789.79, time is 1730861787.44 2024-11-06T02:56:27Z- AHRS_M2 msg: 1730861787.79 epoch sec; seqNo:991 2024-11-06T02:56:27Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 5.30 psi 2024-11-06T02:56:27Z- handleDepth()- Depth_Keller msg: 1730861787.80 epoch sec; seqNo:1061; depth 50.58 2024-11-06T02:56:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861787.81 epoch sec; seqNo:1061; 4061621.04 north; 594303.18 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T02:56:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861785.79 2024-11-06T02:56:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:27Z- waiting for 1730861789.79, time is 1730861787.81 2024-11-06T02:56:28Z- handleDepth()- Depth_Keller msg: 1730861788.22 epoch sec; seqNo:1062; depth 50.57 2024-11-06T02:56:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861788.23 epoch sec; seqNo:1062; 4061621.04 north; 594303.18 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T02:56:28Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:56:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861785.79 2024-11-06T02:56:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:28Z- waiting for 1730861789.79, time is 1730861788.23 2024-11-06T02:56:28Z- AHRS_M2 msg: 1730861788.60 epoch sec; seqNo:992 2024-11-06T02:56:28Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 5.30 psi 2024-11-06T02:56:28Z- handleDepth()- Depth_Keller msg: 1730861788.62 epoch sec; seqNo:1063; depth 50.58 2024-11-06T02:56:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861788.63 epoch sec; seqNo:1063; 4061621.42 north; 594303.18 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T02:56:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861785.79 2024-11-06T02:56:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:28Z- waiting for 1730861789.79, time is 1730861788.63 2024-11-06T02:56:29Z- AHRS_M2 msg: 1730861789.00 epoch sec; seqNo:993 2024-11-06T02:56:29Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 5.30 psi 2024-11-06T02:56:29Z- handleDepth()- Depth_Keller msg: 1730861789.01 epoch sec; seqNo:1064; depth 50.57 2024-11-06T02:56:29Z- handleDVL() - DVL msg epochMillisec: 1730861789.01 2024-11-06T02:56:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:56:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:56:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:56:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:56:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:56:29Z- handleDvl() - RDI_Pathfinder msg: 1730861789.01 sec; seqNo:338; #beams:0 2024-11-06T02:56:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:56:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.06 , 0.01 2024-11-06T02:56:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861789.02 epoch sec; seqNo:1064; 4061621.42 north; 594303.18 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T02:56:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861785.79 2024-11-06T02:56:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:29Z- waiting for 1730861789.79, time is 1730861789.02 2024-11-06T02:56:29Z- AHRS_M2 msg: 1730861789.41 epoch sec; seqNo:994 2024-11-06T02:56:29Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 5.30 psi 2024-11-06T02:56:29Z- handleDepth()- Depth_Keller msg: 1730861789.41 epoch sec; seqNo:1065; depth 50.57 2024-11-06T02:56:29Z- handleDVL() - DVL msg epochMillisec: 1730861789.44 2024-11-06T02:56:29Z- handleDvl() - RDI_Pathfinder msg: 1730861789.44 sec; seqNo:339; #beams:4 2024-11-06T02:56:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 39.35 , 44.45 , 40.78 2024-11-06T02:56:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , 0.02 2024-11-06T02:56:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861789.46 epoch sec; seqNo:1065; 4061621.79 north; 594301.95 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T02:56:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861785.79 2024-11-06T02:56:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:29Z- waiting for 1730861789.79, time is 1730861789.46 2024-11-06T02:56:29Z- AHRS_M2 msg: 1730861789.82 epoch sec; seqNo:995 2024-11-06T02:56:29Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 5.30 psi 2024-11-06T02:56:29Z- handleDepth()- Depth_Keller msg: 1730861789.82 epoch sec; seqNo:1066; depth 50.57 2024-11-06T02:56:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861789.84 epoch sec; seqNo:1066; 4061621.79 north; 594301.95 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T02:56:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861785.79 2024-11-06T02:56:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:29Z- waiting for 1730861789.79, time is 1730861789.84 2024-11-06T02:56:29Z- LcmTrn::updateTrn() >>>> pose time 1730861789.84, meas time 1730861789.44 <<<< 2024-11-06T02:56:29Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861789.84 2024-11-06T02:56:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861789.44 sec, ping # 0; waiting for INS data more recent than 1730861785.79... 2024-11-06T02:56:29Z- TerrainNav::motionUpdate() - currEstimate = (4061622, 594301.95, 50.57) 2024-11-06T02:56:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:56:29Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:56:29Z- TNavPF::Projecting Measurements 2024-11-06T02:56:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:56:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:56:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:56:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:56:29Z- TNavFilter::4 good beams. 2024-11-06T02:56:29Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:56:29Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:56:29Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:56:29Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:56:29Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:56:29Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:56:29Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:56:30Z- TerrainMapOctree: (vehN, vehE ) = (4061621.60, 594302.32). 2024-11-06T02:56:30Z- TerrainMapOctree: Min Distance = 138.65. 2024-11-06T02:56:30Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:56:30Z- Forcing Subcloud Comparison 2024-11-06T02:56:30Z- Weighting particles with subcloud comparison 2024-11-06T02:56:30Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:30Z- meanExpectedMeasDiff: -1.175146 delta_rms_squared: 1.416247 alpha: 0.986075 2024-11-06T02:56:30Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:30Z- meanExpectedMeasDiff: 1.270544 delta_rms_squared: 1.018262 alpha: 0.980737 2024-11-06T02:56:30Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:30Z- meanExpectedMeasDiff: -0.790837 delta_rms_squared: 1.285971 alpha: 0.984686 2024-11-06T02:56:30Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:30Z- meanExpectedMeasDiff: 0.287956 delta_rms_squared: 1.147830 alpha: 0.982874 2024-11-06T02:56:30Z- Subcloud NIS: 0.478209 2024-11-06T02:56:30Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:56:30Z- TNavPF:: sumWeights = 0.474218 2024-11-06T02:56:30Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:56:30Z- TNavPF::First Term of Map Covariance Matrix is 0.51 2024-11-06T02:56:30Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T02:56:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861789.44 sec. 2024-11-06T02:56:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:56:30Z- variances([0], [2], and psi_berg) = 0.251572 0.223153 0.000000 2024-11-06T02:56:30Z- publishEstimatesDecoder() - at 1730861789.84 sec 2024-11-06T02:56:30Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:56:30Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061654.46 594285.49 57.13 2024-11-06T02:56:30Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061654.28 594285.89 56.02 lat/lon: 36.70 -121.94 2024-11-06T02:56:30Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.22 0.72 0.00 2024-11-06T02:56:30Z- AHRS_M2 msg: 1730861790.23 epoch sec; seqNo:996 2024-11-06T02:56:30Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 5.30 psi 2024-11-06T02:56:30Z- handleDepth()- Depth_Keller msg: 1730861790.23 epoch sec; seqNo:1067; depth 50.56 2024-11-06T02:56:30Z- handleDVL() - DVL msg epochMillisec: 1730861790.24 2024-11-06T02:56:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:56:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:56:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:56:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:56:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:56:30Z- handleDvl() - RDI_Pathfinder msg: 1730861790.24 sec; seqNo:340; #beams:0 2024-11-06T02:56:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:56:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , 0.02 2024-11-06T02:56:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861790.25 epoch sec; seqNo:1067; 4061622.17 north; 594301.95 east; 36.70 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T02:56:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861789.84 2024-11-06T02:56:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:30Z- waiting for 1730861793.84, time is 1730861790.25 2024-11-06T02:56:30Z- AHRS_M2 msg: 1730861790.62 epoch sec; seqNo:997 2024-11-06T02:56:30Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 5.30 psi 2024-11-06T02:56:30Z- handleDepth()- Depth_Keller msg: 1730861790.63 epoch sec; seqNo:1068; depth 50.55 2024-11-06T02:56:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861790.66 epoch sec; seqNo:1068; 4061622.17 north; 594301.95 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T02:56:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861789.84 2024-11-06T02:56:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:30Z- waiting for 1730861793.84, time is 1730861790.66 2024-11-06T02:56:31Z- AHRS_M2 msg: 1730861791.03 epoch sec; seqNo:998 2024-11-06T02:56:31Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 5.30 psi 2024-11-06T02:56:31Z- handleDepth()- Depth_Keller msg: 1730861791.04 epoch sec; seqNo:1069; depth 50.53 2024-11-06T02:56:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861791.05 epoch sec; seqNo:1069; 4061622.53 north; 594300.73 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T02:56:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861789.84 2024-11-06T02:56:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:31Z- waiting for 1730861793.84, time is 1730861791.05 2024-11-06T02:56:31Z- AHRS_M2 msg: 1730861791.43 epoch sec; seqNo:999 2024-11-06T02:56:31Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:31Z- handleDepth()- Depth_Keller msg: 1730861791.43 epoch sec; seqNo:1070; depth 50.52 2024-11-06T02:56:31Z- handleDVL() - DVL msg epochMillisec: 1730861791.45 2024-11-06T02:56:31Z- handleDvl() - RDI_Pathfinder msg: 1730861791.45 sec; seqNo:341; #beams:4 2024-11-06T02:56:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 39.55 , 44.45 , 40.78 2024-11-06T02:56:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , -0.03 2024-11-06T02:56:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861791.47 epoch sec; seqNo:1070; 4061622.53 north; 594300.73 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T02:56:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861789.84 2024-11-06T02:56:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:31Z- waiting for 1730861793.84, time is 1730861791.47 2024-11-06T02:56:31Z- AHRS_M2 msg: 1730861791.83 epoch sec; seqNo:1000 2024-11-06T02:56:31Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:31Z- handleDepth()- Depth_Keller msg: 1730861791.84 epoch sec; seqNo:1071; depth 50.49 2024-11-06T02:56:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861791.86 epoch sec; seqNo:1071; 4061622.53 north; 594300.73 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T02:56:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861789.84 2024-11-06T02:56:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:31Z- waiting for 1730861793.84, time is 1730861791.86 2024-11-06T02:56:32Z- AHRS_M2 msg: 1730861792.23 epoch sec; seqNo:1001 2024-11-06T02:56:32Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:32Z- handleDepth()- Depth_Keller msg: 1730861792.24 epoch sec; seqNo:1072; depth 50.47 2024-11-06T02:56:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861792.25 epoch sec; seqNo:1072; 4061622.91 north; 594300.72 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T02:56:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861789.84 2024-11-06T02:56:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:32Z- waiting for 1730861793.84, time is 1730861792.25 2024-11-06T02:56:32Z- handleDepth()- Depth_Keller msg: 1730861792.67 epoch sec; seqNo:1073; depth 50.46 2024-11-06T02:56:32Z- handleDVL() - DVL msg epochMillisec: 1730861792.68 2024-11-06T02:56:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:56:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:56:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:56:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:56:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:56:32Z- handleDvl() - RDI_Pathfinder msg: 1730861792.68 sec; seqNo:342; #beams:0 2024-11-06T02:56:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:56:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , -0.03 2024-11-06T02:56:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861792.69 epoch sec; seqNo:1073; 4061622.90 north; 594299.50 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T02:56:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861789.84 2024-11-06T02:56:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:32Z- waiting for 1730861793.84, time is 1730861792.69 2024-11-06T02:56:33Z- AHRS_M2 msg: 1730861793.05 epoch sec; seqNo:1002 2024-11-06T02:56:33Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.31 psi 2024-11-06T02:56:33Z- handleDepth()- Depth_Keller msg: 1730861793.06 epoch sec; seqNo:1074; depth 50.44 2024-11-06T02:56:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861793.07 epoch sec; seqNo:1074; 4061623.27 north; 594299.50 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T02:56:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861789.84 2024-11-06T02:56:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:33Z- waiting for 1730861793.84, time is 1730861793.07 2024-11-06T02:56:33Z- AHRS_M2 msg: 1730861793.45 epoch sec; seqNo:1003 2024-11-06T02:56:33Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.31 psi 2024-11-06T02:56:33Z- handleDepth()- Depth_Keller msg: 1730861793.45 epoch sec; seqNo:1075; depth 50.43 2024-11-06T02:56:33Z- handleDVL() - DVL msg epochMillisec: 1730861793.48 2024-11-06T02:56:33Z- handleDvl() - RDI_Pathfinder msg: 1730861793.48 sec; seqNo:343; #beams:4 2024-11-06T02:56:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.48 , 39.55 , 44.65 , 40.77 2024-11-06T02:56:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , -0.07 2024-11-06T02:56:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861793.49 epoch sec; seqNo:1075; 4061623.27 north; 594299.50 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T02:56:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861789.84 2024-11-06T02:56:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:33Z- waiting for 1730861793.84, time is 1730861793.49 2024-11-06T02:56:33Z- AHRS_M2 msg: 1730861793.85 epoch sec; seqNo:1004 2024-11-06T02:56:33Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.31 psi 2024-11-06T02:56:33Z- handleDepth()- Depth_Keller msg: 1730861793.86 epoch sec; seqNo:1076; depth 50.40 2024-11-06T02:56:33Z- handleDVL() - DVL msg epochMillisec: 1730861793.86 2024-11-06T02:56:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:56:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:56:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:56:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:56:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:56:33Z- handleDvl() - RDI_Pathfinder msg: 1730861793.86 sec; seqNo:344; #beams:0 2024-11-06T02:56:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:56:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , -0.07 2024-11-06T02:56:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861793.89 epoch sec; seqNo:1076; 4061623.65 north; 594299.49 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T02:56:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861789.84 2024-11-06T02:56:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:34Z- waiting for 1730861793.84, time is 1730861793.89 2024-11-06T02:56:34Z- LcmTrn::updateTrn() >>>> pose time 1730861793.89, meas time 1730861793.86 <<<< 2024-11-06T02:56:34Z- LcmTrn::updateTrn() equating pose 1730861793.86 and meas times 1730861793.86 2024-11-06T02:56:34Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861793.86 2024-11-06T02:56:34Z- TerrainNav::motionUpdate() - currEstimate = (4061624, 594299.49, 50.40) 2024-11-06T02:56:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:56:34Z- TerrainNav:: SubcloudNIS is 0.48, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:56:34Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:56:34Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:56:34Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:56:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:56:34Z- variances([0], [2], and psi_berg) = 0.253575 0.224321 0.000000 2024-11-06T02:56:34Z- publishEstimatesDecoder() - at 1730861793.86 sec 2024-11-06T02:56:34Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:56:34Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061656.26 594282.99 57.16 2024-11-06T02:56:34Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061656.15 594283.43 55.83 lat/lon: 36.70 -121.94 2024-11-06T02:56:34Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.22 0.95 0.00 2024-11-06T02:56:34Z- AHRS_M2 msg: 1730861794.26 epoch sec; seqNo:1005 2024-11-06T02:56:34Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.31 psi 2024-11-06T02:56:34Z- handleDepth()- Depth_Keller msg: 1730861794.27 epoch sec; seqNo:1077; depth 50.37 2024-11-06T02:56:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861794.27 epoch sec; seqNo:1077; 4061623.64 north; 594298.27 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T02:56:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861793.86 2024-11-06T02:56:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:34Z- waiting for 1730861797.86, time is 1730861794.27 2024-11-06T02:56:34Z- AHRS_M2 msg: 1730861794.67 epoch sec; seqNo:1006 2024-11-06T02:56:34Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.31 psi 2024-11-06T02:56:34Z- handleDepth()- Depth_Keller msg: 1730861794.67 epoch sec; seqNo:1078; depth 50.34 2024-11-06T02:56:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861794.68 epoch sec; seqNo:1078; 4061624.02 north; 594298.27 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T02:56:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861793.86 2024-11-06T02:56:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:34Z- waiting for 1730861797.86, time is 1730861794.68 2024-11-06T02:56:35Z- AHRS_M2 msg: 1730861795.07 epoch sec; seqNo:1007 2024-11-06T02:56:35Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:35Z- handleDepth()- Depth_Keller msg: 1730861795.08 epoch sec; seqNo:1079; depth 50.31 2024-11-06T02:56:35Z- handleDVL() - DVL msg epochMillisec: 1730861795.08 2024-11-06T02:56:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:56:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:56:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:56:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:56:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:56:35Z- handleDvl() - RDI_Pathfinder msg: 1730861795.08 sec; seqNo:345; #beams:0 2024-11-06T02:56:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:56:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , -0.07 2024-11-06T02:56:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861795.09 epoch sec; seqNo:1079; 4061624.02 north; 594298.27 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T02:56:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861793.86 2024-11-06T02:56:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:35Z- waiting for 1730861797.86, time is 1730861795.09 2024-11-06T02:56:35Z- AHRS_M2 msg: 1730861795.47 epoch sec; seqNo:1008 2024-11-06T02:56:35Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:35Z- handleDepth()- Depth_Keller msg: 1730861795.47 epoch sec; seqNo:1080; depth 50.28 2024-11-06T02:56:35Z- handleDVL() - DVL msg epochMillisec: 1730861795.50 2024-11-06T02:56:35Z- handleDvl() - RDI_Pathfinder msg: 1730861795.50 sec; seqNo:346; #beams:4 2024-11-06T02:56:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 39.55 , 44.65 , 40.98 2024-11-06T02:56:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.07 , -0.10 2024-11-06T02:56:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861795.52 epoch sec; seqNo:1080; 4061624.40 north; 594298.26 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T02:56:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861793.86 2024-11-06T02:56:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:35Z- waiting for 1730861797.86, time is 1730861795.52 2024-11-06T02:56:35Z- AHRS_M2 msg: 1730861795.87 epoch sec; seqNo:1009 2024-11-06T02:56:35Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:35Z- handleDepth()- Depth_Keller msg: 1730861795.88 epoch sec; seqNo:1081; depth 50.27 2024-11-06T02:56:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861795.89 epoch sec; seqNo:1081; 4061624.38 north; 594297.04 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T02:56:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861793.86 2024-11-06T02:56:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:36Z- waiting for 1730861797.86, time is 1730861795.89 2024-11-06T02:56:36Z- AHRS_M2 msg: 1730861796.28 epoch sec; seqNo:1010 2024-11-06T02:56:36Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:36Z- handleDepth()- Depth_Keller msg: 1730861796.28 epoch sec; seqNo:1082; depth 50.25 2024-11-06T02:56:36Z- handleDVL() - DVL msg epochMillisec: 1730861796.28 2024-11-06T02:56:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:56:36Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:56:36Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:56:36Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:56:36Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:56:36Z- handleDvl() - RDI_Pathfinder msg: 1730861796.28 sec; seqNo:347; #beams:0 2024-11-06T02:56:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:56:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.07 , -0.10 2024-11-06T02:56:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861796.29 epoch sec; seqNo:1082; 4061624.76 north; 594297.04 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T02:56:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861793.86 2024-11-06T02:56:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:36Z- waiting for 1730861797.86, time is 1730861796.29 2024-11-06T02:56:36Z- AHRS_M2 msg: 1730861796.68 epoch sec; seqNo:1011 2024-11-06T02:56:36Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:36Z- handleDepth()- Depth_Keller msg: 1730861796.69 epoch sec; seqNo:1083; depth 50.22 2024-11-06T02:56:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861796.71 epoch sec; seqNo:1083; 4061624.76 north; 594297.04 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T02:56:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861793.86 2024-11-06T02:56:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:36Z- waiting for 1730861797.86, time is 1730861796.71 2024-11-06T02:56:37Z- AHRS_M2 msg: 1730861797.12 epoch sec; seqNo:1012 2024-11-06T02:56:37Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:37Z- handleDepth()- Depth_Keller msg: 1730861797.12 epoch sec; seqNo:1084; depth 50.20 2024-11-06T02:56:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861797.13 epoch sec; seqNo:1084; 4061625.14 north; 594297.04 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T02:56:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861793.86 2024-11-06T02:56:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:37Z- waiting for 1730861797.86, time is 1730861797.13 2024-11-06T02:56:37Z- AHRS_M2 msg: 1730861797.49 epoch sec; seqNo:1013 2024-11-06T02:56:37Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:37Z- handleDepth()- Depth_Keller msg: 1730861797.50 epoch sec; seqNo:1085; depth 50.18 2024-11-06T02:56:37Z- handleDVL() - DVL msg epochMillisec: 1730861797.52 2024-11-06T02:56:37Z- handleDvl() - RDI_Pathfinder msg: 1730861797.52 sec; seqNo:348; #beams:4 2024-11-06T02:56:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 39.55 , 44.65 , 40.78 2024-11-06T02:56:37Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.07 , -0.11 2024-11-06T02:56:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861797.54 epoch sec; seqNo:1085; 4061625.13 north; 594295.81 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T02:56:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861793.86 2024-11-06T02:56:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:37Z- waiting for 1730861797.86, time is 1730861797.54 2024-11-06T02:56:37Z- AHRS_M2 msg: 1730861797.89 epoch sec; seqNo:1014 2024-11-06T02:56:37Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:37Z- handleDepth()- Depth_Keller msg: 1730861797.90 epoch sec; seqNo:1086; depth 50.16 2024-11-06T02:56:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861797.92 epoch sec; seqNo:1086; 4061625.51 north; 594295.81 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T02:56:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861793.86 2024-11-06T02:56:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:38Z- waiting for 1730861797.86, time is 1730861797.92 2024-11-06T02:56:38Z- LcmTrn::updateTrn() >>>> pose time 1730861797.92, meas time 1730861797.52 <<<< 2024-11-06T02:56:38Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861797.92 2024-11-06T02:56:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861797.52 sec, ping # 0; waiting for INS data more recent than 1730861793.86... 2024-11-06T02:56:38Z- TerrainNav::motionUpdate() - currEstimate = (4061626, 594295.81, 50.16) 2024-11-06T02:56:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:56:38Z- TerrainNav:: SubcloudNIS is 0.48, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:56:38Z- TNavPF::Projecting Measurements 2024-11-06T02:56:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:56:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:56:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:56:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:56:38Z- TNavFilter::4 good beams. 2024-11-06T02:56:38Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:56:38Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:56:38Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:56:38Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:56:38Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:56:38Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:56:38Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:56:38Z- TerrainMapOctree: (vehN, vehE ) = (4061625.33, 594296.17). 2024-11-06T02:56:38Z- TerrainMapOctree: Min Distance = 132.95. 2024-11-06T02:56:38Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:56:38Z- Forcing Subcloud Comparison 2024-11-06T02:56:38Z- Weighting particles with subcloud comparison 2024-11-06T02:56:38Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:38Z- meanExpectedMeasDiff: -1.679808 delta_rms_squared: 1.885196 alpha: 0.989502 2024-11-06T02:56:38Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:38Z- meanExpectedMeasDiff: 0.409714 delta_rms_squared: 1.396309 alpha: 0.985879 2024-11-06T02:56:38Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:38Z- meanExpectedMeasDiff: -1.291411 delta_rms_squared: 1.728964 alpha: 0.988565 2024-11-06T02:56:38Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:38Z- meanExpectedMeasDiff: 0.092245 delta_rms_squared: 1.509174 alpha: 0.986921 2024-11-06T02:56:38Z- Subcloud NIS: 0.459392 2024-11-06T02:56:38Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:56:38Z- TNavPF:: sumWeights = 0.446766 2024-11-06T02:56:38Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:56:38Z- TNavPF::First Term of Map Covariance Matrix is 0.59 2024-11-06T02:56:38Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.05 2024-11-06T02:56:38Z- TerrainNav::Resampling particle filter... 2024-11-06T02:56:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861797.52 sec. 2024-11-06T02:56:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:56:38Z- variances([0], [2], and psi_berg) = 0.259354 0.224284 0.000000 2024-11-06T02:56:38Z- publishEstimatesDecoder() - at 1730861797.92 sec 2024-11-06T02:56:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:56:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061657.29 594280.83 56.46 2024-11-06T02:56:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061658.00 594279.74 55.88 lat/lon: 36.70 -121.94 2024-11-06T02:56:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.22 0.75 0.00 2024-11-06T02:56:38Z- AHRS_M2 msg: 1730861798.29 epoch sec; seqNo:1015 2024-11-06T02:56:38Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:38Z- handleDepth()- Depth_Keller msg: 1730861798.32 epoch sec; seqNo:1087; depth 50.15 2024-11-06T02:56:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861798.34 epoch sec; seqNo:1087; 4061625.51 north; 594295.81 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T02:56:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861797.92 2024-11-06T02:56:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:38Z- waiting for 1730861801.92, time is 1730861798.34 2024-11-06T02:56:38Z- AHRS_M2 msg: 1730861798.70 epoch sec; seqNo:1016 2024-11-06T02:56:38Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:38Z- handleDepth()- Depth_Keller msg: 1730861798.71 epoch sec; seqNo:1088; depth 50.14 2024-11-06T02:56:38Z- handleDVL() - DVL msg epochMillisec: 1730861798.71 2024-11-06T02:56:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:56:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:56:38Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:56:38Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:56:38Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:56:38Z- handleDvl() - RDI_Pathfinder msg: 1730861798.71 sec; seqNo:349; #beams:0 2024-11-06T02:56:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:56:38Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.07 , -0.11 2024-11-06T02:56:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861798.72 epoch sec; seqNo:1088; 4061625.87 north; 594294.59 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T02:56:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861797.92 2024-11-06T02:56:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:38Z- waiting for 1730861801.92, time is 1730861798.72 2024-11-06T02:56:39Z- AHRS_M2 msg: 1730861799.10 epoch sec; seqNo:1017 2024-11-06T02:56:39Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.29 psi 2024-11-06T02:56:39Z- handleDepth()- Depth_Keller msg: 1730861799.12 epoch sec; seqNo:1089; depth 50.14 2024-11-06T02:56:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861799.13 epoch sec; seqNo:1089; 4061625.87 north; 594294.59 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T02:56:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861797.92 2024-11-06T02:56:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:39Z- waiting for 1730861801.92, time is 1730861799.13 2024-11-06T02:56:39Z- AHRS_M2 msg: 1730861799.51 epoch sec; seqNo:1018 2024-11-06T02:56:39Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.29 psi 2024-11-06T02:56:39Z- handleDepth()- Depth_Keller msg: 1730861799.52 epoch sec; seqNo:1090; depth 50.12 2024-11-06T02:56:39Z- handleDVL() - DVL msg epochMillisec: 1730861799.54 2024-11-06T02:56:39Z- handleDvl() - RDI_Pathfinder msg: 1730861799.54 sec; seqNo:350; #beams:4 2024-11-06T02:56:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 39.55 , 44.65 , 40.78 2024-11-06T02:56:39Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , -0.08 , -0.05 2024-11-06T02:56:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861799.55 epoch sec; seqNo:1090; 4061626.25 north; 594294.58 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T02:56:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861797.92 2024-11-06T02:56:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:39Z- waiting for 1730861801.92, time is 1730861799.55 2024-11-06T02:56:39Z- AHRS_M2 msg: 1730861799.91 epoch sec; seqNo:1019 2024-11-06T02:56:39Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.29 psi 2024-11-06T02:56:39Z- handleDepth()- Depth_Keller msg: 1730861799.93 epoch sec; seqNo:1091; depth 50.12 2024-11-06T02:56:39Z- handleDVL() - DVL msg epochMillisec: 1730861799.93 2024-11-06T02:56:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:56:39Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:56:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:56:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:56:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:56:39Z- handleDvl() - RDI_Pathfinder msg: 1730861799.93 sec; seqNo:351; #beams:0 2024-11-06T02:56:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:56:39Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , -0.08 , -0.05 2024-11-06T02:56:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861799.95 epoch sec; seqNo:1091; 4061626.25 north; 594294.58 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T02:56:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861797.92 2024-11-06T02:56:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:40Z- waiting for 1730861801.92, time is 1730861799.95 2024-11-06T02:56:40Z- AHRS_M2 msg: 1730861800.32 epoch sec; seqNo:1020 2024-11-06T02:56:40Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.29 psi 2024-11-06T02:56:40Z- handleDepth()- Depth_Keller msg: 1730861800.32 epoch sec; seqNo:1092; depth 50.14 2024-11-06T02:56:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861800.33 epoch sec; seqNo:1092; 4061626.62 north; 594293.36 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T02:56:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861797.92 2024-11-06T02:56:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:40Z- waiting for 1730861801.92, time is 1730861800.33 2024-11-06T02:56:40Z- AHRS_M2 msg: 1730861800.72 epoch sec; seqNo:1021 2024-11-06T02:56:40Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.29 psi 2024-11-06T02:56:40Z- handleDepth()- Depth_Keller msg: 1730861800.73 epoch sec; seqNo:1093; depth 50.13 2024-11-06T02:56:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861800.73 epoch sec; seqNo:1093; 4061626.62 north; 594293.36 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T02:56:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861797.92 2024-11-06T02:56:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:40Z- waiting for 1730861801.92, time is 1730861800.73 2024-11-06T02:56:41Z- AHRS_M2 msg: 1730861801.13 epoch sec; seqNo:1022 2024-11-06T02:56:41Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.29 psi 2024-11-06T02:56:41Z- handleDepth()- Depth_Keller msg: 1730861801.13 epoch sec; seqNo:1094; depth 50.14 2024-11-06T02:56:41Z- handleDVL() - DVL msg epochMillisec: 1730861801.13 2024-11-06T02:56:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:56:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:56:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:56:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:56:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:56:41Z- handleDvl() - RDI_Pathfinder msg: 1730861801.13 sec; seqNo:352; #beams:0 2024-11-06T02:56:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:56:41Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , -0.08 , -0.05 2024-11-06T02:56:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861801.15 epoch sec; seqNo:1094; 4061627.00 north; 594293.35 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T02:56:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861797.92 2024-11-06T02:56:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:41Z- waiting for 1730861801.92, time is 1730861801.15 2024-11-06T02:56:41Z- AHRS_M2 msg: 1730861801.53 epoch sec; seqNo:1023 2024-11-06T02:56:41Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.28 psi 2024-11-06T02:56:41Z- handleDepth()- Depth_Keller msg: 1730861801.53 epoch sec; seqNo:1095; depth 50.13 2024-11-06T02:56:41Z- handleDVL() - DVL msg epochMillisec: 1730861801.55 2024-11-06T02:56:41Z- handleDvl() - RDI_Pathfinder msg: 1730861801.55 sec; seqNo:353; #beams:4 2024-11-06T02:56:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 39.55 , 45.06 , 41.19 2024-11-06T02:56:41Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.07 , 0.05 2024-11-06T02:56:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861801.57 epoch sec; seqNo:1095; 4061627.36 north; 594292.13 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T02:56:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861797.92 2024-11-06T02:56:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:41Z- waiting for 1730861801.92, time is 1730861801.57 2024-11-06T02:56:41Z- AHRS_M2 msg: 1730861801.93 epoch sec; seqNo:1024 2024-11-06T02:56:41Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.28 psi 2024-11-06T02:56:41Z- handleDepth()- Depth_Keller msg: 1730861801.95 epoch sec; seqNo:1096; depth 50.15 2024-11-06T02:56:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861801.96 epoch sec; seqNo:1096; 4061627.36 north; 594292.13 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T02:56:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861797.92 2024-11-06T02:56:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:42Z- waiting for 1730861801.92, time is 1730861801.96 2024-11-06T02:56:42Z- LcmTrn::updateTrn() >>>> pose time 1730861801.96, meas time 1730861801.55 <<<< 2024-11-06T02:56:42Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861801.96 2024-11-06T02:56:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861801.55 sec, ping # 0; waiting for INS data more recent than 1730861797.92... 2024-11-06T02:56:42Z- TerrainNav::motionUpdate() - currEstimate = (4061627, 594292.13, 50.15) 2024-11-06T02:56:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:56:42Z- TerrainNav:: SubcloudNIS is 0.46, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:56:42Z- TNavPF::Projecting Measurements 2024-11-06T02:56:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:56:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:56:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:56:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:56:42Z- TNavFilter::4 good beams. 2024-11-06T02:56:42Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:56:42Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:56:42Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:56:42Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:56:42Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:56:42Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:56:42Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:56:42Z- TerrainMapOctree: (vehN, vehE ) = (4061627.17, 594292.50). 2024-11-06T02:56:42Z- TerrainMapOctree: Min Distance = 129.97. 2024-11-06T02:56:42Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:56:42Z- Forcing Subcloud Comparison 2024-11-06T02:56:42Z- Weighting particles with subcloud comparison 2024-11-06T02:56:42Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:42Z- meanExpectedMeasDiff: -0.863706 delta_rms_squared: 1.596574 alpha: 0.987628 2024-11-06T02:56:42Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:42Z- meanExpectedMeasDiff: 0.757165 delta_rms_squared: 1.172009 alpha: 0.983222 2024-11-06T02:56:42Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:42Z- meanExpectedMeasDiff: -0.519611 delta_rms_squared: 1.453308 alpha: 0.986425 2024-11-06T02:56:42Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:42Z- meanExpectedMeasDiff: 0.729124 delta_rms_squared: 1.275262 alpha: 0.984559 2024-11-06T02:56:42Z- Subcloud NIS: 0.414887 2024-11-06T02:56:42Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:56:42Z- TNavPF:: sumWeights = 0.548648 2024-11-06T02:56:42Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:56:42Z- TNavPF::First Term of Map Covariance Matrix is 0.49 2024-11-06T02:56:42Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T02:56:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861801.55 sec. 2024-11-06T02:56:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:56:42Z- variances([0], [2], and psi_berg) = 0.264691 0.226212 0.000000 2024-11-06T02:56:42Z- publishEstimatesDecoder() - at 1730861801.96 sec 2024-11-06T02:56:42Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:56:42Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061660.10 594276.60 56.26 2024-11-06T02:56:42Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061659.86 594276.05 55.86 lat/lon: 36.70 -121.94 2024-11-06T02:56:42Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.23 0.71 0.00 2024-11-06T02:56:42Z- AHRS_M2 msg: 1730861802.34 epoch sec; seqNo:1025 2024-11-06T02:56:42Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.28 psi 2024-11-06T02:56:42Z- handleDepth()- Depth_Keller msg: 1730861802.34 epoch sec; seqNo:1097; depth 50.14 2024-11-06T02:56:42Z- handleDVL() - DVL msg epochMillisec: 1730861802.34 2024-11-06T02:56:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:56:42Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:56:42Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:56:42Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:56:42Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:56:42Z- handleDvl() - RDI_Pathfinder msg: 1730861802.34 sec; seqNo:354; #beams:0 2024-11-06T02:56:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:56:42Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.07 , 0.05 2024-11-06T02:56:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861802.35 epoch sec; seqNo:1097; 4061627.36 north; 594292.13 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T02:56:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861801.96 2024-11-06T02:56:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:42Z- waiting for 1730861805.96, time is 1730861802.35 2024-11-06T02:56:42Z- AHRS_M2 msg: 1730861802.74 epoch sec; seqNo:1026 2024-11-06T02:56:42Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.28 psi 2024-11-06T02:56:42Z- handleDepth()- Depth_Keller msg: 1730861802.74 epoch sec; seqNo:1098; depth 50.15 2024-11-06T02:56:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861802.75 epoch sec; seqNo:1098; 4061627.74 north; 594292.12 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T02:56:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861801.96 2024-11-06T02:56:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:42Z- waiting for 1730861805.96, time is 1730861802.75 2024-11-06T02:56:43Z- AHRS_M2 msg: 1730861803.15 epoch sec; seqNo:1027 2024-11-06T02:56:43Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.28 psi 2024-11-06T02:56:43Z- handleDepth()- Depth_Keller msg: 1730861803.16 epoch sec; seqNo:1099; depth 50.14 2024-11-06T02:56:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861803.17 epoch sec; seqNo:1099; 4061628.11 north; 594290.90 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T02:56:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861801.96 2024-11-06T02:56:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:43Z- waiting for 1730861805.96, time is 1730861803.17 2024-11-06T02:56:43Z- AHRS_M2 msg: 1730861803.55 epoch sec; seqNo:1028 2024-11-06T02:56:43Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.28 psi 2024-11-06T02:56:43Z- handleDepth()- Depth_Keller msg: 1730861803.56 epoch sec; seqNo:1100; depth 50.13 2024-11-06T02:56:43Z- handleDVL() - DVL msg epochMillisec: 1730861803.58 2024-11-06T02:56:43Z- handleDvl() - RDI_Pathfinder msg: 1730861803.58 sec; seqNo:355; #beams:4 2024-11-06T02:56:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 40.17 , 45.47 , 41.19 2024-11-06T02:56:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , 0.07 2024-11-06T02:56:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861803.60 epoch sec; seqNo:1100; 4061628.11 north; 594290.90 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T02:56:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861801.96 2024-11-06T02:56:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:43Z- waiting for 1730861805.96, time is 1730861803.60 2024-11-06T02:56:43Z- AHRS_M2 msg: 1730861803.96 epoch sec; seqNo:1029 2024-11-06T02:56:43Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.29 psi 2024-11-06T02:56:43Z- handleDepth()- Depth_Keller msg: 1730861803.97 epoch sec; seqNo:1101; depth 50.13 2024-11-06T02:56:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861804.00 epoch sec; seqNo:1101; 4061628.11 north; 594290.90 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T02:56:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861801.96 2024-11-06T02:56:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:44Z- waiting for 1730861805.96, time is 1730861804.00 2024-11-06T02:56:44Z- AHRS_M2 msg: 1730861804.36 epoch sec; seqNo:1030 2024-11-06T02:56:44Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.29 psi 2024-11-06T02:56:44Z- handleDepth()- Depth_Keller msg: 1730861804.37 epoch sec; seqNo:1102; depth 50.12 2024-11-06T02:56:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861804.38 epoch sec; seqNo:1102; 4061628.47 north; 594289.68 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T02:56:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861801.96 2024-11-06T02:56:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:44Z- waiting for 1730861805.96, time is 1730861804.38 2024-11-06T02:56:44Z- AHRS_M2 msg: 1730861804.76 epoch sec; seqNo:1031 2024-11-06T02:56:44Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.29 psi 2024-11-06T02:56:44Z- handleDepth()- Depth_Keller msg: 1730861804.76 epoch sec; seqNo:1103; depth 50.10 2024-11-06T02:56:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861804.77 epoch sec; seqNo:1103; 4061628.47 north; 594289.68 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T02:56:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861801.96 2024-11-06T02:56:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:44Z- waiting for 1730861805.96, time is 1730861804.77 2024-11-06T02:56:45Z- AHRS_M2 msg: 1730861805.16 epoch sec; seqNo:1032 2024-11-06T02:56:45Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:45Z- handleDepth()- Depth_Keller msg: 1730861805.17 epoch sec; seqNo:1104; depth 50.08 2024-11-06T02:56:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861805.18 epoch sec; seqNo:1104; 4061628.85 north; 594289.67 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T02:56:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861801.96 2024-11-06T02:56:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:45Z- waiting for 1730861805.96, time is 1730861805.18 2024-11-06T02:56:45Z- AHRS_M2 msg: 1730861805.57 epoch sec; seqNo:1033 2024-11-06T02:56:45Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:45Z- handleDepth()- Depth_Keller msg: 1730861805.58 epoch sec; seqNo:1105; depth 50.08 2024-11-06T02:56:45Z- handleDVL() - DVL msg epochMillisec: 1730861805.60 2024-11-06T02:56:45Z- handleDvl() - RDI_Pathfinder msg: 1730861805.60 sec; seqNo:356; #beams:4 2024-11-06T02:56:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 40.17 , 45.47 , 41.39 2024-11-06T02:56:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.05 , -0.01 2024-11-06T02:56:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861805.63 epoch sec; seqNo:1105; 4061628.85 north; 594289.67 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T02:56:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861801.96 2024-11-06T02:56:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:45Z- waiting for 1730861805.96, time is 1730861805.63 2024-11-06T02:56:45Z- AHRS_M2 msg: 1730861805.97 epoch sec; seqNo:1034 2024-11-06T02:56:45Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:45Z- handleDepth()- Depth_Keller msg: 1730861805.98 epoch sec; seqNo:1106; depth 50.05 2024-11-06T02:56:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861806.00 epoch sec; seqNo:1106; 4061628.84 north; 594288.45 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T02:56:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861801.96 2024-11-06T02:56:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:46Z- waiting for 1730861805.96, time is 1730861806.00 2024-11-06T02:56:46Z- LcmTrn::updateTrn() >>>> pose time 1730861806.00, meas time 1730861805.60 <<<< 2024-11-06T02:56:46Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861806.00 2024-11-06T02:56:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861805.60 sec, ping # 0; waiting for INS data more recent than 1730861801.96... 2024-11-06T02:56:46Z- TerrainNav::motionUpdate() - currEstimate = (4061629, 594288.45, 50.05) 2024-11-06T02:56:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:56:46Z- TerrainNav:: SubcloudNIS is 0.41, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:56:46Z- TNavPF::Projecting Measurements 2024-11-06T02:56:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:56:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:56:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:56:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:56:46Z- TNavFilter::4 good beams. 2024-11-06T02:56:46Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:56:46Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:56:46Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:56:46Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:56:46Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:56:46Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:56:46Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:56:46Z- TerrainMapOctree: (vehN, vehE ) = (4061628.69, 594288.82). 2024-11-06T02:56:46Z- TerrainMapOctree: Min Distance = 127.37. 2024-11-06T02:56:46Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:56:46Z- Forcing Subcloud Comparison 2024-11-06T02:56:46Z- Weighting particles with subcloud comparison 2024-11-06T02:56:46Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:46Z- meanExpectedMeasDiff: -0.914397 delta_rms_squared: 1.544663 alpha: 0.987218 2024-11-06T02:56:46Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:46Z- meanExpectedMeasDiff: 1.379557 delta_rms_squared: 1.098386 alpha: 0.982117 2024-11-06T02:56:46Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:46Z- meanExpectedMeasDiff: -0.348469 delta_rms_squared: 1.393689 alpha: 0.985853 2024-11-06T02:56:46Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:46Z- meanExpectedMeasDiff: 0.796312 delta_rms_squared: 1.198576 alpha: 0.983587 2024-11-06T02:56:46Z- Subcloud NIS: 0.374487 2024-11-06T02:56:46Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:56:46Z- TNavPF:: sumWeights = 0.480609 2024-11-06T02:56:46Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:56:46Z- TNavPF::First Term of Map Covariance Matrix is 0.51 2024-11-06T02:56:46Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T02:56:46Z- TerrainNav::Resampling particle filter... 2024-11-06T02:56:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861805.60 sec. 2024-11-06T02:56:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:56:46Z- variances([0], [2], and psi_berg) = 0.266980 0.231450 0.000000 2024-11-06T02:56:46Z- publishEstimatesDecoder() - at 1730861806.00 sec 2024-11-06T02:56:46Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:56:46Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061660.62 594273.43 55.93 2024-11-06T02:56:46Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061661.33 594272.37 55.64 lat/lon: 36.70 -121.94 2024-11-06T02:56:46Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.23 0.71 0.00 2024-11-06T02:56:46Z- AHRS_M2 msg: 1730861806.40 epoch sec; seqNo:1035 2024-11-06T02:56:46Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:46Z- handleDepth()- Depth_Keller msg: 1730861806.40 epoch sec; seqNo:1107; depth 50.02 2024-11-06T02:56:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861806.42 epoch sec; seqNo:1107; 4061629.22 north; 594288.45 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T02:56:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861806.00 2024-11-06T02:56:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:46Z- waiting for 1730861810.00, time is 1730861806.42 2024-11-06T02:56:46Z- handleDepth()- Depth_Keller msg: 1730861806.80 epoch sec; seqNo:1108; depth 49.99 2024-11-06T02:56:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861806.81 epoch sec; seqNo:1108; 4061629.22 north; 594288.45 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T02:56:46Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:56:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861806.00 2024-11-06T02:56:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:46Z- waiting for 1730861810.00, time is 1730861806.81 2024-11-06T02:56:47Z- AHRS_M2 msg: 1730861807.19 epoch sec; seqNo:1036 2024-11-06T02:56:47Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:47Z- handleDepth()- Depth_Keller msg: 1730861807.22 epoch sec; seqNo:1109; depth 49.98 2024-11-06T02:56:47Z- handleDVL() - DVL msg epochMillisec: 1730861807.23 2024-11-06T02:56:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:56:47Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:56:47Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:56:47Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:56:47Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:56:47Z- handleDvl() - RDI_Pathfinder msg: 1730861807.23 sec; seqNo:357; #beams:0 2024-11-06T02:56:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:56:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.05 , -0.01 2024-11-06T02:56:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861807.27 epoch sec; seqNo:1109; 4061629.59 north; 594288.44 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T02:56:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861806.00 2024-11-06T02:56:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:47Z- waiting for 1730861810.00, time is 1730861807.27 2024-11-06T02:56:47Z- AHRS_M2 msg: 1730861807.59 epoch sec; seqNo:1037 2024-11-06T02:56:47Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:47Z- handleDepth()- Depth_Keller msg: 1730861807.59 epoch sec; seqNo:1110; depth 49.94 2024-11-06T02:56:47Z- handleDVL() - DVL msg epochMillisec: 1730861807.62 2024-11-06T02:56:47Z- handleDvl() - RDI_Pathfinder msg: 1730861807.62 sec; seqNo:358; #beams:4 2024-11-06T02:56:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.51 , 40.17 , 45.67 , 41.59 2024-11-06T02:56:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.10 2024-11-06T02:56:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861807.65 epoch sec; seqNo:1110; 4061629.59 north; 594288.44 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T02:56:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861806.00 2024-11-06T02:56:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:47Z- waiting for 1730861810.00, time is 1730861807.65 2024-11-06T02:56:48Z- AHRS_M2 msg: 1730861807.99 epoch sec; seqNo:1038 2024-11-06T02:56:48Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:48Z- handleDepth()- Depth_Keller msg: 1730861808.00 epoch sec; seqNo:1111; depth 49.91 2024-11-06T02:56:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861808.01 epoch sec; seqNo:1111; 4061629.96 north; 594287.22 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T02:56:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861806.00 2024-11-06T02:56:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:48Z- waiting for 1730861810.00, time is 1730861808.01 2024-11-06T02:56:48Z- AHRS_M2 msg: 1730861808.40 epoch sec; seqNo:1039 2024-11-06T02:56:48Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:48Z- handleDepth()- Depth_Keller msg: 1730861808.41 epoch sec; seqNo:1112; depth 49.88 2024-11-06T02:56:48Z- handleDVL() - DVL msg epochMillisec: 1730861808.41 2024-11-06T02:56:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:56:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:56:48Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:56:48Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:56:48Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:56:48Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:56:48Z- handleDvl() - RDI_Pathfinder msg: 1730861808.41 sec; seqNo:359; #beams:0 2024-11-06T02:56:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:56:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.10 2024-11-06T02:56:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861808.42 epoch sec; seqNo:1112; 4061629.96 north; 594287.22 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T02:56:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861806.00 2024-11-06T02:56:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:48Z- waiting for 1730861810.00, time is 1730861808.42 2024-11-06T02:56:48Z- AHRS_M2 msg: 1730861808.80 epoch sec; seqNo:1040 2024-11-06T02:56:48Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 5.30 psi 2024-11-06T02:56:48Z- handleDepth()- Depth_Keller msg: 1730861808.80 epoch sec; seqNo:1113; depth 49.86 2024-11-06T02:56:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861808.82 epoch sec; seqNo:1113; 4061630.34 north; 594287.21 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T02:56:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861806.00 2024-11-06T02:56:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:48Z- waiting for 1730861810.00, time is 1730861808.82 2024-11-06T02:56:49Z- AHRS_M2 msg: 1730861809.21 epoch sec; seqNo:1041 2024-11-06T02:56:49Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 5.30 psi 2024-11-06T02:56:49Z- handleDepth()- Depth_Keller msg: 1730861809.22 epoch sec; seqNo:1114; depth 49.83 2024-11-06T02:56:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861809.29 epoch sec; seqNo:1114; 4061630.34 north; 594287.21 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T02:56:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861806.00 2024-11-06T02:56:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:49Z- waiting for 1730861810.00, time is 1730861809.29 2024-11-06T02:56:49Z- AHRS_M2 msg: 1730861809.61 epoch sec; seqNo:1042 2024-11-06T02:56:49Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 5.30 psi 2024-11-06T02:56:49Z- handleDepth()- Depth_Keller msg: 1730861809.62 epoch sec; seqNo:1115; depth 49.80 2024-11-06T02:56:49Z- handleDVL() - DVL msg epochMillisec: 1730861809.64 2024-11-06T02:56:49Z- handleDvl() - RDI_Pathfinder msg: 1730861809.64 sec; seqNo:360; #beams:4 2024-11-06T02:56:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.51 , 40.17 , 45.47 , 41.59 2024-11-06T02:56:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , -0.11 2024-11-06T02:56:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861809.68 epoch sec; seqNo:1115; 4061630.70 north; 594285.99 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T02:56:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861806.00 2024-11-06T02:56:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:49Z- waiting for 1730861810.00, time is 1730861809.68 2024-11-06T02:56:50Z- AHRS_M2 msg: 1730861810.01 epoch sec; seqNo:1043 2024-11-06T02:56:50Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 5.30 psi 2024-11-06T02:56:50Z- handleDepth()- Depth_Keller msg: 1730861810.02 epoch sec; seqNo:1116; depth 49.76 2024-11-06T02:56:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861810.05 epoch sec; seqNo:1116; 4061630.70 north; 594285.99 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T02:56:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861806.00 2024-11-06T02:56:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:50Z- waiting for 1730861810.00, time is 1730861810.05 2024-11-06T02:56:50Z- LcmTrn::updateTrn() >>>> pose time 1730861810.05, meas time 1730861809.64 <<<< 2024-11-06T02:56:50Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861810.05 2024-11-06T02:56:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861809.64 sec, ping # 0; waiting for INS data more recent than 1730861806.00... 2024-11-06T02:56:50Z- TerrainNav::motionUpdate() - currEstimate = (4061631, 594285.99, 49.76) 2024-11-06T02:56:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:56:50Z- TerrainNav:: SubcloudNIS is 0.37, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:56:50Z- TNavPF::Projecting Measurements 2024-11-06T02:56:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:56:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:56:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:56:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:56:50Z- TNavFilter::4 good beams. 2024-11-06T02:56:50Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:56:50Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:56:50Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:56:50Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:56:50Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:56:50Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:56:50Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:56:50Z- TerrainMapOctree: (vehN, vehE ) = (4061630.52, 594286.24). 2024-11-06T02:56:50Z- TerrainMapOctree: Min Distance = 124.86. 2024-11-06T02:56:50Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:56:50Z- Forcing Subcloud Comparison 2024-11-06T02:56:50Z- Weighting particles with subcloud comparison 2024-11-06T02:56:50Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:50Z- meanExpectedMeasDiff: -1.241045 delta_rms_squared: 1.540798 alpha: 0.987186 2024-11-06T02:56:50Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:50Z- meanExpectedMeasDiff: 0.903146 delta_rms_squared: 0.990428 alpha: 0.980206 2024-11-06T02:56:50Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:50Z- meanExpectedMeasDiff: -0.607615 delta_rms_squared: 1.375422 alpha: 0.985667 2024-11-06T02:56:50Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:50Z- meanExpectedMeasDiff: -0.591435 delta_rms_squared: 1.011564 alpha: 0.980612 2024-11-06T02:56:50Z- Subcloud NIS: 0.348353 2024-11-06T02:56:50Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:56:50Z- TNavPF:: sumWeights = 0.528109 2024-11-06T02:56:50Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:56:50Z- TNavPF::First Term of Map Covariance Matrix is 0.55 2024-11-06T02:56:50Z- TNavPF::Calculated NIS Value : 0.21 numBeams normalized NIS: 0.05 2024-11-06T02:56:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861809.64 sec. 2024-11-06T02:56:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:56:50Z- variances([0], [2], and psi_berg) = 0.277015 0.241130 0.000000 2024-11-06T02:56:50Z- publishEstimatesDecoder() - at 1730861810.05 sec 2024-11-06T02:56:50Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:56:50Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061663.41 594268.99 55.96 2024-11-06T02:56:50Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061663.18 594269.90 55.51 lat/lon: 36.70 -121.94 2024-11-06T02:56:50Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.28 0.24 0.74 0.00 2024-11-06T02:56:50Z- AHRS_M2 msg: 1730861810.42 epoch sec; seqNo:1044 2024-11-06T02:56:50Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 5.30 psi 2024-11-06T02:56:50Z- handleDepth()- Depth_Keller msg: 1730861810.42 epoch sec; seqNo:1117; depth 49.73 2024-11-06T02:56:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861810.45 epoch sec; seqNo:1117; 4061631.08 north; 594285.99 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T02:56:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861810.05 2024-11-06T02:56:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:50Z- waiting for 1730861814.05, time is 1730861810.45 2024-11-06T02:56:50Z- AHRS_M2 msg: 1730861810.82 epoch sec; seqNo:1045 2024-11-06T02:56:50Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 5.30 psi 2024-11-06T02:56:50Z- handleDepth()- Depth_Keller msg: 1730861810.83 epoch sec; seqNo:1118; depth 49.73 2024-11-06T02:56:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861810.84 epoch sec; seqNo:1118; 4061631.08 north; 594285.99 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T02:56:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861810.05 2024-11-06T02:56:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:50Z- waiting for 1730861814.05, time is 1730861810.84 2024-11-06T02:56:51Z- AHRS_M2 msg: 1730861811.27 epoch sec; seqNo:1046 2024-11-06T02:56:51Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 5.30 psi 2024-11-06T02:56:51Z- handleDepth()- Depth_Keller msg: 1730861811.29 epoch sec; seqNo:1119; depth 49.69 2024-11-06T02:56:51Z- handleDVL() - DVL msg epochMillisec: 1730861811.35 2024-11-06T02:56:51Z- handleDvl() - RDI_Pathfinder msg: 1730861811.35 sec; seqNo:361; #beams:4 2024-11-06T02:56:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 40.17 , 45.26 , 42.00 2024-11-06T02:56:51Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.06 , -0.08 2024-11-06T02:56:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861811.36 epoch sec; seqNo:1119; 4061631.45 north; 594284.76 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T02:56:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861810.05 2024-11-06T02:56:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:51Z- waiting for 1730861814.05, time is 1730861811.36 2024-11-06T02:56:51Z- AHRS_M2 msg: 1730861811.63 epoch sec; seqNo:1047 2024-11-06T02:56:51Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 5.29 psi 2024-11-06T02:56:51Z- handleDepth()- Depth_Keller msg: 1730861811.63 epoch sec; seqNo:1120; depth 49.67 2024-11-06T02:56:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861811.65 epoch sec; seqNo:1120; 4061631.45 north; 594284.76 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T02:56:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861810.05 2024-11-06T02:56:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:51Z- waiting for 1730861814.05, time is 1730861811.65 2024-11-06T02:56:52Z- AHRS_M2 msg: 1730861812.04 epoch sec; seqNo:1048 2024-11-06T02:56:52Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 5.29 psi 2024-11-06T02:56:52Z- handleDepth()- Depth_Keller msg: 1730861812.04 epoch sec; seqNo:1121; depth 49.67 2024-11-06T02:56:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861812.05 epoch sec; seqNo:1121; 4061631.83 north; 594284.76 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T02:56:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861810.05 2024-11-06T02:56:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:52Z- waiting for 1730861814.05, time is 1730861812.05 2024-11-06T02:56:52Z- AHRS_M2 msg: 1730861812.44 epoch sec; seqNo:1049 2024-11-06T02:56:52Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 5.29 psi 2024-11-06T02:56:52Z- handleDepth()- Depth_Keller msg: 1730861812.45 epoch sec; seqNo:1122; depth 49.67 2024-11-06T02:56:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861812.46 epoch sec; seqNo:1122; 4061631.81 north; 594283.54 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T02:56:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861810.05 2024-11-06T02:56:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:52Z- waiting for 1730861814.05, time is 1730861812.46 2024-11-06T02:56:52Z- AHRS_M2 msg: 1730861812.84 epoch sec; seqNo:1050 2024-11-06T02:56:52Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 5.29 psi 2024-11-06T02:56:52Z- handleDepth()- Depth_Keller msg: 1730861812.84 epoch sec; seqNo:1123; depth 49.65 2024-11-06T02:56:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861812.85 epoch sec; seqNo:1123; 4061632.19 north; 594283.53 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T02:56:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861810.05 2024-11-06T02:56:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:53Z- waiting for 1730861814.05, time is 1730861812.85 2024-11-06T02:56:53Z- AHRS_M2 msg: 1730861813.29 epoch sec; seqNo:1051 2024-11-06T02:56:53Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 5.29 psi 2024-11-06T02:56:53Z- handleDepth()- Depth_Keller msg: 1730861813.30 epoch sec; seqNo:1124; depth 49.65 2024-11-06T02:56:53Z- handleDVL() - DVL msg epochMillisec: 1730861813.32 2024-11-06T02:56:53Z- handleDvl() - RDI_Pathfinder msg: 1730861813.32 sec; seqNo:362; #beams:4 2024-11-06T02:56:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 40.17 , 45.06 , 42.21 2024-11-06T02:56:53Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , -0.09 , -0.03 2024-11-06T02:56:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861813.33 epoch sec; seqNo:1124; 4061632.19 north; 594283.53 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T02:56:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861810.05 2024-11-06T02:56:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:53Z- waiting for 1730861814.05, time is 1730861813.33 2024-11-06T02:56:53Z- AHRS_M2 msg: 1730861813.65 epoch sec; seqNo:1052 2024-11-06T02:56:53Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 5.29 psi 2024-11-06T02:56:53Z- handleDepth()- Depth_Keller msg: 1730861813.66 epoch sec; seqNo:1125; depth 49.65 2024-11-06T02:56:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861813.68 epoch sec; seqNo:1125; 4061632.57 north; 594283.53 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T02:56:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861810.05 2024-11-06T02:56:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:53Z- waiting for 1730861814.05, time is 1730861813.68 2024-11-06T02:56:54Z- AHRS_M2 msg: 1730861814.05 epoch sec; seqNo:1053 2024-11-06T02:56:54Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 5.28 psi 2024-11-06T02:56:54Z- handleDepth()- Depth_Keller msg: 1730861814.06 epoch sec; seqNo:1126; depth 49.64 2024-11-06T02:56:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861814.07 epoch sec; seqNo:1126; 4061632.56 north; 594282.31 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T02:56:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861810.05 2024-11-06T02:56:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:54Z- waiting for 1730861814.05, time is 1730861814.07 2024-11-06T02:56:54Z- LcmTrn::updateTrn() >>>> pose time 1730861814.07, meas time 1730861813.32 <<<< 2024-11-06T02:56:54Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861814.07 2024-11-06T02:56:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861813.32 sec, ping # 0; waiting for INS data more recent than 1730861810.05... 2024-11-06T02:56:54Z- TerrainNav::motionUpdate() - currEstimate = (4061633, 594282.31, 49.64) 2024-11-06T02:56:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:56:54Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:56:54Z- TNavPF::Projecting Measurements 2024-11-06T02:56:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:56:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:56:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:56:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:56:54Z- TNavFilter::4 good beams. 2024-11-06T02:56:54Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:56:54Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:56:54Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:56:54Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:56:54Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:56:54Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:56:54Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:56:54Z- TerrainMapOctree: (vehN, vehE ) = (4061632.21, 594282.99). 2024-11-06T02:56:54Z- TerrainMapOctree: Min Distance = 122.32. 2024-11-06T02:56:54Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:56:54Z- Forcing Subcloud Comparison 2024-11-06T02:56:54Z- Weighting particles with subcloud comparison 2024-11-06T02:56:54Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:54Z- meanExpectedMeasDiff: -1.513821 delta_rms_squared: 1.591541 alpha: 0.987590 2024-11-06T02:56:54Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:54Z- meanExpectedMeasDiff: 0.110667 delta_rms_squared: 0.936733 alpha: 0.979096 2024-11-06T02:56:54Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:54Z- meanExpectedMeasDiff: -0.494254 delta_rms_squared: 1.396349 alpha: 0.985879 2024-11-06T02:56:54Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:54Z- meanExpectedMeasDiff: -0.436519 delta_rms_squared: 1.289328 alpha: 0.984725 2024-11-06T02:56:54Z- Subcloud NIS: 0.330137 2024-11-06T02:56:54Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:56:54Z- TNavPF:: sumWeights = 0.577353 2024-11-06T02:56:54Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:56:54Z- TNavPF::First Term of Map Covariance Matrix is 0.58 2024-11-06T02:56:54Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T02:56:54Z- TerrainNav::Resampling particle filter... 2024-11-06T02:56:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861813.32 sec. 2024-11-06T02:56:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:56:54Z- variances([0], [2], and psi_berg) = 0.280676 0.241749 0.000000 2024-11-06T02:56:54Z- publishEstimatesDecoder() - at 1730861814.07 sec 2024-11-06T02:56:54Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:56:54Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061664.28 594267.19 54.70 2024-11-06T02:56:54Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061665.03 594266.21 55.66 lat/lon: 36.70 -121.94 2024-11-06T02:56:54Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.28 0.24 0.72 0.00 2024-11-06T02:56:54Z- AHRS_M2 msg: 1730861814.45 epoch sec; seqNo:1054 2024-11-06T02:56:54Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 5.28 psi 2024-11-06T02:56:54Z- handleDepth()- Depth_Keller msg: 1730861814.47 epoch sec; seqNo:1127; depth 49.65 2024-11-06T02:56:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861814.47 epoch sec; seqNo:1127; 4061632.94 north; 594282.30 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T02:56:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861814.07 2024-11-06T02:56:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:54Z- waiting for 1730861818.07, time is 1730861814.47 2024-11-06T02:56:54Z- AHRS_M2 msg: 1730861814.86 epoch sec; seqNo:1055 2024-11-06T02:56:54Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 5.28 psi 2024-11-06T02:56:54Z- handleDepth()- Depth_Keller msg: 1730861814.87 epoch sec; seqNo:1128; depth 49.65 2024-11-06T02:56:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861814.88 epoch sec; seqNo:1128; 4061632.94 north; 594282.30 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T02:56:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861814.07 2024-11-06T02:56:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:55Z- waiting for 1730861818.07, time is 1730861814.88 2024-11-06T02:56:55Z- AHRS_M2 msg: 1730861815.30 epoch sec; seqNo:1056 2024-11-06T02:56:55Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 5.28 psi 2024-11-06T02:56:55Z- handleDepth()- Depth_Keller msg: 1730861815.31 epoch sec; seqNo:1129; depth 49.66 2024-11-06T02:56:55Z- handleDVL() - DVL msg epochMillisec: 1730861815.33 2024-11-06T02:56:55Z- handleDvl() - RDI_Pathfinder msg: 1730861815.33 sec; seqNo:363; #beams:4 2024-11-06T02:56:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 40.37 , 45.06 , 42.41 2024-11-06T02:56:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.09 , 0.03 2024-11-06T02:56:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861815.34 epoch sec; seqNo:1129; 4061633.30 north; 594281.08 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T02:56:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861814.07 2024-11-06T02:56:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:55Z- waiting for 1730861818.07, time is 1730861815.34 2024-11-06T02:56:55Z- AHRS_M2 msg: 1730861815.67 epoch sec; seqNo:1057 2024-11-06T02:56:55Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 5.27 psi 2024-11-06T02:56:55Z- handleDepth()- Depth_Keller msg: 1730861815.67 epoch sec; seqNo:1130; depth 49.67 2024-11-06T02:56:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861815.69 epoch sec; seqNo:1130; 4061633.30 north; 594281.08 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T02:56:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861814.07 2024-11-06T02:56:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:55Z- waiting for 1730861818.07, time is 1730861815.69 2024-11-06T02:56:56Z- AHRS_M2 msg: 1730861816.07 epoch sec; seqNo:1058 2024-11-06T02:56:56Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 5.27 psi 2024-11-06T02:56:56Z- handleDepth()- Depth_Keller msg: 1730861816.08 epoch sec; seqNo:1131; depth 49.69 2024-11-06T02:56:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861816.09 epoch sec; seqNo:1131; 4061633.68 north; 594281.08 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T02:56:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861814.07 2024-11-06T02:56:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:56Z- waiting for 1730861818.07, time is 1730861816.09 2024-11-06T02:56:56Z- AHRS_M2 msg: 1730861816.48 epoch sec; seqNo:1059 2024-11-06T02:56:56Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 5.28 psi 2024-11-06T02:56:56Z- handleDepth()- Depth_Keller msg: 1730861816.48 epoch sec; seqNo:1132; depth 49.68 2024-11-06T02:56:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861816.51 epoch sec; seqNo:1132; 4061633.67 north; 594279.85 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T02:56:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861814.07 2024-11-06T02:56:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:56Z- waiting for 1730861818.07, time is 1730861816.51 2024-11-06T02:56:56Z- AHRS_M2 msg: 1730861816.88 epoch sec; seqNo:1060 2024-11-06T02:56:56Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 5.28 psi 2024-11-06T02:56:56Z- handleDepth()- Depth_Keller msg: 1730861816.89 epoch sec; seqNo:1133; depth 49.69 2024-11-06T02:56:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861816.89 epoch sec; seqNo:1133; 4061634.05 north; 594279.85 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T02:56:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861814.07 2024-11-06T02:56:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:57Z- waiting for 1730861818.07, time is 1730861816.89 2024-11-06T02:56:57Z- AHRS_M2 msg: 1730861817.31 epoch sec; seqNo:1061 2024-11-06T02:56:57Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 5.28 psi 2024-11-06T02:56:57Z- handleDepth()- Depth_Keller msg: 1730861817.31 epoch sec; seqNo:1134; depth 49.69 2024-11-06T02:56:57Z- handleDVL() - DVL msg epochMillisec: 1730861817.34 2024-11-06T02:56:57Z- handleDvl() - RDI_Pathfinder msg: 1730861817.34 sec; seqNo:364; #beams:4 2024-11-06T02:56:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 40.37 , 45.06 , 42.82 2024-11-06T02:56:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , 0.04 2024-11-06T02:56:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861817.35 epoch sec; seqNo:1134; 4061634.05 north; 594279.85 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T02:56:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861814.07 2024-11-06T02:56:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:57Z- waiting for 1730861818.07, time is 1730861817.35 2024-11-06T02:56:57Z- handleDepth()- Depth_Keller msg: 1730861817.71 epoch sec; seqNo:1135; depth 49.71 2024-11-06T02:56:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861817.72 epoch sec; seqNo:1135; 4061634.43 north; 594279.85 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T02:56:57Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:56:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861814.07 2024-11-06T02:56:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:57Z- waiting for 1730861818.07, time is 1730861817.72 2024-11-06T02:56:58Z- AHRS_M2 msg: 1730861818.09 epoch sec; seqNo:1062 2024-11-06T02:56:58Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 5.28 psi 2024-11-06T02:56:58Z- handleDepth()- Depth_Keller msg: 1730861818.10 epoch sec; seqNo:1136; depth 49.71 2024-11-06T02:56:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861818.11 epoch sec; seqNo:1136; 4061634.41 north; 594278.63 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T02:56:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861814.07 2024-11-06T02:56:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:58Z- waiting for 1730861818.07, time is 1730861818.11 2024-11-06T02:56:58Z- LcmTrn::updateTrn() >>>> pose time 1730861818.11, meas time 1730861817.34 <<<< 2024-11-06T02:56:58Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861818.11 2024-11-06T02:56:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861817.34 sec, ping # 0; waiting for INS data more recent than 1730861814.07... 2024-11-06T02:56:58Z- TerrainNav::motionUpdate() - currEstimate = (4061634, 594278.63, 49.71) 2024-11-06T02:56:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:56:58Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:56:58Z- TNavPF::Projecting Measurements 2024-11-06T02:56:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:56:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:56:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:56:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:56:58Z- TNavFilter::4 good beams. 2024-11-06T02:56:58Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:56:58Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:56:58Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:56:58Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:56:58Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:56:58Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:56:58Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:56:58Z- TerrainMapOctree: (vehN, vehE ) = (4061634.06, 594279.33). 2024-11-06T02:56:58Z- TerrainMapOctree: Min Distance = 119.60. 2024-11-06T02:56:58Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:56:58Z- Forcing Subcloud Comparison 2024-11-06T02:56:58Z- Weighting particles with subcloud comparison 2024-11-06T02:56:58Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:58Z- meanExpectedMeasDiff: -0.714267 delta_rms_squared: 1.563962 alpha: 0.987373 2024-11-06T02:56:58Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:58Z- meanExpectedMeasDiff: 0.589549 delta_rms_squared: 1.110492 alpha: 0.982309 2024-11-06T02:56:58Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:58Z- meanExpectedMeasDiff: 0.263895 delta_rms_squared: 1.336937 alpha: 0.985261 2024-11-06T02:56:58Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:56:58Z- meanExpectedMeasDiff: 0.117621 delta_rms_squared: 1.280899 alpha: 0.984626 2024-11-06T02:56:58Z- Subcloud NIS: 0.305155 2024-11-06T02:56:58Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:56:58Z- TNavPF:: sumWeights = 0.630071 2024-11-06T02:56:58Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:56:58Z- TNavPF::First Term of Map Covariance Matrix is 0.53 2024-11-06T02:56:58Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T02:56:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861817.34 sec. 2024-11-06T02:56:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:56:58Z- variances([0], [2], and psi_berg) = 0.283963 0.241423 0.000000 2024-11-06T02:56:58Z- publishEstimatesDecoder() - at 1730861818.11 sec 2024-11-06T02:56:58Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:56:58Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061667.01 594262.75 55.93 2024-11-06T02:56:58Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061666.88 594262.53 55.70 lat/lon: 36.70 -121.94 2024-11-06T02:56:58Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.28 0.24 0.70 0.00 2024-11-06T02:56:58Z- AHRS_M2 msg: 1730861818.49 epoch sec; seqNo:1063 2024-11-06T02:56:58Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 5.28 psi 2024-11-06T02:56:58Z- handleDepth()- Depth_Keller msg: 1730861818.50 epoch sec; seqNo:1137; depth 49.71 2024-11-06T02:56:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861818.52 epoch sec; seqNo:1137; 4061634.41 north; 594278.63 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T02:56:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861818.11 2024-11-06T02:56:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:58Z- waiting for 1730861822.11, time is 1730861818.52 2024-11-06T02:56:58Z- AHRS_M2 msg: 1730861818.90 epoch sec; seqNo:1064 2024-11-06T02:56:58Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 5.29 psi 2024-11-06T02:56:58Z- handleDepth()- Depth_Keller msg: 1730861818.90 epoch sec; seqNo:1138; depth 49.71 2024-11-06T02:56:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861818.91 epoch sec; seqNo:1138; 4061634.79 north; 594278.62 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T02:56:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861818.11 2024-11-06T02:56:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:59Z- waiting for 1730861822.11, time is 1730861818.91 2024-11-06T02:56:59Z- AHRS_M2 msg: 1730861819.30 epoch sec; seqNo:1065 2024-11-06T02:56:59Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 5.29 psi 2024-11-06T02:56:59Z- handleDepth()- Depth_Keller msg: 1730861819.31 epoch sec; seqNo:1139; depth 49.72 2024-11-06T02:56:59Z- handleDVL() - DVL msg epochMillisec: 1730861819.34 2024-11-06T02:56:59Z- handleDvl() - RDI_Pathfinder msg: 1730861819.34 sec; seqNo:365; #beams:4 2024-11-06T02:56:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.12 , 40.37 , 45.26 , 43.43 2024-11-06T02:56:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , 0.01 2024-11-06T02:56:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861819.35 epoch sec; seqNo:1139; 4061634.79 north; 594278.62 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T02:56:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:56:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861818.11 2024-11-06T02:56:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:59Z- waiting for 1730861822.11, time is 1730861819.35 2024-11-06T02:56:59Z- AHRS_M2 msg: 1730861819.71 epoch sec; seqNo:1066 2024-11-06T02:56:59Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 5.29 psi 2024-11-06T02:56:59Z- handleDepth()- Depth_Keller msg: 1730861819.71 epoch sec; seqNo:1140; depth 49.73 2024-11-06T02:56:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861819.75 epoch sec; seqNo:1140; 4061635.16 north; 594277.40 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T02:56:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:56:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861818.11 2024-11-06T02:56:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:56:59Z- waiting for 1730861822.11, time is 1730861819.75 2024-11-06T02:57:00Z- AHRS_M2 msg: 1730861820.11 epoch sec; seqNo:1067 2024-11-06T02:57:00Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 5.29 psi 2024-11-06T02:57:00Z- handleDepth()- Depth_Keller msg: 1730861820.12 epoch sec; seqNo:1141; depth 49.71 2024-11-06T02:57:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861820.13 epoch sec; seqNo:1141; 4061635.16 north; 594277.40 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T02:57:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861818.11 2024-11-06T02:57:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:00Z- waiting for 1730861822.11, time is 1730861820.13 2024-11-06T02:57:00Z- AHRS_M2 msg: 1730861820.52 epoch sec; seqNo:1068 2024-11-06T02:57:00Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 5.29 psi 2024-11-06T02:57:00Z- handleDepth()- Depth_Keller msg: 1730861820.52 epoch sec; seqNo:1142; depth 49.71 2024-11-06T02:57:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861820.53 epoch sec; seqNo:1142; 4061635.53 north; 594277.39 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T02:57:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861818.11 2024-11-06T02:57:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:00Z- waiting for 1730861822.11, time is 1730861820.53 2024-11-06T02:57:00Z- AHRS_M2 msg: 1730861820.92 epoch sec; seqNo:1069 2024-11-06T02:57:00Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 5.29 psi 2024-11-06T02:57:00Z- handleDepth()- Depth_Keller msg: 1730861820.93 epoch sec; seqNo:1143; depth 49.70 2024-11-06T02:57:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861820.94 epoch sec; seqNo:1143; 4061635.53 north; 594277.39 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T02:57:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861818.11 2024-11-06T02:57:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:01Z- waiting for 1730861822.11, time is 1730861820.94 2024-11-06T02:57:01Z- AHRS_M2 msg: 1730861821.32 epoch sec; seqNo:1070 2024-11-06T02:57:01Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 5.30 psi 2024-11-06T02:57:01Z- handleDepth()- Depth_Keller msg: 1730861821.33 epoch sec; seqNo:1144; depth 49.69 2024-11-06T02:57:01Z- handleDVL() - DVL msg epochMillisec: 1730861821.36 2024-11-06T02:57:01Z- handleDvl() - RDI_Pathfinder msg: 1730861821.36 sec; seqNo:366; #beams:4 2024-11-06T02:57:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.32 , 40.37 , 45.05 , 43.42 2024-11-06T02:57:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.04 2024-11-06T02:57:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861821.38 epoch sec; seqNo:1144; 4061635.90 north; 594276.17 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T02:57:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861818.11 2024-11-06T02:57:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:01Z- waiting for 1730861822.11, time is 1730861821.38 2024-11-06T02:57:01Z- AHRS_M2 msg: 1730861821.73 epoch sec; seqNo:1071 2024-11-06T02:57:01Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 5.30 psi 2024-11-06T02:57:01Z- handleDepth()- Depth_Keller msg: 1730861821.74 epoch sec; seqNo:1145; depth 49.68 2024-11-06T02:57:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861821.75 epoch sec; seqNo:1145; 4061635.90 north; 594276.17 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T02:57:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861818.11 2024-11-06T02:57:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:01Z- waiting for 1730861822.11, time is 1730861821.75 2024-11-06T02:57:02Z- AHRS_M2 msg: 1730861822.16 epoch sec; seqNo:1072 2024-11-06T02:57:02Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 5.30 psi 2024-11-06T02:57:02Z- handleDepth()- Depth_Keller msg: 1730861822.16 epoch sec; seqNo:1146; depth 49.67 2024-11-06T02:57:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861822.17 epoch sec; seqNo:1146; 4061636.28 north; 594276.16 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T02:57:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861818.11 2024-11-06T02:57:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:02Z- waiting for 1730861822.11, time is 1730861822.17 2024-11-06T02:57:02Z- LcmTrn::updateTrn() >>>> pose time 1730861822.17, meas time 1730861821.36 <<<< 2024-11-06T02:57:02Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861822.17 2024-11-06T02:57:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861821.36 sec, ping # 0; waiting for INS data more recent than 1730861818.11... 2024-11-06T02:57:02Z- TerrainNav::motionUpdate() - currEstimate = (4061636, 594276.16, 49.67) 2024-11-06T02:57:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:57:02Z- TerrainNav:: SubcloudNIS is 0.31, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:57:02Z- TNavPF::Projecting Measurements 2024-11-06T02:57:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:57:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:57:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:57:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:57:02Z- TNavFilter::4 good beams. 2024-11-06T02:57:02Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:57:02Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:57:02Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:57:02Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:57:02Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:57:02Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:57:02Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:57:02Z- TerrainMapOctree: (vehN, vehE ) = (4061635.91, 594276.65). 2024-11-06T02:57:02Z- TerrainMapOctree: Min Distance = 117.16. 2024-11-06T02:57:02Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:57:02Z- Forcing Subcloud Comparison 2024-11-06T02:57:02Z- Weighting particles with subcloud comparison 2024-11-06T02:57:02Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:02Z- meanExpectedMeasDiff: -0.483143 delta_rms_squared: 1.562725 alpha: 0.987364 2024-11-06T02:57:02Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:02Z- meanExpectedMeasDiff: 0.669929 delta_rms_squared: 1.085792 alpha: 0.981913 2024-11-06T02:57:02Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:02Z- meanExpectedMeasDiff: 0.407703 delta_rms_squared: 1.304228 alpha: 0.984897 2024-11-06T02:57:02Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:02Z- meanExpectedMeasDiff: 0.383693 delta_rms_squared: 1.278206 alpha: 0.984594 2024-11-06T02:57:02Z- Subcloud NIS: 0.286014 2024-11-06T02:57:02Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:57:02Z- TNavPF:: sumWeights = 0.635886 2024-11-06T02:57:02Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:57:02Z- TNavPF::First Term of Map Covariance Matrix is 0.52 2024-11-06T02:57:02Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.03 2024-11-06T02:57:02Z- TerrainNav::Resampling particle filter... 2024-11-06T02:57:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861821.36 sec. 2024-11-06T02:57:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:57:02Z- variances([0], [2], and psi_berg) = 0.287278 0.242200 0.000000 2024-11-06T02:57:02Z- publishEstimatesDecoder() - at 1730861822.17 sec 2024-11-06T02:57:02Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:57:02Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061667.96 594261.09 54.88 2024-11-06T02:57:02Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061668.75 594260.06 55.55 lat/lon: 36.70 -121.94 2024-11-06T02:57:02Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.29 0.24 0.70 0.00 2024-11-06T02:57:02Z- AHRS_M2 msg: 1730861822.54 epoch sec; seqNo:1073 2024-11-06T02:57:02Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 5.30 psi 2024-11-06T02:57:02Z- handleDepth()- Depth_Keller msg: 1730861822.55 epoch sec; seqNo:1147; depth 49.66 2024-11-06T02:57:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861822.58 epoch sec; seqNo:1147; 4061636.28 north; 594276.16 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T02:57:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861822.17 2024-11-06T02:57:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:02Z- waiting for 1730861826.17, time is 1730861822.58 2024-11-06T02:57:02Z- AHRS_M2 msg: 1730861822.94 epoch sec; seqNo:1074 2024-11-06T02:57:02Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 5.30 psi 2024-11-06T02:57:02Z- handleDepth()- Depth_Keller msg: 1730861822.95 epoch sec; seqNo:1148; depth 49.65 2024-11-06T02:57:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861822.96 epoch sec; seqNo:1148; 4061636.27 north; 594274.94 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T02:57:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861822.17 2024-11-06T02:57:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:03Z- waiting for 1730861826.17, time is 1730861822.96 2024-11-06T02:57:03Z- AHRS_M2 msg: 1730861823.35 epoch sec; seqNo:1075 2024-11-06T02:57:03Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 5.30 psi 2024-11-06T02:57:03Z- handleDepth()- Depth_Keller msg: 1730861823.35 epoch sec; seqNo:1149; depth 49.64 2024-11-06T02:57:03Z- handleDVL() - DVL msg epochMillisec: 1730861823.37 2024-11-06T02:57:03Z- handleDvl() - RDI_Pathfinder msg: 1730861823.37 sec; seqNo:367; #beams:4 2024-11-06T02:57:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.12 , 40.37 , 44.85 , 43.43 2024-11-06T02:57:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , -0.08 2024-11-06T02:57:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861823.40 epoch sec; seqNo:1149; 4061636.64 north; 594274.94 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T02:57:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861822.17 2024-11-06T02:57:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:03Z- waiting for 1730861826.17, time is 1730861823.40 2024-11-06T02:57:03Z- AHRS_M2 msg: 1730861823.75 epoch sec; seqNo:1076 2024-11-06T02:57:03Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 5.30 psi 2024-11-06T02:57:03Z- handleDepth()- Depth_Keller msg: 1730861823.77 epoch sec; seqNo:1150; depth 49.63 2024-11-06T02:57:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861823.78 epoch sec; seqNo:1150; 4061637.02 north; 594274.94 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T02:57:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861822.17 2024-11-06T02:57:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:03Z- waiting for 1730861826.17, time is 1730861823.78 2024-11-06T02:57:04Z- AHRS_M2 msg: 1730861824.16 epoch sec; seqNo:1077 2024-11-06T02:57:04Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 5.30 psi 2024-11-06T02:57:04Z- handleDepth()- Depth_Keller msg: 1730861824.16 epoch sec; seqNo:1151; depth 49.63 2024-11-06T02:57:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861824.18 epoch sec; seqNo:1151; 4061637.02 north; 594274.94 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T02:57:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861822.17 2024-11-06T02:57:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:04Z- waiting for 1730861826.17, time is 1730861824.18 2024-11-06T02:57:04Z- AHRS_M2 msg: 1730861824.57 epoch sec; seqNo:1078 2024-11-06T02:57:04Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 5.31 psi 2024-11-06T02:57:04Z- handleDepth()- Depth_Keller msg: 1730861824.57 epoch sec; seqNo:1152; depth 49.61 2024-11-06T02:57:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861824.58 epoch sec; seqNo:1152; 4061637.39 north; 594273.71 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T02:57:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861822.17 2024-11-06T02:57:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:04Z- waiting for 1730861826.17, time is 1730861824.58 2024-11-06T02:57:04Z- AHRS_M2 msg: 1730861824.96 epoch sec; seqNo:1079 2024-11-06T02:57:04Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 5.31 psi 2024-11-06T02:57:04Z- handleDepth()- Depth_Keller msg: 1730861824.97 epoch sec; seqNo:1153; depth 49.60 2024-11-06T02:57:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861824.97 epoch sec; seqNo:1153; 4061637.39 north; 594273.71 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T02:57:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861822.17 2024-11-06T02:57:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:05Z- waiting for 1730861826.17, time is 1730861824.97 2024-11-06T02:57:05Z- AHRS_M2 msg: 1730861825.39 epoch sec; seqNo:1080 2024-11-06T02:57:05Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 5.31 psi 2024-11-06T02:57:05Z- handleDepth()- Depth_Keller msg: 1730861825.40 epoch sec; seqNo:1154; depth 49.60 2024-11-06T02:57:05Z- handleDVL() - DVL msg epochMillisec: 1730861825.43 2024-11-06T02:57:05Z- handleDvl() - RDI_Pathfinder msg: 1730861825.43 sec; seqNo:368; #beams:4 2024-11-06T02:57:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.32 , 40.17 , 44.65 , 43.43 2024-11-06T02:57:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.07 , -0.09 2024-11-06T02:57:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861825.45 epoch sec; seqNo:1154; 4061637.77 north; 594273.71 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T02:57:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861822.17 2024-11-06T02:57:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:05Z- waiting for 1730861826.17, time is 1730861825.45 2024-11-06T02:57:05Z- AHRS_M2 msg: 1730861825.77 epoch sec; seqNo:1081 2024-11-06T02:57:05Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 5.31 psi 2024-11-06T02:57:05Z- handleDepth()- Depth_Keller msg: 1730861825.77 epoch sec; seqNo:1155; depth 49.59 2024-11-06T02:57:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861825.80 epoch sec; seqNo:1155; 4061637.77 north; 594273.71 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T02:57:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861822.17 2024-11-06T02:57:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:05Z- waiting for 1730861826.17, time is 1730861825.80 2024-11-06T02:57:06Z- AHRS_M2 msg: 1730861826.17 epoch sec; seqNo:1082 2024-11-06T02:57:06Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 5.31 psi 2024-11-06T02:57:06Z- handleDepth()- Depth_Keller msg: 1730861826.18 epoch sec; seqNo:1156; depth 49.59 2024-11-06T02:57:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861826.19 epoch sec; seqNo:1156; 4061638.13 north; 594272.48 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T02:57:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861822.17 2024-11-06T02:57:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:06Z- waiting for 1730861826.17, time is 1730861826.19 2024-11-06T02:57:06Z- LcmTrn::updateTrn() >>>> pose time 1730861826.19, meas time 1730861825.43 <<<< 2024-11-06T02:57:06Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861826.19 2024-11-06T02:57:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861825.43 sec, ping # 0; waiting for INS data more recent than 1730861822.17... 2024-11-06T02:57:06Z- TerrainNav::motionUpdate() - currEstimate = (4061638, 594272.48, 49.59) 2024-11-06T02:57:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:57:06Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:57:06Z- TNavPF::Projecting Measurements 2024-11-06T02:57:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:57:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:57:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:57:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:57:06Z- TNavFilter::4 good beams. 2024-11-06T02:57:06Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:57:06Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:57:06Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:57:06Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:57:06Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:57:06Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:57:06Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:57:06Z- TerrainMapOctree: (vehN, vehE ) = (4061637.79, 594273.17). 2024-11-06T02:57:06Z- TerrainMapOctree: Min Distance = 114.58. 2024-11-06T02:57:06Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:57:06Z- Forcing Subcloud Comparison 2024-11-06T02:57:06Z- Weighting particles with subcloud comparison 2024-11-06T02:57:06Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:06Z- meanExpectedMeasDiff: -0.725690 delta_rms_squared: 1.568534 alpha: 0.987410 2024-11-06T02:57:06Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:06Z- meanExpectedMeasDiff: 0.261670 delta_rms_squared: 1.079281 alpha: 0.981806 2024-11-06T02:57:06Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:06Z- meanExpectedMeasDiff: 0.024455 delta_rms_squared: 1.296836 alpha: 0.984812 2024-11-06T02:57:06Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:06Z- meanExpectedMeasDiff: -0.069810 delta_rms_squared: 1.299201 alpha: 0.984839 2024-11-06T02:57:06Z- Subcloud NIS: 0.266006 2024-11-06T02:57:06Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:57:06Z- TNavPF:: sumWeights = 0.659332 2024-11-06T02:57:06Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:57:06Z- TNavPF::First Term of Map Covariance Matrix is 0.54 2024-11-06T02:57:06Z- TNavPF::Calculated NIS Value : 0.07 numBeams normalized NIS: 0.02 2024-11-06T02:57:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861825.43 sec. 2024-11-06T02:57:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:57:06Z- variances([0], [2], and psi_berg) = 0.291529 0.245346 0.000000 2024-11-06T02:57:06Z- publishEstimatesDecoder() - at 1730861826.19 sec 2024-11-06T02:57:06Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:57:06Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061670.39 594255.03 55.75 2024-11-06T02:57:06Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061670.60 594256.37 55.51 lat/lon: 36.70 -121.94 2024-11-06T02:57:06Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.29 0.25 0.72 0.00 2024-11-06T02:57:06Z- AHRS_M2 msg: 1730861826.57 epoch sec; seqNo:1083 2024-11-06T02:57:06Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 5.31 psi 2024-11-06T02:57:06Z- handleDepth()- Depth_Keller msg: 1730861826.59 epoch sec; seqNo:1157; depth 49.59 2024-11-06T02:57:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861826.61 epoch sec; seqNo:1157; 4061638.13 north; 594272.48 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T02:57:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861826.19 2024-11-06T02:57:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:06Z- waiting for 1730861830.19, time is 1730861826.61 2024-11-06T02:57:06Z- AHRS_M2 msg: 1730861826.98 epoch sec; seqNo:1084 2024-11-06T02:57:06Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 5.31 psi 2024-11-06T02:57:06Z- handleDepth()- Depth_Keller msg: 1730861826.98 epoch sec; seqNo:1158; depth 49.59 2024-11-06T02:57:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861826.99 epoch sec; seqNo:1158; 4061638.51 north; 594272.48 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T02:57:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861826.19 2024-11-06T02:57:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:07Z- waiting for 1730861830.19, time is 1730861826.99 2024-11-06T02:57:07Z- AHRS_M2 msg: 1730861827.38 epoch sec; seqNo:1085 2024-11-06T02:57:07Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 5.31 psi 2024-11-06T02:57:07Z- handleDepth()- Depth_Keller msg: 1730861827.39 epoch sec; seqNo:1159; depth 49.59 2024-11-06T02:57:07Z- handleDVL() - DVL msg epochMillisec: 1730861827.41 2024-11-06T02:57:07Z- handleDvl() - RDI_Pathfinder msg: 1730861827.41 sec; seqNo:369; #beams:4 2024-11-06T02:57:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.32 , 40.17 , 44.65 , 43.63 2024-11-06T02:57:07Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.10 , -0.05 2024-11-06T02:57:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861827.43 epoch sec; seqNo:1159; 4061638.50 north; 594271.26 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T02:57:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861826.19 2024-11-06T02:57:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:07Z- waiting for 1730861830.19, time is 1730861827.43 2024-11-06T02:57:07Z- AHRS_M2 msg: 1730861827.79 epoch sec; seqNo:1086 2024-11-06T02:57:07Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 5.31 psi 2024-11-06T02:57:07Z- handleDepth()- Depth_Keller msg: 1730861827.80 epoch sec; seqNo:1160; depth 49.61 2024-11-06T02:57:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861827.81 epoch sec; seqNo:1160; 4061638.88 north; 594271.25 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T02:57:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861826.19 2024-11-06T02:57:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:07Z- waiting for 1730861830.19, time is 1730861827.81 2024-11-06T02:57:08Z- AHRS_M2 msg: 1730861828.19 epoch sec; seqNo:1087 2024-11-06T02:57:08Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 5.31 psi 2024-11-06T02:57:08Z- handleDepth()- Depth_Keller msg: 1730861828.20 epoch sec; seqNo:1161; depth 49.61 2024-11-06T02:57:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861828.22 epoch sec; seqNo:1161; 4061638.88 north; 594271.25 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T02:57:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861826.19 2024-11-06T02:57:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:08Z- waiting for 1730861830.19, time is 1730861828.22 2024-11-06T02:57:08Z- AHRS_M2 msg: 1730861828.59 epoch sec; seqNo:1088 2024-11-06T02:57:08Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 5.31 psi 2024-11-06T02:57:08Z- handleDepth()- Depth_Keller msg: 1730861828.60 epoch sec; seqNo:1162; depth 49.61 2024-11-06T02:57:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861828.61 epoch sec; seqNo:1162; 4061639.26 north; 594271.25 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T02:57:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861826.19 2024-11-06T02:57:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:08Z- waiting for 1730861830.19, time is 1730861828.61 2024-11-06T02:57:09Z- AHRS_M2 msg: 1730861829.00 epoch sec; seqNo:1089 2024-11-06T02:57:09Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 5.31 psi 2024-11-06T02:57:09Z- handleDepth()- Depth_Keller msg: 1730861829.00 epoch sec; seqNo:1163; depth 49.64 2024-11-06T02:57:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861829.01 epoch sec; seqNo:1163; 4061639.24 north; 594270.03 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T02:57:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861826.19 2024-11-06T02:57:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:09Z- waiting for 1730861830.19, time is 1730861829.01 2024-11-06T02:57:09Z- AHRS_M2 msg: 1730861829.40 epoch sec; seqNo:1090 2024-11-06T02:57:09Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 5.31 psi 2024-11-06T02:57:09Z- handleDepth()- Depth_Keller msg: 1730861829.42 epoch sec; seqNo:1164; depth 49.66 2024-11-06T02:57:09Z- handleDVL() - DVL msg epochMillisec: 1730861829.44 2024-11-06T02:57:09Z- handleDvl() - RDI_Pathfinder msg: 1730861829.44 sec; seqNo:370; #beams:4 2024-11-06T02:57:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.53 , 40.17 , 44.45 , 43.63 2024-11-06T02:57:09Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.10 , 0.02 2024-11-06T02:57:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861829.45 epoch sec; seqNo:1164; 4061639.62 north; 594270.03 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T02:57:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861826.19 2024-11-06T02:57:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:09Z- waiting for 1730861830.19, time is 1730861829.45 2024-11-06T02:57:09Z- AHRS_M2 msg: 1730861829.81 epoch sec; seqNo:1091 2024-11-06T02:57:09Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 5.31 psi 2024-11-06T02:57:09Z- handleDepth()- Depth_Keller msg: 1730861829.81 epoch sec; seqNo:1165; depth 49.66 2024-11-06T02:57:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861829.83 epoch sec; seqNo:1165; 4061639.62 north; 594270.03 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T02:57:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861826.19 2024-11-06T02:57:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:09Z- waiting for 1730861830.19, time is 1730861829.83 2024-11-06T02:57:10Z- AHRS_M2 msg: 1730861830.21 epoch sec; seqNo:1092 2024-11-06T02:57:10Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 5.31 psi 2024-11-06T02:57:10Z- handleDepth()- Depth_Keller msg: 1730861830.21 epoch sec; seqNo:1166; depth 49.69 2024-11-06T02:57:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861830.22 epoch sec; seqNo:1166; 4061639.99 north; 594268.80 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T02:57:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861826.19 2024-11-06T02:57:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:10Z- waiting for 1730861830.19, time is 1730861830.22 2024-11-06T02:57:10Z- LcmTrn::updateTrn() >>>> pose time 1730861830.22, meas time 1730861829.44 <<<< 2024-11-06T02:57:10Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861830.22 2024-11-06T02:57:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861829.44 sec, ping # 0; waiting for INS data more recent than 1730861826.19... 2024-11-06T02:57:10Z- TerrainNav::motionUpdate() - currEstimate = (4061640, 594268.80, 49.69) 2024-11-06T02:57:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:57:10Z- TerrainNav:: SubcloudNIS is 0.27, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:57:10Z- TNavPF::Projecting Measurements 2024-11-06T02:57:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:57:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:57:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:57:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:57:10Z- TNavFilter::4 good beams. 2024-11-06T02:57:10Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:57:10Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:57:10Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:57:10Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:57:10Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:57:10Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:57:10Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:57:10Z- TerrainMapOctree: (vehN, vehE ) = (4061639.63, 594269.52). 2024-11-06T02:57:10Z- TerrainMapOctree: Min Distance = 112.09. 2024-11-06T02:57:10Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:57:10Z- Forcing Subcloud Comparison 2024-11-06T02:57:10Z- Weighting particles with subcloud comparison 2024-11-06T02:57:10Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:10Z- meanExpectedMeasDiff: -0.358876 delta_rms_squared: 1.583614 alpha: 0.987528 2024-11-06T02:57:10Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:10Z- meanExpectedMeasDiff: -0.668449 delta_rms_squared: 1.102001 alpha: 0.982175 2024-11-06T02:57:10Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:10Z- meanExpectedMeasDiff: 0.193516 delta_rms_squared: 1.296011 alpha: 0.984803 2024-11-06T02:57:10Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:10Z- meanExpectedMeasDiff: 0.007027 delta_rms_squared: 1.187870 alpha: 0.983442 2024-11-06T02:57:10Z- Subcloud NIS: 0.247309 2024-11-06T02:57:10Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:57:10Z- TNavPF:: sumWeights = 0.661031 2024-11-06T02:57:10Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:57:10Z- TNavPF::First Term of Map Covariance Matrix is 0.51 2024-11-06T02:57:10Z- TNavPF::Calculated NIS Value : 0.07 numBeams normalized NIS: 0.02 2024-11-06T02:57:10Z- TerrainNav::Resampling particle filter... 2024-11-06T02:57:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861829.44 sec. 2024-11-06T02:57:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:57:10Z- variances([0], [2], and psi_berg) = 0.296977 0.246815 0.000000 2024-11-06T02:57:10Z- publishEstimatesDecoder() - at 1730861830.22 sec 2024-11-06T02:57:10Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:57:10Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061671.83 594253.60 55.54 2024-11-06T02:57:10Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061672.46 594252.69 55.71 lat/lon: 36.70 -121.94 2024-11-06T02:57:10Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.30 0.25 0.70 0.00 2024-11-06T02:57:10Z- AHRS_M2 msg: 1730861830.62 epoch sec; seqNo:1093 2024-11-06T02:57:10Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 5.31 psi 2024-11-06T02:57:10Z- handleDepth()- Depth_Keller msg: 1730861830.63 epoch sec; seqNo:1167; depth 49.71 2024-11-06T02:57:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861830.64 epoch sec; seqNo:1167; 4061639.99 north; 594268.80 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T02:57:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861830.22 2024-11-06T02:57:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:10Z- waiting for 1730861834.22, time is 1730861830.64 2024-11-06T02:57:11Z- AHRS_M2 msg: 1730861831.06 epoch sec; seqNo:1094 2024-11-06T02:57:11Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 5.32 psi 2024-11-06T02:57:11Z- handleDepth()- Depth_Keller msg: 1730861831.07 epoch sec; seqNo:1168; depth 49.72 2024-11-06T02:57:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861831.08 epoch sec; seqNo:1168; 4061640.37 north; 594268.80 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T02:57:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861830.22 2024-11-06T02:57:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:11Z- waiting for 1730861834.22, time is 1730861831.08 2024-11-06T02:57:11Z- AHRS_M2 msg: 1730861831.42 epoch sec; seqNo:1095 2024-11-06T02:57:11Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 5.32 psi 2024-11-06T02:57:11Z- handleDepth()- Depth_Keller msg: 1730861831.43 epoch sec; seqNo:1169; depth 49.74 2024-11-06T02:57:11Z- handleDVL() - DVL msg epochMillisec: 1730861831.45 2024-11-06T02:57:11Z- handleDvl() - RDI_Pathfinder msg: 1730861831.45 sec; seqNo:371; #beams:4 2024-11-06T02:57:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.93 , 40.37 , 44.65 , 43.63 2024-11-06T02:57:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.08 , 0.04 2024-11-06T02:57:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861831.48 epoch sec; seqNo:1169; 4061640.74 north; 594268.79 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T02:57:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861830.22 2024-11-06T02:57:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:11Z- waiting for 1730861834.22, time is 1730861831.48 2024-11-06T02:57:11Z- AHRS_M2 msg: 1730861831.83 epoch sec; seqNo:1096 2024-11-06T02:57:11Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 5.32 psi 2024-11-06T02:57:11Z- handleDepth()- Depth_Keller msg: 1730861831.84 epoch sec; seqNo:1170; depth 49.75 2024-11-06T02:57:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861831.85 epoch sec; seqNo:1170; 4061640.73 north; 594267.57 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T02:57:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861830.22 2024-11-06T02:57:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:11Z- waiting for 1730861834.22, time is 1730861831.85 2024-11-06T02:57:12Z- AHRS_M2 msg: 1730861832.23 epoch sec; seqNo:1097 2024-11-06T02:57:12Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 5.32 psi 2024-11-06T02:57:12Z- handleDepth()- Depth_Keller msg: 1730861832.24 epoch sec; seqNo:1171; depth 49.76 2024-11-06T02:57:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861832.24 epoch sec; seqNo:1171; 4061640.73 north; 594267.57 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T02:57:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861830.22 2024-11-06T02:57:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:12Z- waiting for 1730861834.22, time is 1730861832.24 2024-11-06T02:57:12Z- AHRS_M2 msg: 1730861832.64 epoch sec; seqNo:1098 2024-11-06T02:57:12Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 5.32 psi 2024-11-06T02:57:12Z- handleDepth()- Depth_Keller msg: 1730861832.65 epoch sec; seqNo:1172; depth 49.77 2024-11-06T02:57:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861832.66 epoch sec; seqNo:1172; 4061641.11 north; 594267.57 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T02:57:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861830.22 2024-11-06T02:57:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:12Z- waiting for 1730861834.22, time is 1730861832.66 2024-11-06T02:57:13Z- AHRS_M2 msg: 1730861833.04 epoch sec; seqNo:1099 2024-11-06T02:57:13Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 5.33 psi 2024-11-06T02:57:13Z- handleDepth()- Depth_Keller msg: 1730861833.04 epoch sec; seqNo:1173; depth 49.80 2024-11-06T02:57:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861833.06 epoch sec; seqNo:1173; 4061641.10 north; 594266.35 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T02:57:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861830.22 2024-11-06T02:57:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:13Z- waiting for 1730861834.22, time is 1730861833.06 2024-11-06T02:57:13Z- AHRS_M2 msg: 1730861833.44 epoch sec; seqNo:1100 2024-11-06T02:57:13Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 5.33 psi 2024-11-06T02:57:13Z- handleDepth()- Depth_Keller msg: 1730861833.45 epoch sec; seqNo:1174; depth 49.80 2024-11-06T02:57:13Z- handleDVL() - DVL msg epochMillisec: 1730861833.47 2024-11-06T02:57:13Z- handleDvl() - RDI_Pathfinder msg: 1730861833.47 sec; seqNo:372; #beams:4 2024-11-06T02:57:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.93 , 40.37 , 44.65 , 43.84 2024-11-06T02:57:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , 0.00 2024-11-06T02:57:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861833.49 epoch sec; seqNo:1174; 4061641.48 north; 594266.34 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T02:57:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861830.22 2024-11-06T02:57:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:13Z- waiting for 1730861834.22, time is 1730861833.49 2024-11-06T02:57:13Z- AHRS_M2 msg: 1730861833.85 epoch sec; seqNo:1101 2024-11-06T02:57:13Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 5.33 psi 2024-11-06T02:57:13Z- handleDepth()- Depth_Keller msg: 1730861833.86 epoch sec; seqNo:1175; depth 49.81 2024-11-06T02:57:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861833.88 epoch sec; seqNo:1175; 4061641.48 north; 594266.34 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T02:57:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861830.22 2024-11-06T02:57:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:14Z- waiting for 1730861834.22, time is 1730861833.88 2024-11-06T02:57:14Z- AHRS_M2 msg: 1730861834.25 epoch sec; seqNo:1102 2024-11-06T02:57:14Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 5.33 psi 2024-11-06T02:57:14Z- handleDepth()- Depth_Keller msg: 1730861834.26 epoch sec; seqNo:1176; depth 49.83 2024-11-06T02:57:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861834.27 epoch sec; seqNo:1176; 4061641.85 north; 594266.34 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T02:57:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861830.22 2024-11-06T02:57:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:14Z- waiting for 1730861834.22, time is 1730861834.27 2024-11-06T02:57:14Z- LcmTrn::updateTrn() >>>> pose time 1730861834.27, meas time 1730861833.47 <<<< 2024-11-06T02:57:14Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861834.27 2024-11-06T02:57:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861833.47 sec, ping # 0; waiting for INS data more recent than 1730861830.22... 2024-11-06T02:57:14Z- TerrainNav::motionUpdate() - currEstimate = (4061642, 594266.34, 49.83) 2024-11-06T02:57:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:57:14Z- TerrainNav:: SubcloudNIS is 0.25, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:57:14Z- TNavPF::Projecting Measurements 2024-11-06T02:57:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:57:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:57:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:57:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:57:14Z- TNavFilter::4 good beams. 2024-11-06T02:57:14Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:57:14Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:57:14Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:57:14Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:57:14Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:57:14Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:57:14Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:57:14Z- TerrainMapOctree: (vehN, vehE ) = (4061641.49, 594266.82). 2024-11-06T02:57:14Z- TerrainMapOctree: Min Distance = 109.81. 2024-11-06T02:57:14Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:57:14Z- Forcing Subcloud Comparison 2024-11-06T02:57:14Z- Weighting particles with subcloud comparison 2024-11-06T02:57:14Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:14Z- meanExpectedMeasDiff: -0.629424 delta_rms_squared: 2.437821 alpha: 0.991863 2024-11-06T02:57:14Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:14Z- meanExpectedMeasDiff: -0.176679 delta_rms_squared: 1.095287 alpha: 0.982067 2024-11-06T02:57:14Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:14Z- meanExpectedMeasDiff: 0.804707 delta_rms_squared: 1.282500 alpha: 0.984645 2024-11-06T02:57:14Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:14Z- meanExpectedMeasDiff: -0.811202 delta_rms_squared: 1.264132 alpha: 0.984425 2024-11-06T02:57:14Z- Subcloud NIS: 0.234271 2024-11-06T02:57:14Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:57:14Z- TNavPF:: sumWeights = 0.562935 2024-11-06T02:57:14Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:57:14Z- TNavPF::First Term of Map Covariance Matrix is 0.74 2024-11-06T02:57:14Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T02:57:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861833.47 sec. 2024-11-06T02:57:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:57:14Z- variances([0], [2], and psi_berg) = 0.303448 0.253618 0.000000 2024-11-06T02:57:14Z- publishEstimatesDecoder() - at 1730861834.27 sec 2024-11-06T02:57:14Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:57:14Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061673.47 594251.34 55.29 2024-11-06T02:57:14Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061674.32 594250.23 55.94 lat/lon: 36.70 -121.94 2024-11-06T02:57:14Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.30 0.25 0.70 0.00 2024-11-06T02:57:14Z- AHRS_M2 msg: 1730861834.67 epoch sec; seqNo:1103 2024-11-06T02:57:14Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 5.33 psi 2024-11-06T02:57:14Z- handleDepth()- Depth_Keller msg: 1730861834.67 epoch sec; seqNo:1177; depth 49.84 2024-11-06T02:57:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861834.69 epoch sec; seqNo:1177; 4061641.84 north; 594265.12 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T02:57:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861834.27 2024-11-06T02:57:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:14Z- waiting for 1730861838.27, time is 1730861834.69 2024-11-06T02:57:15Z- AHRS_M2 msg: 1730861835.06 epoch sec; seqNo:1104 2024-11-06T02:57:15Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:15Z- handleDepth()- Depth_Keller msg: 1730861835.07 epoch sec; seqNo:1178; depth 49.85 2024-11-06T02:57:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861835.08 epoch sec; seqNo:1178; 4061642.22 north; 594265.11 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T02:57:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861834.27 2024-11-06T02:57:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:15Z- waiting for 1730861838.27, time is 1730861835.08 2024-11-06T02:57:15Z- handleDepth()- Depth_Keller msg: 1730861835.49 epoch sec; seqNo:1179; depth 49.85 2024-11-06T02:57:15Z- handleDVL() - DVL msg epochMillisec: 1730861835.51 2024-11-06T02:57:15Z- handleDvl() - RDI_Pathfinder msg: 1730861835.51 sec; seqNo:373; #beams:4 2024-11-06T02:57:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.93 , 40.17 , 44.45 , 43.84 2024-11-06T02:57:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , -0.04 2024-11-06T02:57:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861835.54 epoch sec; seqNo:1179; 4061642.60 north; 594265.11 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T02:57:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861834.27 2024-11-06T02:57:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:15Z- waiting for 1730861838.27, time is 1730861835.54 2024-11-06T02:57:15Z- AHRS_M2 msg: 1730861835.87 epoch sec; seqNo:1105 2024-11-06T02:57:15Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:15Z- handleDepth()- Depth_Keller msg: 1730861835.88 epoch sec; seqNo:1180; depth 49.86 2024-11-06T02:57:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861835.89 epoch sec; seqNo:1180; 4061642.60 north; 594265.11 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T02:57:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861834.27 2024-11-06T02:57:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:16Z- waiting for 1730861838.27, time is 1730861835.89 2024-11-06T02:57:16Z- AHRS_M2 msg: 1730861836.27 epoch sec; seqNo:1106 2024-11-06T02:57:16Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:16Z- handleDepth()- Depth_Keller msg: 1730861836.28 epoch sec; seqNo:1181; depth 49.86 2024-11-06T02:57:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861836.29 epoch sec; seqNo:1181; 4061642.98 north; 594265.11 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T02:57:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861834.27 2024-11-06T02:57:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:16Z- waiting for 1730861838.27, time is 1730861836.29 2024-11-06T02:57:16Z- AHRS_M2 msg: 1730861836.68 epoch sec; seqNo:1107 2024-11-06T02:57:16Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:16Z- handleDepth()- Depth_Keller msg: 1730861836.69 epoch sec; seqNo:1182; depth 49.86 2024-11-06T02:57:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861836.71 epoch sec; seqNo:1182; 4061642.96 north; 594263.89 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T02:57:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861834.27 2024-11-06T02:57:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:16Z- waiting for 1730861838.27, time is 1730861836.71 2024-11-06T02:57:17Z- AHRS_M2 msg: 1730861837.08 epoch sec; seqNo:1108 2024-11-06T02:57:17Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:17Z- handleDepth()- Depth_Keller msg: 1730861837.09 epoch sec; seqNo:1183; depth 49.86 2024-11-06T02:57:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861837.10 epoch sec; seqNo:1183; 4061643.34 north; 594263.88 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T02:57:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861834.27 2024-11-06T02:57:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:17Z- waiting for 1730861838.27, time is 1730861837.10 2024-11-06T02:57:17Z- AHRS_M2 msg: 1730861837.48 epoch sec; seqNo:1109 2024-11-06T02:57:17Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:17Z- handleDepth()- Depth_Keller msg: 1730861837.49 epoch sec; seqNo:1184; depth 49.87 2024-11-06T02:57:17Z- handleDVL() - DVL msg epochMillisec: 1730861837.51 2024-11-06T02:57:17Z- handleDvl() - RDI_Pathfinder msg: 1730861837.51 sec; seqNo:374; #beams:4 2024-11-06T02:57:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.93 , 40.37 , 44.45 , 43.84 2024-11-06T02:57:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , -0.04 2024-11-06T02:57:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861837.53 epoch sec; seqNo:1184; 4061643.34 north; 594263.88 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T02:57:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861834.27 2024-11-06T02:57:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:17Z- waiting for 1730861838.27, time is 1730861837.53 2024-11-06T02:57:17Z- AHRS_M2 msg: 1730861837.89 epoch sec; seqNo:1110 2024-11-06T02:57:17Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:17Z- handleDepth()- Depth_Keller msg: 1730861837.90 epoch sec; seqNo:1185; depth 49.86 2024-11-06T02:57:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861837.92 epoch sec; seqNo:1185; 4061643.72 north; 594263.88 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T02:57:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861834.27 2024-11-06T02:57:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:18Z- waiting for 1730861838.27, time is 1730861837.92 2024-11-06T02:57:18Z- AHRS_M2 msg: 1730861838.29 epoch sec; seqNo:1111 2024-11-06T02:57:18Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:18Z- handleDepth()- Depth_Keller msg: 1730861838.30 epoch sec; seqNo:1186; depth 49.88 2024-11-06T02:57:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861838.31 epoch sec; seqNo:1186; 4061643.71 north; 594262.66 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T02:57:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861834.27 2024-11-06T02:57:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:18Z- waiting for 1730861838.27, time is 1730861838.31 2024-11-06T02:57:18Z- LcmTrn::updateTrn() >>>> pose time 1730861838.31, meas time 1730861837.51 <<<< 2024-11-06T02:57:18Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861838.31 2024-11-06T02:57:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861837.51 sec, ping # 0; waiting for INS data more recent than 1730861834.27... 2024-11-06T02:57:18Z- TerrainNav::motionUpdate() - currEstimate = (4061644, 594262.66, 49.88) 2024-11-06T02:57:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:57:18Z- TerrainNav:: SubcloudNIS is 0.23, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:57:18Z- TNavPF::Projecting Measurements 2024-11-06T02:57:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:57:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:57:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:57:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:57:18Z- TNavFilter::4 good beams. 2024-11-06T02:57:18Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:57:18Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:57:18Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:57:18Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:57:18Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:57:18Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:57:18Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:57:18Z- TerrainMapOctree: (vehN, vehE ) = (4061643.34, 594263.38). 2024-11-06T02:57:18Z- TerrainMapOctree: Min Distance = 107.49. 2024-11-06T02:57:18Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:57:18Z- Forcing Subcloud Comparison 2024-11-06T02:57:18Z- Weighting particles with subcloud comparison 2024-11-06T02:57:18Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:18Z- meanExpectedMeasDiff: 0.248061 delta_rms_squared: 2.388217 alpha: 0.991695 2024-11-06T02:57:18Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:18Z- meanExpectedMeasDiff: -0.138533 delta_rms_squared: 1.091422 alpha: 0.982005 2024-11-06T02:57:18Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:18Z- meanExpectedMeasDiff: 0.898035 delta_rms_squared: 1.262965 alpha: 0.984411 2024-11-06T02:57:18Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:18Z- meanExpectedMeasDiff: -0.662890 delta_rms_squared: 1.319104 alpha: 0.985065 2024-11-06T02:57:18Z- Subcloud NIS: 0.222293 2024-11-06T02:57:18Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:57:18Z- TNavPF:: sumWeights = 0.581180 2024-11-06T02:57:18Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:57:18Z- TNavPF::First Term of Map Covariance Matrix is 0.74 2024-11-06T02:57:18Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T02:57:18Z- TerrainNav::Resampling particle filter... 2024-11-06T02:57:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861837.51 sec. 2024-11-06T02:57:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:57:18Z- variances([0], [2], and psi_berg) = 0.310425 0.250719 0.000000 2024-11-06T02:57:18Z- publishEstimatesDecoder() - at 1730861838.31 sec 2024-11-06T02:57:18Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:57:18Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061675.60 594247.41 55.14 2024-11-06T02:57:18Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061676.17 594246.54 55.96 lat/lon: 36.70 -121.94 2024-11-06T02:57:18Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.31 0.25 0.70 0.00 2024-11-06T02:57:18Z- AHRS_M2 msg: 1730861838.70 epoch sec; seqNo:1112 2024-11-06T02:57:18Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:18Z- handleDepth()- Depth_Keller msg: 1730861838.72 epoch sec; seqNo:1187; depth 49.88 2024-11-06T02:57:18Z- handleDVL() - DVL msg epochMillisec: 1730861838.74 2024-11-06T02:57:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:57:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:57:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:57:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:57:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:57:18Z- handleDvl() - RDI_Pathfinder msg: 1730861838.74 sec; seqNo:375; #beams:0 2024-11-06T02:57:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:57:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , -0.04 2024-11-06T02:57:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861838.76 epoch sec; seqNo:1187; 4061644.09 north; 594262.65 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T02:57:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861838.31 2024-11-06T02:57:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:18Z- waiting for 1730861842.31, time is 1730861838.76 2024-11-06T02:57:19Z- AHRS_M2 msg: 1730861839.10 epoch sec; seqNo:1113 2024-11-06T02:57:19Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 5.35 psi 2024-11-06T02:57:19Z- handleDepth()- Depth_Keller msg: 1730861839.11 epoch sec; seqNo:1188; depth 49.87 2024-11-06T02:57:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861839.12 epoch sec; seqNo:1188; 4061644.09 north; 594262.65 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T02:57:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861838.31 2024-11-06T02:57:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:19Z- waiting for 1730861842.31, time is 1730861839.12 2024-11-06T02:57:19Z- AHRS_M2 msg: 1730861839.51 epoch sec; seqNo:1114 2024-11-06T02:57:19Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 5.35 psi 2024-11-06T02:57:19Z- handleDepth()- Depth_Keller msg: 1730861839.51 epoch sec; seqNo:1189; depth 49.88 2024-11-06T02:57:19Z- handleDVL() - DVL msg epochMillisec: 1730861839.54 2024-11-06T02:57:19Z- handleDvl() - RDI_Pathfinder msg: 1730861839.54 sec; seqNo:376; #beams:4 2024-11-06T02:57:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.73 , 40.37 , 44.24 , 43.63 2024-11-06T02:57:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.07 , -0.05 2024-11-06T02:57:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861839.55 epoch sec; seqNo:1189; 4061644.45 north; 594261.43 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T02:57:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861838.31 2024-11-06T02:57:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:19Z- waiting for 1730861842.31, time is 1730861839.55 2024-11-06T02:57:19Z- AHRS_M2 msg: 1730861839.91 epoch sec; seqNo:1115 2024-11-06T02:57:19Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 5.35 psi 2024-11-06T02:57:19Z- handleDepth()- Depth_Keller msg: 1730861839.92 epoch sec; seqNo:1190; depth 49.87 2024-11-06T02:57:19Z- handleDVL() - DVL msg epochMillisec: 1730861839.92 2024-11-06T02:57:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:57:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:57:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:57:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:57:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:57:19Z- handleDvl() - RDI_Pathfinder msg: 1730861839.92 sec; seqNo:377; #beams:0 2024-11-06T02:57:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:57:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.07 , -0.05 2024-11-06T02:57:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861839.95 epoch sec; seqNo:1190; 4061644.45 north; 594261.43 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T02:57:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861838.31 2024-11-06T02:57:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:20Z- waiting for 1730861842.31, time is 1730861839.95 2024-11-06T02:57:20Z- AHRS_M2 msg: 1730861840.31 epoch sec; seqNo:1116 2024-11-06T02:57:20Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 5.35 psi 2024-11-06T02:57:20Z- handleDepth()- Depth_Keller msg: 1730861840.32 epoch sec; seqNo:1191; depth 49.88 2024-11-06T02:57:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861840.33 epoch sec; seqNo:1191; 4061644.83 north; 594261.42 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T02:57:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861838.31 2024-11-06T02:57:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:20Z- waiting for 1730861842.31, time is 1730861840.33 2024-11-06T02:57:20Z- AHRS_M2 msg: 1730861840.72 epoch sec; seqNo:1117 2024-11-06T02:57:20Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 5.35 psi 2024-11-06T02:57:20Z- handleDepth()- Depth_Keller msg: 1730861840.72 epoch sec; seqNo:1192; depth 49.89 2024-11-06T02:57:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861840.74 epoch sec; seqNo:1192; 4061644.83 north; 594261.42 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T02:57:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861838.31 2024-11-06T02:57:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:20Z- waiting for 1730861842.31, time is 1730861840.74 2024-11-06T02:57:21Z- handleDepth()- Depth_Keller msg: 1730861841.14 epoch sec; seqNo:1193; depth 49.89 2024-11-06T02:57:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861841.17 epoch sec; seqNo:1193; 4061645.20 north; 594260.20 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T02:57:21Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:57:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861838.31 2024-11-06T02:57:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:21Z- waiting for 1730861842.31, time is 1730861841.17 2024-11-06T02:57:21Z- AHRS_M2 msg: 1730861841.53 epoch sec; seqNo:1118 2024-11-06T02:57:21Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 5.35 psi 2024-11-06T02:57:21Z- handleDepth()- Depth_Keller msg: 1730861841.54 epoch sec; seqNo:1194; depth 49.89 2024-11-06T02:57:21Z- handleDVL() - DVL msg epochMillisec: 1730861841.56 2024-11-06T02:57:21Z- handleDvl() - RDI_Pathfinder msg: 1730861841.56 sec; seqNo:378; #beams:4 2024-11-06T02:57:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.32 , 40.17 , 44.04 , 43.63 2024-11-06T02:57:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.08 , -0.06 2024-11-06T02:57:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861841.59 epoch sec; seqNo:1194; 4061645.58 north; 594260.20 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T02:57:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861838.31 2024-11-06T02:57:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:21Z- waiting for 1730861842.31, time is 1730861841.59 2024-11-06T02:57:21Z- AHRS_M2 msg: 1730861841.93 epoch sec; seqNo:1119 2024-11-06T02:57:21Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 5.35 psi 2024-11-06T02:57:21Z- handleDepth()- Depth_Keller msg: 1730861841.94 epoch sec; seqNo:1195; depth 49.89 2024-11-06T02:57:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861841.97 epoch sec; seqNo:1195; 4061645.58 north; 594260.20 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T02:57:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861838.31 2024-11-06T02:57:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:22Z- waiting for 1730861842.31, time is 1730861841.97 2024-11-06T02:57:22Z- AHRS_M2 msg: 1730861842.33 epoch sec; seqNo:1120 2024-11-06T02:57:22Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 5.35 psi 2024-11-06T02:57:22Z- handleDepth()- Depth_Keller msg: 1730861842.34 epoch sec; seqNo:1196; depth 49.90 2024-11-06T02:57:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861842.37 epoch sec; seqNo:1196; 4061645.95 north; 594260.19 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T02:57:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861838.31 2024-11-06T02:57:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:22Z- waiting for 1730861842.31, time is 1730861842.37 2024-11-06T02:57:22Z- LcmTrn::updateTrn() >>>> pose time 1730861842.37, meas time 1730861841.56 <<<< 2024-11-06T02:57:22Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861842.37 2024-11-06T02:57:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861841.56 sec, ping # 0; waiting for INS data more recent than 1730861838.31... 2024-11-06T02:57:22Z- TerrainNav::motionUpdate() - currEstimate = (4061646, 594260.19, 49.90) 2024-11-06T02:57:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:57:22Z- TerrainNav:: SubcloudNIS is 0.22, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:57:22Z- TNavPF::Projecting Measurements 2024-11-06T02:57:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:57:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:57:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:57:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:57:22Z- TNavFilter::4 good beams. 2024-11-06T02:57:22Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:57:22Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:57:22Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:57:22Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:57:22Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:57:22Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:57:22Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:57:22Z- TerrainMapOctree: (vehN, vehE ) = (4061645.51, 594260.68). 2024-11-06T02:57:22Z- TerrainMapOctree: Min Distance = 105.04. 2024-11-06T02:57:22Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:57:22Z- Forcing Subcloud Comparison 2024-11-06T02:57:22Z- Weighting particles with subcloud comparison 2024-11-06T02:57:22Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:22Z- meanExpectedMeasDiff: -0.259466 delta_rms_squared: 2.447222 alpha: 0.991894 2024-11-06T02:57:22Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:22Z- meanExpectedMeasDiff: -0.500079 delta_rms_squared: 1.071558 alpha: 0.981678 2024-11-06T02:57:22Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:22Z- meanExpectedMeasDiff: 0.110919 delta_rms_squared: 2.207863 alpha: 0.991023 2024-11-06T02:57:22Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:22Z- meanExpectedMeasDiff: -0.845319 delta_rms_squared: 1.283785 alpha: 0.984660 2024-11-06T02:57:22Z- Subcloud NIS: 0.218509 2024-11-06T02:57:22Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:57:22Z- TNavPF:: sumWeights = 0.571249 2024-11-06T02:57:22Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:57:22Z- TNavPF::First Term of Map Covariance Matrix is 0.71 2024-11-06T02:57:22Z- TNavPF::Calculated NIS Value : 0.07 numBeams normalized NIS: 0.02 2024-11-06T02:57:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861841.56 sec. 2024-11-06T02:57:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:57:22Z- variances([0], [2], and psi_berg) = 0.317876 0.256672 0.000000 2024-11-06T02:57:22Z- publishEstimatesDecoder() - at 1730861842.37 sec 2024-11-06T02:57:22Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:57:22Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061679.34 594243.72 57.57 2024-11-06T02:57:22Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061678.41 594244.07 56.14 lat/lon: 36.70 -121.94 2024-11-06T02:57:22Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.32 0.26 0.66 0.00 2024-11-06T02:57:22Z- AHRS_M2 msg: 1730861842.75 epoch sec; seqNo:1121 2024-11-06T02:57:22Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 5.35 psi 2024-11-06T02:57:22Z- handleDepth()- Depth_Keller msg: 1730861842.76 epoch sec; seqNo:1197; depth 49.92 2024-11-06T02:57:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861842.78 epoch sec; seqNo:1197; 4061645.94 north; 594258.97 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T02:57:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861842.37 2024-11-06T02:57:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:22Z- waiting for 1730861846.37, time is 1730861842.78 2024-11-06T02:57:23Z- AHRS_M2 msg: 1730861843.14 epoch sec; seqNo:1122 2024-11-06T02:57:23Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 5.35 psi 2024-11-06T02:57:23Z- handleDepth()- Depth_Keller msg: 1730861843.14 epoch sec; seqNo:1198; depth 49.92 2024-11-06T02:57:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861843.16 epoch sec; seqNo:1198; 4061646.32 north; 594258.97 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T02:57:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861842.37 2024-11-06T02:57:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:23Z- waiting for 1730861846.37, time is 1730861843.16 2024-11-06T02:57:23Z- AHRS_M2 msg: 1730861843.54 epoch sec; seqNo:1123 2024-11-06T02:57:23Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:23Z- handleDepth()- Depth_Keller msg: 1730861843.55 epoch sec; seqNo:1199; depth 49.95 2024-11-06T02:57:23Z- handleDVL() - DVL msg epochMillisec: 1730861843.58 2024-11-06T02:57:23Z- handleDvl() - RDI_Pathfinder msg: 1730861843.58 sec; seqNo:379; #beams:4 2024-11-06T02:57:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.51 , 40.37 , 43.84 , 43.63 2024-11-06T02:57:23Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.09 , -0.04 2024-11-06T02:57:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861843.59 epoch sec; seqNo:1199; 4061646.32 north; 594258.97 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T02:57:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861842.37 2024-11-06T02:57:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:23Z- waiting for 1730861846.37, time is 1730861843.59 2024-11-06T02:57:23Z- AHRS_M2 msg: 1730861843.95 epoch sec; seqNo:1124 2024-11-06T02:57:23Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:23Z- handleDepth()- Depth_Keller msg: 1730861843.96 epoch sec; seqNo:1200; depth 49.94 2024-11-06T02:57:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861843.98 epoch sec; seqNo:1200; 4061646.69 north; 594257.74 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T02:57:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861842.37 2024-11-06T02:57:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:24Z- waiting for 1730861846.37, time is 1730861843.98 2024-11-06T02:57:24Z- handleDepth()- Depth_Keller msg: 1730861844.38 epoch sec; seqNo:1201; depth 49.97 2024-11-06T02:57:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861844.39 epoch sec; seqNo:1201; 4061646.69 north; 594257.74 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T02:57:24Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:57:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861842.37 2024-11-06T02:57:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:24Z- waiting for 1730861846.37, time is 1730861844.39 2024-11-06T02:57:24Z- AHRS_M2 msg: 1730861844.76 epoch sec; seqNo:1125 2024-11-06T02:57:24Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:24Z- handleDepth()- Depth_Keller msg: 1730861844.76 epoch sec; seqNo:1202; depth 49.97 2024-11-06T02:57:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861844.79 epoch sec; seqNo:1202; 4061647.06 north; 594257.74 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T02:57:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861842.37 2024-11-06T02:57:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:24Z- waiting for 1730861846.37, time is 1730861844.79 2024-11-06T02:57:25Z- AHRS_M2 msg: 1730861845.16 epoch sec; seqNo:1126 2024-11-06T02:57:25Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:25Z- handleDepth()- Depth_Keller msg: 1730861845.16 epoch sec; seqNo:1203; depth 50.00 2024-11-06T02:57:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861845.17 epoch sec; seqNo:1203; 4061647.44 north; 594257.73 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T02:57:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861842.37 2024-11-06T02:57:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:25Z- waiting for 1730861846.37, time is 1730861845.17 2024-11-06T02:57:25Z- AHRS_M2 msg: 1730861845.56 epoch sec; seqNo:1127 2024-11-06T02:57:25Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:25Z- handleDepth()- Depth_Keller msg: 1730861845.57 epoch sec; seqNo:1204; depth 50.01 2024-11-06T02:57:25Z- handleDVL() - DVL msg epochMillisec: 1730861845.59 2024-11-06T02:57:25Z- handleDvl() - RDI_Pathfinder msg: 1730861845.59 sec; seqNo:380; #beams:4 2024-11-06T02:57:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.51 , 40.57 , 44.04 , 43.02 2024-11-06T02:57:25Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.09 , 0.00 2024-11-06T02:57:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861845.61 epoch sec; seqNo:1204; 4061647.43 north; 594256.51 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T02:57:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861842.37 2024-11-06T02:57:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:25Z- waiting for 1730861846.37, time is 1730861845.61 2024-11-06T02:57:25Z- AHRS_M2 msg: 1730861845.97 epoch sec; seqNo:1128 2024-11-06T02:57:25Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:25Z- handleDepth()- Depth_Keller msg: 1730861845.98 epoch sec; seqNo:1205; depth 50.03 2024-11-06T02:57:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861846.00 epoch sec; seqNo:1205; 4061647.81 north; 594256.51 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T02:57:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861842.37 2024-11-06T02:57:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:26Z- waiting for 1730861846.37, time is 1730861846.00 2024-11-06T02:57:26Z- AHRS_M2 msg: 1730861846.37 epoch sec; seqNo:1129 2024-11-06T02:57:26Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:26Z- handleDepth()- Depth_Keller msg: 1730861846.38 epoch sec; seqNo:1206; depth 50.06 2024-11-06T02:57:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861846.39 epoch sec; seqNo:1206; 4061647.81 north; 594256.51 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T02:57:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861842.37 2024-11-06T02:57:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:26Z- waiting for 1730861846.37, time is 1730861846.39 2024-11-06T02:57:26Z- LcmTrn::updateTrn() >>>> pose time 1730861846.39, meas time 1730861845.59 <<<< 2024-11-06T02:57:26Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861846.39 2024-11-06T02:57:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861845.59 sec, ping # 0; waiting for INS data more recent than 1730861842.37... 2024-11-06T02:57:26Z- TerrainNav::motionUpdate() - currEstimate = (4061648, 594256.51, 50.06) 2024-11-06T02:57:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:57:26Z- TerrainNav:: SubcloudNIS is 0.22, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:57:26Z- TNavPF::Projecting Measurements 2024-11-06T02:57:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:57:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:57:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:57:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:57:26Z- TNavFilter::4 good beams. 2024-11-06T02:57:26Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:57:26Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:57:26Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:57:26Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:57:26Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:57:26Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:57:26Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:57:26Z- TerrainMapOctree: (vehN, vehE ) = (4061647.44, 594257.24). 2024-11-06T02:57:26Z- TerrainMapOctree: Min Distance = 102.81. 2024-11-06T02:57:26Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:57:26Z- Forcing Subcloud Comparison 2024-11-06T02:57:26Z- Weighting particles with subcloud comparison 2024-11-06T02:57:26Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:26Z- meanExpectedMeasDiff: -1.065764 delta_rms_squared: 1.423253 alpha: 0.986142 2024-11-06T02:57:26Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:26Z- meanExpectedMeasDiff: 0.126453 delta_rms_squared: 1.037677 alpha: 0.981091 2024-11-06T02:57:26Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:26Z- meanExpectedMeasDiff: -0.187286 delta_rms_squared: 1.178326 alpha: 0.983310 2024-11-06T02:57:26Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:26Z- meanExpectedMeasDiff: -1.134878 delta_rms_squared: 1.238596 alpha: 0.984109 2024-11-06T02:57:26Z- Subcloud NIS: 0.206773 2024-11-06T02:57:26Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:57:26Z- TNavPF:: sumWeights = 0.593235 2024-11-06T02:57:26Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:57:26Z- TNavPF::First Term of Map Covariance Matrix is 0.47 2024-11-06T02:57:26Z- TNavPF::Calculated NIS Value : 0.14 numBeams normalized NIS: 0.04 2024-11-06T02:57:26Z- TerrainNav::Resampling particle filter... 2024-11-06T02:57:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861845.59 sec. 2024-11-06T02:57:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:57:26Z- variances([0], [2], and psi_berg) = 0.320036 0.259753 0.000000 2024-11-06T02:57:26Z- publishEstimatesDecoder() - at 1730861846.39 sec 2024-11-06T02:57:26Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:57:26Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061679.60 594241.33 56.35 2024-11-06T02:57:26Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061680.28 594240.38 56.55 lat/lon: 36.70 -121.95 2024-11-06T02:57:26Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.32 0.26 0.69 0.00 2024-11-06T02:57:26Z- AHRS_M2 msg: 1730861846.83 epoch sec; seqNo:1130 2024-11-06T02:57:26Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:26Z- handleDepth()- Depth_Keller msg: 1730861846.85 epoch sec; seqNo:1207; depth 50.08 2024-11-06T02:57:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861846.87 epoch sec; seqNo:1207; 4061648.17 north; 594255.29 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T02:57:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861846.39 2024-11-06T02:57:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:26Z- waiting for 1730861850.39, time is 1730861846.87 2024-11-06T02:57:27Z- AHRS_M2 msg: 1730861847.18 epoch sec; seqNo:1131 2024-11-06T02:57:27Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:27Z- handleDepth()- Depth_Keller msg: 1730861847.18 epoch sec; seqNo:1208; depth 50.11 2024-11-06T02:57:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861847.19 epoch sec; seqNo:1208; 4061648.17 north; 594255.29 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T02:57:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861846.39 2024-11-06T02:57:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:27Z- waiting for 1730861850.39, time is 1730861847.19 2024-11-06T02:57:27Z- AHRS_M2 msg: 1730861847.58 epoch sec; seqNo:1132 2024-11-06T02:57:27Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:27Z- handleDepth()- Depth_Keller msg: 1730861847.59 epoch sec; seqNo:1209; depth 50.13 2024-11-06T02:57:27Z- handleDVL() - DVL msg epochMillisec: 1730861847.61 2024-11-06T02:57:27Z- handleDvl() - RDI_Pathfinder msg: 1730861847.61 sec; seqNo:381; #beams:4 2024-11-06T02:57:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.91 , 40.37 , 44.04 , 43.22 2024-11-06T02:57:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.08 , 0.04 2024-11-06T02:57:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861847.63 epoch sec; seqNo:1209; 4061648.55 north; 594255.28 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T02:57:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861846.39 2024-11-06T02:57:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:27Z- waiting for 1730861850.39, time is 1730861847.63 2024-11-06T02:57:27Z- AHRS_M2 msg: 1730861847.99 epoch sec; seqNo:1133 2024-11-06T02:57:27Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:28Z- handleDepth()- Depth_Keller msg: 1730861848.00 epoch sec; seqNo:1210; depth 50.13 2024-11-06T02:57:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861848.03 epoch sec; seqNo:1210; 4061648.55 north; 594255.28 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T02:57:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861846.39 2024-11-06T02:57:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:28Z- waiting for 1730861850.39, time is 1730861848.03 2024-11-06T02:57:28Z- AHRS_M2 msg: 1730861848.39 epoch sec; seqNo:1134 2024-11-06T02:57:28Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:28Z- handleDepth()- Depth_Keller msg: 1730861848.40 epoch sec; seqNo:1211; depth 50.16 2024-11-06T02:57:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861848.40 epoch sec; seqNo:1211; 4061648.92 north; 594254.06 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T02:57:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861846.39 2024-11-06T02:57:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:28Z- waiting for 1730861850.39, time is 1730861848.40 2024-11-06T02:57:28Z- AHRS_M2 msg: 1730861848.80 epoch sec; seqNo:1135 2024-11-06T02:57:28Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:28Z- handleDepth()- Depth_Keller msg: 1730861848.80 epoch sec; seqNo:1212; depth 50.18 2024-11-06T02:57:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861848.81 epoch sec; seqNo:1212; 4061648.92 north; 594254.06 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T02:57:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861846.39 2024-11-06T02:57:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:28Z- waiting for 1730861850.39, time is 1730861848.81 2024-11-06T02:57:29Z- AHRS_M2 msg: 1730861849.20 epoch sec; seqNo:1136 2024-11-06T02:57:29Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.34 psi 2024-11-06T02:57:29Z- handleDepth()- Depth_Keller msg: 1730861849.21 epoch sec; seqNo:1213; depth 50.18 2024-11-06T02:57:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861849.22 epoch sec; seqNo:1213; 4061649.30 north; 594254.05 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T02:57:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861846.39 2024-11-06T02:57:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:29Z- waiting for 1730861850.39, time is 1730861849.22 2024-11-06T02:57:29Z- AHRS_M2 msg: 1730861849.60 epoch sec; seqNo:1137 2024-11-06T02:57:29Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.35 psi 2024-11-06T02:57:29Z- handleDepth()- Depth_Keller msg: 1730861849.61 epoch sec; seqNo:1214; depth 50.20 2024-11-06T02:57:29Z- handleDVL() - DVL msg epochMillisec: 1730861849.64 2024-11-06T02:57:29Z- handleDvl() - RDI_Pathfinder msg: 1730861849.64 sec; seqNo:382; #beams:4 2024-11-06T02:57:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.91 , 40.57 , 44.04 , 43.43 2024-11-06T02:57:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.02 2024-11-06T02:57:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861849.66 epoch sec; seqNo:1214; 4061649.30 north; 594254.05 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T02:57:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861846.39 2024-11-06T02:57:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:29Z- waiting for 1730861850.39, time is 1730861849.66 2024-11-06T02:57:30Z- AHRS_M2 msg: 1730861850.01 epoch sec; seqNo:1138 2024-11-06T02:57:30Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.35 psi 2024-11-06T02:57:30Z- handleDepth()- Depth_Keller msg: 1730861850.01 epoch sec; seqNo:1215; depth 50.21 2024-11-06T02:57:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861850.03 epoch sec; seqNo:1215; 4061649.66 north; 594252.83 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T02:57:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861846.39 2024-11-06T02:57:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:30Z- waiting for 1730861850.39, time is 1730861850.03 2024-11-06T02:57:30Z- AHRS_M2 msg: 1730861850.42 epoch sec; seqNo:1139 2024-11-06T02:57:30Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.35 psi 2024-11-06T02:57:30Z- handleDepth()- Depth_Keller msg: 1730861850.42 epoch sec; seqNo:1216; depth 50.21 2024-11-06T02:57:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861850.43 epoch sec; seqNo:1216; 4061649.66 north; 594252.83 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T02:57:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861846.39 2024-11-06T02:57:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:30Z- waiting for 1730861850.39, time is 1730861850.43 2024-11-06T02:57:30Z- LcmTrn::updateTrn() >>>> pose time 1730861850.43, meas time 1730861849.64 <<<< 2024-11-06T02:57:30Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861850.43 2024-11-06T02:57:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861849.64 sec, ping # 0; waiting for INS data more recent than 1730861846.39... 2024-11-06T02:57:30Z- TerrainNav::motionUpdate() - currEstimate = (4061650, 594252.83, 50.21) 2024-11-06T02:57:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:57:30Z- TerrainNav:: SubcloudNIS is 0.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:57:30Z- TNavPF::Projecting Measurements 2024-11-06T02:57:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:57:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:57:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:57:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:57:30Z- TNavFilter::4 good beams. 2024-11-06T02:57:30Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:57:30Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:57:30Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:57:30Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:57:30Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:57:30Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:57:30Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:57:30Z- TerrainMapOctree: (vehN, vehE ) = (4061649.30, 594253.55). 2024-11-06T02:57:30Z- TerrainMapOctree: Min Distance = 100.76. 2024-11-06T02:57:30Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:57:30Z- Forcing Subcloud Comparison 2024-11-06T02:57:30Z- Weighting particles with subcloud comparison 2024-11-06T02:57:30Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:30Z- meanExpectedMeasDiff: -0.110417 delta_rms_squared: 1.471659 alpha: 0.986592 2024-11-06T02:57:30Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:30Z- meanExpectedMeasDiff: 0.543588 delta_rms_squared: 1.075813 alpha: 0.981749 2024-11-06T02:57:30Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:30Z- meanExpectedMeasDiff: 0.250378 delta_rms_squared: 1.221926 alpha: 0.983896 2024-11-06T02:57:30Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:30Z- meanExpectedMeasDiff: -0.213529 delta_rms_squared: 1.280615 alpha: 0.984623 2024-11-06T02:57:30Z- Subcloud NIS: 0.198283 2024-11-06T02:57:30Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:57:30Z- TNavPF:: sumWeights = 0.673578 2024-11-06T02:57:30Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:57:30Z- TNavPF::First Term of Map Covariance Matrix is 0.49 2024-11-06T02:57:30Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.01 2024-11-06T02:57:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861849.64 sec. 2024-11-06T02:57:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:57:30Z- variances([0], [2], and psi_berg) = 0.320353 0.262394 0.000000 2024-11-06T02:57:30Z- publishEstimatesDecoder() - at 1730861850.43 sec 2024-11-06T02:57:30Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:57:30Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061682.41 594237.01 56.13 2024-11-06T02:57:30Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061682.13 594236.70 56.65 lat/lon: 36.70 -121.95 2024-11-06T02:57:30Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.32 0.26 0.70 0.00 2024-11-06T02:57:30Z- AHRS_M2 msg: 1730861850.82 epoch sec; seqNo:1140 2024-11-06T02:57:30Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.35 psi 2024-11-06T02:57:30Z- handleDepth()- Depth_Keller msg: 1730861850.83 epoch sec; seqNo:1217; depth 50.23 2024-11-06T02:57:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861850.84 epoch sec; seqNo:1217; 4061650.04 north; 594252.82 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T02:57:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861850.43 2024-11-06T02:57:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:30Z- waiting for 1730861854.43, time is 1730861850.84 2024-11-06T02:57:31Z- AHRS_M2 msg: 1730861851.24 epoch sec; seqNo:1141 2024-11-06T02:57:31Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.35 psi 2024-11-06T02:57:31Z- handleDepth()- Depth_Keller msg: 1730861851.26 epoch sec; seqNo:1218; depth 50.24 2024-11-06T02:57:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861851.29 epoch sec; seqNo:1218; 4061650.04 north; 594252.82 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T02:57:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861850.43 2024-11-06T02:57:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:31Z- waiting for 1730861854.43, time is 1730861851.29 2024-11-06T02:57:31Z- AHRS_M2 msg: 1730861851.63 epoch sec; seqNo:1142 2024-11-06T02:57:31Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.35 psi 2024-11-06T02:57:31Z- handleDepth()- Depth_Keller msg: 1730861851.63 epoch sec; seqNo:1219; depth 50.24 2024-11-06T02:57:31Z- handleDVL() - DVL msg epochMillisec: 1730861851.66 2024-11-06T02:57:31Z- handleDvl() - RDI_Pathfinder msg: 1730861851.66 sec; seqNo:383; #beams:4 2024-11-06T02:57:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 40.37 , 44.04 , 43.63 2024-11-06T02:57:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , -0.03 2024-11-06T02:57:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861851.68 epoch sec; seqNo:1219; 4061650.41 north; 594251.60 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T02:57:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861850.43 2024-11-06T02:57:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:31Z- waiting for 1730861854.43, time is 1730861851.68 2024-11-06T02:57:32Z- AHRS_M2 msg: 1730861852.03 epoch sec; seqNo:1143 2024-11-06T02:57:32Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.35 psi 2024-11-06T02:57:32Z- handleDepth()- Depth_Keller msg: 1730861852.04 epoch sec; seqNo:1220; depth 50.25 2024-11-06T02:57:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861852.05 epoch sec; seqNo:1220; 4061650.41 north; 594251.60 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T02:57:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861850.43 2024-11-06T02:57:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:32Z- waiting for 1730861854.43, time is 1730861852.05 2024-11-06T02:57:32Z- AHRS_M2 msg: 1730861852.44 epoch sec; seqNo:1144 2024-11-06T02:57:32Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.35 psi 2024-11-06T02:57:32Z- handleDepth()- Depth_Keller msg: 1730861852.45 epoch sec; seqNo:1221; depth 50.25 2024-11-06T02:57:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861852.46 epoch sec; seqNo:1221; 4061650.79 north; 594251.60 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T02:57:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861850.43 2024-11-06T02:57:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:32Z- waiting for 1730861854.43, time is 1730861852.46 2024-11-06T02:57:32Z- AHRS_M2 msg: 1730861852.85 epoch sec; seqNo:1145 2024-11-06T02:57:32Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 5.35 psi 2024-11-06T02:57:32Z- handleDepth()- Depth_Keller msg: 1730861852.85 epoch sec; seqNo:1222; depth 50.24 2024-11-06T02:57:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861852.86 epoch sec; seqNo:1222; 4061650.79 north; 594251.60 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T02:57:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861850.43 2024-11-06T02:57:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:33Z- waiting for 1730861854.43, time is 1730861852.86 2024-11-06T02:57:33Z- AHRS_M2 msg: 1730861853.28 epoch sec; seqNo:1146 2024-11-06T02:57:33Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 5.35 psi 2024-11-06T02:57:33Z- handleDepth()- Depth_Keller msg: 1730861853.29 epoch sec; seqNo:1223; depth 50.23 2024-11-06T02:57:33Z- handleDVL() - DVL msg epochMillisec: 1730861853.32 2024-11-06T02:57:33Z- handleDvl() - RDI_Pathfinder msg: 1730861853.32 sec; seqNo:384; #beams:4 2024-11-06T02:57:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 40.57 , 44.04 , 43.43 2024-11-06T02:57:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , -0.06 2024-11-06T02:57:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861853.33 epoch sec; seqNo:1223; 4061651.15 north; 594250.37 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T02:57:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861850.43 2024-11-06T02:57:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:33Z- waiting for 1730861854.43, time is 1730861853.33 2024-11-06T02:57:33Z- AHRS_M2 msg: 1730861853.65 epoch sec; seqNo:1147 2024-11-06T02:57:33Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 5.35 psi 2024-11-06T02:57:33Z- handleDepth()- Depth_Keller msg: 1730861853.66 epoch sec; seqNo:1224; depth 50.24 2024-11-06T02:57:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861853.69 epoch sec; seqNo:1224; 4061651.53 north; 594250.37 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T02:57:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861850.43 2024-11-06T02:57:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:33Z- waiting for 1730861854.43, time is 1730861853.69 2024-11-06T02:57:34Z- AHRS_M2 msg: 1730861854.06 epoch sec; seqNo:1148 2024-11-06T02:57:34Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 5.35 psi 2024-11-06T02:57:34Z- handleDepth()- Depth_Keller msg: 1730861854.06 epoch sec; seqNo:1225; depth 50.26 2024-11-06T02:57:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861854.07 epoch sec; seqNo:1225; 4061651.53 north; 594250.37 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T02:57:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861850.43 2024-11-06T02:57:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:34Z- waiting for 1730861854.43, time is 1730861854.07 2024-11-06T02:57:34Z- AHRS_M2 msg: 1730861854.46 epoch sec; seqNo:1149 2024-11-06T02:57:34Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 5.35 psi 2024-11-06T02:57:34Z- handleDepth()- Depth_Keller msg: 1730861854.46 epoch sec; seqNo:1226; depth 50.25 2024-11-06T02:57:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861854.47 epoch sec; seqNo:1226; 4061651.91 north; 594250.36 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T02:57:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861850.43 2024-11-06T02:57:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:34Z- waiting for 1730861854.43, time is 1730861854.47 2024-11-06T02:57:34Z- LcmTrn::updateTrn() >>>> pose time 1730861854.47, meas time 1730861853.32 <<<< 2024-11-06T02:57:34Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861854.47 2024-11-06T02:57:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861853.32 sec, ping # 0; waiting for INS data more recent than 1730861850.43... 2024-11-06T02:57:34Z- TerrainNav::motionUpdate() - currEstimate = (4061652, 594250.36, 50.25) 2024-11-06T02:57:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:57:34Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:57:34Z- TNavPF::Projecting Measurements 2024-11-06T02:57:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:57:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:57:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:57:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:57:34Z- TNavFilter::4 good beams. 2024-11-06T02:57:34Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:57:34Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:57:34Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:57:34Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:57:34Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:57:34Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:57:34Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:57:34Z- TerrainMapOctree: (vehN, vehE ) = (4061651.27, 594251.06). 2024-11-06T02:57:34Z- TerrainMapOctree: Min Distance = 98.74. 2024-11-06T02:57:34Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:57:34Z- Forcing Subcloud Comparison 2024-11-06T02:57:34Z- Weighting particles with subcloud comparison 2024-11-06T02:57:34Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:34Z- meanExpectedMeasDiff: -0.264902 delta_rms_squared: 1.515042 alpha: 0.986971 2024-11-06T02:57:34Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:34Z- meanExpectedMeasDiff: 0.538087 delta_rms_squared: 1.110232 alpha: 0.982305 2024-11-06T02:57:34Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:34Z- meanExpectedMeasDiff: 0.133089 delta_rms_squared: 1.267431 alpha: 0.984465 2024-11-06T02:57:34Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:34Z- meanExpectedMeasDiff: -0.059269 delta_rms_squared: 1.311766 alpha: 0.984982 2024-11-06T02:57:34Z- Subcloud NIS: 0.188392 2024-11-06T02:57:34Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:57:34Z- TNavPF:: sumWeights = 0.669228 2024-11-06T02:57:34Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:57:34Z- TNavPF::First Term of Map Covariance Matrix is 0.50 2024-11-06T02:57:34Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.01 2024-11-06T02:57:34Z- TerrainNav::Resampling particle filter... 2024-11-06T02:57:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861853.32 sec. 2024-11-06T02:57:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:57:34Z- variances([0], [2], and psi_berg) = 0.321465 0.263652 0.000000 2024-11-06T02:57:34Z- publishEstimatesDecoder() - at 1730861854.47 sec 2024-11-06T02:57:34Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:57:34Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061683.82 594235.21 55.71 2024-11-06T02:57:34Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061684.38 594234.24 56.65 lat/lon: 36.70 -121.95 2024-11-06T02:57:34Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.32 0.26 0.69 0.00 2024-11-06T02:57:34Z- AHRS_M2 msg: 1730861854.87 epoch sec; seqNo:1150 2024-11-06T02:57:34Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 5.35 psi 2024-11-06T02:57:34Z- handleDepth()- Depth_Keller msg: 1730861854.87 epoch sec; seqNo:1227; depth 50.24 2024-11-06T02:57:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861854.89 epoch sec; seqNo:1227; 4061651.90 north; 594249.14 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T02:57:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861854.47 2024-11-06T02:57:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:35Z- waiting for 1730861858.47, time is 1730861854.89 2024-11-06T02:57:35Z- AHRS_M2 msg: 1730861855.29 epoch sec; seqNo:1151 2024-11-06T02:57:35Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 5.35 psi 2024-11-06T02:57:35Z- handleDepth()- Depth_Keller msg: 1730861855.30 epoch sec; seqNo:1228; depth 50.26 2024-11-06T02:57:35Z- handleDVL() - DVL msg epochMillisec: 1730861855.32 2024-11-06T02:57:35Z- handleDvl() - RDI_Pathfinder msg: 1730861855.32 sec; seqNo:385; #beams:4 2024-11-06T02:57:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.51 , 40.57 , 44.04 , 43.22 2024-11-06T02:57:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.08 , -0.07 2024-11-06T02:57:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861855.33 epoch sec; seqNo:1228; 4061652.27 north; 594249.14 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T02:57:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861854.47 2024-11-06T02:57:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:35Z- waiting for 1730861858.47, time is 1730861855.33 2024-11-06T02:57:35Z- AHRS_M2 msg: 1730861855.67 epoch sec; seqNo:1152 2024-11-06T02:57:35Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 5.35 psi 2024-11-06T02:57:35Z- handleDepth()- Depth_Keller msg: 1730861855.68 epoch sec; seqNo:1229; depth 50.25 2024-11-06T02:57:35Z- handleDVL() - DVL msg epochMillisec: 1730861855.68 2024-11-06T02:57:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:57:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:57:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:57:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:57:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:57:35Z- handleDvl() - RDI_Pathfinder msg: 1730861855.68 sec; seqNo:386; #beams:0 2024-11-06T02:57:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:57:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.08 , -0.07 2024-11-06T02:57:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861855.69 epoch sec; seqNo:1229; 4061652.27 north; 594249.14 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T02:57:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861854.47 2024-11-06T02:57:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:35Z- waiting for 1730861858.47, time is 1730861855.69 2024-11-06T02:57:36Z- handleDepth()- Depth_Keller msg: 1730861856.10 epoch sec; seqNo:1230; depth 50.25 2024-11-06T02:57:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861856.11 epoch sec; seqNo:1230; 4061652.65 north; 594249.13 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T02:57:36Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:57:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861854.47 2024-11-06T02:57:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:36Z- waiting for 1730861858.47, time is 1730861856.11 2024-11-06T02:57:36Z- AHRS_M2 msg: 1730861856.48 epoch sec; seqNo:1153 2024-11-06T02:57:36Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 5.35 psi 2024-11-06T02:57:36Z- handleDepth()- Depth_Keller msg: 1730861856.49 epoch sec; seqNo:1231; depth 50.23 2024-11-06T02:57:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861856.50 epoch sec; seqNo:1231; 4061652.64 north; 594247.91 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T02:57:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861854.47 2024-11-06T02:57:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:36Z- waiting for 1730861858.47, time is 1730861856.50 2024-11-06T02:57:36Z- AHRS_M2 msg: 1730861856.88 epoch sec; seqNo:1154 2024-11-06T02:57:36Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 5.35 psi 2024-11-06T02:57:36Z- handleDepth()- Depth_Keller msg: 1730861856.89 epoch sec; seqNo:1232; depth 50.24 2024-11-06T02:57:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861856.90 epoch sec; seqNo:1232; 4061653.02 north; 594247.91 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T02:57:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861854.47 2024-11-06T02:57:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:37Z- waiting for 1730861858.47, time is 1730861856.90 2024-11-06T02:57:37Z- AHRS_M2 msg: 1730861857.30 epoch sec; seqNo:1155 2024-11-06T02:57:37Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 5.35 psi 2024-11-06T02:57:37Z- handleDepth()- Depth_Keller msg: 1730861857.30 epoch sec; seqNo:1233; depth 50.25 2024-11-06T02:57:37Z- handleDVL() - DVL msg epochMillisec: 1730861857.33 2024-11-06T02:57:37Z- handleDvl() - RDI_Pathfinder msg: 1730861857.33 sec; seqNo:387; #beams:4 2024-11-06T02:57:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.51 , 40.37 , 44.04 , 42.82 2024-11-06T02:57:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.09 , -0.05 2024-11-06T02:57:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861857.35 epoch sec; seqNo:1233; 4061653.02 north; 594247.91 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T02:57:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861854.47 2024-11-06T02:57:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:37Z- waiting for 1730861858.47, time is 1730861857.35 2024-11-06T02:57:37Z- AHRS_M2 msg: 1730861857.69 epoch sec; seqNo:1156 2024-11-06T02:57:37Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 5.35 psi 2024-11-06T02:57:37Z- handleDepth()- Depth_Keller msg: 1730861857.70 epoch sec; seqNo:1234; depth 50.24 2024-11-06T02:57:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861857.72 epoch sec; seqNo:1234; 4061653.38 north; 594246.68 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T02:57:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861854.47 2024-11-06T02:57:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:37Z- waiting for 1730861858.47, time is 1730861857.72 2024-11-06T02:57:38Z- AHRS_M2 msg: 1730861858.10 epoch sec; seqNo:1157 2024-11-06T02:57:38Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 5.35 psi 2024-11-06T02:57:38Z- handleDepth()- Depth_Keller msg: 1730861858.11 epoch sec; seqNo:1235; depth 50.25 2024-11-06T02:57:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861858.12 epoch sec; seqNo:1235; 4061653.38 north; 594246.68 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T02:57:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861854.47 2024-11-06T02:57:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:38Z- waiting for 1730861858.47, time is 1730861858.12 2024-11-06T02:57:38Z- AHRS_M2 msg: 1730861858.50 epoch sec; seqNo:1158 2024-11-06T02:57:38Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 5.35 psi 2024-11-06T02:57:38Z- handleDepth()- Depth_Keller msg: 1730861858.51 epoch sec; seqNo:1236; depth 50.25 2024-11-06T02:57:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861858.52 epoch sec; seqNo:1236; 4061653.76 north; 594246.68 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T02:57:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861854.47 2024-11-06T02:57:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:38Z- waiting for 1730861858.47, time is 1730861858.52 2024-11-06T02:57:38Z- LcmTrn::updateTrn() >>>> pose time 1730861858.52, meas time 1730861857.33 <<<< 2024-11-06T02:57:38Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861858.52 2024-11-06T02:57:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861857.33 sec, ping # 0; waiting for INS data more recent than 1730861854.47... 2024-11-06T02:57:38Z- TerrainNav::motionUpdate() - currEstimate = (4061654, 594246.68, 50.25) 2024-11-06T02:57:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:57:38Z- TerrainNav:: SubcloudNIS is 0.19, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:57:38Z- TNavPF::Projecting Measurements 2024-11-06T02:57:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:57:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:57:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:57:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:57:38Z- TNavFilter::4 good beams. 2024-11-06T02:57:38Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:57:38Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:57:38Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:57:38Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:57:38Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:57:38Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:57:38Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:57:38Z- TerrainMapOctree: (vehN, vehE ) = (4061653.22, 594247.76). 2024-11-06T02:57:38Z- TerrainMapOctree: Min Distance = 96.81. 2024-11-06T02:57:38Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:57:38Z- Forcing Subcloud Comparison 2024-11-06T02:57:38Z- Weighting particles with subcloud comparison 2024-11-06T02:57:38Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:38Z- meanExpectedMeasDiff: -0.472906 delta_rms_squared: 1.497680 alpha: 0.986822 2024-11-06T02:57:38Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:38Z- meanExpectedMeasDiff: 0.309881 delta_rms_squared: 1.097740 alpha: 0.982107 2024-11-06T02:57:38Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:38Z- meanExpectedMeasDiff: -0.157040 delta_rms_squared: 1.268362 alpha: 0.984476 2024-11-06T02:57:38Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:38Z- meanExpectedMeasDiff: -0.416176 delta_rms_squared: 1.280112 alpha: 0.984617 2024-11-06T02:57:38Z- Subcloud NIS: 0.178344 2024-11-06T02:57:38Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:57:38Z- TNavPF:: sumWeights = 0.665749 2024-11-06T02:57:38Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:57:38Z- TNavPF::First Term of Map Covariance Matrix is 0.50 2024-11-06T02:57:38Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.01 2024-11-06T02:57:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861857.33 sec. 2024-11-06T02:57:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:57:38Z- variances([0], [2], and psi_berg) = 0.323899 0.265786 0.000000 2024-11-06T02:57:38Z- publishEstimatesDecoder() - at 1730861858.52 sec 2024-11-06T02:57:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:57:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061685.86 594230.89 57.66 2024-11-06T02:57:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061686.24 594230.55 56.73 lat/lon: 36.70 -121.95 2024-11-06T02:57:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.32 0.27 0.71 0.00 2024-11-06T02:57:38Z- AHRS_M2 msg: 1730861858.92 epoch sec; seqNo:1159 2024-11-06T02:57:38Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 5.35 psi 2024-11-06T02:57:38Z- handleDepth()- Depth_Keller msg: 1730861858.93 epoch sec; seqNo:1237; depth 50.27 2024-11-06T02:57:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861858.94 epoch sec; seqNo:1237; 4061654.14 north; 594246.68 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T02:57:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861858.52 2024-11-06T02:57:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:39Z- waiting for 1730861862.52, time is 1730861858.94 2024-11-06T02:57:39Z- AHRS_M2 msg: 1730861859.30 epoch sec; seqNo:1160 2024-11-06T02:57:39Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 5.35 psi 2024-11-06T02:57:39Z- handleDepth()- Depth_Keller msg: 1730861859.31 epoch sec; seqNo:1238; depth 50.27 2024-11-06T02:57:39Z- handleDVL() - DVL msg epochMillisec: 1730861859.33 2024-11-06T02:57:39Z- handleDvl() - RDI_Pathfinder msg: 1730861859.33 sec; seqNo:388; #beams:4 2024-11-06T02:57:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 40.57 , 44.45 , 42.82 2024-11-06T02:57:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.09 , -0.00 2024-11-06T02:57:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861859.35 epoch sec; seqNo:1238; 4061654.13 north; 594245.46 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T02:57:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861858.52 2024-11-06T02:57:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:39Z- waiting for 1730861862.52, time is 1730861859.35 2024-11-06T02:57:39Z- AHRS_M2 msg: 1730861859.71 epoch sec; seqNo:1161 2024-11-06T02:57:39Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 5.35 psi 2024-11-06T02:57:39Z- handleDepth()- Depth_Keller msg: 1730861859.72 epoch sec; seqNo:1239; depth 50.27 2024-11-06T02:57:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861859.74 epoch sec; seqNo:1239; 4061654.51 north; 594245.45 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T02:57:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861858.52 2024-11-06T02:57:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:39Z- waiting for 1730861862.52, time is 1730861859.74 2024-11-06T02:57:40Z- AHRS_M2 msg: 1730861860.11 epoch sec; seqNo:1162 2024-11-06T02:57:40Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 5.35 psi 2024-11-06T02:57:40Z- handleDepth()- Depth_Keller msg: 1730861860.12 epoch sec; seqNo:1240; depth 50.28 2024-11-06T02:57:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861860.14 epoch sec; seqNo:1240; 4061654.51 north; 594245.45 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T02:57:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861858.52 2024-11-06T02:57:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:40Z- waiting for 1730861862.52, time is 1730861860.14 2024-11-06T02:57:40Z- AHRS_M2 msg: 1730861860.52 epoch sec; seqNo:1163 2024-11-06T02:57:40Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.34 psi 2024-11-06T02:57:40Z- handleDepth()- Depth_Keller msg: 1730861860.53 epoch sec; seqNo:1241; depth 50.29 2024-11-06T02:57:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861860.54 epoch sec; seqNo:1241; 4061654.89 north; 594245.45 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T02:57:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861858.52 2024-11-06T02:57:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:40Z- waiting for 1730861862.52, time is 1730861860.54 2024-11-06T02:57:40Z- AHRS_M2 msg: 1730861860.92 epoch sec; seqNo:1164 2024-11-06T02:57:40Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.34 psi 2024-11-06T02:57:40Z- handleDepth()- Depth_Keller msg: 1730861860.93 epoch sec; seqNo:1242; depth 50.28 2024-11-06T02:57:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861860.94 epoch sec; seqNo:1242; 4061654.87 north; 594244.23 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T02:57:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861858.52 2024-11-06T02:57:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:41Z- waiting for 1730861862.52, time is 1730861860.94 2024-11-06T02:57:41Z- AHRS_M2 msg: 1730861861.33 epoch sec; seqNo:1165 2024-11-06T02:57:41Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.34 psi 2024-11-06T02:57:41Z- handleDepth()- Depth_Keller msg: 1730861861.33 epoch sec; seqNo:1243; depth 50.30 2024-11-06T02:57:41Z- handleDVL() - DVL msg epochMillisec: 1730861861.35 2024-11-06T02:57:41Z- handleDvl() - RDI_Pathfinder msg: 1730861861.35 sec; seqNo:389; #beams:4 2024-11-06T02:57:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 40.57 , 44.65 , 42.61 2024-11-06T02:57:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.08 , 0.03 2024-11-06T02:57:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861861.36 epoch sec; seqNo:1243; 4061655.25 north; 594244.22 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T02:57:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861858.52 2024-11-06T02:57:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:41Z- waiting for 1730861862.52, time is 1730861861.36 2024-11-06T02:57:41Z- AHRS_M2 msg: 1730861861.73 epoch sec; seqNo:1166 2024-11-06T02:57:41Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.34 psi 2024-11-06T02:57:41Z- handleDepth()- Depth_Keller msg: 1730861861.73 epoch sec; seqNo:1244; depth 50.30 2024-11-06T02:57:41Z- handleDVL() - DVL msg epochMillisec: 1730861861.73 2024-11-06T02:57:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:57:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:57:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:57:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:57:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:57:41Z- handleDvl() - RDI_Pathfinder msg: 1730861861.73 sec; seqNo:390; #beams:0 2024-11-06T02:57:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:57:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.08 , 0.03 2024-11-06T02:57:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861861.75 epoch sec; seqNo:1244; 4061655.25 north; 594244.22 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T02:57:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861858.52 2024-11-06T02:57:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:41Z- waiting for 1730861862.52, time is 1730861861.75 2024-11-06T02:57:42Z- AHRS_M2 msg: 1730861862.14 epoch sec; seqNo:1167 2024-11-06T02:57:42Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.34 psi 2024-11-06T02:57:42Z- handleDepth()- Depth_Keller msg: 1730861862.17 epoch sec; seqNo:1245; depth 50.31 2024-11-06T02:57:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861862.18 epoch sec; seqNo:1245; 4061655.62 north; 594243.00 east; 36.70 lat; -121.95 lon; 50.31 depth; utmZone: 10 2024-11-06T02:57:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861858.52 2024-11-06T02:57:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:42Z- waiting for 1730861862.52, time is 1730861862.18 2024-11-06T02:57:42Z- AHRS_M2 msg: 1730861862.54 epoch sec; seqNo:1168 2024-11-06T02:57:42Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.34 psi 2024-11-06T02:57:42Z- handleDepth()- Depth_Keller msg: 1730861862.54 epoch sec; seqNo:1246; depth 50.30 2024-11-06T02:57:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861862.56 epoch sec; seqNo:1246; 4061655.62 north; 594243.00 east; 36.70 lat; -121.95 lon; 50.30 depth; utmZone: 10 2024-11-06T02:57:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861858.52 2024-11-06T02:57:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:42Z- waiting for 1730861862.52, time is 1730861862.56 2024-11-06T02:57:42Z- LcmTrn::updateTrn() >>>> pose time 1730861862.56, meas time 1730861861.73 <<<< 2024-11-06T02:57:42Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861862.56 2024-11-06T02:57:42Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:57:42Z- TerrainNav::motionUpdate() - currEstimate = (4061656, 594243.00, 50.30) 2024-11-06T02:57:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:57:42Z- TerrainNav:: SubcloudNIS is 0.18, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:57:42Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:57:42Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:57:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:57:42Z- variances([0], [2], and psi_berg) = 0.325737 0.267697 0.000000 2024-11-06T02:57:42Z- publishEstimatesDecoder() - at 1730861862.56 sec 2024-11-06T02:57:42Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:57:42Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061687.78 594227.15 58.43 2024-11-06T02:57:42Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061688.09 594226.87 56.78 lat/lon: 36.70 -121.95 2024-11-06T02:57:42Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.27 0.95 0.00 2024-11-06T02:57:42Z- AHRS_M2 msg: 1730861862.94 epoch sec; seqNo:1169 2024-11-06T02:57:42Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.34 psi 2024-11-06T02:57:42Z- handleDepth()- Depth_Keller msg: 1730861862.95 epoch sec; seqNo:1247; depth 50.32 2024-11-06T02:57:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861862.95 epoch sec; seqNo:1247; 4061656.00 north; 594242.99 east; 36.70 lat; -121.95 lon; 50.32 depth; utmZone: 10 2024-11-06T02:57:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861862.56 2024-11-06T02:57:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:43Z- waiting for 1730861866.56, time is 1730861862.95 2024-11-06T02:57:43Z- AHRS_M2 msg: 1730861863.34 epoch sec; seqNo:1170 2024-11-06T02:57:43Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.34 psi 2024-11-06T02:57:43Z- handleDepth()- Depth_Keller msg: 1730861863.35 epoch sec; seqNo:1248; depth 50.31 2024-11-06T02:57:43Z- handleDVL() - DVL msg epochMillisec: 1730861863.38 2024-11-06T02:57:43Z- handleDvl() - RDI_Pathfinder msg: 1730861863.38 sec; seqNo:391; #beams:4 2024-11-06T02:57:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 40.57 , 44.85 , 42.82 2024-11-06T02:57:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , 0.03 2024-11-06T02:57:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861863.39 epoch sec; seqNo:1248; 4061656.37 north; 594242.99 east; 36.70 lat; -121.95 lon; 50.31 depth; utmZone: 10 2024-11-06T02:57:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861862.56 2024-11-06T02:57:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:43Z- waiting for 1730861866.56, time is 1730861863.39 2024-11-06T02:57:43Z- AHRS_M2 msg: 1730861863.75 epoch sec; seqNo:1171 2024-11-06T02:57:43Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.34 psi 2024-11-06T02:57:43Z- handleDepth()- Depth_Keller msg: 1730861863.75 epoch sec; seqNo:1249; depth 50.32 2024-11-06T02:57:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861863.77 epoch sec; seqNo:1249; 4061656.36 north; 594241.77 east; 36.70 lat; -121.95 lon; 50.32 depth; utmZone: 10 2024-11-06T02:57:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861862.56 2024-11-06T02:57:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:43Z- waiting for 1730861866.56, time is 1730861863.77 2024-11-06T02:57:44Z- AHRS_M2 msg: 1730861864.15 epoch sec; seqNo:1172 2024-11-06T02:57:44Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.34 psi 2024-11-06T02:57:44Z- handleDepth()- Depth_Keller msg: 1730861864.16 epoch sec; seqNo:1250; depth 50.31 2024-11-06T02:57:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861864.16 epoch sec; seqNo:1250; 4061656.74 north; 594241.77 east; 36.70 lat; -121.95 lon; 50.31 depth; utmZone: 10 2024-11-06T02:57:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861862.56 2024-11-06T02:57:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:44Z- waiting for 1730861866.56, time is 1730861864.16 2024-11-06T02:57:44Z- AHRS_M2 msg: 1730861864.56 epoch sec; seqNo:1173 2024-11-06T02:57:44Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.33 psi 2024-11-06T02:57:44Z- handleDepth()- Depth_Keller msg: 1730861864.57 epoch sec; seqNo:1251; depth 50.31 2024-11-06T02:57:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861864.57 epoch sec; seqNo:1251; 4061656.74 north; 594241.77 east; 36.70 lat; -121.95 lon; 50.31 depth; utmZone: 10 2024-11-06T02:57:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861862.56 2024-11-06T02:57:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:44Z- waiting for 1730861866.56, time is 1730861864.57 2024-11-06T02:57:44Z- AHRS_M2 msg: 1730861864.96 epoch sec; seqNo:1174 2024-11-06T02:57:44Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.33 psi 2024-11-06T02:57:44Z- handleDepth()- Depth_Keller msg: 1730861864.97 epoch sec; seqNo:1252; depth 50.31 2024-11-06T02:57:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861864.98 epoch sec; seqNo:1252; 4061657.12 north; 594241.76 east; 36.70 lat; -121.95 lon; 50.31 depth; utmZone: 10 2024-11-06T02:57:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861862.56 2024-11-06T02:57:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:45Z- waiting for 1730861866.56, time is 1730861864.98 2024-11-06T02:57:45Z- AHRS_M2 msg: 1730861865.37 epoch sec; seqNo:1175 2024-11-06T02:57:45Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.33 psi 2024-11-06T02:57:45Z- handleDepth()- Depth_Keller msg: 1730861865.37 epoch sec; seqNo:1253; depth 50.32 2024-11-06T02:57:45Z- handleDVL() - DVL msg epochMillisec: 1730861865.40 2024-11-06T02:57:45Z- handleDvl() - RDI_Pathfinder msg: 1730861865.40 sec; seqNo:392; #beams:4 2024-11-06T02:57:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.51 , 40.78 , 44.85 , 43.02 2024-11-06T02:57:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , 0.01 2024-11-06T02:57:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861865.41 epoch sec; seqNo:1253; 4061657.11 north; 594240.54 east; 36.70 lat; -121.95 lon; 50.32 depth; utmZone: 10 2024-11-06T02:57:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861862.56 2024-11-06T02:57:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:45Z- waiting for 1730861866.56, time is 1730861865.41 2024-11-06T02:57:45Z- AHRS_M2 msg: 1730861865.77 epoch sec; seqNo:1176 2024-11-06T02:57:45Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.33 psi 2024-11-06T02:57:45Z- handleDepth()- Depth_Keller msg: 1730861865.77 epoch sec; seqNo:1254; depth 50.30 2024-11-06T02:57:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861865.79 epoch sec; seqNo:1254; 4061657.48 north; 594240.54 east; 36.70 lat; -121.95 lon; 50.30 depth; utmZone: 10 2024-11-06T02:57:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861862.56 2024-11-06T02:57:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:45Z- waiting for 1730861866.56, time is 1730861865.79 2024-11-06T02:57:46Z- AHRS_M2 msg: 1730861866.17 epoch sec; seqNo:1177 2024-11-06T02:57:46Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.33 psi 2024-11-06T02:57:46Z- handleDepth()- Depth_Keller msg: 1730861866.18 epoch sec; seqNo:1255; depth 50.30 2024-11-06T02:57:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861866.19 epoch sec; seqNo:1255; 4061657.48 north; 594240.54 east; 36.70 lat; -121.95 lon; 50.30 depth; utmZone: 10 2024-11-06T02:57:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861862.56 2024-11-06T02:57:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:46Z- waiting for 1730861866.56, time is 1730861866.19 2024-11-06T02:57:46Z- AHRS_M2 msg: 1730861866.58 epoch sec; seqNo:1178 2024-11-06T02:57:46Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.33 psi 2024-11-06T02:57:46Z- handleDepth()- Depth_Keller msg: 1730861866.58 epoch sec; seqNo:1256; depth 50.28 2024-11-06T02:57:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861866.59 epoch sec; seqNo:1256; 4061657.48 north; 594240.54 east; 36.70 lat; -121.95 lon; 50.28 depth; utmZone: 10 2024-11-06T02:57:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861862.56 2024-11-06T02:57:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:46Z- waiting for 1730861866.56, time is 1730861866.59 2024-11-06T02:57:46Z- LcmTrn::updateTrn() >>>> pose time 1730861866.59, meas time 1730861865.40 <<<< 2024-11-06T02:57:46Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861866.59 2024-11-06T02:57:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861865.40 sec, ping # 0; waiting for INS data more recent than 1730861862.56... 2024-11-06T02:57:46Z- TerrainNav::motionUpdate() - currEstimate = (4061657, 594240.54, 50.28) 2024-11-06T02:57:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:57:46Z- TerrainNav:: SubcloudNIS is 0.18, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:57:46Z- TNavPF::Projecting Measurements 2024-11-06T02:57:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:57:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:57:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:57:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:57:46Z- TNavFilter::4 good beams. 2024-11-06T02:57:46Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:57:46Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:57:46Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:57:46Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:57:46Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:57:46Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:57:46Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:57:46Z- TerrainMapOctree: (vehN, vehE ) = (4061656.93, 594241.27). 2024-11-06T02:57:46Z- TerrainMapOctree: Min Distance = 93.48. 2024-11-06T02:57:46Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:57:46Z- Forcing Subcloud Comparison 2024-11-06T02:57:46Z- Weighting particles with subcloud comparison 2024-11-06T02:57:46Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:46Z- meanExpectedMeasDiff: -1.313283 delta_rms_squared: 1.874853 alpha: 0.989445 2024-11-06T02:57:46Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:46Z- meanExpectedMeasDiff: -0.368870 delta_rms_squared: 1.399541 alpha: 0.985911 2024-11-06T02:57:46Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:46Z- meanExpectedMeasDiff: 0.479166 delta_rms_squared: 1.630280 alpha: 0.987881 2024-11-06T02:57:46Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:46Z- meanExpectedMeasDiff: 0.303745 delta_rms_squared: 1.593351 alpha: 0.987603 2024-11-06T02:57:46Z- Subcloud NIS: 0.171571 2024-11-06T02:57:46Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:57:46Z- TNavPF:: sumWeights = 0.538568 2024-11-06T02:57:46Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:57:46Z- TNavPF::First Term of Map Covariance Matrix is 0.59 2024-11-06T02:57:46Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T02:57:46Z- TerrainNav::Resampling particle filter... 2024-11-06T02:57:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861865.40 sec. 2024-11-06T02:57:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:57:46Z- variances([0], [2], and psi_berg) = 0.327098 0.267311 0.000000 2024-11-06T02:57:46Z- publishEstimatesDecoder() - at 1730861866.59 sec 2024-11-06T02:57:46Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:57:46Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061689.55 594225.29 55.51 2024-11-06T02:57:46Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061689.96 594224.40 56.87 lat/lon: 36.70 -121.95 2024-11-06T02:57:46Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.27 0.76 0.00 2024-11-06T02:57:46Z- AHRS_M2 msg: 1730861866.98 epoch sec; seqNo:1179 2024-11-06T02:57:47Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.33 psi 2024-11-06T02:57:47Z- handleDepth()- Depth_Keller msg: 1730861867.00 epoch sec; seqNo:1257; depth 50.27 2024-11-06T02:57:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861867.02 epoch sec; seqNo:1257; 4061657.85 north; 594239.31 east; 36.70 lat; -121.95 lon; 50.27 depth; utmZone: 10 2024-11-06T02:57:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861866.59 2024-11-06T02:57:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:47Z- waiting for 1730861870.59, time is 1730861867.02 2024-11-06T02:57:47Z- AHRS_M2 msg: 1730861867.39 epoch sec; seqNo:1180 2024-11-06T02:57:47Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.33 psi 2024-11-06T02:57:47Z- handleDepth()- Depth_Keller msg: 1730861867.39 epoch sec; seqNo:1258; depth 50.26 2024-11-06T02:57:47Z- handleDVL() - DVL msg epochMillisec: 1730861867.41 2024-11-06T02:57:47Z- handleDvl() - RDI_Pathfinder msg: 1730861867.41 sec; seqNo:393; #beams:4 2024-11-06T02:57:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 41.19 , 44.85 , 43.22 2024-11-06T02:57:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , -0.02 2024-11-06T02:57:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861867.43 epoch sec; seqNo:1258; 4061657.85 north; 594239.31 east; 36.70 lat; -121.95 lon; 50.26 depth; utmZone: 10 2024-11-06T02:57:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861866.59 2024-11-06T02:57:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:47Z- waiting for 1730861870.59, time is 1730861867.43 2024-11-06T02:57:47Z- AHRS_M2 msg: 1730861867.79 epoch sec; seqNo:1181 2024-11-06T02:57:47Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.33 psi 2024-11-06T02:57:47Z- handleDepth()- Depth_Keller msg: 1730861867.79 epoch sec; seqNo:1259; depth 50.25 2024-11-06T02:57:47Z- handleDVL() - DVL msg epochMillisec: 1730861867.79 2024-11-06T02:57:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:57:47Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:57:47Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:57:47Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:57:47Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:57:47Z- handleDvl() - RDI_Pathfinder msg: 1730861867.79 sec; seqNo:394; #beams:0 2024-11-06T02:57:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:57:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , -0.02 2024-11-06T02:57:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861867.82 epoch sec; seqNo:1259; 4061658.23 north; 594239.31 east; 36.70 lat; -121.95 lon; 50.25 depth; utmZone: 10 2024-11-06T02:57:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861866.59 2024-11-06T02:57:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:47Z- waiting for 1730861870.59, time is 1730861867.82 2024-11-06T02:57:48Z- AHRS_M2 msg: 1730861868.19 epoch sec; seqNo:1182 2024-11-06T02:57:48Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.33 psi 2024-11-06T02:57:48Z- handleDepth()- Depth_Keller msg: 1730861868.21 epoch sec; seqNo:1260; depth 50.24 2024-11-06T02:57:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861868.24 epoch sec; seqNo:1260; 4061658.23 north; 594239.31 east; 36.70 lat; -121.95 lon; 50.24 depth; utmZone: 10 2024-11-06T02:57:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861866.59 2024-11-06T02:57:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:48Z- waiting for 1730861870.59, time is 1730861868.24 2024-11-06T02:57:48Z- AHRS_M2 msg: 1730861868.60 epoch sec; seqNo:1183 2024-11-06T02:57:48Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.33 psi 2024-11-06T02:57:48Z- handleDepth()- Depth_Keller msg: 1730861868.60 epoch sec; seqNo:1261; depth 50.23 2024-11-06T02:57:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861868.62 epoch sec; seqNo:1261; 4061658.59 north; 594238.08 east; 36.70 lat; -121.95 lon; 50.23 depth; utmZone: 10 2024-11-06T02:57:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861866.59 2024-11-06T02:57:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:48Z- waiting for 1730861870.59, time is 1730861868.62 2024-11-06T02:57:49Z- AHRS_M2 msg: 1730861869.00 epoch sec; seqNo:1184 2024-11-06T02:57:49Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.32 psi 2024-11-06T02:57:49Z- handleDepth()- Depth_Keller msg: 1730861869.01 epoch sec; seqNo:1262; depth 50.21 2024-11-06T02:57:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861869.02 epoch sec; seqNo:1262; 4061658.59 north; 594238.08 east; 36.70 lat; -121.95 lon; 50.21 depth; utmZone: 10 2024-11-06T02:57:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861866.59 2024-11-06T02:57:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:49Z- waiting for 1730861870.59, time is 1730861869.02 2024-11-06T02:57:49Z- AHRS_M2 msg: 1730861869.41 epoch sec; seqNo:1185 2024-11-06T02:57:49Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.32 psi 2024-11-06T02:57:49Z- handleDepth()- Depth_Keller msg: 1730861869.41 epoch sec; seqNo:1263; depth 50.20 2024-11-06T02:57:49Z- handleDVL() - DVL msg epochMillisec: 1730861869.44 2024-11-06T02:57:49Z- handleDvl() - RDI_Pathfinder msg: 1730861869.44 sec; seqNo:395; #beams:4 2024-11-06T02:57:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 41.39 , 44.85 , 43.22 2024-11-06T02:57:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , -0.07 2024-11-06T02:57:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861869.46 epoch sec; seqNo:1263; 4061658.97 north; 594238.08 east; 36.70 lat; -121.95 lon; 50.20 depth; utmZone: 10 2024-11-06T02:57:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861866.59 2024-11-06T02:57:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:49Z- waiting for 1730861870.59, time is 1730861869.46 2024-11-06T02:57:49Z- AHRS_M2 msg: 1730861869.81 epoch sec; seqNo:1186 2024-11-06T02:57:49Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.32 psi 2024-11-06T02:57:49Z- handleDepth()- Depth_Keller msg: 1730861869.81 epoch sec; seqNo:1264; depth 50.19 2024-11-06T02:57:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861869.83 epoch sec; seqNo:1264; 4061658.97 north; 594238.08 east; 36.70 lat; -121.95 lon; 50.19 depth; utmZone: 10 2024-11-06T02:57:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861866.59 2024-11-06T02:57:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:49Z- waiting for 1730861870.59, time is 1730861869.83 2024-11-06T02:57:50Z- AHRS_M2 msg: 1730861870.21 epoch sec; seqNo:1187 2024-11-06T02:57:50Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.32 psi 2024-11-06T02:57:50Z- handleDepth()- Depth_Keller msg: 1730861870.22 epoch sec; seqNo:1265; depth 50.17 2024-11-06T02:57:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861870.23 epoch sec; seqNo:1265; 4061659.34 north; 594236.86 east; 36.70 lat; -121.95 lon; 50.17 depth; utmZone: 10 2024-11-06T02:57:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861866.59 2024-11-06T02:57:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:50Z- waiting for 1730861870.59, time is 1730861870.23 2024-11-06T02:57:50Z- AHRS_M2 msg: 1730861870.62 epoch sec; seqNo:1188 2024-11-06T02:57:50Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.32 psi 2024-11-06T02:57:50Z- handleDepth()- Depth_Keller msg: 1730861870.62 epoch sec; seqNo:1266; depth 50.16 2024-11-06T02:57:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861870.65 epoch sec; seqNo:1266; 4061659.34 north; 594236.86 east; 36.70 lat; -121.95 lon; 50.16 depth; utmZone: 10 2024-11-06T02:57:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861866.59 2024-11-06T02:57:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:50Z- waiting for 1730861870.59, time is 1730861870.65 2024-11-06T02:57:50Z- LcmTrn::updateTrn() >>>> pose time 1730861870.65, meas time 1730861869.44 <<<< 2024-11-06T02:57:50Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861870.65 2024-11-06T02:57:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861869.44 sec, ping # 0; waiting for INS data more recent than 1730861866.59... 2024-11-06T02:57:50Z- TerrainNav::motionUpdate() - currEstimate = (4061659, 594236.86, 50.16) 2024-11-06T02:57:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:57:50Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:57:50Z- TNavPF::Projecting Measurements 2024-11-06T02:57:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:57:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:57:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:57:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:57:50Z- TNavFilter::4 good beams. 2024-11-06T02:57:50Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:57:50Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:57:50Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:57:50Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:57:50Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:57:50Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:57:50Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:57:50Z- TerrainMapOctree: (vehN, vehE ) = (4061658.79, 594237.95). 2024-11-06T02:57:50Z- TerrainMapOctree: Min Distance = 92.01. 2024-11-06T02:57:50Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:57:50Z- Forcing Subcloud Comparison 2024-11-06T02:57:50Z- Weighting particles with subcloud comparison 2024-11-06T02:57:50Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:50Z- meanExpectedMeasDiff: -1.608723 delta_rms_squared: 1.568229 alpha: 0.987407 2024-11-06T02:57:50Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:50Z- meanExpectedMeasDiff: 0.226796 delta_rms_squared: 1.177450 alpha: 0.983298 2024-11-06T02:57:50Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:50Z- meanExpectedMeasDiff: 0.518273 delta_rms_squared: 1.367671 alpha: 0.985587 2024-11-06T02:57:50Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:50Z- meanExpectedMeasDiff: 0.544986 delta_rms_squared: 1.336722 alpha: 0.985259 2024-11-06T02:57:50Z- Subcloud NIS: 0.161962 2024-11-06T02:57:50Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:57:50Z- TNavPF:: sumWeights = 0.508162 2024-11-06T02:57:50Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:57:50Z- TNavPF::First Term of Map Covariance Matrix is 0.68 2024-11-06T02:57:50Z- TNavPF::Calculated NIS Value : 0.21 numBeams normalized NIS: 0.05 2024-11-06T02:57:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861869.44 sec. 2024-11-06T02:57:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:57:50Z- variances([0], [2], and psi_berg) = 0.330090 0.269246 0.000000 2024-11-06T02:57:50Z- publishEstimatesDecoder() - at 1730861870.65 sec 2024-11-06T02:57:50Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:57:50Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061691.72 594221.62 57.46 2024-11-06T02:57:50Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061691.81 594220.72 56.78 lat/lon: 36.70 -121.95 2024-11-06T02:57:50Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.27 0.74 0.00 2024-11-06T02:57:51Z- AHRS_M2 msg: 1730861871.02 epoch sec; seqNo:1189 2024-11-06T02:57:51Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.32 psi 2024-11-06T02:57:51Z- handleDepth()- Depth_Keller msg: 1730861871.04 epoch sec; seqNo:1267; depth 50.16 2024-11-06T02:57:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861871.06 epoch sec; seqNo:1267; 4061659.72 north; 594236.85 east; 36.70 lat; -121.95 lon; 50.16 depth; utmZone: 10 2024-11-06T02:57:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861870.65 2024-11-06T02:57:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:51Z- waiting for 1730861874.65, time is 1730861871.06 2024-11-06T02:57:51Z- AHRS_M2 msg: 1730861871.42 epoch sec; seqNo:1190 2024-11-06T02:57:51Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.32 psi 2024-11-06T02:57:51Z- handleDepth()- Depth_Keller msg: 1730861871.43 epoch sec; seqNo:1268; depth 50.16 2024-11-06T02:57:51Z- handleDVL() - DVL msg epochMillisec: 1730861871.45 2024-11-06T02:57:51Z- handleDvl() - RDI_Pathfinder msg: 1730861871.45 sec; seqNo:396; #beams:4 2024-11-06T02:57:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 41.39 , 44.45 , 43.43 2024-11-06T02:57:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.09 , -0.07 2024-11-06T02:57:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861871.48 epoch sec; seqNo:1268; 4061659.72 north; 594236.85 east; 36.70 lat; -121.95 lon; 50.16 depth; utmZone: 10 2024-11-06T02:57:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861870.65 2024-11-06T02:57:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:51Z- waiting for 1730861874.65, time is 1730861871.48 2024-11-06T02:57:51Z- handleDepth()- Depth_Keller msg: 1730861871.85 epoch sec; seqNo:1269; depth 50.15 2024-11-06T02:57:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861871.87 epoch sec; seqNo:1269; 4061660.08 north; 594235.63 east; 36.70 lat; -121.95 lon; 50.15 depth; utmZone: 10 2024-11-06T02:57:52Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:57:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861870.65 2024-11-06T02:57:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:52Z- waiting for 1730861874.65, time is 1730861871.87 2024-11-06T02:57:52Z- AHRS_M2 msg: 1730861872.23 epoch sec; seqNo:1191 2024-11-06T02:57:52Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 5.31 psi 2024-11-06T02:57:52Z- handleDepth()- Depth_Keller msg: 1730861872.24 epoch sec; seqNo:1270; depth 50.15 2024-11-06T02:57:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861872.25 epoch sec; seqNo:1270; 4061660.08 north; 594235.63 east; 36.70 lat; -121.95 lon; 50.15 depth; utmZone: 10 2024-11-06T02:57:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861870.65 2024-11-06T02:57:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:52Z- waiting for 1730861874.65, time is 1730861872.25 2024-11-06T02:57:52Z- AHRS_M2 msg: 1730861872.64 epoch sec; seqNo:1192 2024-11-06T02:57:52Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 5.31 psi 2024-11-06T02:57:52Z- handleDepth()- Depth_Keller msg: 1730861872.64 epoch sec; seqNo:1271; depth 50.12 2024-11-06T02:57:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861872.67 epoch sec; seqNo:1271; 4061660.46 north; 594235.62 east; 36.70 lat; -121.95 lon; 50.12 depth; utmZone: 10 2024-11-06T02:57:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861870.65 2024-11-06T02:57:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:52Z- waiting for 1730861874.65, time is 1730861872.67 2024-11-06T02:57:53Z- AHRS_M2 msg: 1730861873.04 epoch sec; seqNo:1193 2024-11-06T02:57:53Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 5.30 psi 2024-11-06T02:57:53Z- handleDepth()- Depth_Keller msg: 1730861873.04 epoch sec; seqNo:1272; depth 50.12 2024-11-06T02:57:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861873.05 epoch sec; seqNo:1272; 4061660.45 north; 594234.40 east; 36.70 lat; -121.95 lon; 50.12 depth; utmZone: 10 2024-11-06T02:57:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861870.65 2024-11-06T02:57:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:53Z- waiting for 1730861874.65, time is 1730861873.05 2024-11-06T02:57:53Z- AHRS_M2 msg: 1730861873.44 epoch sec; seqNo:1194 2024-11-06T02:57:53Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 5.30 psi 2024-11-06T02:57:53Z- handleDepth()- Depth_Keller msg: 1730861873.45 epoch sec; seqNo:1273; depth 50.13 2024-11-06T02:57:53Z- handleDVL() - DVL msg epochMillisec: 1730861873.48 2024-11-06T02:57:53Z- handleDvl() - RDI_Pathfinder msg: 1730861873.48 sec; seqNo:397; #beams:4 2024-11-06T02:57:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 41.39 , 44.85 , 43.02 2024-11-06T02:57:53Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.08 , 0.00 2024-11-06T02:57:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861873.52 epoch sec; seqNo:1273; 4061660.83 north; 594234.40 east; 36.70 lat; -121.95 lon; 50.13 depth; utmZone: 10 2024-11-06T02:57:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861870.65 2024-11-06T02:57:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:53Z- waiting for 1730861874.65, time is 1730861873.52 2024-11-06T02:57:53Z- AHRS_M2 msg: 1730861873.86 epoch sec; seqNo:1195 2024-11-06T02:57:53Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 5.30 psi 2024-11-06T02:57:53Z- handleDepth()- Depth_Keller msg: 1730861873.86 epoch sec; seqNo:1274; depth 50.14 2024-11-06T02:57:53Z- handleDVL() - DVL msg epochMillisec: 1730861873.87 2024-11-06T02:57:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:57:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:57:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:57:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:57:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:57:53Z- handleDvl() - RDI_Pathfinder msg: 1730861873.87 sec; seqNo:398; #beams:0 2024-11-06T02:57:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:57:53Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.08 , 0.00 2024-11-06T02:57:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861873.89 epoch sec; seqNo:1274; 4061661.21 north; 594234.39 east; 36.70 lat; -121.95 lon; 50.14 depth; utmZone: 10 2024-11-06T02:57:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861870.65 2024-11-06T02:57:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:54Z- waiting for 1730861874.65, time is 1730861873.89 2024-11-06T02:57:54Z- AHRS_M2 msg: 1730861874.25 epoch sec; seqNo:1196 2024-11-06T02:57:54Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 5.30 psi 2024-11-06T02:57:54Z- handleDepth()- Depth_Keller msg: 1730861874.26 epoch sec; seqNo:1275; depth 50.13 2024-11-06T02:57:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861874.29 epoch sec; seqNo:1275; 4061661.21 north; 594234.39 east; 36.70 lat; -121.95 lon; 50.13 depth; utmZone: 10 2024-11-06T02:57:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861870.65 2024-11-06T02:57:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:54Z- waiting for 1730861874.65, time is 1730861874.29 2024-11-06T02:57:54Z- handleDepth()- Depth_Keller msg: 1730861874.69 epoch sec; seqNo:1276; depth 50.11 2024-11-06T02:57:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861874.70 epoch sec; seqNo:1276; 4061661.57 north; 594233.17 east; 36.70 lat; -121.95 lon; 50.11 depth; utmZone: 10 2024-11-06T02:57:54Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:57:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861870.65 2024-11-06T02:57:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:54Z- waiting for 1730861874.65, time is 1730861874.70 2024-11-06T02:57:54Z- LcmTrn::updateTrn() >>>> pose time 1730861874.70, meas time 1730861873.87 <<<< 2024-11-06T02:57:54Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861874.70 2024-11-06T02:57:54Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:57:54Z- TerrainNav::motionUpdate() - currEstimate = (4061662, 594233.17, 50.11) 2024-11-06T02:57:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:57:54Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:57:54Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:57:54Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:57:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:57:54Z- variances([0], [2], and psi_berg) = 0.333416 0.271595 0.000000 2024-11-06T02:57:54Z- publishEstimatesDecoder() - at 1730861874.70 sec 2024-11-06T02:57:54Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:57:54Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061694.01 594217.91 57.66 2024-11-06T02:57:54Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061694.04 594217.03 56.73 lat/lon: 36.70 -121.95 2024-11-06T02:57:54Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.27 0.97 0.00 2024-11-06T02:57:55Z- AHRS_M2 msg: 1730861875.06 epoch sec; seqNo:1197 2024-11-06T02:57:55Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 5.30 psi 2024-11-06T02:57:55Z- handleDepth()- Depth_Keller msg: 1730861875.08 epoch sec; seqNo:1277; depth 50.12 2024-11-06T02:57:55Z- handleDVL() - DVL msg epochMillisec: 1730861875.08 2024-11-06T02:57:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:57:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:57:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:57:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:57:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:57:55Z- handleDvl() - RDI_Pathfinder msg: 1730861875.08 sec; seqNo:399; #beams:0 2024-11-06T02:57:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:57:55Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.08 , 0.00 2024-11-06T02:57:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861875.10 epoch sec; seqNo:1277; 4061661.57 north; 594233.17 east; 36.70 lat; -121.95 lon; 50.12 depth; utmZone: 10 2024-11-06T02:57:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861874.70 2024-11-06T02:57:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:55Z- waiting for 1730861878.70, time is 1730861875.10 2024-11-06T02:57:55Z- AHRS_M2 msg: 1730861875.47 epoch sec; seqNo:1198 2024-11-06T02:57:55Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 5.30 psi 2024-11-06T02:57:55Z- handleDepth()- Depth_Keller msg: 1730861875.47 epoch sec; seqNo:1278; depth 50.11 2024-11-06T02:57:55Z- handleDVL() - DVL msg epochMillisec: 1730861875.52 2024-11-06T02:57:55Z- handleDvl() - RDI_Pathfinder msg: 1730861875.52 sec; seqNo:400; #beams:4 2024-11-06T02:57:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 41.39 , 45.26 , 43.02 2024-11-06T02:57:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.08 , 0.07 2024-11-06T02:57:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861875.55 epoch sec; seqNo:1278; 4061661.95 north; 594233.17 east; 36.70 lat; -121.95 lon; 50.11 depth; utmZone: 10 2024-11-06T02:57:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861874.70 2024-11-06T02:57:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:55Z- waiting for 1730861878.70, time is 1730861875.55 2024-11-06T02:57:55Z- AHRS_M2 msg: 1730861875.87 epoch sec; seqNo:1199 2024-11-06T02:57:55Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 5.30 psi 2024-11-06T02:57:55Z- handleDepth()- Depth_Keller msg: 1730861875.88 epoch sec; seqNo:1279; depth 50.13 2024-11-06T02:57:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861875.89 epoch sec; seqNo:1279; 4061661.94 north; 594231.94 east; 36.70 lat; -121.95 lon; 50.13 depth; utmZone: 10 2024-11-06T02:57:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861874.70 2024-11-06T02:57:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:56Z- waiting for 1730861878.70, time is 1730861875.89 2024-11-06T02:57:56Z- AHRS_M2 msg: 1730861876.27 epoch sec; seqNo:1200 2024-11-06T02:57:56Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 5.30 psi 2024-11-06T02:57:56Z- handleDepth()- Depth_Keller msg: 1730861876.28 epoch sec; seqNo:1280; depth 50.11 2024-11-06T02:57:56Z- handleDVL() - DVL msg epochMillisec: 1730861876.28 2024-11-06T02:57:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:57:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:57:56Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:57:56Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:57:56Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:57:56Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:57:56Z- handleDvl() - RDI_Pathfinder msg: 1730861876.28 sec; seqNo:401; #beams:0 2024-11-06T02:57:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:57:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.08 , 0.07 2024-11-06T02:57:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861876.29 epoch sec; seqNo:1280; 4061661.94 north; 594231.94 east; 36.70 lat; -121.95 lon; 50.11 depth; utmZone: 10 2024-11-06T02:57:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861874.70 2024-11-06T02:57:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:56Z- waiting for 1730861878.70, time is 1730861876.29 2024-11-06T02:57:56Z- AHRS_M2 msg: 1730861876.67 epoch sec; seqNo:1201 2024-11-06T02:57:56Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 5.30 psi 2024-11-06T02:57:56Z- handleDepth()- Depth_Keller msg: 1730861876.69 epoch sec; seqNo:1281; depth 50.11 2024-11-06T02:57:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861876.70 epoch sec; seqNo:1281; 4061662.32 north; 594231.94 east; 36.70 lat; -121.95 lon; 50.11 depth; utmZone: 10 2024-11-06T02:57:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861874.70 2024-11-06T02:57:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:56Z- waiting for 1730861878.70, time is 1730861876.70 2024-11-06T02:57:57Z- AHRS_M2 msg: 1730861877.08 epoch sec; seqNo:1202 2024-11-06T02:57:57Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 5.30 psi 2024-11-06T02:57:57Z- handleDepth()- Depth_Keller msg: 1730861877.08 epoch sec; seqNo:1282; depth 50.12 2024-11-06T02:57:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861877.09 epoch sec; seqNo:1282; 4061662.32 north; 594231.94 east; 36.70 lat; -121.95 lon; 50.12 depth; utmZone: 10 2024-11-06T02:57:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861874.70 2024-11-06T02:57:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:57Z- waiting for 1730861878.70, time is 1730861877.09 2024-11-06T02:57:57Z- AHRS_M2 msg: 1730861877.48 epoch sec; seqNo:1203 2024-11-06T02:57:57Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 5.31 psi 2024-11-06T02:57:57Z- handleDepth()- Depth_Keller msg: 1730861877.49 epoch sec; seqNo:1283; depth 50.10 2024-11-06T02:57:57Z- handleDVL() - DVL msg epochMillisec: 1730861877.51 2024-11-06T02:57:57Z- handleDvl() - RDI_Pathfinder msg: 1730861877.51 sec; seqNo:402; #beams:4 2024-11-06T02:57:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.12 , 41.39 , 45.47 , 43.63 2024-11-06T02:57:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , 0.05 2024-11-06T02:57:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861877.56 epoch sec; seqNo:1283; 4061662.68 north; 594230.72 east; 36.70 lat; -121.95 lon; 50.10 depth; utmZone: 10 2024-11-06T02:57:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861874.70 2024-11-06T02:57:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:57Z- waiting for 1730861878.70, time is 1730861877.56 2024-11-06T02:57:57Z- AHRS_M2 msg: 1730861877.89 epoch sec; seqNo:1204 2024-11-06T02:57:57Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 5.31 psi 2024-11-06T02:57:57Z- handleDepth()- Depth_Keller msg: 1730861877.89 epoch sec; seqNo:1284; depth 50.08 2024-11-06T02:57:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861877.91 epoch sec; seqNo:1284; 4061662.68 north; 594230.72 east; 36.70 lat; -121.95 lon; 50.08 depth; utmZone: 10 2024-11-06T02:57:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861874.70 2024-11-06T02:57:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:58Z- waiting for 1730861878.70, time is 1730861877.91 2024-11-06T02:57:58Z- AHRS_M2 msg: 1730861878.29 epoch sec; seqNo:1205 2024-11-06T02:57:58Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 5.32 psi 2024-11-06T02:57:58Z- handleDepth()- Depth_Keller msg: 1730861878.30 epoch sec; seqNo:1285; depth 50.07 2024-11-06T02:57:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861878.31 epoch sec; seqNo:1285; 4061663.06 north; 594230.71 east; 36.70 lat; -121.95 lon; 50.07 depth; utmZone: 10 2024-11-06T02:57:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861874.70 2024-11-06T02:57:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:58Z- waiting for 1730861878.70, time is 1730861878.31 2024-11-06T02:57:58Z- AHRS_M2 msg: 1730861878.69 epoch sec; seqNo:1206 2024-11-06T02:57:58Z- AHRS_M2 msg: 0.18 phi; 0.02 theta; 5.33 psi 2024-11-06T02:57:58Z- handleDepth()- Depth_Keller msg: 1730861878.70 epoch sec; seqNo:1286; depth 50.06 2024-11-06T02:57:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861878.71 epoch sec; seqNo:1286; 4061663.06 north; 594230.71 east; 36.70 lat; -121.95 lon; 50.06 depth; utmZone: 10 2024-11-06T02:57:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861874.70 2024-11-06T02:57:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:58Z- waiting for 1730861878.70, time is 1730861878.71 2024-11-06T02:57:58Z- LcmTrn::updateTrn() >>>> pose time 1730861878.71, meas time 1730861877.51 <<<< 2024-11-06T02:57:58Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861878.71 2024-11-06T02:57:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861877.51 sec, ping # 0; waiting for INS data more recent than 1730861874.70... 2024-11-06T02:57:58Z- TerrainNav::motionUpdate() - currEstimate = (4061663, 594230.71, 50.06) 2024-11-06T02:57:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:57:58Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:57:58Z- TNavPF::Projecting Measurements 2024-11-06T02:57:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:57:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:57:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:57:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:57:58Z- TNavFilter::4 good beams. 2024-11-06T02:57:58Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:57:58Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:57:58Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:57:58Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:57:58Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:57:58Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:57:58Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:57:58Z- TerrainMapOctree: (vehN, vehE ) = (4061662.62, 594231.44). 2024-11-06T02:57:58Z- TerrainMapOctree: Min Distance = 89.33. 2024-11-06T02:57:58Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:57:59Z- Forcing Subcloud Comparison 2024-11-06T02:57:59Z- Weighting particles with subcloud comparison 2024-11-06T02:57:59Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:59Z- meanExpectedMeasDiff: -2.343389 delta_rms_squared: 2.046033 alpha: 0.990320 2024-11-06T02:57:59Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:59Z- meanExpectedMeasDiff: 0.795321 delta_rms_squared: 1.391563 alpha: 0.985831 2024-11-06T02:57:59Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:59Z- meanExpectedMeasDiff: -0.603779 delta_rms_squared: 1.709549 alpha: 0.988436 2024-11-06T02:57:59Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:57:59Z- meanExpectedMeasDiff: 0.733071 delta_rms_squared: 1.641041 alpha: 0.987959 2024-11-06T02:57:59Z- Subcloud NIS: 0.166906 2024-11-06T02:57:59Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:57:59Z- TNavPF:: sumWeights = 0.344071 2024-11-06T02:57:59Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:57:59Z- TNavPF::First Term of Map Covariance Matrix is 0.97 2024-11-06T02:57:59Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.04 2024-11-06T02:57:59Z- TerrainNav::Resampling particle filter... 2024-11-06T02:57:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861877.51 sec. 2024-11-06T02:57:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:57:59Z- variances([0], [2], and psi_berg) = 0.337047 0.271703 0.000000 2024-11-06T02:57:59Z- publishEstimatesDecoder() - at 1730861878.71 sec 2024-11-06T02:57:59Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:57:59Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061695.02 594215.45 55.55 2024-11-06T02:57:59Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061695.54 594214.57 56.85 lat/lon: 36.70 -121.95 2024-11-06T02:57:59Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.34 0.27 0.75 0.00 2024-11-06T02:57:59Z- AHRS_M2 msg: 1730861879.11 epoch sec; seqNo:1207 2024-11-06T02:57:59Z- AHRS_M2 msg: 0.19 phi; 0.01 theta; 5.35 psi 2024-11-06T02:57:59Z- handleDepth()- Depth_Keller msg: 1730861879.12 epoch sec; seqNo:1287; depth 50.04 2024-11-06T02:57:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861879.15 epoch sec; seqNo:1287; 4061663.42 north; 594229.49 east; 36.70 lat; -121.95 lon; 50.04 depth; utmZone: 10 2024-11-06T02:57:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:57:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861878.71 2024-11-06T02:57:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:59Z- waiting for 1730861882.71, time is 1730861879.15 2024-11-06T02:57:59Z- AHRS_M2 msg: 1730861879.52 epoch sec; seqNo:1208 2024-11-06T02:57:59Z- AHRS_M2 msg: 0.22 phi; 0.01 theta; 5.36 psi 2024-11-06T02:57:59Z- handleDepth()- Depth_Keller msg: 1730861879.53 epoch sec; seqNo:1288; depth 50.04 2024-11-06T02:57:59Z- handleDVL() - DVL msg epochMillisec: 1730861879.55 2024-11-06T02:57:59Z- handleDvl() - RDI_Pathfinder msg: 1730861879.55 sec; seqNo:403; #beams:4 2024-11-06T02:57:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.54 , 41.19 , 45.67 , 44.24 2024-11-06T02:57:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , -0.03 2024-11-06T02:57:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861879.59 epoch sec; seqNo:1288; 4061663.42 north; 594229.49 east; 36.70 lat; -121.95 lon; 50.04 depth; utmZone: 10 2024-11-06T02:57:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:57:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861878.71 2024-11-06T02:57:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:57:59Z- waiting for 1730861882.71, time is 1730861879.59 2024-11-06T02:57:59Z- AHRS_M2 msg: 1730861879.91 epoch sec; seqNo:1209 2024-11-06T02:57:59Z- AHRS_M2 msg: 0.25 phi; 0.01 theta; 5.38 psi 2024-11-06T02:57:59Z- handleDepth()- Depth_Keller msg: 1730861879.91 epoch sec; seqNo:1289; depth 50.02 2024-11-06T02:57:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861879.94 epoch sec; seqNo:1289; 4061663.80 north; 594229.48 east; 36.70 lat; -121.95 lon; 50.02 depth; utmZone: 10 2024-11-06T02:58:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861878.71 2024-11-06T02:58:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:00Z- waiting for 1730861882.71, time is 1730861879.94 2024-11-06T02:58:00Z- AHRS_M2 msg: 1730861880.31 epoch sec; seqNo:1210 2024-11-06T02:58:00Z- AHRS_M2 msg: 0.28 phi; 0.00 theta; 5.41 psi 2024-11-06T02:58:00Z- handleDepth()- Depth_Keller msg: 1730861880.32 epoch sec; seqNo:1290; depth 50.01 2024-11-06T02:58:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861880.33 epoch sec; seqNo:1290; 4061663.80 north; 594229.48 east; 36.70 lat; -121.95 lon; 50.01 depth; utmZone: 10 2024-11-06T02:58:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861878.71 2024-11-06T02:58:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:00Z- waiting for 1730861882.71, time is 1730861880.33 2024-11-06T02:58:00Z- AHRS_M2 msg: 1730861880.72 epoch sec; seqNo:1211 2024-11-06T02:58:00Z- AHRS_M2 msg: 0.31 phi; -0.01 theta; 5.44 psi 2024-11-06T02:58:00Z- handleDepth()- Depth_Keller msg: 1730861880.72 epoch sec; seqNo:1291; depth 49.99 2024-11-06T02:58:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861880.73 epoch sec; seqNo:1291; 4061664.17 north; 594228.26 east; 36.70 lat; -121.95 lon; 49.99 depth; utmZone: 10 2024-11-06T02:58:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861878.71 2024-11-06T02:58:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:00Z- waiting for 1730861882.71, time is 1730861880.73 2024-11-06T02:58:01Z- AHRS_M2 msg: 1730861881.12 epoch sec; seqNo:1212 2024-11-06T02:58:01Z- AHRS_M2 msg: 0.35 phi; -0.02 theta; 5.47 psi 2024-11-06T02:58:01Z- handleDepth()- Depth_Keller msg: 1730861881.13 epoch sec; seqNo:1292; depth 49.99 2024-11-06T02:58:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861881.14 epoch sec; seqNo:1292; 4061664.17 north; 594228.26 east; 36.70 lat; -121.95 lon; 49.99 depth; utmZone: 10 2024-11-06T02:58:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861878.71 2024-11-06T02:58:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:01Z- waiting for 1730861882.71, time is 1730861881.14 2024-11-06T02:58:01Z- AHRS_M2 msg: 1730861881.52 epoch sec; seqNo:1213 2024-11-06T02:58:01Z- AHRS_M2 msg: 0.38 phi; -0.03 theta; 5.50 psi 2024-11-06T02:58:01Z- handleDepth()- Depth_Keller msg: 1730861881.53 epoch sec; seqNo:1293; depth 49.96 2024-11-06T02:58:01Z- handleDVL() - DVL msg epochMillisec: 1730861881.57 2024-11-06T02:58:01Z- handleDvl() - RDI_Pathfinder msg: 1730861881.57 sec; seqNo:404; #beams:4 2024-11-06T02:58:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 55.05 , 39.96 , 46.89 , 45.88 2024-11-06T02:58:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.06 2024-11-06T02:58:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861881.58 epoch sec; seqNo:1293; 4061664.55 north; 594228.25 east; 36.70 lat; -121.95 lon; 49.96 depth; utmZone: 10 2024-11-06T02:58:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861878.71 2024-11-06T02:58:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:01Z- waiting for 1730861882.71, time is 1730861881.58 2024-11-06T02:58:01Z- AHRS_M2 msg: 1730861881.93 epoch sec; seqNo:1214 2024-11-06T02:58:01Z- AHRS_M2 msg: 0.41 phi; -0.04 theta; 5.54 psi 2024-11-06T02:58:01Z- handleDepth()- Depth_Keller msg: 1730861881.94 epoch sec; seqNo:1294; depth 49.94 2024-11-06T02:58:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861881.95 epoch sec; seqNo:1294; 4061664.93 north; 594228.25 east; 36.70 lat; -121.95 lon; 49.94 depth; utmZone: 10 2024-11-06T02:58:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861878.71 2024-11-06T02:58:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:02Z- waiting for 1730861882.71, time is 1730861881.95 2024-11-06T02:58:02Z- AHRS_M2 msg: 1730861882.33 epoch sec; seqNo:1215 2024-11-06T02:58:02Z- AHRS_M2 msg: 0.43 phi; -0.05 theta; 5.57 psi 2024-11-06T02:58:02Z- handleDepth()- Depth_Keller msg: 1730861882.34 epoch sec; seqNo:1295; depth 49.95 2024-11-06T02:58:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861882.35 epoch sec; seqNo:1295; 4061664.93 north; 594228.25 east; 36.70 lat; -121.95 lon; 49.95 depth; utmZone: 10 2024-11-06T02:58:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861878.71 2024-11-06T02:58:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:02Z- waiting for 1730861882.71, time is 1730861882.35 2024-11-06T02:58:02Z- AHRS_M2 msg: 1730861882.73 epoch sec; seqNo:1216 2024-11-06T02:58:02Z- AHRS_M2 msg: 0.44 phi; -0.06 theta; 5.61 psi 2024-11-06T02:58:02Z- handleDepth()- Depth_Keller msg: 1730861882.74 epoch sec; seqNo:1296; depth 49.93 2024-11-06T02:58:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861882.76 epoch sec; seqNo:1296; 4061665.29 north; 594227.03 east; 36.70 lat; -121.95 lon; 49.93 depth; utmZone: 10 2024-11-06T02:58:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861878.71 2024-11-06T02:58:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:02Z- waiting for 1730861882.71, time is 1730861882.76 2024-11-06T02:58:02Z- LcmTrn::updateTrn() >>>> pose time 1730861882.76, meas time 1730861881.57 <<<< 2024-11-06T02:58:02Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861882.76 2024-11-06T02:58:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861881.57 sec, ping # 0; waiting for INS data more recent than 1730861878.71... 2024-11-06T02:58:02Z- TerrainNav::motionUpdate() - currEstimate = (4061665, 594227.03, 49.93) 2024-11-06T02:58:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:58:02Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:58:02Z- TNavPF::Projecting Measurements 2024-11-06T02:58:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:58:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:58:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:58:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:58:02Z- TNavFilter::4 good beams. 2024-11-06T02:58:02Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:58:02Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:58:02Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:58:02Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:58:02Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:58:02Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:58:02Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:58:02Z- TerrainMapOctree: (vehN, vehE ) = (4061664.64, 594228.11). 2024-11-06T02:58:02Z- TerrainMapOctree: Min Distance = 88.13. 2024-11-06T02:58:02Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:58:03Z- Forcing Subcloud Comparison 2024-11-06T02:58:03Z- Weighting particles with subcloud comparison 2024-11-06T02:58:03Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:03Z- meanExpectedMeasDiff: -7.605822 delta_rms_squared: 4.340863 alpha: 0.995414 2024-11-06T02:58:03Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:03Z- meanExpectedMeasDiff: 1.530454 delta_rms_squared: 1.016841 alpha: 0.980711 2024-11-06T02:58:03Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:03Z- meanExpectedMeasDiff: -1.304683 delta_rms_squared: 1.717331 alpha: 0.988488 2024-11-06T02:58:03Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:03Z- meanExpectedMeasDiff: -2.132870 delta_rms_squared: 1.592331 alpha: 0.987596 2024-11-06T02:58:03Z- Subcloud NIS: 0.198930 2024-11-06T02:58:03Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:58:03Z- TNavPF:: sumWeights = 0.017832 2024-11-06T02:58:03Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:58:03Z- TNavPF::First Term of Map Covariance Matrix is 0.49 2024-11-06T02:58:03Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T02:58:03Z- TerrainNav::Resampling particle filter... 2024-11-06T02:58:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861881.57 sec. 2024-11-06T02:58:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:58:03Z- variances([0], [2], and psi_berg) = 0.332500 0.236284 0.000000 2024-11-06T02:58:03Z- publishEstimatesDecoder() - at 1730861882.76 sec 2024-11-06T02:58:03Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:58:03Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061696.96 594211.81 56.81 2024-11-06T02:58:03Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061697.69 594211.02 57.78 lat/lon: 36.70 -121.95 2024-11-06T02:58:03Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.24 0.66 0.00 2024-11-06T02:58:03Z- AHRS_M2 msg: 1730861883.14 epoch sec; seqNo:1217 2024-11-06T02:58:03Z- AHRS_M2 msg: 0.45 phi; -0.07 theta; 5.65 psi 2024-11-06T02:58:03Z- handleDepth()- Depth_Keller msg: 1730861883.14 epoch sec; seqNo:1297; depth 49.94 2024-11-06T02:58:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861883.16 epoch sec; seqNo:1297; 4061665.67 north; 594227.02 east; 36.70 lat; -121.95 lon; 49.94 depth; utmZone: 10 2024-11-06T02:58:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861882.76 2024-11-06T02:58:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:03Z- waiting for 1730861886.76, time is 1730861883.16 2024-11-06T02:58:03Z- AHRS_M2 msg: 1730861883.57 epoch sec; seqNo:1218 2024-11-06T02:58:03Z- AHRS_M2 msg: 0.45 phi; -0.08 theta; 5.68 psi 2024-11-06T02:58:03Z- handleDepth()- Depth_Keller msg: 1730861883.58 epoch sec; seqNo:1298; depth 49.93 2024-11-06T02:58:03Z- handleDVL() - DVL msg epochMillisec: 1730861883.61 2024-11-06T02:58:03Z- handleDvl() - RDI_Pathfinder msg: 1730861883.61 sec; seqNo:405; #beams:4 2024-11-06T02:58:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.60 , 37.99 , 48.15 , 49.92 2024-11-06T02:58:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , -0.05 2024-11-06T02:58:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861883.62 epoch sec; seqNo:1298; 4061666.05 north; 594227.02 east; 36.70 lat; -121.95 lon; 49.93 depth; utmZone: 10 2024-11-06T02:58:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861882.76 2024-11-06T02:58:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:03Z- waiting for 1730861886.76, time is 1730861883.62 2024-11-06T02:58:03Z- AHRS_M2 msg: 1730861883.97 epoch sec; seqNo:1219 2024-11-06T02:58:03Z- AHRS_M2 msg: 0.44 phi; -0.09 theta; 5.72 psi 2024-11-06T02:58:03Z- handleDepth()- Depth_Keller msg: 1730861883.97 epoch sec; seqNo:1299; depth 49.94 2024-11-06T02:58:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861883.99 epoch sec; seqNo:1299; 4061666.43 north; 594227.01 east; 36.70 lat; -121.95 lon; 49.94 depth; utmZone: 10 2024-11-06T02:58:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861882.76 2024-11-06T02:58:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:04Z- waiting for 1730861886.76, time is 1730861883.99 2024-11-06T02:58:04Z- AHRS_M2 msg: 1730861884.38 epoch sec; seqNo:1220 2024-11-06T02:58:04Z- AHRS_M2 msg: 0.43 phi; -0.09 theta; 5.75 psi 2024-11-06T02:58:04Z- handleDepth()- Depth_Keller msg: 1730861884.38 epoch sec; seqNo:1300; depth 49.95 2024-11-06T02:58:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861884.39 epoch sec; seqNo:1300; 4061666.43 north; 594227.01 east; 36.70 lat; -121.95 lon; 49.95 depth; utmZone: 10 2024-11-06T02:58:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861882.76 2024-11-06T02:58:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:04Z- waiting for 1730861886.76, time is 1730861884.39 2024-11-06T02:58:04Z- AHRS_M2 msg: 1730861884.78 epoch sec; seqNo:1221 2024-11-06T02:58:04Z- AHRS_M2 msg: 0.42 phi; -0.10 theta; 5.79 psi 2024-11-06T02:58:04Z- handleDepth()- Depth_Keller msg: 1730861884.79 epoch sec; seqNo:1301; depth 49.95 2024-11-06T02:58:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861884.79 epoch sec; seqNo:1301; 4061666.81 north; 594227.01 east; 36.70 lat; -121.95 lon; 49.95 depth; utmZone: 10 2024-11-06T02:58:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861882.76 2024-11-06T02:58:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:04Z- waiting for 1730861886.76, time is 1730861884.79 2024-11-06T02:58:05Z- AHRS_M2 msg: 1730861885.18 epoch sec; seqNo:1222 2024-11-06T02:58:05Z- AHRS_M2 msg: 0.42 phi; -0.10 theta; 5.82 psi 2024-11-06T02:58:05Z- handleDepth()- Depth_Keller msg: 1730861885.18 epoch sec; seqNo:1302; depth 49.99 2024-11-06T02:58:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861885.19 epoch sec; seqNo:1302; 4061667.17 north; 594225.78 east; 36.70 lat; -121.95 lon; 49.99 depth; utmZone: 10 2024-11-06T02:58:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861882.76 2024-11-06T02:58:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:05Z- waiting for 1730861886.76, time is 1730861885.19 2024-11-06T02:58:05Z- AHRS_M2 msg: 1730861885.58 epoch sec; seqNo:1223 2024-11-06T02:58:05Z- AHRS_M2 msg: 0.41 phi; -0.10 theta; 5.86 psi 2024-11-06T02:58:05Z- handleDepth()- Depth_Keller msg: 1730861885.59 epoch sec; seqNo:1303; depth 50.00 2024-11-06T02:58:05Z- handleDVL() - DVL msg epochMillisec: 1730861885.61 2024-11-06T02:58:05Z- handleDvl() - RDI_Pathfinder msg: 1730861885.61 sec; seqNo:406; #beams:4 2024-11-06T02:58:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.41 , 38.06 , 46.86 , 51.14 2024-11-06T02:58:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , -0.02 2024-11-06T02:58:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861885.67 epoch sec; seqNo:1303; 4061667.55 north; 594225.78 east; 36.70 lat; -121.95 lon; 50.00 depth; utmZone: 10 2024-11-06T02:58:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861882.76 2024-11-06T02:58:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:05Z- waiting for 1730861886.76, time is 1730861885.67 2024-11-06T02:58:05Z- AHRS_M2 msg: 1730861885.99 epoch sec; seqNo:1224 2024-11-06T02:58:05Z- AHRS_M2 msg: 0.40 phi; -0.11 theta; 5.89 psi 2024-11-06T02:58:05Z- handleDepth()- Depth_Keller msg: 1730861885.99 epoch sec; seqNo:1304; depth 50.03 2024-11-06T02:58:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861886.01 epoch sec; seqNo:1304; 4061667.93 north; 594225.78 east; 36.70 lat; -121.95 lon; 50.03 depth; utmZone: 10 2024-11-06T02:58:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861882.76 2024-11-06T02:58:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:06Z- waiting for 1730861886.76, time is 1730861886.01 2024-11-06T02:58:06Z- AHRS_M2 msg: 1730861886.39 epoch sec; seqNo:1225 2024-11-06T02:58:06Z- AHRS_M2 msg: 0.40 phi; -0.11 theta; 5.92 psi 2024-11-06T02:58:06Z- handleDepth()- Depth_Keller msg: 1730861886.39 epoch sec; seqNo:1305; depth 50.06 2024-11-06T02:58:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861886.40 epoch sec; seqNo:1305; 4061667.93 north; 594225.78 east; 36.70 lat; -121.95 lon; 50.06 depth; utmZone: 10 2024-11-06T02:58:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861882.76 2024-11-06T02:58:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:06Z- waiting for 1730861886.76, time is 1730861886.40 2024-11-06T02:58:06Z- AHRS_M2 msg: 1730861886.79 epoch sec; seqNo:1226 2024-11-06T02:58:06Z- AHRS_M2 msg: 0.40 phi; -0.11 theta; 5.96 psi 2024-11-06T02:58:06Z- handleDepth()- Depth_Keller msg: 1730861886.80 epoch sec; seqNo:1306; depth 50.09 2024-11-06T02:58:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861886.81 epoch sec; seqNo:1306; 4061668.31 north; 594225.77 east; 36.70 lat; -121.95 lon; 50.09 depth; utmZone: 10 2024-11-06T02:58:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861882.76 2024-11-06T02:58:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:06Z- waiting for 1730861886.76, time is 1730861886.81 2024-11-06T02:58:06Z- LcmTrn::updateTrn() >>>> pose time 1730861886.81, meas time 1730861885.61 <<<< 2024-11-06T02:58:06Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861886.81 2024-11-06T02:58:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861885.61 sec, ping # 0; waiting for INS data more recent than 1730861882.76... 2024-11-06T02:58:06Z- TerrainNav::motionUpdate() - currEstimate = (4061668, 594225.77, 50.09) 2024-11-06T02:58:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:58:06Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:58:06Z- TNavPF::Projecting Measurements 2024-11-06T02:58:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:58:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:58:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:58:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:58:06Z- TNavFilter::4 good beams. 2024-11-06T02:58:06Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:58:06Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:58:06Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:58:06Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:58:06Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:58:06Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:58:06Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:58:06Z- TerrainMapOctree: (vehN, vehE ) = (4061667.42, 594226.14). 2024-11-06T02:58:06Z- TerrainMapOctree: Min Distance = 85.96. 2024-11-06T02:58:06Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:58:07Z- Forcing Subcloud Comparison 2024-11-06T02:58:07Z- Weighting particles with subcloud comparison 2024-11-06T02:58:07Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:07Z- meanExpectedMeasDiff: -0.251463 delta_rms_squared: 2.546159 alpha: 0.992206 2024-11-06T02:58:07Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:07Z- meanExpectedMeasDiff: 0.883995 delta_rms_squared: 0.900763 alpha: 0.978279 2024-11-06T02:58:07Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:07Z- meanExpectedMeasDiff: 1.134232 delta_rms_squared: 2.391018 alpha: 0.991705 2024-11-06T02:58:07Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:07Z- meanExpectedMeasDiff: 0.308688 delta_rms_squared: 1.603991 alpha: 0.987685 2024-11-06T02:58:07Z- Subcloud NIS: 0.190514 2024-11-06T02:58:07Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:58:07Z- TNavPF:: sumWeights = 0.556705 2024-11-06T02:58:07Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:58:07Z- TNavPF::First Term of Map Covariance Matrix is 0.72 2024-11-06T02:58:07Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.03 2024-11-06T02:58:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861885.61 sec. 2024-11-06T02:58:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:58:07Z- variances([0], [2], and psi_berg) = 0.333047 0.241904 0.000000 2024-11-06T02:58:07Z- publishEstimatesDecoder() - at 1730861886.81 sec 2024-11-06T02:58:07Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:58:07Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061699.32 594209.04 58.16 2024-11-06T02:58:07Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061700.69 594209.75 57.70 lat/lon: 36.70 -121.95 2024-11-06T02:58:07Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.24 0.66 0.00 2024-11-06T02:58:07Z- handleDepth()- Depth_Keller msg: 1730861887.26 epoch sec; seqNo:1307; depth 50.12 2024-11-06T02:58:07Z- handleDVL() - DVL msg epochMillisec: 1730861887.34 2024-11-06T02:58:07Z- handleDvl() - RDI_Pathfinder msg: 1730861887.34 sec; seqNo:407; #beams:4 2024-11-06T02:58:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.99 , 38.30 , 45.67 , 51.14 2024-11-06T02:58:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.10 , 0.00 2024-11-06T02:58:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861887.37 epoch sec; seqNo:1307; 4061668.69 north; 594225.77 east; 36.70 lat; -121.95 lon; 50.12 depth; utmZone: 10 2024-11-06T02:58:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861886.81 2024-11-06T02:58:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:07Z- waiting for 1730861890.81, time is 1730861887.37 2024-11-06T02:58:07Z- AHRS_M2 msg: 1730861887.76 epoch sec; seqNo:1227 2024-11-06T02:58:07Z- AHRS_M2 msg: 0.40 phi; -0.11 theta; 6.06 psi 2024-11-06T02:58:07Z- handleDepth()- Depth_Keller msg: 1730861887.77 epoch sec; seqNo:1308; depth 50.16 2024-11-06T02:58:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861887.78 epoch sec; seqNo:1308; 4061669.07 north; 594225.76 east; 36.70 lat; -121.95 lon; 50.16 depth; utmZone: 10 2024-11-06T02:58:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861886.81 2024-11-06T02:58:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:07Z- waiting for 1730861890.81, time is 1730861887.78 2024-11-06T02:58:08Z- AHRS_M2 msg: 1730861888.16 epoch sec; seqNo:1228 2024-11-06T02:58:08Z- AHRS_M2 msg: 0.40 phi; -0.11 theta; 6.09 psi 2024-11-06T02:58:08Z- handleDepth()- Depth_Keller msg: 1730861888.16 epoch sec; seqNo:1309; depth 50.22 2024-11-06T02:58:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861888.18 epoch sec; seqNo:1309; 4061669.45 north; 594225.76 east; 36.70 lat; -121.95 lon; 50.22 depth; utmZone: 10 2024-11-06T02:58:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861886.81 2024-11-06T02:58:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:08Z- waiting for 1730861890.81, time is 1730861888.18 2024-11-06T02:58:08Z- AHRS_M2 msg: 1730861888.56 epoch sec; seqNo:1229 2024-11-06T02:58:08Z- AHRS_M2 msg: 0.40 phi; -0.11 theta; 6.13 psi 2024-11-06T02:58:08Z- handleDepth()- Depth_Keller msg: 1730861888.57 epoch sec; seqNo:1310; depth 50.25 2024-11-06T02:58:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861888.57 epoch sec; seqNo:1310; 4061669.82 north; 594225.76 east; 36.70 lat; -121.95 lon; 50.25 depth; utmZone: 10 2024-11-06T02:58:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861886.81 2024-11-06T02:58:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:08Z- waiting for 1730861890.81, time is 1730861888.57 2024-11-06T02:58:08Z- AHRS_M2 msg: 1730861888.97 epoch sec; seqNo:1230 2024-11-06T02:58:08Z- AHRS_M2 msg: 0.40 phi; -0.11 theta; 6.16 psi 2024-11-06T02:58:08Z- handleDepth()- Depth_Keller msg: 1730861888.97 epoch sec; seqNo:1311; depth 50.29 2024-11-06T02:58:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861888.98 epoch sec; seqNo:1311; 4061670.19 north; 594224.53 east; 36.70 lat; -121.95 lon; 50.29 depth; utmZone: 10 2024-11-06T02:58:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861886.81 2024-11-06T02:58:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:09Z- waiting for 1730861890.81, time is 1730861888.98 2024-11-06T02:58:09Z- AHRS_M2 msg: 1730861889.37 epoch sec; seqNo:1231 2024-11-06T02:58:09Z- AHRS_M2 msg: 0.40 phi; -0.11 theta; 6.19 psi 2024-11-06T02:58:09Z- handleDepth()- Depth_Keller msg: 1730861889.38 epoch sec; seqNo:1312; depth 50.32 2024-11-06T02:58:09Z- handleDVL() - DVL msg epochMillisec: 1730861889.40 2024-11-06T02:58:09Z- handleDvl() - RDI_Pathfinder msg: 1730861889.40 sec; seqNo:408; #beams:4 2024-11-06T02:58:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.98 , 38.77 , 45.67 , 50.66 2024-11-06T02:58:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.09 , 0.01 2024-11-06T02:58:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861889.41 epoch sec; seqNo:1312; 4061670.57 north; 594224.53 east; 36.70 lat; -121.95 lon; 50.32 depth; utmZone: 10 2024-11-06T02:58:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861886.81 2024-11-06T02:58:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:09Z- waiting for 1730861890.81, time is 1730861889.41 2024-11-06T02:58:09Z- handleDepth()- Depth_Keller msg: 1730861889.81 epoch sec; seqNo:1313; depth 50.36 2024-11-06T02:58:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861889.83 epoch sec; seqNo:1313; 4061670.57 north; 594224.53 east; 36.70 lat; -121.95 lon; 50.36 depth; utmZone: 10 2024-11-06T02:58:09Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:58:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861886.81 2024-11-06T02:58:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:09Z- waiting for 1730861890.81, time is 1730861889.83 2024-11-06T02:58:10Z- AHRS_M2 msg: 1730861890.19 epoch sec; seqNo:1232 2024-11-06T02:58:10Z- AHRS_M2 msg: 0.39 phi; -0.10 theta; 6.26 psi 2024-11-06T02:58:10Z- handleDepth()- Depth_Keller msg: 1730861890.20 epoch sec; seqNo:1314; depth 50.40 2024-11-06T02:58:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861890.21 epoch sec; seqNo:1314; 4061670.95 north; 594224.52 east; 36.70 lat; -121.95 lon; 50.40 depth; utmZone: 10 2024-11-06T02:58:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861886.81 2024-11-06T02:58:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:10Z- waiting for 1730861890.81, time is 1730861890.21 2024-11-06T02:58:10Z- AHRS_M2 msg: 1730861890.60 epoch sec; seqNo:1233 2024-11-06T02:58:10Z- AHRS_M2 msg: 0.39 phi; -0.10 theta; 0.01 psi 2024-11-06T02:58:10Z- handleDepth()- Depth_Keller msg: 1730861890.60 epoch sec; seqNo:1315; depth 50.43 2024-11-06T02:58:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861890.61 epoch sec; seqNo:1315; 4061671.33 north; 594224.52 east; 36.70 lat; -121.95 lon; 50.43 depth; utmZone: 10 2024-11-06T02:58:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861886.81 2024-11-06T02:58:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:10Z- waiting for 1730861890.81, time is 1730861890.61 2024-11-06T02:58:11Z- AHRS_M2 msg: 1730861891.00 epoch sec; seqNo:1234 2024-11-06T02:58:11Z- AHRS_M2 msg: 0.39 phi; -0.09 theta; 0.04 psi 2024-11-06T02:58:11Z- handleDepth()- Depth_Keller msg: 1730861891.01 epoch sec; seqNo:1316; depth 50.46 2024-11-06T02:58:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861891.02 epoch sec; seqNo:1316; 4061671.71 north; 594224.51 east; 36.70 lat; -121.95 lon; 50.46 depth; utmZone: 10 2024-11-06T02:58:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861886.81 2024-11-06T02:58:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:11Z- waiting for 1730861890.81, time is 1730861891.02 2024-11-06T02:58:11Z- LcmTrn::updateTrn() >>>> pose time 1730861891.02, meas time 1730861889.40 <<<< 2024-11-06T02:58:11Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861891.02 2024-11-06T02:58:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861889.40 sec, ping # 0; waiting for INS data more recent than 1730861886.81... 2024-11-06T02:58:11Z- TerrainNav::motionUpdate() - currEstimate = (4061672, 594224.51, 50.46) 2024-11-06T02:58:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:58:11Z- TerrainNav:: SubcloudNIS is 0.19, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:58:11Z- TNavPF::Projecting Measurements 2024-11-06T02:58:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:58:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:58:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:58:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:58:11Z- TNavFilter::4 good beams. 2024-11-06T02:58:11Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:58:11Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:58:11Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:58:11Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:58:11Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:58:11Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:58:11Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:58:11Z- TerrainMapOctree: (vehN, vehE ) = (4061670.40, 594225.00). 2024-11-06T02:58:11Z- TerrainMapOctree: Min Distance = 83.43. 2024-11-06T02:58:11Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:58:11Z- Forcing Subcloud Comparison 2024-11-06T02:58:11Z- Weighting particles with subcloud comparison 2024-11-06T02:58:11Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:11Z- meanExpectedMeasDiff: 3.078546 delta_rms_squared: 2.480003 alpha: 0.992000 2024-11-06T02:58:11Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:11Z- meanExpectedMeasDiff: 1.544704 delta_rms_squared: 0.920246 alpha: 0.978729 2024-11-06T02:58:11Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:11Z- meanExpectedMeasDiff: 2.143391 delta_rms_squared: 1.261670 alpha: 0.984395 2024-11-06T02:58:11Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:11Z- meanExpectedMeasDiff: 1.418303 delta_rms_squared: 1.617902 alpha: 0.987789 2024-11-06T02:58:11Z- Subcloud NIS: 0.205864 2024-11-06T02:58:11Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:58:11Z- TNavPF:: sumWeights = 0.331647 2024-11-06T02:58:11Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:58:11Z- TNavPF::First Term of Map Covariance Matrix is 0.86 2024-11-06T02:58:11Z- TNavPF::Calculated NIS Value : 0.09 numBeams normalized NIS: 0.02 2024-11-06T02:58:11Z- TerrainNav::Resampling particle filter... 2024-11-06T02:58:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861889.40 sec. 2024-11-06T02:58:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:58:11Z- variances([0], [2], and psi_berg) = 0.323483 0.235037 0.000000 2024-11-06T02:58:11Z- publishEstimatesDecoder() - at 1730861891.02 sec 2024-11-06T02:58:11Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:58:11Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061703.30 594209.20 57.13 2024-11-06T02:58:11Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061704.07 594208.53 57.29 lat/lon: 36.70 -121.95 2024-11-06T02:58:11Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.32 0.24 0.68 0.00 2024-11-06T02:58:11Z- handleDepth()- Depth_Keller msg: 1730861891.45 epoch sec; seqNo:1317; depth 50.50 2024-11-06T02:58:11Z- handleDVL() - DVL msg epochMillisec: 1730861891.47 2024-11-06T02:58:11Z- handleDvl() - RDI_Pathfinder msg: 1730861891.47 sec; seqNo:409; #beams:4 2024-11-06T02:58:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.75 , 39.01 , 45.67 , 50.19 2024-11-06T02:58:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.04 , 0.00 2024-11-06T02:58:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861891.49 epoch sec; seqNo:1317; 4061672.08 north; 594224.51 east; 36.70 lat; -121.95 lon; 50.50 depth; utmZone: 10 2024-11-06T02:58:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861891.02 2024-11-06T02:58:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:11Z- waiting for 1730861895.02, time is 1730861891.49 2024-11-06T02:58:11Z- AHRS_M2 msg: 1730861891.81 epoch sec; seqNo:1235 2024-11-06T02:58:11Z- AHRS_M2 msg: 0.39 phi; -0.08 theta; 0.11 psi 2024-11-06T02:58:11Z- handleDepth()- Depth_Keller msg: 1730861891.82 epoch sec; seqNo:1318; depth 50.54 2024-11-06T02:58:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861891.83 epoch sec; seqNo:1318; 4061672.08 north; 594224.51 east; 36.70 lat; -121.95 lon; 50.54 depth; utmZone: 10 2024-11-06T02:58:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861891.02 2024-11-06T02:58:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:11Z- waiting for 1730861895.02, time is 1730861891.83 2024-11-06T02:58:12Z- AHRS_M2 msg: 1730861892.21 epoch sec; seqNo:1236 2024-11-06T02:58:12Z- AHRS_M2 msg: 0.39 phi; -0.08 theta; 0.15 psi 2024-11-06T02:58:12Z- handleDepth()- Depth_Keller msg: 1730861892.21 epoch sec; seqNo:1319; depth 50.57 2024-11-06T02:58:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861892.22 epoch sec; seqNo:1319; 4061672.46 north; 594224.51 east; 36.70 lat; -121.95 lon; 50.57 depth; utmZone: 10 2024-11-06T02:58:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861891.02 2024-11-06T02:58:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:12Z- waiting for 1730861895.02, time is 1730861892.22 2024-11-06T02:58:12Z- AHRS_M2 msg: 1730861892.62 epoch sec; seqNo:1237 2024-11-06T02:58:12Z- AHRS_M2 msg: 0.39 phi; -0.07 theta; 0.18 psi 2024-11-06T02:58:12Z- handleDepth()- Depth_Keller msg: 1730861892.62 epoch sec; seqNo:1320; depth 50.59 2024-11-06T02:58:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861892.64 epoch sec; seqNo:1320; 4061672.84 north; 594224.50 east; 36.70 lat; -121.95 lon; 50.59 depth; utmZone: 10 2024-11-06T02:58:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861891.02 2024-11-06T02:58:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:12Z- waiting for 1730861895.02, time is 1730861892.64 2024-11-06T02:58:13Z- AHRS_M2 msg: 1730861893.04 epoch sec; seqNo:1238 2024-11-06T02:58:13Z- AHRS_M2 msg: 0.39 phi; -0.07 theta; 0.21 psi 2024-11-06T02:58:13Z- handleDepth()- Depth_Keller msg: 1730861893.04 epoch sec; seqNo:1321; depth 50.62 2024-11-06T02:58:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861893.05 epoch sec; seqNo:1321; 4061673.22 north; 594224.50 east; 36.70 lat; -121.95 lon; 50.62 depth; utmZone: 10 2024-11-06T02:58:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861891.02 2024-11-06T02:58:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:13Z- waiting for 1730861895.02, time is 1730861893.05 2024-11-06T02:58:13Z- handleDepth()- Depth_Keller msg: 1730861893.44 epoch sec; seqNo:1322; depth 50.64 2024-11-06T02:58:13Z- handleDVL() - DVL msg epochMillisec: 1730861893.46 2024-11-06T02:58:13Z- handleDvl() - RDI_Pathfinder msg: 1730861893.46 sec; seqNo:410; #beams:4 2024-11-06T02:58:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.80 , 38.77 , 45.91 , 49.00 2024-11-06T02:58:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.01 , -0.04 2024-11-06T02:58:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861893.47 epoch sec; seqNo:1322; 4061673.60 north; 594224.49 east; 36.70 lat; -121.95 lon; 50.64 depth; utmZone: 10 2024-11-06T02:58:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861891.02 2024-11-06T02:58:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:13Z- waiting for 1730861895.02, time is 1730861893.47 2024-11-06T02:58:13Z- AHRS_M2 msg: 1730861893.83 epoch sec; seqNo:1239 2024-11-06T02:58:13Z- AHRS_M2 msg: 0.38 phi; -0.06 theta; 0.28 psi 2024-11-06T02:58:13Z- handleDepth()- Depth_Keller msg: 1730861893.83 epoch sec; seqNo:1323; depth 50.67 2024-11-06T02:58:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861893.86 epoch sec; seqNo:1323; 4061673.60 north; 594224.49 east; 36.70 lat; -121.95 lon; 50.67 depth; utmZone: 10 2024-11-06T02:58:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861891.02 2024-11-06T02:58:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:13Z- waiting for 1730861895.02, time is 1730861893.86 2024-11-06T02:58:14Z- AHRS_M2 msg: 1730861894.23 epoch sec; seqNo:1240 2024-11-06T02:58:14Z- AHRS_M2 msg: 0.38 phi; -0.05 theta; 0.31 psi 2024-11-06T02:58:14Z- handleDepth()- Depth_Keller msg: 1730861894.24 epoch sec; seqNo:1324; depth 50.68 2024-11-06T02:58:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861894.24 epoch sec; seqNo:1324; 4061673.99 north; 594225.71 east; 36.70 lat; -121.95 lon; 50.68 depth; utmZone: 10 2024-11-06T02:58:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861891.02 2024-11-06T02:58:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:14Z- waiting for 1730861895.02, time is 1730861894.24 2024-11-06T02:58:14Z- AHRS_M2 msg: 1730861894.64 epoch sec; seqNo:1241 2024-11-06T02:58:14Z- AHRS_M2 msg: 0.39 phi; -0.05 theta; 0.34 psi 2024-11-06T02:58:14Z- handleDepth()- Depth_Keller msg: 1730861894.65 epoch sec; seqNo:1325; depth 50.70 2024-11-06T02:58:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861894.66 epoch sec; seqNo:1325; 4061674.37 north; 594225.71 east; 36.70 lat; -121.95 lon; 50.70 depth; utmZone: 10 2024-11-06T02:58:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861891.02 2024-11-06T02:58:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:14Z- waiting for 1730861895.02, time is 1730861894.66 2024-11-06T02:58:15Z- AHRS_M2 msg: 1730861895.04 epoch sec; seqNo:1242 2024-11-06T02:58:15Z- AHRS_M2 msg: 0.39 phi; -0.04 theta; 0.37 psi 2024-11-06T02:58:15Z- handleDepth()- Depth_Keller msg: 1730861895.04 epoch sec; seqNo:1326; depth 50.72 2024-11-06T02:58:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861895.06 epoch sec; seqNo:1326; 4061674.75 north; 594225.70 east; 36.70 lat; -121.95 lon; 50.72 depth; utmZone: 10 2024-11-06T02:58:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861891.02 2024-11-06T02:58:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:15Z- waiting for 1730861895.02, time is 1730861895.06 2024-11-06T02:58:15Z- LcmTrn::updateTrn() >>>> pose time 1730861895.06, meas time 1730861893.46 <<<< 2024-11-06T02:58:15Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861895.06 2024-11-06T02:58:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861893.46 sec, ping # 0; waiting for INS data more recent than 1730861891.02... 2024-11-06T02:58:15Z- TerrainNav::motionUpdate() - currEstimate = (4061675, 594225.70, 50.72) 2024-11-06T02:58:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:58:15Z- TerrainNav:: SubcloudNIS is 0.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:58:15Z- TNavPF::Projecting Measurements 2024-11-06T02:58:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:58:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:58:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:58:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:58:15Z- TNavFilter::4 good beams. 2024-11-06T02:58:15Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:58:15Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:58:15Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:58:15Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:58:15Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:58:15Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:58:15Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:58:15Z- TerrainMapOctree: (vehN, vehE ) = (4061673.54, 594225.23). 2024-11-06T02:58:15Z- TerrainMapOctree: Min Distance = 80.37. 2024-11-06T02:58:15Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:58:15Z- Forcing Subcloud Comparison 2024-11-06T02:58:15Z- Weighting particles with subcloud comparison 2024-11-06T02:58:15Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:15Z- meanExpectedMeasDiff: -1.550930 delta_rms_squared: 2.535085 alpha: 0.992172 2024-11-06T02:58:15Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:15Z- meanExpectedMeasDiff: 1.125239 delta_rms_squared: 0.957224 alpha: 0.979534 2024-11-06T02:58:15Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:15Z- meanExpectedMeasDiff: 1.055340 delta_rms_squared: 1.362929 alpha: 0.985538 2024-11-06T02:58:15Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:15Z- meanExpectedMeasDiff: -0.743315 delta_rms_squared: 1.591557 alpha: 0.987590 2024-11-06T02:58:15Z- Subcloud NIS: 0.190299 2024-11-06T02:58:15Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:58:15Z- TNavPF:: sumWeights = 0.438249 2024-11-06T02:58:15Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:58:15Z- TNavPF::First Term of Map Covariance Matrix is 0.88 2024-11-06T02:58:15Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.06 2024-11-06T02:58:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861893.46 sec. 2024-11-06T02:58:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:58:15Z- variances([0], [2], and psi_berg) = 0.325230 0.237342 0.000000 2024-11-06T02:58:15Z- publishEstimatesDecoder() - at 1730861895.06 sec 2024-11-06T02:58:15Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:58:15Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061707.19 594210.34 57.58 2024-11-06T02:58:15Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061707.11 594209.72 57.52 lat/lon: 36.70 -121.95 2024-11-06T02:58:15Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.24 0.71 0.00 2024-11-06T02:58:15Z- handleDepth()- Depth_Keller msg: 1730861895.47 epoch sec; seqNo:1327; depth 50.74 2024-11-06T02:58:15Z- handleDVL() - DVL msg epochMillisec: 1730861895.49 2024-11-06T02:58:15Z- handleDvl() - RDI_Pathfinder msg: 1730861895.49 sec; seqNo:411; #beams:4 2024-11-06T02:58:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.56 , 38.77 , 46.38 , 48.29 2024-11-06T02:58:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.04 , -0.06 2024-11-06T02:58:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861895.51 epoch sec; seqNo:1327; 4061674.75 north; 594225.70 east; 36.70 lat; -121.95 lon; 50.74 depth; utmZone: 10 2024-11-06T02:58:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861895.06 2024-11-06T02:58:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:15Z- waiting for 1730861899.06, time is 1730861895.51 2024-11-06T02:58:15Z- AHRS_M2 msg: 1730861895.85 epoch sec; seqNo:1243 2024-11-06T02:58:15Z- AHRS_M2 msg: 0.39 phi; -0.03 theta; 0.43 psi 2024-11-06T02:58:15Z- handleDepth()- Depth_Keller msg: 1730861895.86 epoch sec; seqNo:1328; depth 50.73 2024-11-06T02:58:15Z- handleDVL() - DVL msg epochMillisec: 1730861895.86 2024-11-06T02:58:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:58:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:58:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:58:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:58:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:58:15Z- handleDvl() - RDI_Pathfinder msg: 1730861895.86 sec; seqNo:412; #beams:0 2024-11-06T02:58:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:58:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.04 , -0.06 2024-11-06T02:58:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861895.88 epoch sec; seqNo:1328; 4061675.13 north; 594225.70 east; 36.70 lat; -121.95 lon; 50.73 depth; utmZone: 10 2024-11-06T02:58:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861895.06 2024-11-06T02:58:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:16Z- waiting for 1730861899.06, time is 1730861895.88 2024-11-06T02:58:16Z- AHRS_M2 msg: 1730861896.25 epoch sec; seqNo:1244 2024-11-06T02:58:16Z- AHRS_M2 msg: 0.39 phi; -0.03 theta; 0.46 psi 2024-11-06T02:58:16Z- handleDepth()- Depth_Keller msg: 1730861896.26 epoch sec; seqNo:1329; depth 50.77 2024-11-06T02:58:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861896.27 epoch sec; seqNo:1329; 4061675.51 north; 594225.69 east; 36.70 lat; -121.95 lon; 50.77 depth; utmZone: 10 2024-11-06T02:58:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861895.06 2024-11-06T02:58:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:16Z- waiting for 1730861899.06, time is 1730861896.27 2024-11-06T02:58:16Z- AHRS_M2 msg: 1730861896.67 epoch sec; seqNo:1245 2024-11-06T02:58:16Z- AHRS_M2 msg: 0.39 phi; -0.03 theta; 0.49 psi 2024-11-06T02:58:16Z- handleDepth()- Depth_Keller msg: 1730861896.72 epoch sec; seqNo:1330; depth 50.77 2024-11-06T02:58:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861896.76 epoch sec; seqNo:1330; 4061675.89 north; 594225.69 east; 36.70 lat; -121.95 lon; 50.77 depth; utmZone: 10 2024-11-06T02:58:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861895.06 2024-11-06T02:58:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:16Z- waiting for 1730861899.06, time is 1730861896.76 2024-11-06T02:58:17Z- AHRS_M2 msg: 1730861897.06 epoch sec; seqNo:1246 2024-11-06T02:58:17Z- AHRS_M2 msg: 0.39 phi; -0.03 theta; 0.52 psi 2024-11-06T02:58:17Z- handleDepth()- Depth_Keller msg: 1730861897.06 epoch sec; seqNo:1331; depth 50.79 2024-11-06T02:58:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861897.07 epoch sec; seqNo:1331; 4061675.89 north; 594225.69 east; 36.70 lat; -121.95 lon; 50.79 depth; utmZone: 10 2024-11-06T02:58:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861895.06 2024-11-06T02:58:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:17Z- waiting for 1730861899.06, time is 1730861897.07 2024-11-06T02:58:17Z- handleDepth()- Depth_Keller msg: 1730861897.49 epoch sec; seqNo:1332; depth 50.79 2024-11-06T02:58:17Z- handleDVL() - DVL msg epochMillisec: 1730861897.51 2024-11-06T02:58:17Z- handleDvl() - RDI_Pathfinder msg: 1730861897.51 sec; seqNo:413; #beams:4 2024-11-06T02:58:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.08 , 38.53 , 46.86 , 48.05 2024-11-06T02:58:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.07 , -0.06 2024-11-06T02:58:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861897.54 epoch sec; seqNo:1332; 4061676.27 north; 594225.68 east; 36.70 lat; -121.95 lon; 50.79 depth; utmZone: 10 2024-11-06T02:58:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861895.06 2024-11-06T02:58:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:17Z- waiting for 1730861899.06, time is 1730861897.54 2024-11-06T02:58:17Z- AHRS_M2 msg: 1730861897.87 epoch sec; seqNo:1247 2024-11-06T02:58:17Z- AHRS_M2 msg: 0.38 phi; -0.02 theta; 0.58 psi 2024-11-06T02:58:17Z- handleDepth()- Depth_Keller msg: 1730861897.87 epoch sec; seqNo:1333; depth 50.81 2024-11-06T02:58:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861897.90 epoch sec; seqNo:1333; 4061676.66 north; 594226.90 east; 36.70 lat; -121.95 lon; 50.81 depth; utmZone: 10 2024-11-06T02:58:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861895.06 2024-11-06T02:58:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:18Z- waiting for 1730861899.06, time is 1730861897.90 2024-11-06T02:58:18Z- AHRS_M2 msg: 1730861898.27 epoch sec; seqNo:1248 2024-11-06T02:58:18Z- AHRS_M2 msg: 0.38 phi; -0.02 theta; 0.61 psi 2024-11-06T02:58:18Z- handleDepth()- Depth_Keller msg: 1730861898.28 epoch sec; seqNo:1334; depth 50.81 2024-11-06T02:58:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861898.29 epoch sec; seqNo:1334; 4061676.66 north; 594226.90 east; 36.70 lat; -121.95 lon; 50.81 depth; utmZone: 10 2024-11-06T02:58:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861895.06 2024-11-06T02:58:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:18Z- waiting for 1730861899.06, time is 1730861898.29 2024-11-06T02:58:18Z- AHRS_M2 msg: 1730861898.69 epoch sec; seqNo:1249 2024-11-06T02:58:18Z- AHRS_M2 msg: 0.38 phi; -0.02 theta; 0.64 psi 2024-11-06T02:58:18Z- handleDepth()- Depth_Keller msg: 1730861898.70 epoch sec; seqNo:1335; depth 50.82 2024-11-06T02:58:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861898.74 epoch sec; seqNo:1335; 4061677.04 north; 594226.90 east; 36.70 lat; -121.95 lon; 50.82 depth; utmZone: 10 2024-11-06T02:58:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861895.06 2024-11-06T02:58:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:18Z- waiting for 1730861899.06, time is 1730861898.74 2024-11-06T02:58:19Z- AHRS_M2 msg: 1730861899.08 epoch sec; seqNo:1250 2024-11-06T02:58:19Z- AHRS_M2 msg: 0.38 phi; -0.01 theta; 0.67 psi 2024-11-06T02:58:19Z- handleDepth()- Depth_Keller msg: 1730861899.09 epoch sec; seqNo:1336; depth 50.83 2024-11-06T02:58:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861899.10 epoch sec; seqNo:1336; 4061677.42 north; 594226.89 east; 36.70 lat; -121.95 lon; 50.83 depth; utmZone: 10 2024-11-06T02:58:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861895.06 2024-11-06T02:58:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:19Z- waiting for 1730861899.06, time is 1730861899.10 2024-11-06T02:58:19Z- LcmTrn::updateTrn() >>>> pose time 1730861899.10, meas time 1730861897.51 <<<< 2024-11-06T02:58:19Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861899.10 2024-11-06T02:58:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861897.51 sec, ping # 0; waiting for INS data more recent than 1730861895.06... 2024-11-06T02:58:19Z- TerrainNav::motionUpdate() - currEstimate = (4061677, 594226.89, 50.83) 2024-11-06T02:58:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:58:19Z- TerrainNav:: SubcloudNIS is 0.19, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:58:19Z- TNavPF::Projecting Measurements 2024-11-06T02:58:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:58:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:58:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:58:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:58:19Z- TNavFilter::4 good beams. 2024-11-06T02:58:19Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:58:19Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:58:19Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:58:19Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:58:19Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:58:19Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:58:19Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:58:19Z- TerrainMapOctree: (vehN, vehE ) = (4061676.36, 594226.42). 2024-11-06T02:58:19Z- TerrainMapOctree: Min Distance = 77.32. 2024-11-06T02:58:19Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:58:19Z- Forcing Subcloud Comparison 2024-11-06T02:58:19Z- Weighting particles with subcloud comparison 2024-11-06T02:58:19Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:19Z- meanExpectedMeasDiff: -1.514126 delta_rms_squared: 2.532749 alpha: 0.992165 2024-11-06T02:58:19Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:19Z- meanExpectedMeasDiff: 1.072497 delta_rms_squared: 0.970029 alpha: 0.979799 2024-11-06T02:58:19Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:19Z- meanExpectedMeasDiff: 1.354190 delta_rms_squared: 1.436035 alpha: 0.986264 2024-11-06T02:58:19Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:19Z- meanExpectedMeasDiff: -0.325553 delta_rms_squared: 2.049680 alpha: 0.990337 2024-11-06T02:58:19Z- Subcloud NIS: 0.194903 2024-11-06T02:58:19Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:58:19Z- TNavPF:: sumWeights = 0.427778 2024-11-06T02:58:19Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:58:19Z- TNavPF::First Term of Map Covariance Matrix is 0.88 2024-11-06T02:58:19Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T02:58:19Z- TerrainNav::Resampling particle filter... 2024-11-06T02:58:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861897.51 sec. 2024-11-06T02:58:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:58:19Z- variances([0], [2], and psi_berg) = 0.322571 0.231722 0.000000 2024-11-06T02:58:19Z- publishEstimatesDecoder() - at 1730861899.10 sec 2024-11-06T02:58:19Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:58:19Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061708.98 594211.53 57.17 2024-11-06T02:58:19Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061709.77 594210.93 57.55 lat/lon: 36.70 -121.95 2024-11-06T02:58:19Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.32 0.23 0.68 0.00 2024-11-06T02:58:19Z- AHRS_M2 msg: 1730861899.49 epoch sec; seqNo:1251 2024-11-06T02:58:19Z- AHRS_M2 msg: 0.38 phi; -0.01 theta; 0.70 psi 2024-11-06T02:58:19Z- handleDepth()- Depth_Keller msg: 1730861899.51 epoch sec; seqNo:1337; depth 50.84 2024-11-06T02:58:19Z- handleDVL() - DVL msg epochMillisec: 1730861899.53 2024-11-06T02:58:19Z- handleDvl() - RDI_Pathfinder msg: 1730861899.53 sec; seqNo:414; #beams:4 2024-11-06T02:58:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.07 , 38.65 , 46.83 , 47.93 2024-11-06T02:58:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.07 , -0.02 2024-11-06T02:58:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861899.55 epoch sec; seqNo:1337; 4061677.42 north; 594226.89 east; 36.70 lat; -121.95 lon; 50.84 depth; utmZone: 10 2024-11-06T02:58:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861899.10 2024-11-06T02:58:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:19Z- waiting for 1730861903.10, time is 1730861899.55 2024-11-06T02:58:19Z- AHRS_M2 msg: 1730861899.89 epoch sec; seqNo:1252 2024-11-06T02:58:19Z- AHRS_M2 msg: 0.38 phi; -0.01 theta; 0.73 psi 2024-11-06T02:58:19Z- handleDepth()- Depth_Keller msg: 1730861899.89 epoch sec; seqNo:1338; depth 50.84 2024-11-06T02:58:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861899.91 epoch sec; seqNo:1338; 4061677.79 north; 594226.89 east; 36.70 lat; -121.95 lon; 50.84 depth; utmZone: 10 2024-11-06T02:58:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861899.10 2024-11-06T02:58:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:20Z- waiting for 1730861903.10, time is 1730861899.91 2024-11-06T02:58:20Z- AHRS_M2 msg: 1730861900.30 epoch sec; seqNo:1253 2024-11-06T02:58:20Z- AHRS_M2 msg: 0.38 phi; -0.01 theta; 0.76 psi 2024-11-06T02:58:20Z- handleDepth()- Depth_Keller msg: 1730861900.30 epoch sec; seqNo:1339; depth 50.85 2024-11-06T02:58:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861900.31 epoch sec; seqNo:1339; 4061677.79 north; 594226.89 east; 36.70 lat; -121.95 lon; 50.85 depth; utmZone: 10 2024-11-06T02:58:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861899.10 2024-11-06T02:58:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:20Z- waiting for 1730861903.10, time is 1730861900.31 2024-11-06T02:58:20Z- AHRS_M2 msg: 1730861900.69 epoch sec; seqNo:1254 2024-11-06T02:58:20Z- AHRS_M2 msg: 0.39 phi; -0.01 theta; 0.79 psi 2024-11-06T02:58:20Z- handleDepth()- Depth_Keller msg: 1730861900.70 epoch sec; seqNo:1340; depth 50.86 2024-11-06T02:58:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861900.71 epoch sec; seqNo:1340; 4061678.19 north; 594228.10 east; 36.70 lat; -121.95 lon; 50.86 depth; utmZone: 10 2024-11-06T02:58:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861899.10 2024-11-06T02:58:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:20Z- waiting for 1730861903.10, time is 1730861900.71 2024-11-06T02:58:21Z- AHRS_M2 msg: 1730861901.10 epoch sec; seqNo:1255 2024-11-06T02:58:21Z- AHRS_M2 msg: 0.39 phi; -0.00 theta; 0.82 psi 2024-11-06T02:58:21Z- handleDepth()- Depth_Keller msg: 1730861901.10 epoch sec; seqNo:1341; depth 50.87 2024-11-06T02:58:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861901.11 epoch sec; seqNo:1341; 4061678.57 north; 594228.10 east; 36.70 lat; -121.95 lon; 50.87 depth; utmZone: 10 2024-11-06T02:58:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861899.10 2024-11-06T02:58:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:21Z- waiting for 1730861903.10, time is 1730861901.11 2024-11-06T02:58:21Z- AHRS_M2 msg: 1730861901.50 epoch sec; seqNo:1256 2024-11-06T02:58:21Z- AHRS_M2 msg: 0.39 phi; -0.00 theta; 0.86 psi 2024-11-06T02:58:21Z- handleDepth()- Depth_Keller msg: 1730861901.51 epoch sec; seqNo:1342; depth 50.88 2024-11-06T02:58:21Z- handleDVL() - DVL msg epochMillisec: 1730861901.54 2024-11-06T02:58:21Z- handleDvl() - RDI_Pathfinder msg: 1730861901.54 sec; seqNo:415; #beams:4 2024-11-06T02:58:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.29 , 37.55 , 47.27 , 47.71 2024-11-06T02:58:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.03 , 0.00 2024-11-06T02:58:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861901.55 epoch sec; seqNo:1342; 4061678.57 north; 594228.10 east; 36.70 lat; -121.95 lon; 50.88 depth; utmZone: 10 2024-11-06T02:58:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861899.10 2024-11-06T02:58:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:21Z- waiting for 1730861903.10, time is 1730861901.55 2024-11-06T02:58:21Z- AHRS_M2 msg: 1730861901.91 epoch sec; seqNo:1257 2024-11-06T02:58:21Z- AHRS_M2 msg: 0.39 phi; 0.00 theta; 0.92 psi 2024-11-06T02:58:21Z- handleDepth()- Depth_Keller msg: 1730861901.92 epoch sec; seqNo:1343; depth 50.87 2024-11-06T02:58:21Z- handleDVL() - DVL msg epochMillisec: 1730861901.92 2024-11-06T02:58:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:58:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:58:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:58:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:58:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:58:21Z- handleDvl() - RDI_Pathfinder msg: 1730861901.92 sec; seqNo:416; #beams:0 2024-11-06T02:58:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:58:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.03 , 0.00 2024-11-06T02:58:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861901.94 epoch sec; seqNo:1343; 4061678.94 north; 594228.10 east; 36.70 lat; -121.95 lon; 50.87 depth; utmZone: 10 2024-11-06T02:58:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861899.10 2024-11-06T02:58:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:22Z- waiting for 1730861903.10, time is 1730861901.94 2024-11-06T02:58:22Z- AHRS_M2 msg: 1730861902.32 epoch sec; seqNo:1258 2024-11-06T02:58:22Z- AHRS_M2 msg: 0.39 phi; 0.00 theta; 0.95 psi 2024-11-06T02:58:22Z- handleDepth()- Depth_Keller msg: 1730861902.32 epoch sec; seqNo:1344; depth 50.88 2024-11-06T02:58:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861902.33 epoch sec; seqNo:1344; 4061678.94 north; 594228.10 east; 36.70 lat; -121.95 lon; 50.88 depth; utmZone: 10 2024-11-06T02:58:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861899.10 2024-11-06T02:58:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:22Z- waiting for 1730861903.10, time is 1730861902.33 2024-11-06T02:58:22Z- AHRS_M2 msg: 1730861902.71 epoch sec; seqNo:1259 2024-11-06T02:58:22Z- AHRS_M2 msg: 0.39 phi; 0.00 theta; 0.99 psi 2024-11-06T02:58:22Z- handleDepth()- Depth_Keller msg: 1730861902.72 epoch sec; seqNo:1345; depth 50.87 2024-11-06T02:58:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861902.73 epoch sec; seqNo:1345; 4061679.32 north; 594228.09 east; 36.70 lat; -121.95 lon; 50.87 depth; utmZone: 10 2024-11-06T02:58:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861899.10 2024-11-06T02:58:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:22Z- waiting for 1730861903.10, time is 1730861902.73 2024-11-06T02:58:23Z- AHRS_M2 msg: 1730861903.12 epoch sec; seqNo:1260 2024-11-06T02:58:23Z- AHRS_M2 msg: 0.39 phi; 0.01 theta; 1.02 psi 2024-11-06T02:58:23Z- handleDepth()- Depth_Keller msg: 1730861903.13 epoch sec; seqNo:1346; depth 50.88 2024-11-06T02:58:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861903.14 epoch sec; seqNo:1346; 4061679.34 north; 594229.31 east; 36.70 lat; -121.95 lon; 50.88 depth; utmZone: 10 2024-11-06T02:58:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861899.10 2024-11-06T02:58:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:23Z- waiting for 1730861903.10, time is 1730861903.14 2024-11-06T02:58:23Z- LcmTrn::updateTrn() >>>> pose time 1730861903.14, meas time 1730861901.92 <<<< 2024-11-06T02:58:23Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861903.14 2024-11-06T02:58:23Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:58:23Z- TerrainNav::motionUpdate() - currEstimate = (4061679, 594229.31, 50.88) 2024-11-06T02:58:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:58:23Z- TerrainNav:: SubcloudNIS is 0.19, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:58:23Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:58:23Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:58:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:58:23Z- variances([0], [2], and psi_berg) = 0.324403 0.233102 0.000000 2024-11-06T02:58:23Z- publishEstimatesDecoder() - at 1730861903.14 sec 2024-11-06T02:58:23Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:58:23Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061710.90 594213.90 57.31 2024-11-06T02:58:23Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061711.69 594213.35 57.59 lat/lon: 36.70 -121.95 2024-11-06T02:58:23Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.32 0.23 0.92 0.00 2024-11-06T02:58:23Z- AHRS_M2 msg: 1730861903.52 epoch sec; seqNo:1261 2024-11-06T02:58:23Z- AHRS_M2 msg: 0.39 phi; 0.01 theta; 1.05 psi 2024-11-06T02:58:23Z- handleDepth()- Depth_Keller msg: 1730861903.53 epoch sec; seqNo:1347; depth 50.88 2024-11-06T02:58:23Z- handleDVL() - DVL msg epochMillisec: 1730861903.56 2024-11-06T02:58:23Z- handleDvl() - RDI_Pathfinder msg: 1730861903.56 sec; seqNo:417; #beams:4 2024-11-06T02:58:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.62 , 38.65 , 47.27 , 47.49 2024-11-06T02:58:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.02 , -0.01 2024-11-06T02:58:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861903.57 epoch sec; seqNo:1347; 4061679.34 north; 594229.31 east; 36.70 lat; -121.95 lon; 50.88 depth; utmZone: 10 2024-11-06T02:58:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861903.14 2024-11-06T02:58:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:23Z- waiting for 1730861907.14, time is 1730861903.57 2024-11-06T02:58:23Z- AHRS_M2 msg: 1730861903.93 epoch sec; seqNo:1262 2024-11-06T02:58:23Z- AHRS_M2 msg: 0.39 phi; 0.01 theta; 1.08 psi 2024-11-06T02:58:23Z- handleDepth()- Depth_Keller msg: 1730861903.93 epoch sec; seqNo:1348; depth 50.88 2024-11-06T02:58:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861903.95 epoch sec; seqNo:1348; 4061679.72 north; 594229.31 east; 36.70 lat; -121.95 lon; 50.88 depth; utmZone: 10 2024-11-06T02:58:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861903.14 2024-11-06T02:58:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:24Z- waiting for 1730861907.14, time is 1730861903.95 2024-11-06T02:58:24Z- AHRS_M2 msg: 1730861904.33 epoch sec; seqNo:1263 2024-11-06T02:58:24Z- AHRS_M2 msg: 0.39 phi; 0.01 theta; 1.12 psi 2024-11-06T02:58:24Z- handleDepth()- Depth_Keller msg: 1730861904.34 epoch sec; seqNo:1349; depth 50.89 2024-11-06T02:58:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861904.35 epoch sec; seqNo:1349; 4061679.72 north; 594229.31 east; 36.70 lat; -121.95 lon; 50.89 depth; utmZone: 10 2024-11-06T02:58:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861903.14 2024-11-06T02:58:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:24Z- waiting for 1730861907.14, time is 1730861904.35 2024-11-06T02:58:24Z- AHRS_M2 msg: 1730861904.74 epoch sec; seqNo:1264 2024-11-06T02:58:24Z- AHRS_M2 msg: 0.39 phi; 0.01 theta; 1.15 psi 2024-11-06T02:58:24Z- handleDepth()- Depth_Keller msg: 1730861904.74 epoch sec; seqNo:1350; depth 50.89 2024-11-06T02:58:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861904.75 epoch sec; seqNo:1350; 4061679.72 north; 594229.31 east; 36.70 lat; -121.95 lon; 50.89 depth; utmZone: 10 2024-11-06T02:58:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861903.14 2024-11-06T02:58:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:24Z- waiting for 1730861907.14, time is 1730861904.75 2024-11-06T02:58:25Z- AHRS_M2 msg: 1730861905.14 epoch sec; seqNo:1265 2024-11-06T02:58:25Z- AHRS_M2 msg: 0.39 phi; 0.01 theta; 1.18 psi 2024-11-06T02:58:25Z- handleDepth()- Depth_Keller msg: 1730861905.14 epoch sec; seqNo:1351; depth 50.89 2024-11-06T02:58:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861905.16 epoch sec; seqNo:1351; 4061680.11 north; 594230.52 east; 36.70 lat; -121.95 lon; 50.89 depth; utmZone: 10 2024-11-06T02:58:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861903.14 2024-11-06T02:58:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:25Z- waiting for 1730861907.14, time is 1730861905.16 2024-11-06T02:58:25Z- AHRS_M2 msg: 1730861905.54 epoch sec; seqNo:1266 2024-11-06T02:58:25Z- AHRS_M2 msg: 0.39 phi; 0.01 theta; 1.22 psi 2024-11-06T02:58:25Z- handleDepth()- Depth_Keller msg: 1730861905.55 epoch sec; seqNo:1352; depth 50.88 2024-11-06T02:58:25Z- handleDVL() - DVL msg epochMillisec: 1730861905.57 2024-11-06T02:58:25Z- handleDvl() - RDI_Pathfinder msg: 1730861905.57 sec; seqNo:418; #beams:4 2024-11-06T02:58:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.51 , 37.77 , 47.05 , 47.05 2024-11-06T02:58:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.00 , -0.03 2024-11-06T02:58:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861905.59 epoch sec; seqNo:1352; 4061680.11 north; 594230.52 east; 36.70 lat; -121.95 lon; 50.88 depth; utmZone: 10 2024-11-06T02:58:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861903.14 2024-11-06T02:58:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:25Z- waiting for 1730861907.14, time is 1730861905.59 2024-11-06T02:58:25Z- AHRS_M2 msg: 1730861905.95 epoch sec; seqNo:1267 2024-11-06T02:58:25Z- AHRS_M2 msg: 0.39 phi; 0.01 theta; 1.25 psi 2024-11-06T02:58:25Z- handleDepth()- Depth_Keller msg: 1730861905.95 epoch sec; seqNo:1353; depth 50.88 2024-11-06T02:58:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861905.97 epoch sec; seqNo:1353; 4061680.11 north; 594230.52 east; 36.70 lat; -121.95 lon; 50.88 depth; utmZone: 10 2024-11-06T02:58:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861903.14 2024-11-06T02:58:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:26Z- waiting for 1730861907.14, time is 1730861905.97 2024-11-06T02:58:26Z- AHRS_M2 msg: 1730861906.35 epoch sec; seqNo:1268 2024-11-06T02:58:26Z- AHRS_M2 msg: 0.39 phi; 0.01 theta; 1.28 psi 2024-11-06T02:58:26Z- handleDepth()- Depth_Keller msg: 1730861906.36 epoch sec; seqNo:1354; depth 50.88 2024-11-06T02:58:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861906.37 epoch sec; seqNo:1354; 4061680.49 north; 594230.52 east; 36.70 lat; -121.95 lon; 50.88 depth; utmZone: 10 2024-11-06T02:58:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861903.14 2024-11-06T02:58:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:26Z- waiting for 1730861907.14, time is 1730861906.37 2024-11-06T02:58:26Z- AHRS_M2 msg: 1730861906.76 epoch sec; seqNo:1269 2024-11-06T02:58:26Z- AHRS_M2 msg: 0.39 phi; 0.01 theta; 1.31 psi 2024-11-06T02:58:26Z- handleDepth()- Depth_Keller msg: 1730861906.76 epoch sec; seqNo:1355; depth 50.88 2024-11-06T02:58:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861906.77 epoch sec; seqNo:1355; 4061680.50 north; 594231.74 east; 36.70 lat; -121.95 lon; 50.88 depth; utmZone: 10 2024-11-06T02:58:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861903.14 2024-11-06T02:58:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:26Z- waiting for 1730861907.14, time is 1730861906.77 2024-11-06T02:58:27Z- AHRS_M2 msg: 1730861907.16 epoch sec; seqNo:1270 2024-11-06T02:58:27Z- AHRS_M2 msg: 0.39 phi; 0.01 theta; 1.35 psi 2024-11-06T02:58:27Z- handleDepth()- Depth_Keller msg: 1730861907.16 epoch sec; seqNo:1356; depth 50.88 2024-11-06T02:58:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861907.21 epoch sec; seqNo:1356; 4061680.50 north; 594231.74 east; 36.70 lat; -121.95 lon; 50.88 depth; utmZone: 10 2024-11-06T02:58:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861903.14 2024-11-06T02:58:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:27Z- waiting for 1730861907.14, time is 1730861907.21 2024-11-06T02:58:27Z- LcmTrn::updateTrn() >>>> pose time 1730861907.21, meas time 1730861905.57 <<<< 2024-11-06T02:58:27Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861907.21 2024-11-06T02:58:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861905.57 sec, ping # 0; waiting for INS data more recent than 1730861903.14... 2024-11-06T02:58:27Z- TerrainNav::motionUpdate() - currEstimate = (4061680, 594231.74, 50.88) 2024-11-06T02:58:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:58:27Z- TerrainNav:: SubcloudNIS is 0.19, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:58:27Z- TNavPF::Projecting Measurements 2024-11-06T02:58:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:58:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:58:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:58:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:58:27Z- TNavFilter::4 good beams. 2024-11-06T02:58:27Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:58:27Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:58:27Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:58:27Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:58:27Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:58:27Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:58:27Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:58:27Z- TerrainMapOctree: (vehN, vehE ) = (4061680.03, 594230.76). 2024-11-06T02:58:27Z- TerrainMapOctree: Min Distance = 72.57. 2024-11-06T02:58:27Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:58:27Z- Forcing Subcloud Comparison 2024-11-06T02:58:27Z- Weighting particles with subcloud comparison 2024-11-06T02:58:27Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:27Z- meanExpectedMeasDiff: 1.728443 delta_rms_squared: 3.130410 alpha: 0.993652 2024-11-06T02:58:27Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:27Z- meanExpectedMeasDiff: 0.348176 delta_rms_squared: 1.180478 alpha: 0.983340 2024-11-06T02:58:27Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:27Z- meanExpectedMeasDiff: 0.422391 delta_rms_squared: 1.837999 alpha: 0.989236 2024-11-06T02:58:27Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:27Z- meanExpectedMeasDiff: -0.219398 delta_rms_squared: 1.796166 alpha: 0.988988 2024-11-06T02:58:27Z- Subcloud NIS: 0.197816 2024-11-06T02:58:27Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:58:27Z- TNavPF:: sumWeights = 0.552391 2024-11-06T02:58:27Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:58:27Z- TNavPF::First Term of Map Covariance Matrix is 0.93 2024-11-06T02:58:27Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.03 2024-11-06T02:58:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861905.57 sec. 2024-11-06T02:58:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:58:27Z- variances([0], [2], and psi_berg) = 0.324176 0.233606 0.000000 2024-11-06T02:58:27Z- publishEstimatesDecoder() - at 1730861907.21 sec 2024-11-06T02:58:27Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:58:27Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061711.77 594214.98 56.84 2024-11-06T02:58:27Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061712.85 594215.78 57.35 lat/lon: 36.70 -121.95 2024-11-06T02:58:27Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.32 0.23 0.74 0.00 2024-11-06T02:58:27Z- AHRS_M2 msg: 1730861907.58 epoch sec; seqNo:1271 2024-11-06T02:58:27Z- AHRS_M2 msg: 0.39 phi; 0.01 theta; 1.38 psi 2024-11-06T02:58:27Z- handleDepth()- Depth_Keller msg: 1730861907.61 epoch sec; seqNo:1357; depth 50.86 2024-11-06T02:58:27Z- handleDVL() - DVL msg epochMillisec: 1730861907.64 2024-11-06T02:58:27Z- handleDvl() - RDI_Pathfinder msg: 1730861907.64 sec; seqNo:419; #beams:4 2024-11-06T02:58:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.07 , 38.65 , 46.60 , 47.05 2024-11-06T02:58:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.04 , -0.03 2024-11-06T02:58:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861907.68 epoch sec; seqNo:1357; 4061680.50 north; 594231.74 east; 36.70 lat; -121.95 lon; 50.86 depth; utmZone: 10 2024-11-06T02:58:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861907.21 2024-11-06T02:58:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:27Z- waiting for 1730861911.21, time is 1730861907.68 2024-11-06T02:58:27Z- AHRS_M2 msg: 1730861907.97 epoch sec; seqNo:1272 2024-11-06T02:58:27Z- AHRS_M2 msg: 0.39 phi; 0.02 theta; 1.41 psi 2024-11-06T02:58:27Z- handleDepth()- Depth_Keller msg: 1730861907.97 epoch sec; seqNo:1358; depth 50.86 2024-11-06T02:58:27Z- handleDVL() - DVL msg epochMillisec: 1730861907.97 2024-11-06T02:58:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:58:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:58:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:58:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:58:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:58:27Z- handleDvl() - RDI_Pathfinder msg: 1730861907.97 sec; seqNo:420; #beams:0 2024-11-06T02:58:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:58:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , -0.04 , -0.03 2024-11-06T02:58:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861908.00 epoch sec; seqNo:1358; 4061680.50 north; 594231.74 east; 36.70 lat; -121.95 lon; 50.86 depth; utmZone: 10 2024-11-06T02:58:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861907.21 2024-11-06T02:58:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:28Z- waiting for 1730861911.21, time is 1730861908.00 2024-11-06T02:58:28Z- AHRS_M2 msg: 1730861908.37 epoch sec; seqNo:1273 2024-11-06T02:58:28Z- AHRS_M2 msg: 0.39 phi; 0.02 theta; 1.45 psi 2024-11-06T02:58:28Z- handleDepth()- Depth_Keller msg: 1730861908.38 epoch sec; seqNo:1359; depth 50.86 2024-11-06T02:58:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861908.39 epoch sec; seqNo:1359; 4061680.51 north; 594232.96 east; 36.70 lat; -121.95 lon; 50.86 depth; utmZone: 10 2024-11-06T02:58:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861907.21 2024-11-06T02:58:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:28Z- waiting for 1730861911.21, time is 1730861908.39 2024-11-06T02:58:28Z- AHRS_M2 msg: 1730861908.77 epoch sec; seqNo:1274 2024-11-06T02:58:28Z- AHRS_M2 msg: 0.39 phi; 0.02 theta; 1.48 psi 2024-11-06T02:58:28Z- handleDepth()- Depth_Keller msg: 1730861908.78 epoch sec; seqNo:1360; depth 50.86 2024-11-06T02:58:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861908.79 epoch sec; seqNo:1360; 4061680.51 north; 594232.96 east; 36.70 lat; -121.95 lon; 50.86 depth; utmZone: 10 2024-11-06T02:58:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861907.21 2024-11-06T02:58:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:28Z- waiting for 1730861911.21, time is 1730861908.79 2024-11-06T02:58:29Z- AHRS_M2 msg: 1730861909.18 epoch sec; seqNo:1275 2024-11-06T02:58:29Z- AHRS_M2 msg: 0.39 phi; 0.02 theta; 1.51 psi 2024-11-06T02:58:29Z- handleDepth()- Depth_Keller msg: 1730861909.18 epoch sec; seqNo:1361; depth 50.86 2024-11-06T02:58:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861909.19 epoch sec; seqNo:1361; 4061680.51 north; 594232.96 east; 36.70 lat; -121.95 lon; 50.86 depth; utmZone: 10 2024-11-06T02:58:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861907.21 2024-11-06T02:58:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:29Z- waiting for 1730861911.21, time is 1730861909.19 2024-11-06T02:58:29Z- AHRS_M2 msg: 1730861909.58 epoch sec; seqNo:1276 2024-11-06T02:58:29Z- AHRS_M2 msg: 0.39 phi; 0.02 theta; 1.55 psi 2024-11-06T02:58:29Z- handleDepth()- Depth_Keller msg: 1730861909.60 epoch sec; seqNo:1362; depth 50.85 2024-11-06T02:58:29Z- handleDVL() - DVL msg epochMillisec: 1730861909.61 2024-11-06T02:58:29Z- handleDvl() - RDI_Pathfinder msg: 1730861909.61 sec; seqNo:421; #beams:4 2024-11-06T02:58:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.85 , 38.65 , 46.38 , 46.83 2024-11-06T02:58:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.04 , 0.01 2024-11-06T02:58:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861909.63 epoch sec; seqNo:1362; 4061680.89 north; 594232.96 east; 36.70 lat; -121.95 lon; 50.85 depth; utmZone: 10 2024-11-06T02:58:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861907.21 2024-11-06T02:58:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:29Z- waiting for 1730861911.21, time is 1730861909.63 2024-11-06T02:58:30Z- AHRS_M2 msg: 1730861909.99 epoch sec; seqNo:1277 2024-11-06T02:58:30Z- AHRS_M2 msg: 0.39 phi; 0.02 theta; 1.58 psi 2024-11-06T02:58:30Z- handleDepth()- Depth_Keller msg: 1730861910.00 epoch sec; seqNo:1363; depth 50.84 2024-11-06T02:58:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861910.01 epoch sec; seqNo:1363; 4061680.91 north; 594234.18 east; 36.70 lat; -121.95 lon; 50.84 depth; utmZone: 10 2024-11-06T02:58:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861907.21 2024-11-06T02:58:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:30Z- waiting for 1730861911.21, time is 1730861910.01 2024-11-06T02:58:30Z- AHRS_M2 msg: 1730861910.40 epoch sec; seqNo:1278 2024-11-06T02:58:30Z- AHRS_M2 msg: 0.39 phi; 0.02 theta; 1.61 psi 2024-11-06T02:58:30Z- handleDepth()- Depth_Keller msg: 1730861910.40 epoch sec; seqNo:1364; depth 50.83 2024-11-06T02:58:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861910.41 epoch sec; seqNo:1364; 4061680.91 north; 594234.18 east; 36.70 lat; -121.95 lon; 50.83 depth; utmZone: 10 2024-11-06T02:58:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861907.21 2024-11-06T02:58:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:30Z- waiting for 1730861911.21, time is 1730861910.41 2024-11-06T02:58:30Z- AHRS_M2 msg: 1730861910.79 epoch sec; seqNo:1279 2024-11-06T02:58:30Z- AHRS_M2 msg: 0.39 phi; 0.02 theta; 1.65 psi 2024-11-06T02:58:30Z- handleDepth()- Depth_Keller msg: 1730861910.81 epoch sec; seqNo:1365; depth 50.83 2024-11-06T02:58:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861910.82 epoch sec; seqNo:1365; 4061680.91 north; 594234.18 east; 36.70 lat; -121.95 lon; 50.83 depth; utmZone: 10 2024-11-06T02:58:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861907.21 2024-11-06T02:58:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:30Z- waiting for 1730861911.21, time is 1730861910.82 2024-11-06T02:58:31Z- handleDepth()- Depth_Keller msg: 1730861911.21 epoch sec; seqNo:1366; depth 50.82 2024-11-06T02:58:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861911.25 epoch sec; seqNo:1366; 4061680.91 north; 594234.18 east; 36.70 lat; -121.95 lon; 50.82 depth; utmZone: 10 2024-11-06T02:58:31Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:58:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861907.21 2024-11-06T02:58:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:31Z- waiting for 1730861911.21, time is 1730861911.25 2024-11-06T02:58:31Z- LcmTrn::updateTrn() >>>> pose time 1730861911.25, meas time 1730861909.61 <<<< 2024-11-06T02:58:31Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861911.25 2024-11-06T02:58:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861909.61 sec, ping # 0; waiting for INS data more recent than 1730861907.21... 2024-11-06T02:58:31Z- TerrainNav::motionUpdate() - currEstimate = (4061681, 594234.18, 50.82) 2024-11-06T02:58:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:58:31Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:58:31Z- TNavPF::Projecting Measurements 2024-11-06T02:58:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:58:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:58:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:58:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:58:31Z- TNavFilter::4 good beams. 2024-11-06T02:58:31Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:58:31Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:58:31Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:58:31Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:58:31Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:58:31Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:58:31Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:58:31Z- TerrainMapOctree: (vehN, vehE ) = (4061680.74, 594233.19). 2024-11-06T02:58:31Z- TerrainMapOctree: Min Distance = 71.27. 2024-11-06T02:58:31Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:58:31Z- Forcing Subcloud Comparison 2024-11-06T02:58:31Z- Weighting particles with subcloud comparison 2024-11-06T02:58:31Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:31Z- meanExpectedMeasDiff: 0.636282 delta_rms_squared: 2.697981 alpha: 0.992642 2024-11-06T02:58:31Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:31Z- meanExpectedMeasDiff: 0.955425 delta_rms_squared: 1.017200 alpha: 0.980717 2024-11-06T02:58:31Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:31Z- meanExpectedMeasDiff: -0.757568 delta_rms_squared: 1.596249 alpha: 0.987626 2024-11-06T02:58:31Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:31Z- meanExpectedMeasDiff: 0.638278 delta_rms_squared: 1.531906 alpha: 0.987113 2024-11-06T02:58:31Z- Subcloud NIS: 0.197745 2024-11-06T02:58:31Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:58:31Z- TNavPF:: sumWeights = 0.554065 2024-11-06T02:58:31Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:58:31Z- TNavPF::First Term of Map Covariance Matrix is 0.70 2024-11-06T02:58:31Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T02:58:31Z- TerrainNav::Resampling particle filter... 2024-11-06T02:58:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861909.61 sec. 2024-11-06T02:58:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:58:31Z- variances([0], [2], and psi_berg) = 0.323184 0.233415 0.000000 2024-11-06T02:58:31Z- publishEstimatesDecoder() - at 1730861911.25 sec 2024-11-06T02:58:31Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:58:31Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061712.49 594218.77 56.33 2024-11-06T02:58:31Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061713.26 594218.21 57.15 lat/lon: 36.70 -121.95 2024-11-06T02:58:31Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.32 0.23 0.70 0.00 2024-11-06T02:58:31Z- AHRS_M2 msg: 1730861911.61 epoch sec; seqNo:1280 2024-11-06T02:58:31Z- AHRS_M2 msg: 0.39 phi; 0.02 theta; 1.72 psi 2024-11-06T02:58:31Z- handleDepth()- Depth_Keller msg: 1730861911.64 epoch sec; seqNo:1367; depth 50.81 2024-11-06T02:58:31Z- handleDVL() - DVL msg epochMillisec: 1730861911.69 2024-11-06T02:58:31Z- handleDvl() - RDI_Pathfinder msg: 1730861911.69 sec; seqNo:422; #beams:4 2024-11-06T02:58:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.18 , 38.65 , 46.38 , 47.05 2024-11-06T02:58:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.01 , 0.03 2024-11-06T02:58:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861911.70 epoch sec; seqNo:1367; 4061680.54 north; 594235.40 east; 36.70 lat; -121.95 lon; 50.81 depth; utmZone: 10 2024-11-06T02:58:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861911.25 2024-11-06T02:58:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:31Z- waiting for 1730861915.25, time is 1730861911.70 2024-11-06T02:58:32Z- AHRS_M2 msg: 1730861912.02 epoch sec; seqNo:1281 2024-11-06T02:58:32Z- AHRS_M2 msg: 0.38 phi; 0.02 theta; 1.75 psi 2024-11-06T02:58:32Z- handleDepth()- Depth_Keller msg: 1730861912.02 epoch sec; seqNo:1368; depth 50.80 2024-11-06T02:58:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861912.04 epoch sec; seqNo:1368; 4061680.54 north; 594235.40 east; 36.70 lat; -121.95 lon; 50.80 depth; utmZone: 10 2024-11-06T02:58:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861911.25 2024-11-06T02:58:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:32Z- waiting for 1730861915.25, time is 1730861912.04 2024-11-06T02:58:32Z- AHRS_M2 msg: 1730861912.41 epoch sec; seqNo:1282 2024-11-06T02:58:32Z- AHRS_M2 msg: 0.38 phi; 0.02 theta; 1.79 psi 2024-11-06T02:58:32Z- handleDepth()- Depth_Keller msg: 1730861912.42 epoch sec; seqNo:1369; depth 50.79 2024-11-06T02:58:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861912.43 epoch sec; seqNo:1369; 4061680.54 north; 594235.40 east; 36.70 lat; -121.95 lon; 50.79 depth; utmZone: 10 2024-11-06T02:58:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861911.25 2024-11-06T02:58:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:32Z- waiting for 1730861915.25, time is 1730861912.43 2024-11-06T02:58:32Z- AHRS_M2 msg: 1730861912.82 epoch sec; seqNo:1283 2024-11-06T02:58:32Z- AHRS_M2 msg: 0.38 phi; 0.02 theta; 1.82 psi 2024-11-06T02:58:32Z- handleDepth()- Depth_Keller msg: 1730861912.82 epoch sec; seqNo:1370; depth 50.78 2024-11-06T02:58:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861912.83 epoch sec; seqNo:1370; 4061680.54 north; 594235.40 east; 36.70 lat; -121.95 lon; 50.78 depth; utmZone: 10 2024-11-06T02:58:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861911.25 2024-11-06T02:58:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:32Z- waiting for 1730861915.25, time is 1730861912.83 2024-11-06T02:58:33Z- AHRS_M2 msg: 1730861913.23 epoch sec; seqNo:1284 2024-11-06T02:58:33Z- AHRS_M2 msg: 0.38 phi; 0.02 theta; 1.86 psi 2024-11-06T02:58:33Z- handleDepth()- Depth_Keller msg: 1730861913.25 epoch sec; seqNo:1371; depth 50.78 2024-11-06T02:58:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861913.29 epoch sec; seqNo:1371; 4061680.55 north; 594236.62 east; 36.70 lat; -121.95 lon; 50.78 depth; utmZone: 10 2024-11-06T02:58:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861911.25 2024-11-06T02:58:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:33Z- waiting for 1730861915.25, time is 1730861913.29 2024-11-06T02:58:33Z- AHRS_M2 msg: 1730861913.62 epoch sec; seqNo:1285 2024-11-06T02:58:33Z- AHRS_M2 msg: 0.38 phi; 0.02 theta; 1.89 psi 2024-11-06T02:58:33Z- handleDepth()- Depth_Keller msg: 1730861913.64 epoch sec; seqNo:1372; depth 50.76 2024-11-06T02:58:33Z- handleDVL() - DVL msg epochMillisec: 1730861913.67 2024-11-06T02:58:33Z- handleDvl() - RDI_Pathfinder msg: 1730861913.67 sec; seqNo:423; #beams:4 2024-11-06T02:58:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 58.97 , 38.87 , 46.16 , 46.83 2024-11-06T02:58:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.01 , -0.00 2024-11-06T02:58:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861913.70 epoch sec; seqNo:1372; 4061680.17 north; 594236.63 east; 36.70 lat; -121.95 lon; 50.76 depth; utmZone: 10 2024-11-06T02:58:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861911.25 2024-11-06T02:58:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:33Z- waiting for 1730861915.25, time is 1730861913.70 2024-11-06T02:58:34Z- handleDepth()- Depth_Keller msg: 1730861914.05 epoch sec; seqNo:1373; depth 50.75 2024-11-06T02:58:34Z- handleDVL() - DVL msg epochMillisec: 1730861914.06 2024-11-06T02:58:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:58:34Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:58:34Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:58:34Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:58:34Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:58:34Z- handleDvl() - RDI_Pathfinder msg: 1730861914.06 sec; seqNo:424; #beams:0 2024-11-06T02:58:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:58:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.01 , -0.00 2024-11-06T02:58:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861914.07 epoch sec; seqNo:1373; 4061680.17 north; 594236.63 east; 36.70 lat; -121.95 lon; 50.75 depth; utmZone: 10 2024-11-06T02:58:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861911.25 2024-11-06T02:58:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:34Z- waiting for 1730861915.25, time is 1730861914.07 2024-11-06T02:58:34Z- AHRS_M2 msg: 1730861914.44 epoch sec; seqNo:1286 2024-11-06T02:58:34Z- AHRS_M2 msg: 0.38 phi; 0.03 theta; 1.96 psi 2024-11-06T02:58:34Z- handleDepth()- Depth_Keller msg: 1730861914.46 epoch sec; seqNo:1374; depth 50.74 2024-11-06T02:58:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861914.48 epoch sec; seqNo:1374; 4061680.17 north; 594236.63 east; 36.70 lat; -121.95 lon; 50.74 depth; utmZone: 10 2024-11-06T02:58:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861911.25 2024-11-06T02:58:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:34Z- waiting for 1730861915.25, time is 1730861914.48 2024-11-06T02:58:34Z- AHRS_M2 msg: 1730861914.84 epoch sec; seqNo:1287 2024-11-06T02:58:34Z- AHRS_M2 msg: 0.37 phi; 0.03 theta; 1.99 psi 2024-11-06T02:58:34Z- handleDepth()- Depth_Keller msg: 1730861914.84 epoch sec; seqNo:1375; depth 50.72 2024-11-06T02:58:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861914.85 epoch sec; seqNo:1375; 4061680.17 north; 594236.63 east; 36.70 lat; -121.95 lon; 50.72 depth; utmZone: 10 2024-11-06T02:58:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861911.25 2024-11-06T02:58:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:35Z- waiting for 1730861915.25, time is 1730861914.85 2024-11-06T02:58:35Z- AHRS_M2 msg: 1730861915.28 epoch sec; seqNo:1288 2024-11-06T02:58:35Z- AHRS_M2 msg: 0.37 phi; 0.03 theta; 2.02 psi 2024-11-06T02:58:35Z- handleDepth()- Depth_Keller msg: 1730861915.29 epoch sec; seqNo:1376; depth 50.70 2024-11-06T02:58:35Z- handleDVL() - DVL msg epochMillisec: 1730861915.32 2024-11-06T02:58:35Z- handleDvl() - RDI_Pathfinder msg: 1730861915.32 sec; seqNo:425; #beams:4 2024-11-06T02:58:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 58.75 , 38.87 , 46.16 , 46.38 2024-11-06T02:58:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.02 , -0.04 2024-11-06T02:58:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861915.33 epoch sec; seqNo:1376; 4061679.81 north; 594237.85 east; 36.70 lat; -121.95 lon; 50.70 depth; utmZone: 10 2024-11-06T02:58:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861911.25 2024-11-06T02:58:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:35Z- waiting for 1730861915.25, time is 1730861915.33 2024-11-06T02:58:35Z- LcmTrn::updateTrn() >>>> pose time 1730861915.33, meas time 1730861915.32 <<<< 2024-11-06T02:58:35Z- LcmTrn::updateTrn() equating pose 1730861915.32 and meas times 1730861915.32 2024-11-06T02:58:35Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861915.32 2024-11-06T02:58:35Z- TerrainNav::motionUpdate() - currEstimate = (4061680, 594237.85, 50.70) 2024-11-06T02:58:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:58:35Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:58:35Z- TNavPF::Projecting Measurements 2024-11-06T02:58:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:58:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:58:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:58:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:58:35Z- TNavFilter::4 good beams. 2024-11-06T02:58:35Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:58:35Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:58:35Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:58:35Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:58:35Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:58:35Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:58:35Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:58:35Z- TerrainMapOctree: (vehN, vehE ) = (4061679.81, 594237.85). 2024-11-06T02:58:35Z- TerrainMapOctree: Min Distance = 71.23. 2024-11-06T02:58:35Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:58:35Z- Forcing Subcloud Comparison 2024-11-06T02:58:35Z- Weighting particles with subcloud comparison 2024-11-06T02:58:35Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:35Z- meanExpectedMeasDiff: -1.215588 delta_rms_squared: 2.433874 alpha: 0.991850 2024-11-06T02:58:35Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:35Z- meanExpectedMeasDiff: 0.750751 delta_rms_squared: 0.977669 alpha: 0.979953 2024-11-06T02:58:35Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:35Z- meanExpectedMeasDiff: -0.017900 delta_rms_squared: 1.519463 alpha: 0.987008 2024-11-06T02:58:35Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:35Z- meanExpectedMeasDiff: 0.569802 delta_rms_squared: 1.416368 alpha: 0.986076 2024-11-06T02:58:35Z- Subcloud NIS: 0.197981 2024-11-06T02:58:35Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:58:35Z- TNavPF:: sumWeights = 0.561689 2024-11-06T02:58:35Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:58:35Z- TNavPF::First Term of Map Covariance Matrix is 0.81 2024-11-06T02:58:35Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T02:58:35Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861915.32 sec, ping # 0. 2024-11-06T02:58:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:58:35Z- variances([0], [2], and psi_berg) = 0.324241 0.239397 0.000000 2024-11-06T02:58:35Z- publishEstimatesDecoder() - at 1730861915.32 sec 2024-11-06T02:58:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:58:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061712.00 594221.78 57.00 2024-11-06T02:58:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061712.17 594221.89 57.01 lat/lon: 36.70 -121.95 2024-11-06T02:58:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.32 0.24 0.45 0.00 2024-11-06T02:58:35Z- AHRS_M2 msg: 1730861915.69 epoch sec; seqNo:1289 2024-11-06T02:58:35Z- AHRS_M2 msg: 0.36 phi; 0.03 theta; 2.05 psi 2024-11-06T02:58:35Z- handleDepth()- Depth_Keller msg: 1730861915.70 epoch sec; seqNo:1377; depth 50.70 2024-11-06T02:58:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861915.72 epoch sec; seqNo:1377; 4061679.81 north; 594237.85 east; 36.70 lat; -121.95 lon; 50.70 depth; utmZone: 10 2024-11-06T02:58:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861915.32 2024-11-06T02:58:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:35Z- waiting for 1730861919.32, time is 1730861915.72 2024-11-06T02:58:36Z- AHRS_M2 msg: 1730861916.05 epoch sec; seqNo:1290 2024-11-06T02:58:36Z- AHRS_M2 msg: 0.35 phi; 0.03 theta; 2.08 psi 2024-11-06T02:58:36Z- handleDepth()- Depth_Keller msg: 1730861916.05 epoch sec; seqNo:1378; depth 50.69 2024-11-06T02:58:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861916.06 epoch sec; seqNo:1378; 4061679.43 north; 594237.85 east; 36.70 lat; -121.95 lon; 50.69 depth; utmZone: 10 2024-11-06T02:58:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861915.32 2024-11-06T02:58:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:36Z- waiting for 1730861919.32, time is 1730861916.06 2024-11-06T02:58:36Z- AHRS_M2 msg: 1730861916.45 epoch sec; seqNo:1291 2024-11-06T02:58:36Z- AHRS_M2 msg: 0.34 phi; 0.03 theta; 2.10 psi 2024-11-06T02:58:36Z- handleDepth()- Depth_Keller msg: 1730861916.45 epoch sec; seqNo:1379; depth 50.68 2024-11-06T02:58:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861916.47 epoch sec; seqNo:1379; 4061679.43 north; 594237.85 east; 36.70 lat; -121.95 lon; 50.68 depth; utmZone: 10 2024-11-06T02:58:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861915.32 2024-11-06T02:58:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:36Z- waiting for 1730861919.32, time is 1730861916.47 2024-11-06T02:58:36Z- AHRS_M2 msg: 1730861916.86 epoch sec; seqNo:1292 2024-11-06T02:58:36Z- AHRS_M2 msg: 0.33 phi; 0.04 theta; 2.12 psi 2024-11-06T02:58:36Z- handleDepth()- Depth_Keller msg: 1730861916.87 epoch sec; seqNo:1380; depth 50.67 2024-11-06T02:58:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861916.88 epoch sec; seqNo:1380; 4061679.06 north; 594239.08 east; 36.70 lat; -121.95 lon; 50.67 depth; utmZone: 10 2024-11-06T02:58:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861915.32 2024-11-06T02:58:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:37Z- waiting for 1730861919.32, time is 1730861916.88 2024-11-06T02:58:37Z- AHRS_M2 msg: 1730861917.29 epoch sec; seqNo:1293 2024-11-06T02:58:37Z- AHRS_M2 msg: 0.32 phi; 0.04 theta; 2.14 psi 2024-11-06T02:58:37Z- handleDepth()- Depth_Keller msg: 1730861917.30 epoch sec; seqNo:1381; depth 50.65 2024-11-06T02:58:37Z- handleDVL() - DVL msg epochMillisec: 1730861917.33 2024-11-06T02:58:37Z- handleDvl() - RDI_Pathfinder msg: 1730861917.33 sec; seqNo:426; #beams:4 2024-11-06T02:58:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 55.66 , 38.87 , 45.94 , 45.06 2024-11-06T02:58:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , 0.04 , -0.04 2024-11-06T02:58:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861917.34 epoch sec; seqNo:1381; 4061679.06 north; 594239.08 east; 36.70 lat; -121.95 lon; 50.65 depth; utmZone: 10 2024-11-06T02:58:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861915.32 2024-11-06T02:58:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:37Z- waiting for 1730861919.32, time is 1730861917.34 2024-11-06T02:58:37Z- AHRS_M2 msg: 1730861917.67 epoch sec; seqNo:1294 2024-11-06T02:58:37Z- AHRS_M2 msg: 0.31 phi; 0.05 theta; 2.16 psi 2024-11-06T02:58:37Z- handleDepth()- Depth_Keller msg: 1730861917.68 epoch sec; seqNo:1382; depth 50.65 2024-11-06T02:58:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861917.69 epoch sec; seqNo:1382; 4061678.69 north; 594239.08 east; 36.70 lat; -121.95 lon; 50.65 depth; utmZone: 10 2024-11-06T02:58:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861915.32 2024-11-06T02:58:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:37Z- waiting for 1730861919.32, time is 1730861917.69 2024-11-06T02:58:38Z- AHRS_M2 msg: 1730861918.07 epoch sec; seqNo:1295 2024-11-06T02:58:38Z- AHRS_M2 msg: 0.30 phi; 0.05 theta; 2.18 psi 2024-11-06T02:58:38Z- handleDepth()- Depth_Keller msg: 1730861918.07 epoch sec; seqNo:1383; depth 50.63 2024-11-06T02:58:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861918.10 epoch sec; seqNo:1383; 4061678.69 north; 594239.08 east; 36.70 lat; -121.95 lon; 50.63 depth; utmZone: 10 2024-11-06T02:58:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861915.32 2024-11-06T02:58:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:38Z- waiting for 1730861919.32, time is 1730861918.10 2024-11-06T02:58:38Z- AHRS_M2 msg: 1730861918.47 epoch sec; seqNo:1296 2024-11-06T02:58:38Z- AHRS_M2 msg: 0.29 phi; 0.06 theta; 2.19 psi 2024-11-06T02:58:38Z- handleDepth()- Depth_Keller msg: 1730861918.47 epoch sec; seqNo:1384; depth 50.61 2024-11-06T02:58:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861918.48 epoch sec; seqNo:1384; 4061678.31 north; 594239.09 east; 36.70 lat; -121.95 lon; 50.61 depth; utmZone: 10 2024-11-06T02:58:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861915.32 2024-11-06T02:58:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:38Z- waiting for 1730861919.32, time is 1730861918.48 2024-11-06T02:58:38Z- handleDepth()- Depth_Keller msg: 1730861918.90 epoch sec; seqNo:1385; depth 50.61 2024-11-06T02:58:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861918.91 epoch sec; seqNo:1385; 4061678.32 north; 594240.31 east; 36.70 lat; -121.95 lon; 50.61 depth; utmZone: 10 2024-11-06T02:58:39Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:58:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861915.32 2024-11-06T02:58:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:39Z- waiting for 1730861919.32, time is 1730861918.91 2024-11-06T02:58:39Z- handleDepth()- Depth_Keller msg: 1730861919.33 epoch sec; seqNo:1386; depth 50.59 2024-11-06T02:58:39Z- handleDVL() - DVL msg epochMillisec: 1730861919.36 2024-11-06T02:58:39Z- handleDvl() - RDI_Pathfinder msg: 1730861919.36 sec; seqNo:427; #beams:4 2024-11-06T02:58:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 52.35 , 39.10 , 45.94 , 43.96 2024-11-06T02:58:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , 0.04 , 0.02 2024-11-06T02:58:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861919.37 epoch sec; seqNo:1386; 4061677.94 north; 594240.31 east; 36.70 lat; -121.95 lon; 50.59 depth; utmZone: 10 2024-11-06T02:58:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861915.32 2024-11-06T02:58:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:39Z- waiting for 1730861919.32, time is 1730861919.37 2024-11-06T02:58:39Z- LcmTrn::updateTrn() >>>> pose time 1730861919.37, meas time 1730861919.36 <<<< 2024-11-06T02:58:39Z- LcmTrn::updateTrn() equating pose 1730861919.36 and meas times 1730861919.36 2024-11-06T02:58:39Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861919.36 2024-11-06T02:58:39Z- TerrainNav::motionUpdate() - currEstimate = (4061678, 594240.31, 50.59) 2024-11-06T02:58:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:58:39Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:58:39Z- TNavPF::Projecting Measurements 2024-11-06T02:58:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:58:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:58:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:58:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:58:39Z- TNavFilter::4 good beams. 2024-11-06T02:58:39Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:58:39Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:58:39Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:58:39Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:58:39Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:58:39Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:58:39Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:58:39Z- TerrainMapOctree: (vehN, vehE ) = (4061677.94, 594240.31). 2024-11-06T02:58:39Z- TerrainMapOctree: Min Distance = 72.71. 2024-11-06T02:58:39Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:58:39Z- Forcing Subcloud Comparison 2024-11-06T02:58:39Z- Weighting particles with subcloud comparison 2024-11-06T02:58:39Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:39Z- meanExpectedMeasDiff: -2.002973 delta_rms_squared: 1.088160 alpha: 0.981952 2024-11-06T02:58:39Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:39Z- meanExpectedMeasDiff: 0.193673 delta_rms_squared: 0.736928 alpha: 0.973577 2024-11-06T02:58:39Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:39Z- meanExpectedMeasDiff: -0.075618 delta_rms_squared: 1.092815 alpha: 0.982028 2024-11-06T02:58:39Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:39Z- meanExpectedMeasDiff: -0.067844 delta_rms_squared: 0.946547 alpha: 0.979308 2024-11-06T02:58:39Z- Subcloud NIS: 0.198834 2024-11-06T02:58:39Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:58:39Z- TNavPF:: sumWeights = 0.559808 2024-11-06T02:58:39Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:58:39Z- TNavPF::First Term of Map Covariance Matrix is 0.74 2024-11-06T02:58:39Z- TNavPF::Calculated NIS Value : 0.21 numBeams normalized NIS: 0.05 2024-11-06T02:58:39Z- TerrainNav::Resampling particle filter... 2024-11-06T02:58:39Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861919.36 sec, ping # 0. 2024-11-06T02:58:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:58:39Z- variances([0], [2], and psi_berg) = 0.318891 0.238773 0.000000 2024-11-06T02:58:39Z- publishEstimatesDecoder() - at 1730861919.36 sec 2024-11-06T02:58:39Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:58:39Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061709.51 594224.91 56.14 2024-11-06T02:58:39Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061710.29 594224.40 57.03 lat/lon: 36.70 -121.95 2024-11-06T02:58:39Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.32 0.24 0.42 0.00 2024-11-06T02:58:39Z- AHRS_M2 msg: 1730861919.69 epoch sec; seqNo:1297 2024-11-06T02:58:39Z- AHRS_M2 msg: 0.26 phi; 0.09 theta; 2.23 psi 2024-11-06T02:58:39Z- handleDepth()- Depth_Keller msg: 1730861919.70 epoch sec; seqNo:1387; depth 50.57 2024-11-06T02:58:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861919.75 epoch sec; seqNo:1387; 4061677.94 north; 594240.31 east; 36.70 lat; -121.95 lon; 50.57 depth; utmZone: 10 2024-11-06T02:58:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861919.36 2024-11-06T02:58:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:39Z- waiting for 1730861923.36, time is 1730861919.75 2024-11-06T02:58:40Z- AHRS_M2 msg: 1730861920.10 epoch sec; seqNo:1298 2024-11-06T02:58:40Z- AHRS_M2 msg: 0.25 phi; 0.10 theta; 2.24 psi 2024-11-06T02:58:40Z- handleDepth()- Depth_Keller msg: 1730861920.10 epoch sec; seqNo:1388; depth 50.55 2024-11-06T02:58:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861920.11 epoch sec; seqNo:1388; 4061677.56 north; 594240.32 east; 36.70 lat; -121.95 lon; 50.55 depth; utmZone: 10 2024-11-06T02:58:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861919.36 2024-11-06T02:58:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:40Z- waiting for 1730861923.36, time is 1730861920.11 2024-11-06T02:58:40Z- AHRS_M2 msg: 1730861920.49 epoch sec; seqNo:1299 2024-11-06T02:58:40Z- AHRS_M2 msg: 0.24 phi; 0.11 theta; 2.25 psi 2024-11-06T02:58:40Z- handleDepth()- Depth_Keller msg: 1730861920.50 epoch sec; seqNo:1389; depth 50.53 2024-11-06T02:58:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861920.51 epoch sec; seqNo:1389; 4061677.56 north; 594240.32 east; 36.70 lat; -121.95 lon; 50.53 depth; utmZone: 10 2024-11-06T02:58:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861919.36 2024-11-06T02:58:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:40Z- waiting for 1730861923.36, time is 1730861920.51 2024-11-06T02:58:40Z- AHRS_M2 msg: 1730861920.89 epoch sec; seqNo:1300 2024-11-06T02:58:40Z- AHRS_M2 msg: 0.22 phi; 0.12 theta; 2.25 psi 2024-11-06T02:58:40Z- handleDepth()- Depth_Keller msg: 1730861920.90 epoch sec; seqNo:1390; depth 50.49 2024-11-06T02:58:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861920.92 epoch sec; seqNo:1390; 4061677.18 north; 594240.32 east; 36.70 lat; -121.95 lon; 50.49 depth; utmZone: 10 2024-11-06T02:58:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861919.36 2024-11-06T02:58:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:41Z- waiting for 1730861923.36, time is 1730861920.92 2024-11-06T02:58:41Z- handleDepth()- Depth_Keller msg: 1730861921.33 epoch sec; seqNo:1391; depth 50.48 2024-11-06T02:58:41Z- handleDVL() - DVL msg epochMillisec: 1730861921.35 2024-11-06T02:58:41Z- handleDvl() - RDI_Pathfinder msg: 1730861921.35 sec; seqNo:428; #beams:4 2024-11-06T02:58:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.80 , 39.75 , 47.04 , 42.85 2024-11-06T02:58:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.03 , 0.06 2024-11-06T02:58:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861921.36 epoch sec; seqNo:1391; 4061677.20 north; 594241.54 east; 36.70 lat; -121.95 lon; 50.48 depth; utmZone: 10 2024-11-06T02:58:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861919.36 2024-11-06T02:58:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:41Z- waiting for 1730861923.36, time is 1730861921.36 2024-11-06T02:58:41Z- AHRS_M2 msg: 1730861921.71 epoch sec; seqNo:1301 2024-11-06T02:58:41Z- AHRS_M2 msg: 0.20 phi; 0.13 theta; 2.26 psi 2024-11-06T02:58:41Z- handleDepth()- Depth_Keller msg: 1730861921.72 epoch sec; seqNo:1392; depth 50.45 2024-11-06T02:58:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861921.73 epoch sec; seqNo:1392; 4061676.82 north; 594241.54 east; 36.70 lat; -121.95 lon; 50.45 depth; utmZone: 10 2024-11-06T02:58:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861919.36 2024-11-06T02:58:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:41Z- waiting for 1730861923.36, time is 1730861921.73 2024-11-06T02:58:42Z- AHRS_M2 msg: 1730861922.11 epoch sec; seqNo:1302 2024-11-06T02:58:42Z- AHRS_M2 msg: 0.19 phi; 0.14 theta; 2.27 psi 2024-11-06T02:58:42Z- handleDepth()- Depth_Keller msg: 1730861922.12 epoch sec; seqNo:1393; depth 50.42 2024-11-06T02:58:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861922.13 epoch sec; seqNo:1393; 4061676.82 north; 594241.54 east; 36.70 lat; -121.95 lon; 50.42 depth; utmZone: 10 2024-11-06T02:58:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861919.36 2024-11-06T02:58:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:42Z- waiting for 1730861923.36, time is 1730861922.13 2024-11-06T02:58:42Z- AHRS_M2 msg: 1730861922.52 epoch sec; seqNo:1303 2024-11-06T02:58:42Z- AHRS_M2 msg: 0.18 phi; 0.15 theta; 2.28 psi 2024-11-06T02:58:42Z- handleDepth()- Depth_Keller msg: 1730861922.52 epoch sec; seqNo:1394; depth 50.39 2024-11-06T02:58:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861922.53 epoch sec; seqNo:1394; 4061676.44 north; 594241.55 east; 36.70 lat; -121.95 lon; 50.39 depth; utmZone: 10 2024-11-06T02:58:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861919.36 2024-11-06T02:58:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:42Z- waiting for 1730861923.36, time is 1730861922.53 2024-11-06T02:58:42Z- AHRS_M2 msg: 1730861922.92 epoch sec; seqNo:1304 2024-11-06T02:58:42Z- AHRS_M2 msg: 0.18 phi; 0.16 theta; 2.28 psi 2024-11-06T02:58:42Z- handleDepth()- Depth_Keller msg: 1730861922.92 epoch sec; seqNo:1395; depth 50.35 2024-11-06T02:58:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861922.93 epoch sec; seqNo:1395; 4061676.44 north; 594241.55 east; 36.70 lat; -121.95 lon; 50.35 depth; utmZone: 10 2024-11-06T02:58:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861919.36 2024-11-06T02:58:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:43Z- waiting for 1730861923.36, time is 1730861922.93 2024-11-06T02:58:43Z- AHRS_M2 msg: 1730861923.32 epoch sec; seqNo:1305 2024-11-06T02:58:43Z- AHRS_M2 msg: 0.18 phi; 0.16 theta; 2.29 psi 2024-11-06T02:58:43Z- handleDepth()- Depth_Keller msg: 1730861923.33 epoch sec; seqNo:1396; depth 50.32 2024-11-06T02:58:43Z- handleDVL() - DVL msg epochMillisec: 1730861923.35 2024-11-06T02:58:43Z- handleDvl() - RDI_Pathfinder msg: 1730861923.35 sec; seqNo:429; #beams:4 2024-11-06T02:58:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.93 , 40.98 , 48.32 , 41.80 2024-11-06T02:58:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , 0.06 2024-11-06T02:58:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861923.36 epoch sec; seqNo:1396; 4061676.07 north; 594242.77 east; 36.70 lat; -121.95 lon; 50.32 depth; utmZone: 10 2024-11-06T02:58:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861919.36 2024-11-06T02:58:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:43Z- waiting for 1730861923.36, time is 1730861923.36 2024-11-06T02:58:43Z- LcmTrn::updateTrn() >>>> pose time 1730861923.36, meas time 1730861923.35 <<<< 2024-11-06T02:58:43Z- LcmTrn::updateTrn() equating pose 1730861923.35 and meas times 1730861923.35 2024-11-06T02:58:43Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861923.35 2024-11-06T02:58:43Z- TerrainNav::motionUpdate() - currEstimate = (4061676, 594242.77, 50.32) 2024-11-06T02:58:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:58:43Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:58:43Z- TNavPF::Projecting Measurements 2024-11-06T02:58:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:58:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:58:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:58:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:58:43Z- TNavFilter::4 good beams. 2024-11-06T02:58:43Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:58:43Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:58:43Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:58:43Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:58:43Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:58:43Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:58:43Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:58:43Z- TerrainMapOctree: (vehN, vehE ) = (4061676.07, 594242.77). 2024-11-06T02:58:43Z- TerrainMapOctree: Min Distance = 74.28. 2024-11-06T02:58:43Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:58:43Z- Forcing Subcloud Comparison 2024-11-06T02:58:43Z- Weighting particles with subcloud comparison 2024-11-06T02:58:43Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:43Z- meanExpectedMeasDiff: -0.818105 delta_rms_squared: 0.783731 alpha: 0.975116 2024-11-06T02:58:43Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:43Z- meanExpectedMeasDiff: 0.097944 delta_rms_squared: 0.775606 alpha: 0.974862 2024-11-06T02:58:43Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:43Z- meanExpectedMeasDiff: -0.338993 delta_rms_squared: 1.164004 alpha: 0.983108 2024-11-06T02:58:43Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:43Z- meanExpectedMeasDiff: 0.703281 delta_rms_squared: 0.783723 alpha: 0.975116 2024-11-06T02:58:43Z- Subcloud NIS: 0.199029 2024-11-06T02:58:43Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:58:43Z- TNavPF:: sumWeights = 0.675045 2024-11-06T02:58:43Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:58:43Z- TNavPF::First Term of Map Covariance Matrix is 0.40 2024-11-06T02:58:43Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T02:58:43Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861923.35 sec, ping # 0. 2024-11-06T02:58:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:58:43Z- variances([0], [2], and psi_berg) = 0.317976 0.239348 0.000000 2024-11-06T02:58:43Z- publishEstimatesDecoder() - at 1730861923.35 sec 2024-11-06T02:58:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:58:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061708.38 594228.01 56.66 2024-11-06T02:58:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061708.42 594226.89 56.75 lat/lon: 36.70 -121.95 2024-11-06T02:58:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.32 0.24 0.41 0.00 2024-11-06T02:58:43Z- AHRS_M2 msg: 1730861923.78 epoch sec; seqNo:1306 2024-11-06T02:58:43Z- AHRS_M2 msg: 0.17 phi; 0.17 theta; 2.29 psi 2024-11-06T02:58:43Z- handleDepth()- Depth_Keller msg: 1730861923.79 epoch sec; seqNo:1397; depth 50.27 2024-11-06T02:58:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861923.80 epoch sec; seqNo:1397; 4061675.70 north; 594242.78 east; 36.70 lat; -121.95 lon; 50.27 depth; utmZone: 10 2024-11-06T02:58:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861923.35 2024-11-06T02:58:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:43Z- waiting for 1730861927.35, time is 1730861923.80 2024-11-06T02:58:44Z- AHRS_M2 msg: 1730861924.17 epoch sec; seqNo:1307 2024-11-06T02:58:44Z- AHRS_M2 msg: 0.17 phi; 0.18 theta; 2.30 psi 2024-11-06T02:58:44Z- handleDepth()- Depth_Keller msg: 1730861924.17 epoch sec; seqNo:1398; depth 50.22 2024-11-06T02:58:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861924.18 epoch sec; seqNo:1398; 4061675.70 north; 594242.78 east; 36.70 lat; -121.95 lon; 50.22 depth; utmZone: 10 2024-11-06T02:58:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861923.35 2024-11-06T02:58:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:44Z- waiting for 1730861927.35, time is 1730861924.18 2024-11-06T02:58:44Z- AHRS_M2 msg: 1730861924.58 epoch sec; seqNo:1308 2024-11-06T02:58:44Z- AHRS_M2 msg: 0.17 phi; 0.18 theta; 2.31 psi 2024-11-06T02:58:44Z- handleDepth()- Depth_Keller msg: 1730861924.59 epoch sec; seqNo:1399; depth 50.18 2024-11-06T02:58:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861924.60 epoch sec; seqNo:1399; 4061675.32 north; 594242.78 east; 36.70 lat; -121.95 lon; 50.18 depth; utmZone: 10 2024-11-06T02:58:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861923.35 2024-11-06T02:58:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:44Z- waiting for 1730861927.35, time is 1730861924.60 2024-11-06T02:58:44Z- AHRS_M2 msg: 1730861924.97 epoch sec; seqNo:1309 2024-11-06T02:58:44Z- AHRS_M2 msg: 0.17 phi; 0.19 theta; 2.31 psi 2024-11-06T02:58:44Z- handleDepth()- Depth_Keller msg: 1730861924.98 epoch sec; seqNo:1400; depth 50.10 2024-11-06T02:58:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861925.00 epoch sec; seqNo:1400; 4061674.95 north; 594244.01 east; 36.70 lat; -121.95 lon; 50.10 depth; utmZone: 10 2024-11-06T02:58:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861923.35 2024-11-06T02:58:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:45Z- waiting for 1730861927.35, time is 1730861925.00 2024-11-06T02:58:45Z- AHRS_M2 msg: 1730861925.38 epoch sec; seqNo:1310 2024-11-06T02:58:45Z- AHRS_M2 msg: 0.16 phi; 0.19 theta; 2.31 psi 2024-11-06T02:58:45Z- handleDepth()- Depth_Keller msg: 1730861925.38 epoch sec; seqNo:1401; depth 50.05 2024-11-06T02:58:45Z- handleDVL() - DVL msg epochMillisec: 1730861925.41 2024-11-06T02:58:45Z- handleDvl() - RDI_Pathfinder msg: 1730861925.41 sec; seqNo:430; #beams:4 2024-11-06T02:58:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.93 , 41.80 , 49.95 , 41.59 2024-11-06T02:58:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , 0.00 2024-11-06T02:58:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861925.42 epoch sec; seqNo:1401; 4061674.95 north; 594244.01 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T02:58:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861923.35 2024-11-06T02:58:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:45Z- waiting for 1730861927.35, time is 1730861925.42 2024-11-06T02:58:45Z- AHRS_M2 msg: 1730861925.78 epoch sec; seqNo:1311 2024-11-06T02:58:45Z- AHRS_M2 msg: 0.16 phi; 0.19 theta; 2.32 psi 2024-11-06T02:58:45Z- handleDepth()- Depth_Keller msg: 1730861925.79 epoch sec; seqNo:1402; depth 49.99 2024-11-06T02:58:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861925.80 epoch sec; seqNo:1402; 4061674.57 north; 594244.01 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T02:58:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861923.35 2024-11-06T02:58:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:45Z- waiting for 1730861927.35, time is 1730861925.80 2024-11-06T02:58:46Z- AHRS_M2 msg: 1730861926.19 epoch sec; seqNo:1312 2024-11-06T02:58:46Z- AHRS_M2 msg: 0.16 phi; 0.19 theta; 2.32 psi 2024-11-06T02:58:46Z- handleDepth()- Depth_Keller msg: 1730861926.19 epoch sec; seqNo:1403; depth 49.92 2024-11-06T02:58:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861926.21 epoch sec; seqNo:1403; 4061674.19 north; 594244.01 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T02:58:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861923.35 2024-11-06T02:58:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:46Z- waiting for 1730861927.35, time is 1730861926.21 2024-11-06T02:58:46Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T02:58:46Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T02:58:46Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T02:58:46Z- AHRS_M2 msg: 1730861926.58 epoch sec; seqNo:1313 2024-11-06T02:58:46Z- AHRS_M2 msg: 0.16 phi; 0.19 theta; 2.32 psi 2024-11-06T02:58:46Z- handleDepth()- Depth_Keller msg: 1730861926.61 epoch sec; seqNo:1404; depth 49.86 2024-11-06T02:58:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861926.63 epoch sec; seqNo:1404; 4061674.19 north; 594244.01 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T02:58:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861923.35 2024-11-06T02:58:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:46Z- waiting for 1730861927.35, time is 1730861926.63 2024-11-06T02:58:47Z- AHRS_M2 msg: 1730861926.99 epoch sec; seqNo:1314 2024-11-06T02:58:47Z- AHRS_M2 msg: 0.16 phi; 0.19 theta; 2.32 psi 2024-11-06T02:58:47Z- handleDepth()- Depth_Keller msg: 1730861927.00 epoch sec; seqNo:1405; depth 49.80 2024-11-06T02:58:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861927.01 epoch sec; seqNo:1405; 4061673.83 north; 594245.24 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T02:58:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861923.35 2024-11-06T02:58:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:47Z- waiting for 1730861927.35, time is 1730861927.01 2024-11-06T02:58:47Z- AHRS_M2 msg: 1730861927.40 epoch sec; seqNo:1315 2024-11-06T02:58:47Z- AHRS_M2 msg: 0.15 phi; 0.19 theta; 2.32 psi 2024-11-06T02:58:47Z- handleDepth()- Depth_Keller msg: 1730861927.40 epoch sec; seqNo:1406; depth 49.73 2024-11-06T02:58:47Z- handleDVL() - DVL msg epochMillisec: 1730861927.42 2024-11-06T02:58:47Z- handleDvl() - RDI_Pathfinder msg: 1730861927.42 sec; seqNo:431; #beams:4 2024-11-06T02:58:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.59 , 42.41 , 51.02 , 41.53 2024-11-06T02:58:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , -0.04 2024-11-06T02:58:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861927.44 epoch sec; seqNo:1406; 4061673.83 north; 594245.24 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T02:58:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861923.35 2024-11-06T02:58:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:47Z- waiting for 1730861927.35, time is 1730861927.44 2024-11-06T02:58:47Z- LcmTrn::updateTrn() >>>> pose time 1730861927.44, meas time 1730861927.42 <<<< 2024-11-06T02:58:47Z- LcmTrn::updateTrn() equating pose 1730861927.42 and meas times 1730861927.42 2024-11-06T02:58:47Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861927.42 2024-11-06T02:58:47Z- TerrainNav::motionUpdate() - currEstimate = (4061674, 594245.24, 49.73) 2024-11-06T02:58:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:58:47Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:58:47Z- TNavPF::Projecting Measurements 2024-11-06T02:58:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:58:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:58:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:58:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:58:47Z- TNavFilter::4 good beams. 2024-11-06T02:58:47Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:58:47Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:58:47Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:58:47Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:58:47Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:58:47Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:58:47Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:58:47Z- TerrainMapOctree: (vehN, vehE ) = (4061673.83, 594245.24). 2024-11-06T02:58:47Z- TerrainMapOctree: Min Distance = 76.32. 2024-11-06T02:58:47Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:58:47Z- Forcing Subcloud Comparison 2024-11-06T02:58:47Z- Weighting particles with subcloud comparison 2024-11-06T02:58:47Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:47Z- meanExpectedMeasDiff: -0.431253 delta_rms_squared: 1.136103 alpha: 0.982701 2024-11-06T02:58:47Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:47Z- meanExpectedMeasDiff: 0.281458 delta_rms_squared: 0.792910 alpha: 0.975397 2024-11-06T02:58:47Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:47Z- meanExpectedMeasDiff: 0.547620 delta_rms_squared: 1.204957 alpha: 0.983673 2024-11-06T02:58:47Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:47Z- meanExpectedMeasDiff: 0.388325 delta_rms_squared: 0.755621 alpha: 0.974214 2024-11-06T02:58:47Z- Subcloud NIS: 0.196342 2024-11-06T02:58:47Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:58:47Z- TNavPF:: sumWeights = 0.714243 2024-11-06T02:58:47Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:58:47Z- TNavPF::First Term of Map Covariance Matrix is 0.48 2024-11-06T02:58:47Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T02:58:47Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861927.42 sec, ping # 0. 2024-11-06T02:58:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:58:47Z- variances([0], [2], and psi_berg) = 0.318881 0.242889 0.000000 2024-11-06T02:58:47Z- publishEstimatesDecoder() - at 1730861927.42 sec 2024-11-06T02:58:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:58:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061704.44 594229.60 56.00 2024-11-06T02:58:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061706.17 594229.36 56.09 lat/lon: 36.70 -121.95 2024-11-06T02:58:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.32 0.24 0.38 0.00 2024-11-06T02:58:47Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T02:58:47Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T02:58:47Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T02:58:47Z- AHRS_M2 msg: 1730861927.80 epoch sec; seqNo:1316 2024-11-06T02:58:47Z- AHRS_M2 msg: 0.15 phi; 0.19 theta; 2.32 psi 2024-11-06T02:58:47Z- handleDepth()- Depth_Keller msg: 1730861927.83 epoch sec; seqNo:1407; depth 49.66 2024-11-06T02:58:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861927.87 epoch sec; seqNo:1407; 4061673.45 north; 594245.24 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T02:58:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861927.42 2024-11-06T02:58:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:47Z- waiting for 1730861931.42, time is 1730861927.87 2024-11-06T02:58:48Z- AHRS_M2 msg: 1730861928.23 epoch sec; seqNo:1317 2024-11-06T02:58:48Z- AHRS_M2 msg: 0.14 phi; 0.19 theta; 2.31 psi 2024-11-06T02:58:48Z- handleDepth()- Depth_Keller msg: 1730861928.23 epoch sec; seqNo:1408; depth 49.58 2024-11-06T02:58:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861928.24 epoch sec; seqNo:1408; 4061673.07 north; 594245.25 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T02:58:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861927.42 2024-11-06T02:58:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:48Z- waiting for 1730861931.42, time is 1730861928.24 2024-11-06T02:58:48Z- AHRS_M2 msg: 1730861928.63 epoch sec; seqNo:1318 2024-11-06T02:58:48Z- AHRS_M2 msg: 0.13 phi; 0.19 theta; 2.31 psi 2024-11-06T02:58:48Z- handleDepth()- Depth_Keller msg: 1730861928.64 epoch sec; seqNo:1409; depth 49.51 2024-11-06T02:58:48Z- handleDVL() - DVL msg epochMillisec: 1730861928.64 2024-11-06T02:58:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:58:48Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:58:48Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:58:48Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:58:48Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:58:48Z- handleDvl() - RDI_Pathfinder msg: 1730861928.64 sec; seqNo:432; #beams:0 2024-11-06T02:58:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:58:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , -0.04 2024-11-06T02:58:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861928.66 epoch sec; seqNo:1409; 4061673.08 north; 594246.47 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T02:58:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861927.42 2024-11-06T02:58:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:48Z- waiting for 1730861931.42, time is 1730861928.66 2024-11-06T02:58:49Z- AHRS_M2 msg: 1730861929.04 epoch sec; seqNo:1319 2024-11-06T02:58:49Z- AHRS_M2 msg: 0.12 phi; 0.19 theta; 2.31 psi 2024-11-06T02:58:49Z- handleDepth()- Depth_Keller msg: 1730861929.05 epoch sec; seqNo:1410; depth 49.44 2024-11-06T02:58:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861929.05 epoch sec; seqNo:1410; 4061672.71 north; 594246.47 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T02:58:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861927.42 2024-11-06T02:58:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:49Z- waiting for 1730861931.42, time is 1730861929.05 2024-11-06T02:58:49Z- AHRS_M2 msg: 1730861929.44 epoch sec; seqNo:1320 2024-11-06T02:58:49Z- AHRS_M2 msg: 0.12 phi; 0.19 theta; 2.31 psi 2024-11-06T02:58:49Z- handleDepth()- Depth_Keller msg: 1730861929.45 epoch sec; seqNo:1411; depth 49.35 2024-11-06T02:58:49Z- handleDVL() - DVL msg epochMillisec: 1730861929.48 2024-11-06T02:58:49Z- handleDvl() - RDI_Pathfinder msg: 1730861929.48 sec; seqNo:433; #beams:4 2024-11-06T02:58:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.93 , 43.07 , 50.58 , 41.53 2024-11-06T02:58:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , -0.06 2024-11-06T02:58:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861929.49 epoch sec; seqNo:1411; 4061672.33 north; 594246.48 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T02:58:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861927.42 2024-11-06T02:58:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:49Z- waiting for 1730861931.42, time is 1730861929.49 2024-11-06T02:58:49Z- AHRS_M2 msg: 1730861929.84 epoch sec; seqNo:1321 2024-11-06T02:58:49Z- AHRS_M2 msg: 0.12 phi; 0.18 theta; 2.31 psi 2024-11-06T02:58:49Z- handleDepth()- Depth_Keller msg: 1730861929.85 epoch sec; seqNo:1412; depth 49.27 2024-11-06T02:58:49Z- handleDVL() - DVL msg epochMillisec: 1730861929.85 2024-11-06T02:58:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:58:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:58:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:58:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:58:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:58:49Z- handleDvl() - RDI_Pathfinder msg: 1730861929.85 sec; seqNo:434; #beams:0 2024-11-06T02:58:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:58:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , -0.06 2024-11-06T02:58:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861929.87 epoch sec; seqNo:1412; 4061672.33 north; 594246.48 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T02:58:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861927.42 2024-11-06T02:58:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:50Z- waiting for 1730861931.42, time is 1730861929.87 2024-11-06T02:58:50Z- AHRS_M2 msg: 1730861930.26 epoch sec; seqNo:1322 2024-11-06T02:58:50Z- AHRS_M2 msg: 0.12 phi; 0.18 theta; 2.30 psi 2024-11-06T02:58:50Z- handleDepth()- Depth_Keller msg: 1730861930.27 epoch sec; seqNo:1413; depth 49.20 2024-11-06T02:58:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861930.28 epoch sec; seqNo:1413; 4061671.95 north; 594246.48 east; 36.70 lat; -121.94 lon; 49.20 depth; utmZone: 10 2024-11-06T02:58:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861927.42 2024-11-06T02:58:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:50Z- waiting for 1730861931.42, time is 1730861930.28 2024-11-06T02:58:50Z- AHRS_M2 msg: 1730861930.66 epoch sec; seqNo:1323 2024-11-06T02:58:50Z- AHRS_M2 msg: 0.12 phi; 0.17 theta; 2.30 psi 2024-11-06T02:58:50Z- handleDepth()- Depth_Keller msg: 1730861930.67 epoch sec; seqNo:1414; depth 49.14 2024-11-06T02:58:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861930.68 epoch sec; seqNo:1414; 4061671.58 north; 594247.70 east; 36.70 lat; -121.94 lon; 49.14 depth; utmZone: 10 2024-11-06T02:58:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861927.42 2024-11-06T02:58:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:50Z- waiting for 1730861931.42, time is 1730861930.68 2024-11-06T02:58:51Z- AHRS_M2 msg: 1730861931.06 epoch sec; seqNo:1324 2024-11-06T02:58:51Z- AHRS_M2 msg: 0.12 phi; 0.17 theta; 2.30 psi 2024-11-06T02:58:51Z- handleDepth()- Depth_Keller msg: 1730861931.07 epoch sec; seqNo:1415; depth 49.04 2024-11-06T02:58:51Z- handleDVL() - DVL msg epochMillisec: 1730861931.07 2024-11-06T02:58:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:58:51Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:58:51Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:58:51Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:58:51Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:58:51Z- handleDvl() - RDI_Pathfinder msg: 1730861931.07 sec; seqNo:435; #beams:0 2024-11-06T02:58:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:58:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , -0.06 2024-11-06T02:58:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861931.08 epoch sec; seqNo:1415; 4061671.58 north; 594247.70 east; 36.70 lat; -121.94 lon; 49.04 depth; utmZone: 10 2024-11-06T02:58:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861927.42 2024-11-06T02:58:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:51Z- waiting for 1730861931.42, time is 1730861931.08 2024-11-06T02:58:51Z- AHRS_M2 msg: 1730861931.46 epoch sec; seqNo:1325 2024-11-06T02:58:51Z- AHRS_M2 msg: 0.12 phi; 0.16 theta; 2.30 psi 2024-11-06T02:58:51Z- handleDepth()- Depth_Keller msg: 1730861931.47 epoch sec; seqNo:1416; depth 48.98 2024-11-06T02:58:51Z- handleDVL() - DVL msg epochMillisec: 1730861931.49 2024-11-06T02:58:51Z- handleDvl() - RDI_Pathfinder msg: 1730861931.49 sec; seqNo:436; #beams:4 2024-11-06T02:58:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.15 , 43.29 , 50.36 , 42.19 2024-11-06T02:58:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.08 , -0.03 2024-11-06T02:58:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861931.53 epoch sec; seqNo:1416; 4061671.20 north; 594247.71 east; 36.70 lat; -121.94 lon; 48.98 depth; utmZone: 10 2024-11-06T02:58:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861927.42 2024-11-06T02:58:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:51Z- waiting for 1730861931.42, time is 1730861931.53 2024-11-06T02:58:51Z- LcmTrn::updateTrn() >>>> pose time 1730861931.53, meas time 1730861931.49 <<<< 2024-11-06T02:58:51Z- LcmTrn::updateTrn() equating pose 1730861931.49 and meas times 1730861931.49 2024-11-06T02:58:51Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861931.49 2024-11-06T02:58:51Z- TerrainNav::motionUpdate() - currEstimate = (4061671, 594247.71, 48.98) 2024-11-06T02:58:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:58:51Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:58:51Z- TNavPF::Projecting Measurements 2024-11-06T02:58:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:58:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:58:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:58:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:58:51Z- TNavFilter::4 good beams. 2024-11-06T02:58:51Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:58:51Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:58:51Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:58:51Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:58:51Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:58:51Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:58:51Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:58:51Z- TerrainMapOctree: (vehN, vehE ) = (4061671.20, 594247.71). 2024-11-06T02:58:51Z- TerrainMapOctree: Min Distance = 78.83. 2024-11-06T02:58:51Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:58:51Z- Forcing Subcloud Comparison 2024-11-06T02:58:51Z- Weighting particles with subcloud comparison 2024-11-06T02:58:51Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:51Z- meanExpectedMeasDiff: -0.449892 delta_rms_squared: 0.985037 alpha: 0.980100 2024-11-06T02:58:51Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:51Z- meanExpectedMeasDiff: 0.795513 delta_rms_squared: 0.673426 alpha: 0.971158 2024-11-06T02:58:51Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:51Z- meanExpectedMeasDiff: 0.389265 delta_rms_squared: 1.041918 alpha: 0.981166 2024-11-06T02:58:51Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:51Z- meanExpectedMeasDiff: -0.150057 delta_rms_squared: 0.707220 alpha: 0.972498 2024-11-06T02:58:51Z- Subcloud NIS: 0.192928 2024-11-06T02:58:51Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:58:51Z- TNavPF:: sumWeights = 0.704183 2024-11-06T02:58:51Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:58:51Z- TNavPF::First Term of Map Covariance Matrix is 0.45 2024-11-06T02:58:51Z- TNavPF::Calculated NIS Value : 0.14 numBeams normalized NIS: 0.04 2024-11-06T02:58:51Z- TerrainNav::Resampling particle filter... 2024-11-06T02:58:51Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861931.49 sec, ping # 0. 2024-11-06T02:58:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:58:51Z- variances([0], [2], and psi_berg) = 0.325871 0.249924 0.000000 2024-11-06T02:58:51Z- publishEstimatesDecoder() - at 1730861931.49 sec 2024-11-06T02:58:51Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:58:51Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061702.74 594232.30 54.18 2024-11-06T02:58:51Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061703.57 594231.83 55.29 lat/lon: 36.70 -121.95 2024-11-06T02:58:51Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.25 0.38 0.00 2024-11-06T02:58:51Z- AHRS_M2 msg: 1730861931.87 epoch sec; seqNo:1326 2024-11-06T02:58:51Z- AHRS_M2 msg: 0.12 phi; 0.16 theta; 2.29 psi 2024-11-06T02:58:51Z- handleDepth()- Depth_Keller msg: 1730861931.88 epoch sec; seqNo:1417; depth 48.90 2024-11-06T02:58:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861931.89 epoch sec; seqNo:1417; 4061670.82 north; 594247.71 east; 36.70 lat; -121.94 lon; 48.90 depth; utmZone: 10 2024-11-06T02:58:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861931.49 2024-11-06T02:58:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:52Z- waiting for 1730861935.49, time is 1730861931.89 2024-11-06T02:58:52Z- AHRS_M2 msg: 1730861932.27 epoch sec; seqNo:1327 2024-11-06T02:58:52Z- AHRS_M2 msg: 0.12 phi; 0.15 theta; 2.29 psi 2024-11-06T02:58:52Z- handleDepth()- Depth_Keller msg: 1730861932.28 epoch sec; seqNo:1418; depth 48.84 2024-11-06T02:58:52Z- handleDVL() - DVL msg epochMillisec: 1730861932.28 2024-11-06T02:58:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:58:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:58:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:58:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:58:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:58:52Z- handleDvl() - RDI_Pathfinder msg: 1730861932.28 sec; seqNo:437; #beams:0 2024-11-06T02:58:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:58:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.08 , -0.03 2024-11-06T02:58:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861932.30 epoch sec; seqNo:1418; 4061670.82 north; 594247.71 east; 36.70 lat; -121.94 lon; 48.84 depth; utmZone: 10 2024-11-06T02:58:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861931.49 2024-11-06T02:58:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:52Z- waiting for 1730861935.49, time is 1730861932.30 2024-11-06T02:58:52Z- AHRS_M2 msg: 1730861932.68 epoch sec; seqNo:1328 2024-11-06T02:58:52Z- AHRS_M2 msg: 0.12 phi; 0.14 theta; 2.29 psi 2024-11-06T02:58:52Z- handleDepth()- Depth_Keller msg: 1730861932.68 epoch sec; seqNo:1419; depth 48.76 2024-11-06T02:58:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861932.69 epoch sec; seqNo:1419; 4061670.45 north; 594247.72 east; 36.70 lat; -121.94 lon; 48.76 depth; utmZone: 10 2024-11-06T02:58:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861931.49 2024-11-06T02:58:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:52Z- waiting for 1730861935.49, time is 1730861932.69 2024-11-06T02:58:53Z- AHRS_M2 msg: 1730861933.08 epoch sec; seqNo:1329 2024-11-06T02:58:53Z- AHRS_M2 msg: 0.12 phi; 0.14 theta; 2.28 psi 2024-11-06T02:58:53Z- handleDepth()- Depth_Keller msg: 1730861933.09 epoch sec; seqNo:1420; depth 48.72 2024-11-06T02:58:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861933.10 epoch sec; seqNo:1420; 4061670.46 north; 594248.94 east; 36.70 lat; -121.94 lon; 48.72 depth; utmZone: 10 2024-11-06T02:58:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861931.49 2024-11-06T02:58:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:53Z- waiting for 1730861935.49, time is 1730861933.10 2024-11-06T02:58:53Z- handleDepth()- Depth_Keller msg: 1730861933.50 epoch sec; seqNo:1421; depth 48.65 2024-11-06T02:58:53Z- handleDVL() - DVL msg epochMillisec: 1730861933.52 2024-11-06T02:58:53Z- handleDvl() - RDI_Pathfinder msg: 1730861933.52 sec; seqNo:438; #beams:4 2024-11-06T02:58:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 43.07 , 49.26 , 42.63 2024-11-06T02:58:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.09 , 0.02 2024-11-06T02:58:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861933.53 epoch sec; seqNo:1421; 4061670.08 north; 594248.94 east; 36.70 lat; -121.94 lon; 48.65 depth; utmZone: 10 2024-11-06T02:58:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861931.49 2024-11-06T02:58:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:53Z- waiting for 1730861935.49, time is 1730861933.53 2024-11-06T02:58:53Z- AHRS_M2 msg: 1730861933.89 epoch sec; seqNo:1330 2024-11-06T02:58:53Z- AHRS_M2 msg: 0.11 phi; 0.12 theta; 2.27 psi 2024-11-06T02:58:53Z- handleDepth()- Depth_Keller msg: 1730861933.90 epoch sec; seqNo:1422; depth 48.58 2024-11-06T02:58:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861933.92 epoch sec; seqNo:1422; 4061669.70 north; 594248.95 east; 36.70 lat; -121.94 lon; 48.58 depth; utmZone: 10 2024-11-06T02:58:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861931.49 2024-11-06T02:58:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:54Z- waiting for 1730861935.49, time is 1730861933.92 2024-11-06T02:58:54Z- AHRS_M2 msg: 1730861934.29 epoch sec; seqNo:1331 2024-11-06T02:58:54Z- AHRS_M2 msg: 0.11 phi; 0.12 theta; 2.27 psi 2024-11-06T02:58:54Z- handleDepth()- Depth_Keller msg: 1730861934.30 epoch sec; seqNo:1423; depth 48.52 2024-11-06T02:58:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861934.31 epoch sec; seqNo:1423; 4061669.70 north; 594248.95 east; 36.70 lat; -121.94 lon; 48.52 depth; utmZone: 10 2024-11-06T02:58:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861931.49 2024-11-06T02:58:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:54Z- waiting for 1730861935.49, time is 1730861934.31 2024-11-06T02:58:54Z- AHRS_M2 msg: 1730861934.70 epoch sec; seqNo:1332 2024-11-06T02:58:54Z- AHRS_M2 msg: 0.11 phi; 0.11 theta; 2.26 psi 2024-11-06T02:58:54Z- handleDepth()- Depth_Keller msg: 1730861934.70 epoch sec; seqNo:1424; depth 48.48 2024-11-06T02:58:54Z- handleDVL() - DVL msg epochMillisec: 1730861934.71 2024-11-06T02:58:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:58:54Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:58:54Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:58:54Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:58:54Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:58:54Z- handleDvl() - RDI_Pathfinder msg: 1730861934.71 sec; seqNo:439; #beams:0 2024-11-06T02:58:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:58:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.09 , 0.02 2024-11-06T02:58:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861934.72 epoch sec; seqNo:1424; 4061669.32 north; 594248.95 east; 36.70 lat; -121.94 lon; 48.48 depth; utmZone: 10 2024-11-06T02:58:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861931.49 2024-11-06T02:58:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:54Z- waiting for 1730861935.49, time is 1730861934.72 2024-11-06T02:58:55Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T02:58:55Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T02:58:55Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T02:58:55Z- AHRS_M2 msg: 1730861935.09 epoch sec; seqNo:1333 2024-11-06T02:58:55Z- AHRS_M2 msg: 0.11 phi; 0.11 theta; 2.26 psi 2024-11-06T02:58:55Z- handleDepth()- Depth_Keller msg: 1730861935.13 epoch sec; seqNo:1425; depth 48.43 2024-11-06T02:58:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861935.14 epoch sec; seqNo:1425; 4061668.96 north; 594250.17 east; 36.70 lat; -121.94 lon; 48.43 depth; utmZone: 10 2024-11-06T02:58:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861931.49 2024-11-06T02:58:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:55Z- waiting for 1730861935.49, time is 1730861935.14 2024-11-06T02:58:55Z- AHRS_M2 msg: 1730861935.51 epoch sec; seqNo:1334 2024-11-06T02:58:55Z- AHRS_M2 msg: 0.11 phi; 0.10 theta; 2.26 psi 2024-11-06T02:58:55Z- handleDepth()- Depth_Keller msg: 1730861935.52 epoch sec; seqNo:1426; depth 48.38 2024-11-06T02:58:55Z- handleDVL() - DVL msg epochMillisec: 1730861935.55 2024-11-06T02:58:55Z- handleDvl() - RDI_Pathfinder msg: 1730861935.55 sec; seqNo:440; #beams:4 2024-11-06T02:58:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 43.51 , 48.59 , 43.51 2024-11-06T02:58:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.07 , 0.06 2024-11-06T02:58:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861935.56 epoch sec; seqNo:1426; 4061668.96 north; 594250.17 east; 36.70 lat; -121.94 lon; 48.38 depth; utmZone: 10 2024-11-06T02:58:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861931.49 2024-11-06T02:58:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:55Z- waiting for 1730861935.49, time is 1730861935.56 2024-11-06T02:58:55Z- LcmTrn::updateTrn() >>>> pose time 1730861935.56, meas time 1730861935.55 <<<< 2024-11-06T02:58:55Z- LcmTrn::updateTrn() equating pose 1730861935.55 and meas times 1730861935.55 2024-11-06T02:58:55Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861935.55 2024-11-06T02:58:55Z- TerrainNav::motionUpdate() - currEstimate = (4061669, 594250.17, 48.38) 2024-11-06T02:58:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:58:55Z- TerrainNav:: SubcloudNIS is 0.19, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:58:55Z- TNavPF::Projecting Measurements 2024-11-06T02:58:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:58:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:58:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:58:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:58:55Z- TNavFilter::4 good beams. 2024-11-06T02:58:55Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:58:55Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:58:55Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:58:55Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:58:55Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:58:55Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:58:55Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:58:55Z- TerrainMapOctree: (vehN, vehE ) = (4061668.96, 594250.17). 2024-11-06T02:58:55Z- TerrainMapOctree: Min Distance = 81.04. 2024-11-06T02:58:55Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:58:55Z- Forcing Subcloud Comparison 2024-11-06T02:58:55Z- Weighting particles with subcloud comparison 2024-11-06T02:58:55Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:55Z- meanExpectedMeasDiff: -1.038077 delta_rms_squared: 0.977462 alpha: 0.979949 2024-11-06T02:58:55Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:55Z- meanExpectedMeasDiff: 0.785457 delta_rms_squared: 0.741406 alpha: 0.973733 2024-11-06T02:58:55Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:55Z- meanExpectedMeasDiff: 0.286227 delta_rms_squared: 0.959704 alpha: 0.979586 2024-11-06T02:58:55Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:55Z- meanExpectedMeasDiff: -0.658702 delta_rms_squared: 0.760755 alpha: 0.974384 2024-11-06T02:58:55Z- Subcloud NIS: 0.193479 2024-11-06T02:58:55Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:58:55Z- TNavPF:: sumWeights = 0.629502 2024-11-06T02:58:55Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:58:55Z- TNavPF::First Term of Map Covariance Matrix is 0.45 2024-11-06T02:58:55Z- TNavPF::Calculated NIS Value : 0.24 numBeams normalized NIS: 0.06 2024-11-06T02:58:55Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861935.55 sec, ping # 0. 2024-11-06T02:58:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:58:55Z- variances([0], [2], and psi_berg) = 0.331043 0.251449 0.000000 2024-11-06T02:58:55Z- publishEstimatesDecoder() - at 1730861935.55 sec 2024-11-06T02:58:55Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:58:55Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061703.19 594233.40 54.95 2024-11-06T02:58:55Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061701.32 594234.30 54.75 lat/lon: 36.70 -121.95 2024-11-06T02:58:55Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.25 0.38 0.00 2024-11-06T02:58:55Z- AHRS_M2 msg: 1730861935.91 epoch sec; seqNo:1335 2024-11-06T02:58:55Z- AHRS_M2 msg: 0.11 phi; 0.09 theta; 2.25 psi 2024-11-06T02:58:55Z- handleDepth()- Depth_Keller msg: 1730861935.91 epoch sec; seqNo:1427; depth 48.34 2024-11-06T02:58:55Z- handleDVL() - DVL msg epochMillisec: 1730861935.92 2024-11-06T02:58:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:58:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:58:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:58:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:58:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:58:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:58:55Z- handleDvl() - RDI_Pathfinder msg: 1730861935.92 sec; seqNo:441; #beams:0 2024-11-06T02:58:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:58:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.07 , 0.06 2024-11-06T02:58:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861935.95 epoch sec; seqNo:1427; 4061668.58 north; 594250.18 east; 36.70 lat; -121.94 lon; 48.34 depth; utmZone: 10 2024-11-06T02:58:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861935.55 2024-11-06T02:58:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:56Z- waiting for 1730861939.55, time is 1730861935.95 2024-11-06T02:58:56Z- AHRS_M2 msg: 1730861936.31 epoch sec; seqNo:1336 2024-11-06T02:58:56Z- AHRS_M2 msg: 0.11 phi; 0.08 theta; 2.25 psi 2024-11-06T02:58:56Z- handleDepth()- Depth_Keller msg: 1730861936.32 epoch sec; seqNo:1428; depth 48.29 2024-11-06T02:58:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861936.33 epoch sec; seqNo:1428; 4061668.58 north; 594250.18 east; 36.70 lat; -121.94 lon; 48.29 depth; utmZone: 10 2024-11-06T02:58:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861935.55 2024-11-06T02:58:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:56Z- waiting for 1730861939.55, time is 1730861936.33 2024-11-06T02:58:56Z- AHRS_M2 msg: 1730861936.71 epoch sec; seqNo:1337 2024-11-06T02:58:56Z- AHRS_M2 msg: 0.10 phi; 0.08 theta; 2.24 psi 2024-11-06T02:58:56Z- handleDepth()- Depth_Keller msg: 1730861936.73 epoch sec; seqNo:1429; depth 48.25 2024-11-06T02:58:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861936.73 epoch sec; seqNo:1429; 4061668.20 north; 594250.18 east; 36.70 lat; -121.94 lon; 48.25 depth; utmZone: 10 2024-11-06T02:58:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861935.55 2024-11-06T02:58:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:56Z- waiting for 1730861939.55, time is 1730861936.73 2024-11-06T02:58:57Z- AHRS_M2 msg: 1730861937.12 epoch sec; seqNo:1338 2024-11-06T02:58:57Z- AHRS_M2 msg: 0.09 phi; 0.07 theta; 2.23 psi 2024-11-06T02:58:57Z- handleDepth()- Depth_Keller msg: 1730861937.12 epoch sec; seqNo:1430; depth 48.21 2024-11-06T02:58:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861937.13 epoch sec; seqNo:1430; 4061667.83 north; 594251.41 east; 36.70 lat; -121.94 lon; 48.21 depth; utmZone: 10 2024-11-06T02:58:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861935.55 2024-11-06T02:58:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:57Z- waiting for 1730861939.55, time is 1730861937.13 2024-11-06T02:58:57Z- AHRS_M2 msg: 1730861937.52 epoch sec; seqNo:1339 2024-11-06T02:58:57Z- AHRS_M2 msg: 0.09 phi; 0.06 theta; 2.23 psi 2024-11-06T02:58:57Z- handleDepth()- Depth_Keller msg: 1730861937.53 epoch sec; seqNo:1431; depth 48.18 2024-11-06T02:58:57Z- handleDVL() - DVL msg epochMillisec: 1730861937.57 2024-11-06T02:58:57Z- handleDvl() - RDI_Pathfinder msg: 1730861937.57 sec; seqNo:442; #beams:4 2024-11-06T02:58:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 43.73 , 47.49 , 44.17 2024-11-06T02:58:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , 0.07 2024-11-06T02:58:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861937.58 epoch sec; seqNo:1431; 4061667.83 north; 594251.41 east; 36.70 lat; -121.94 lon; 48.18 depth; utmZone: 10 2024-11-06T02:58:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861935.55 2024-11-06T02:58:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:57Z- waiting for 1730861939.55, time is 1730861937.58 2024-11-06T02:58:57Z- AHRS_M2 msg: 1730861937.93 epoch sec; seqNo:1340 2024-11-06T02:58:57Z- AHRS_M2 msg: 0.09 phi; 0.06 theta; 2.22 psi 2024-11-06T02:58:57Z- handleDepth()- Depth_Keller msg: 1730861937.93 epoch sec; seqNo:1432; depth 48.14 2024-11-06T02:58:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861937.95 epoch sec; seqNo:1432; 4061667.45 north; 594251.41 east; 36.70 lat; -121.94 lon; 48.14 depth; utmZone: 10 2024-11-06T02:58:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861935.55 2024-11-06T02:58:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:58Z- waiting for 1730861939.55, time is 1730861937.95 2024-11-06T02:58:58Z- handleDepth()- Depth_Keller msg: 1730861938.36 epoch sec; seqNo:1433; depth 48.11 2024-11-06T02:58:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861938.37 epoch sec; seqNo:1433; 4061667.45 north; 594251.41 east; 36.70 lat; -121.94 lon; 48.11 depth; utmZone: 10 2024-11-06T02:58:58Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:58:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861935.55 2024-11-06T02:58:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:58Z- waiting for 1730861939.55, time is 1730861938.37 2024-11-06T02:58:58Z- AHRS_M2 msg: 1730861938.73 epoch sec; seqNo:1341 2024-11-06T02:58:58Z- AHRS_M2 msg: 0.07 phi; 0.05 theta; 2.20 psi 2024-11-06T02:58:58Z- handleDepth()- Depth_Keller msg: 1730861938.74 epoch sec; seqNo:1434; depth 48.10 2024-11-06T02:58:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861938.76 epoch sec; seqNo:1434; 4061667.09 north; 594252.64 east; 36.70 lat; -121.94 lon; 48.10 depth; utmZone: 10 2024-11-06T02:58:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861935.55 2024-11-06T02:58:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:58Z- waiting for 1730861939.55, time is 1730861938.76 2024-11-06T02:58:59Z- AHRS_M2 msg: 1730861939.14 epoch sec; seqNo:1342 2024-11-06T02:58:59Z- AHRS_M2 msg: 0.07 phi; 0.04 theta; 2.20 psi 2024-11-06T02:58:59Z- handleDepth()- Depth_Keller msg: 1730861939.14 epoch sec; seqNo:1435; depth 48.05 2024-11-06T02:58:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861939.15 epoch sec; seqNo:1435; 4061666.71 north; 594252.64 east; 36.70 lat; -121.94 lon; 48.05 depth; utmZone: 10 2024-11-06T02:58:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:58:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861935.55 2024-11-06T02:58:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:59Z- waiting for 1730861939.55, time is 1730861939.15 2024-11-06T02:58:59Z- AHRS_M2 msg: 1730861939.54 epoch sec; seqNo:1343 2024-11-06T02:58:59Z- AHRS_M2 msg: 0.06 phi; 0.03 theta; 2.19 psi 2024-11-06T02:58:59Z- handleDepth()- Depth_Keller msg: 1730861939.55 epoch sec; seqNo:1436; depth 48.03 2024-11-06T02:58:59Z- handleDVL() - DVL msg epochMillisec: 1730861939.60 2024-11-06T02:58:59Z- handleDvl() - RDI_Pathfinder msg: 1730861939.60 sec; seqNo:443; #beams:4 2024-11-06T02:58:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.83 , 44.40 , 46.83 , 45.06 2024-11-06T02:58:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , 0.04 2024-11-06T02:58:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861939.61 epoch sec; seqNo:1436; 4061666.71 north; 594252.64 east; 36.70 lat; -121.94 lon; 48.03 depth; utmZone: 10 2024-11-06T02:58:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:58:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861935.55 2024-11-06T02:58:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:58:59Z- waiting for 1730861939.55, time is 1730861939.61 2024-11-06T02:58:59Z- LcmTrn::updateTrn() >>>> pose time 1730861939.61, meas time 1730861939.60 <<<< 2024-11-06T02:58:59Z- LcmTrn::updateTrn() equating pose 1730861939.60 and meas times 1730861939.60 2024-11-06T02:58:59Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861939.60 2024-11-06T02:58:59Z- TerrainNav::motionUpdate() - currEstimate = (4061667, 594252.64, 48.03) 2024-11-06T02:58:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:58:59Z- TerrainNav:: SubcloudNIS is 0.19, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:58:59Z- TNavPF::Projecting Measurements 2024-11-06T02:58:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:58:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:58:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:58:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:58:59Z- TNavFilter::4 good beams. 2024-11-06T02:58:59Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:58:59Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:58:59Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:58:59Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:58:59Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:58:59Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:58:59Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:58:59Z- TerrainMapOctree: (vehN, vehE ) = (4061666.71, 594252.64). 2024-11-06T02:58:59Z- TerrainMapOctree: Min Distance = 83.33. 2024-11-06T02:58:59Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:58:59Z- Forcing Subcloud Comparison 2024-11-06T02:58:59Z- Weighting particles with subcloud comparison 2024-11-06T02:58:59Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:59Z- meanExpectedMeasDiff: -0.215117 delta_rms_squared: 0.877029 alpha: 0.977704 2024-11-06T02:58:59Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:59Z- meanExpectedMeasDiff: 0.418404 delta_rms_squared: 0.765257 alpha: 0.974531 2024-11-06T02:58:59Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:59Z- meanExpectedMeasDiff: 0.441967 delta_rms_squared: 0.852421 alpha: 0.977075 2024-11-06T02:58:59Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:58:59Z- meanExpectedMeasDiff: -0.643528 delta_rms_squared: 0.786206 alpha: 0.975192 2024-11-06T02:58:59Z- Subcloud NIS: 0.191352 2024-11-06T02:58:59Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:58:59Z- TNavPF:: sumWeights = 0.721291 2024-11-06T02:58:59Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:58:59Z- TNavPF::First Term of Map Covariance Matrix is 0.39 2024-11-06T02:58:59Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T02:58:59Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861939.60 sec, ping # 0. 2024-11-06T02:58:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:58:59Z- variances([0], [2], and psi_berg) = 0.333531 0.252886 0.000000 2024-11-06T02:58:59Z- publishEstimatesDecoder() - at 1730861939.60 sec 2024-11-06T02:58:59Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:58:59Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061699.72 594238.28 54.71 2024-11-06T02:58:59Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061699.07 594236.76 54.41 lat/lon: 36.70 -121.95 2024-11-06T02:58:59Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.25 0.38 0.00 2024-11-06T02:58:59Z- AHRS_M2 msg: 1730861939.95 epoch sec; seqNo:1344 2024-11-06T02:58:59Z- AHRS_M2 msg: 0.05 phi; 0.03 theta; 2.18 psi 2024-11-06T02:58:59Z- handleDepth()- Depth_Keller msg: 1730861939.96 epoch sec; seqNo:1437; depth 48.00 2024-11-06T02:58:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861939.97 epoch sec; seqNo:1437; 4061666.33 north; 594252.64 east; 36.70 lat; -121.94 lon; 48.00 depth; utmZone: 10 2024-11-06T02:59:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861939.60 2024-11-06T02:59:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:00Z- waiting for 1730861943.60, time is 1730861939.97 2024-11-06T02:59:00Z- AHRS_M2 msg: 1730861940.35 epoch sec; seqNo:1345 2024-11-06T02:59:00Z- AHRS_M2 msg: 0.05 phi; 0.02 theta; 2.17 psi 2024-11-06T02:59:00Z- handleDepth()- Depth_Keller msg: 1730861940.38 epoch sec; seqNo:1438; depth 47.99 2024-11-06T02:59:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861940.39 epoch sec; seqNo:1438; 4061666.33 north; 594252.64 east; 36.70 lat; -121.94 lon; 47.99 depth; utmZone: 10 2024-11-06T02:59:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861939.60 2024-11-06T02:59:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:00Z- waiting for 1730861943.60, time is 1730861940.39 2024-11-06T02:59:00Z- handleDepth()- Depth_Keller msg: 1730861940.77 epoch sec; seqNo:1439; depth 47.95 2024-11-06T02:59:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861940.79 epoch sec; seqNo:1439; 4061665.97 north; 594253.87 east; 36.70 lat; -121.94 lon; 47.95 depth; utmZone: 10 2024-11-06T02:59:00Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:59:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861939.60 2024-11-06T02:59:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:00Z- waiting for 1730861943.60, time is 1730861940.79 2024-11-06T02:59:01Z- AHRS_M2 msg: 1730861941.16 epoch sec; seqNo:1346 2024-11-06T02:59:01Z- AHRS_M2 msg: 0.03 phi; 0.01 theta; 2.14 psi 2024-11-06T02:59:01Z- handleDepth()- Depth_Keller msg: 1730861941.16 epoch sec; seqNo:1440; depth 47.95 2024-11-06T02:59:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861941.17 epoch sec; seqNo:1440; 4061665.97 north; 594253.87 east; 36.70 lat; -121.94 lon; 47.95 depth; utmZone: 10 2024-11-06T02:59:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861939.60 2024-11-06T02:59:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:01Z- waiting for 1730861943.60, time is 1730861941.17 2024-11-06T02:59:01Z- AHRS_M2 msg: 1730861941.57 epoch sec; seqNo:1347 2024-11-06T02:59:01Z- AHRS_M2 msg: 0.03 phi; 0.01 theta; 2.13 psi 2024-11-06T02:59:01Z- handleDepth()- Depth_Keller msg: 1730861941.57 epoch sec; seqNo:1441; depth 47.92 2024-11-06T02:59:01Z- handleDVL() - DVL msg epochMillisec: 1730861941.69 2024-11-06T02:59:01Z- handleDvl() - RDI_Pathfinder msg: 1730861941.69 sec; seqNo:444; #beams:4 2024-11-06T02:59:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.16 , 45.28 , 45.94 , 45.72 2024-11-06T02:59:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , 0.01 2024-11-06T02:59:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861941.70 epoch sec; seqNo:1441; 4061665.59 north; 594253.87 east; 36.70 lat; -121.94 lon; 47.92 depth; utmZone: 10 2024-11-06T02:59:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861939.60 2024-11-06T02:59:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:01Z- waiting for 1730861943.60, time is 1730861941.70 2024-11-06T02:59:01Z- AHRS_M2 msg: 1730861941.97 epoch sec; seqNo:1348 2024-11-06T02:59:01Z- AHRS_M2 msg: 0.03 phi; 0.00 theta; 2.12 psi 2024-11-06T02:59:01Z- handleDepth()- Depth_Keller msg: 1730861941.97 epoch sec; seqNo:1442; depth 47.89 2024-11-06T02:59:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861941.99 epoch sec; seqNo:1442; 4061665.21 north; 594253.88 east; 36.70 lat; -121.94 lon; 47.89 depth; utmZone: 10 2024-11-06T02:59:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861939.60 2024-11-06T02:59:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:02Z- waiting for 1730861943.60, time is 1730861941.99 2024-11-06T02:59:02Z- AHRS_M2 msg: 1730861942.37 epoch sec; seqNo:1349 2024-11-06T02:59:02Z- AHRS_M2 msg: 0.02 phi; -0.00 theta; 2.11 psi 2024-11-06T02:59:02Z- handleDepth()- Depth_Keller msg: 1730861942.38 epoch sec; seqNo:1443; depth 47.88 2024-11-06T02:59:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861942.39 epoch sec; seqNo:1443; 4061665.22 north; 594255.10 east; 36.70 lat; -121.94 lon; 47.88 depth; utmZone: 10 2024-11-06T02:59:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861939.60 2024-11-06T02:59:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:02Z- waiting for 1730861943.60, time is 1730861942.39 2024-11-06T02:59:02Z- AHRS_M2 msg: 1730861942.78 epoch sec; seqNo:1350 2024-11-06T02:59:02Z- AHRS_M2 msg: 0.03 phi; -0.01 theta; 2.10 psi 2024-11-06T02:59:02Z- handleDepth()- Depth_Keller msg: 1730861942.78 epoch sec; seqNo:1444; depth 47.86 2024-11-06T02:59:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861942.79 epoch sec; seqNo:1444; 4061664.84 north; 594255.10 east; 36.70 lat; -121.94 lon; 47.86 depth; utmZone: 10 2024-11-06T02:59:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861939.60 2024-11-06T02:59:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:02Z- waiting for 1730861943.60, time is 1730861942.79 2024-11-06T02:59:03Z- AHRS_M2 msg: 1730861943.18 epoch sec; seqNo:1351 2024-11-06T02:59:03Z- AHRS_M2 msg: 0.03 phi; -0.01 theta; 2.09 psi 2024-11-06T02:59:03Z- handleDepth()- Depth_Keller msg: 1730861943.18 epoch sec; seqNo:1445; depth 47.85 2024-11-06T02:59:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861943.19 epoch sec; seqNo:1445; 4061664.84 north; 594255.10 east; 36.70 lat; -121.94 lon; 47.85 depth; utmZone: 10 2024-11-06T02:59:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861939.60 2024-11-06T02:59:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:03Z- waiting for 1730861943.60, time is 1730861943.19 2024-11-06T02:59:03Z- AHRS_M2 msg: 1730861943.58 epoch sec; seqNo:1352 2024-11-06T02:59:03Z- AHRS_M2 msg: 0.03 phi; -0.02 theta; 2.08 psi 2024-11-06T02:59:03Z- handleDepth()- Depth_Keller msg: 1730861943.59 epoch sec; seqNo:1446; depth 47.84 2024-11-06T02:59:03Z- handleDVL() - DVL msg epochMillisec: 1730861943.62 2024-11-06T02:59:03Z- handleDvl() - RDI_Pathfinder msg: 1730861943.62 sec; seqNo:445; #beams:4 2024-11-06T02:59:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.50 , 45.72 , 45.28 , 46.60 2024-11-06T02:59:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.05 , -0.01 2024-11-06T02:59:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861943.64 epoch sec; seqNo:1446; 4061664.48 north; 594256.33 east; 36.70 lat; -121.94 lon; 47.84 depth; utmZone: 10 2024-11-06T02:59:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861939.60 2024-11-06T02:59:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:03Z- waiting for 1730861943.60, time is 1730861943.64 2024-11-06T02:59:03Z- LcmTrn::updateTrn() >>>> pose time 1730861943.64, meas time 1730861943.62 <<<< 2024-11-06T02:59:03Z- LcmTrn::updateTrn() equating pose 1730861943.62 and meas times 1730861943.62 2024-11-06T02:59:03Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861943.62 2024-11-06T02:59:03Z- TerrainNav::motionUpdate() - currEstimate = (4061664, 594256.33, 47.84) 2024-11-06T02:59:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:59:03Z- TerrainNav:: SubcloudNIS is 0.19, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:59:03Z- TNavPF::Projecting Measurements 2024-11-06T02:59:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:59:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:59:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:59:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:59:03Z- TNavFilter::4 good beams. 2024-11-06T02:59:03Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:59:03Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:59:03Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:59:03Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:59:03Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:59:03Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:59:03Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:59:03Z- TerrainMapOctree: (vehN, vehE ) = (4061664.48, 594256.33). 2024-11-06T02:59:03Z- TerrainMapOctree: Min Distance = 85.76. 2024-11-06T02:59:03Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:59:03Z- Forcing Subcloud Comparison 2024-11-06T02:59:03Z- Weighting particles with subcloud comparison 2024-11-06T02:59:03Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:03Z- meanExpectedMeasDiff: -0.862320 delta_rms_squared: 0.871197 alpha: 0.977558 2024-11-06T02:59:03Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:03Z- meanExpectedMeasDiff: 0.876514 delta_rms_squared: 0.814445 alpha: 0.976032 2024-11-06T02:59:03Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:03Z- meanExpectedMeasDiff: 0.082953 delta_rms_squared: 0.827422 alpha: 0.976399 2024-11-06T02:59:03Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:03Z- meanExpectedMeasDiff: -0.553948 delta_rms_squared: 0.857194 alpha: 0.977200 2024-11-06T02:59:03Z- Subcloud NIS: 0.193598 2024-11-06T02:59:03Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:59:03Z- TNavPF:: sumWeights = 0.652227 2024-11-06T02:59:03Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:59:03Z- TNavPF::First Term of Map Covariance Matrix is 0.41 2024-11-06T02:59:03Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.05 2024-11-06T02:59:03Z- TerrainNav::Resampling particle filter... 2024-11-06T02:59:03Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730861943.62 sec, ping # 0. 2024-11-06T02:59:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:59:03Z- variances([0], [2], and psi_berg) = 0.334471 0.254325 0.000000 2024-11-06T02:59:03Z- publishEstimatesDecoder() - at 1730861943.62 sec 2024-11-06T02:59:03Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:59:03Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061696.44 594240.61 54.92 2024-11-06T02:59:03Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061696.84 594240.45 54.25 lat/lon: 36.70 -121.95 2024-11-06T02:59:03Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.25 0.40 0.00 2024-11-06T02:59:04Z- AHRS_M2 msg: 1730861944.01 epoch sec; seqNo:1353 2024-11-06T02:59:04Z- AHRS_M2 msg: 0.03 phi; -0.02 theta; 2.07 psi 2024-11-06T02:59:04Z- handleDepth()- Depth_Keller msg: 1730861944.02 epoch sec; seqNo:1447; depth 47.82 2024-11-06T02:59:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861944.04 epoch sec; seqNo:1447; 4061664.48 north; 594256.33 east; 36.70 lat; -121.94 lon; 47.82 depth; utmZone: 10 2024-11-06T02:59:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861943.62 2024-11-06T02:59:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:04Z- waiting for 1730861947.62, time is 1730861944.04 2024-11-06T02:59:04Z- handleDepth()- Depth_Keller msg: 1730861944.41 epoch sec; seqNo:1448; depth 47.81 2024-11-06T02:59:04Z- handleDVL() - DVL msg epochMillisec: 1730861944.42 2024-11-06T02:59:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:59:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:59:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:59:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:59:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:59:04Z- handleDvl() - RDI_Pathfinder msg: 1730861944.42 sec; seqNo:446; #beams:0 2024-11-06T02:59:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:59:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.05 , -0.01 2024-11-06T02:59:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861944.42 epoch sec; seqNo:1448; 4061664.10 north; 594256.33 east; 36.70 lat; -121.94 lon; 47.81 depth; utmZone: 10 2024-11-06T02:59:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861943.62 2024-11-06T02:59:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:04Z- waiting for 1730861947.62, time is 1730861944.42 2024-11-06T02:59:04Z- AHRS_M2 msg: 1730861944.79 epoch sec; seqNo:1354 2024-11-06T02:59:04Z- AHRS_M2 msg: 0.02 phi; -0.03 theta; 2.05 psi 2024-11-06T02:59:04Z- handleDepth()- Depth_Keller msg: 1730861944.81 epoch sec; seqNo:1449; depth 47.80 2024-11-06T02:59:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861944.82 epoch sec; seqNo:1449; 4061664.10 north; 594256.33 east; 36.70 lat; -121.94 lon; 47.80 depth; utmZone: 10 2024-11-06T02:59:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861943.62 2024-11-06T02:59:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:04Z- waiting for 1730861947.62, time is 1730861944.82 2024-11-06T02:59:05Z- AHRS_M2 msg: 1730861945.20 epoch sec; seqNo:1355 2024-11-06T02:59:05Z- AHRS_M2 msg: 0.02 phi; -0.03 theta; 2.04 psi 2024-11-06T02:59:05Z- handleDepth()- Depth_Keller msg: 1730861945.21 epoch sec; seqNo:1450; depth 47.80 2024-11-06T02:59:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861945.25 epoch sec; seqNo:1450; 4061663.73 north; 594257.56 east; 36.70 lat; -121.94 lon; 47.80 depth; utmZone: 10 2024-11-06T02:59:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861943.62 2024-11-06T02:59:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:05Z- waiting for 1730861947.62, time is 1730861945.25 2024-11-06T02:59:05Z- AHRS_M2 msg: 1730861945.61 epoch sec; seqNo:1356 2024-11-06T02:59:05Z- AHRS_M2 msg: 0.02 phi; -0.03 theta; 2.02 psi 2024-11-06T02:59:05Z- handleDepth()- Depth_Keller msg: 1730861945.61 epoch sec; seqNo:1451; depth 47.79 2024-11-06T02:59:05Z- handleDVL() - DVL msg epochMillisec: 1730861945.68 2024-11-06T02:59:05Z- handleDvl() - RDI_Pathfinder msg: 1730861945.68 sec; seqNo:447; #beams:4 2024-11-06T02:59:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.72 , 45.50 , 44.84 , 46.83 2024-11-06T02:59:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.06 , -0.03 2024-11-06T02:59:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861945.70 epoch sec; seqNo:1451; 4061663.73 north; 594257.56 east; 36.70 lat; -121.94 lon; 47.79 depth; utmZone: 10 2024-11-06T02:59:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861943.62 2024-11-06T02:59:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:05Z- waiting for 1730861947.62, time is 1730861945.70 2024-11-06T02:59:06Z- handleDepth()- Depth_Keller msg: 1730861946.04 epoch sec; seqNo:1452; depth 47.78 2024-11-06T02:59:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861946.06 epoch sec; seqNo:1452; 4061663.35 north; 594257.56 east; 36.70 lat; -121.94 lon; 47.78 depth; utmZone: 10 2024-11-06T02:59:06Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:59:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861943.62 2024-11-06T02:59:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:06Z- waiting for 1730861947.62, time is 1730861946.06 2024-11-06T02:59:06Z- AHRS_M2 msg: 1730861946.41 epoch sec; seqNo:1357 2024-11-06T02:59:06Z- AHRS_M2 msg: 0.02 phi; -0.04 theta; 2.00 psi 2024-11-06T02:59:06Z- handleDepth()- Depth_Keller msg: 1730861946.42 epoch sec; seqNo:1453; depth 47.78 2024-11-06T02:59:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861946.43 epoch sec; seqNo:1453; 4061663.35 north; 594257.56 east; 36.70 lat; -121.94 lon; 47.78 depth; utmZone: 10 2024-11-06T02:59:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861943.62 2024-11-06T02:59:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:06Z- waiting for 1730861947.62, time is 1730861946.43 2024-11-06T02:59:06Z- AHRS_M2 msg: 1730861946.82 epoch sec; seqNo:1358 2024-11-06T02:59:06Z- AHRS_M2 msg: 0.02 phi; -0.05 theta; 1.99 psi 2024-11-06T02:59:06Z- handleDepth()- Depth_Keller msg: 1730861946.84 epoch sec; seqNo:1454; depth 47.77 2024-11-06T02:59:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861946.85 epoch sec; seqNo:1454; 4061662.99 north; 594258.78 east; 36.70 lat; -121.94 lon; 47.77 depth; utmZone: 10 2024-11-06T02:59:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861943.62 2024-11-06T02:59:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:06Z- waiting for 1730861947.62, time is 1730861946.85 2024-11-06T02:59:07Z- AHRS_M2 msg: 1730861947.29 epoch sec; seqNo:1359 2024-11-06T02:59:07Z- AHRS_M2 msg: 0.02 phi; -0.05 theta; 1.98 psi 2024-11-06T02:59:07Z- handleDepth()- Depth_Keller msg: 1730861947.30 epoch sec; seqNo:1455; depth 47.77 2024-11-06T02:59:07Z- handleDVL() - DVL msg epochMillisec: 1730861947.32 2024-11-06T02:59:07Z- handleDvl() - RDI_Pathfinder msg: 1730861947.32 sec; seqNo:448; #beams:4 2024-11-06T02:59:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.50 , 45.72 , 44.17 , 47.27 2024-11-06T02:59:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.07 , -0.04 2024-11-06T02:59:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861947.34 epoch sec; seqNo:1455; 4061662.99 north; 594258.78 east; 36.70 lat; -121.94 lon; 47.77 depth; utmZone: 10 2024-11-06T02:59:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861943.62 2024-11-06T02:59:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:07Z- waiting for 1730861947.62, time is 1730861947.34 2024-11-06T02:59:07Z- AHRS_M2 msg: 1730861947.62 epoch sec; seqNo:1360 2024-11-06T02:59:07Z- AHRS_M2 msg: 0.02 phi; -0.05 theta; 1.97 psi 2024-11-06T02:59:07Z- handleDepth()- Depth_Keller msg: 1730861947.63 epoch sec; seqNo:1456; depth 47.78 2024-11-06T02:59:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861947.65 epoch sec; seqNo:1456; 4061662.99 north; 594258.78 east; 36.70 lat; -121.94 lon; 47.78 depth; utmZone: 10 2024-11-06T02:59:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861943.62 2024-11-06T02:59:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:07Z- waiting for 1730861947.62, time is 1730861947.65 2024-11-06T02:59:07Z- LcmTrn::updateTrn() >>>> pose time 1730861947.65, meas time 1730861947.32 <<<< 2024-11-06T02:59:07Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861947.65 2024-11-06T02:59:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861947.32 sec, ping # 0; waiting for INS data more recent than 1730861943.62... 2024-11-06T02:59:07Z- TerrainNav::motionUpdate() - currEstimate = (4061663, 594258.78, 47.78) 2024-11-06T02:59:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:59:07Z- TerrainNav:: SubcloudNIS is 0.19, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:59:07Z- TNavPF::Projecting Measurements 2024-11-06T02:59:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:59:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:59:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:59:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:59:07Z- TNavFilter::4 good beams. 2024-11-06T02:59:07Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:59:07Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:59:07Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:59:07Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:59:07Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:59:07Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:59:07Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:59:07Z- TerrainMapOctree: (vehN, vehE ) = (4061663.11, 594258.58). 2024-11-06T02:59:07Z- TerrainMapOctree: Min Distance = 87.31. 2024-11-06T02:59:07Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:59:07Z- Forcing Subcloud Comparison 2024-11-06T02:59:07Z- Weighting particles with subcloud comparison 2024-11-06T02:59:07Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:07Z- meanExpectedMeasDiff: -0.736070 delta_rms_squared: 0.872778 alpha: 0.977598 2024-11-06T02:59:07Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:07Z- meanExpectedMeasDiff: 0.610008 delta_rms_squared: 0.828409 alpha: 0.976426 2024-11-06T02:59:07Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:07Z- meanExpectedMeasDiff: -0.181525 delta_rms_squared: 0.800563 alpha: 0.975626 2024-11-06T02:59:07Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:07Z- meanExpectedMeasDiff: -0.980316 delta_rms_squared: 0.902250 alpha: 0.978314 2024-11-06T02:59:07Z- Subcloud NIS: 0.195335 2024-11-06T02:59:07Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:59:07Z- TNavPF:: sumWeights = 0.658984 2024-11-06T02:59:07Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:59:07Z- TNavPF::First Term of Map Covariance Matrix is 0.37 2024-11-06T02:59:07Z- TNavPF::Calculated NIS Value : 0.19 numBeams normalized NIS: 0.05 2024-11-06T02:59:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861947.32 sec. 2024-11-06T02:59:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:59:07Z- variances([0], [2], and psi_berg) = 0.334154 0.254700 0.000000 2024-11-06T02:59:07Z- publishEstimatesDecoder() - at 1730861947.65 sec 2024-11-06T02:59:07Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:59:07Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061693.69 594243.27 54.46 2024-11-06T02:59:07Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061695.35 594242.90 54.30 lat/lon: 36.70 -121.95 2024-11-06T02:59:07Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.25 0.64 0.00 2024-11-06T02:59:08Z- AHRS_M2 msg: 1730861948.03 epoch sec; seqNo:1361 2024-11-06T02:59:08Z- AHRS_M2 msg: 0.02 phi; -0.06 theta; 1.95 psi 2024-11-06T02:59:08Z- handleDepth()- Depth_Keller msg: 1730861948.04 epoch sec; seqNo:1457; depth 47.79 2024-11-06T02:59:08Z- handleDVL() - DVL msg epochMillisec: 1730861948.04 2024-11-06T02:59:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:59:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:59:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:59:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:59:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:59:08Z- handleDvl() - RDI_Pathfinder msg: 1730861948.04 sec; seqNo:449; #beams:0 2024-11-06T02:59:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:59:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.07 , -0.04 2024-11-06T02:59:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861948.05 epoch sec; seqNo:1457; 4061662.61 north; 594258.79 east; 36.70 lat; -121.94 lon; 47.79 depth; utmZone: 10 2024-11-06T02:59:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861947.65 2024-11-06T02:59:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:08Z- waiting for 1730861951.65, time is 1730861948.05 2024-11-06T02:59:08Z- AHRS_M2 msg: 1730861948.43 epoch sec; seqNo:1362 2024-11-06T02:59:08Z- AHRS_M2 msg: 0.02 phi; -0.06 theta; 1.94 psi 2024-11-06T02:59:08Z- handleDepth()- Depth_Keller msg: 1730861948.44 epoch sec; seqNo:1458; depth 47.80 2024-11-06T02:59:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861948.45 epoch sec; seqNo:1458; 4061662.62 north; 594260.01 east; 36.70 lat; -121.94 lon; 47.80 depth; utmZone: 10 2024-11-06T02:59:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861947.65 2024-11-06T02:59:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:08Z- waiting for 1730861951.65, time is 1730861948.45 2024-11-06T02:59:08Z- AHRS_M2 msg: 1730861948.84 epoch sec; seqNo:1363 2024-11-06T02:59:08Z- AHRS_M2 msg: 0.03 phi; -0.06 theta; 1.93 psi 2024-11-06T02:59:08Z- handleDepth()- Depth_Keller msg: 1730861948.84 epoch sec; seqNo:1459; depth 47.81 2024-11-06T02:59:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861948.85 epoch sec; seqNo:1459; 4061662.25 north; 594260.01 east; 36.70 lat; -121.94 lon; 47.81 depth; utmZone: 10 2024-11-06T02:59:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861947.65 2024-11-06T02:59:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:08Z- waiting for 1730861951.65, time is 1730861948.85 2024-11-06T02:59:09Z- AHRS_M2 msg: 1730861949.28 epoch sec; seqNo:1364 2024-11-06T02:59:09Z- AHRS_M2 msg: 0.03 phi; -0.07 theta; 1.90 psi 2024-11-06T02:59:09Z- handleDepth()- Depth_Keller msg: 1730861949.29 epoch sec; seqNo:1460; depth 47.82 2024-11-06T02:59:09Z- handleDVL() - DVL msg epochMillisec: 1730861949.31 2024-11-06T02:59:09Z- handleDvl() - RDI_Pathfinder msg: 1730861949.31 sec; seqNo:450; #beams:4 2024-11-06T02:59:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.06 , 45.28 , 43.73 , 47.71 2024-11-06T02:59:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.05 , -0.01 2024-11-06T02:59:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861949.32 epoch sec; seqNo:1460; 4061662.25 north; 594260.01 east; 36.70 lat; -121.94 lon; 47.82 depth; utmZone: 10 2024-11-06T02:59:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861947.65 2024-11-06T02:59:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:09Z- waiting for 1730861951.65, time is 1730861949.32 2024-11-06T02:59:09Z- handleDepth()- Depth_Keller msg: 1730861949.68 epoch sec; seqNo:1461; depth 47.84 2024-11-06T02:59:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861949.70 epoch sec; seqNo:1461; 4061662.25 north; 594260.01 east; 36.70 lat; -121.94 lon; 47.84 depth; utmZone: 10 2024-11-06T02:59:09Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:59:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861947.65 2024-11-06T02:59:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:09Z- waiting for 1730861951.65, time is 1730861949.70 2024-11-06T02:59:10Z- AHRS_M2 msg: 1730861950.05 epoch sec; seqNo:1365 2024-11-06T02:59:10Z- AHRS_M2 msg: 0.03 phi; -0.07 theta; 1.88 psi 2024-11-06T02:59:10Z- handleDepth()- Depth_Keller msg: 1730861950.06 epoch sec; seqNo:1462; depth 47.86 2024-11-06T02:59:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861950.09 epoch sec; seqNo:1462; 4061662.25 north; 594260.01 east; 36.70 lat; -121.94 lon; 47.86 depth; utmZone: 10 2024-11-06T02:59:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861947.65 2024-11-06T02:59:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:10Z- waiting for 1730861951.65, time is 1730861950.09 2024-11-06T02:59:10Z- AHRS_M2 msg: 1730861950.45 epoch sec; seqNo:1366 2024-11-06T02:59:10Z- AHRS_M2 msg: 0.02 phi; -0.07 theta; 1.87 psi 2024-11-06T02:59:10Z- handleDepth()- Depth_Keller msg: 1730861950.46 epoch sec; seqNo:1463; depth 47.88 2024-11-06T02:59:10Z- handleDVL() - DVL msg epochMillisec: 1730861950.46 2024-11-06T02:59:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:59:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:59:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:59:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:59:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:59:10Z- handleDvl() - RDI_Pathfinder msg: 1730861950.46 sec; seqNo:451; #beams:0 2024-11-06T02:59:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:59:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.05 , -0.01 2024-11-06T02:59:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861950.47 epoch sec; seqNo:1463; 4061661.88 north; 594261.24 east; 36.70 lat; -121.94 lon; 47.88 depth; utmZone: 10 2024-11-06T02:59:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861947.65 2024-11-06T02:59:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:10Z- waiting for 1730861951.65, time is 1730861950.47 2024-11-06T02:59:10Z- AHRS_M2 msg: 1730861950.87 epoch sec; seqNo:1367 2024-11-06T02:59:10Z- AHRS_M2 msg: 0.02 phi; -0.08 theta; 1.84 psi 2024-11-06T02:59:10Z- handleDepth()- Depth_Keller msg: 1730861950.87 epoch sec; seqNo:1464; depth 47.90 2024-11-06T02:59:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861950.88 epoch sec; seqNo:1464; 4061661.88 north; 594261.24 east; 36.70 lat; -121.94 lon; 47.90 depth; utmZone: 10 2024-11-06T02:59:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861947.65 2024-11-06T02:59:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:11Z- waiting for 1730861951.65, time is 1730861950.88 2024-11-06T02:59:11Z- AHRS_M2 msg: 1730861951.29 epoch sec; seqNo:1368 2024-11-06T02:59:11Z- AHRS_M2 msg: 0.02 phi; -0.08 theta; 1.83 psi 2024-11-06T02:59:11Z- handleDepth()- Depth_Keller msg: 1730861951.29 epoch sec; seqNo:1465; depth 47.92 2024-11-06T02:59:11Z- handleDVL() - DVL msg epochMillisec: 1730861951.31 2024-11-06T02:59:11Z- handleDvl() - RDI_Pathfinder msg: 1730861951.31 sec; seqNo:452; #beams:4 2024-11-06T02:59:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.06 , 45.50 , 43.29 , 47.93 2024-11-06T02:59:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.03 , 0.05 2024-11-06T02:59:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861951.34 epoch sec; seqNo:1465; 4061661.88 north; 594261.24 east; 36.70 lat; -121.94 lon; 47.92 depth; utmZone: 10 2024-11-06T02:59:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861947.65 2024-11-06T02:59:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:11Z- waiting for 1730861951.65, time is 1730861951.34 2024-11-06T02:59:11Z- AHRS_M2 msg: 1730861951.67 epoch sec; seqNo:1369 2024-11-06T02:59:11Z- AHRS_M2 msg: 0.02 phi; -0.08 theta; 1.81 psi 2024-11-06T02:59:11Z- handleDepth()- Depth_Keller msg: 1730861951.68 epoch sec; seqNo:1466; depth 47.95 2024-11-06T02:59:11Z- handleDVL() - DVL msg epochMillisec: 1730861951.68 2024-11-06T02:59:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:59:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:59:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:59:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:59:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:59:11Z- handleDvl() - RDI_Pathfinder msg: 1730861951.68 sec; seqNo:453; #beams:0 2024-11-06T02:59:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:59:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.03 , 0.05 2024-11-06T02:59:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861951.71 epoch sec; seqNo:1466; 4061661.88 north; 594261.24 east; 36.70 lat; -121.94 lon; 47.95 depth; utmZone: 10 2024-11-06T02:59:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861947.65 2024-11-06T02:59:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:11Z- waiting for 1730861951.65, time is 1730861951.71 2024-11-06T02:59:11Z- LcmTrn::updateTrn() >>>> pose time 1730861951.71, meas time 1730861951.68 <<<< 2024-11-06T02:59:11Z- LcmTrn::updateTrn() equating pose 1730861951.68 and meas times 1730861951.68 2024-11-06T02:59:11Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861951.68 2024-11-06T02:59:11Z- TerrainNav::motionUpdate() - currEstimate = (4061662, 594261.24, 47.95) 2024-11-06T02:59:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:59:11Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:59:11Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:59:11Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:59:11Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:59:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:59:11Z- variances([0], [2], and psi_berg) = 0.335299 0.256320 0.000000 2024-11-06T02:59:11Z- publishEstimatesDecoder() - at 1730861951.68 sec 2024-11-06T02:59:11Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:59:11Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061692.64 594245.76 54.90 2024-11-06T02:59:11Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061694.24 594245.36 54.47 lat/lon: 36.70 -121.94 2024-11-06T02:59:11Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.34 0.26 0.88 0.00 2024-11-06T02:59:12Z- AHRS_M2 msg: 1730861952.07 epoch sec; seqNo:1370 2024-11-06T02:59:12Z- AHRS_M2 msg: 0.02 phi; -0.08 theta; 1.80 psi 2024-11-06T02:59:12Z- handleDepth()- Depth_Keller msg: 1730861952.07 epoch sec; seqNo:1467; depth 47.97 2024-11-06T02:59:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861952.08 epoch sec; seqNo:1467; 4061661.51 north; 594262.46 east; 36.70 lat; -121.94 lon; 47.97 depth; utmZone: 10 2024-11-06T02:59:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861951.68 2024-11-06T02:59:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:12Z- waiting for 1730861955.68, time is 1730861952.08 2024-11-06T02:59:12Z- AHRS_M2 msg: 1730861952.47 epoch sec; seqNo:1371 2024-11-06T02:59:12Z- AHRS_M2 msg: 0.02 phi; -0.08 theta; 1.79 psi 2024-11-06T02:59:12Z- handleDepth()- Depth_Keller msg: 1730861952.47 epoch sec; seqNo:1468; depth 47.99 2024-11-06T02:59:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861952.48 epoch sec; seqNo:1468; 4061661.51 north; 594262.46 east; 36.70 lat; -121.94 lon; 47.99 depth; utmZone: 10 2024-11-06T02:59:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861951.68 2024-11-06T02:59:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:12Z- waiting for 1730861955.68, time is 1730861952.48 2024-11-06T02:59:12Z- AHRS_M2 msg: 1730861952.87 epoch sec; seqNo:1372 2024-11-06T02:59:12Z- AHRS_M2 msg: 0.03 phi; -0.09 theta; 1.78 psi 2024-11-06T02:59:12Z- handleDepth()- Depth_Keller msg: 1730861952.88 epoch sec; seqNo:1469; depth 48.02 2024-11-06T02:59:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861952.89 epoch sec; seqNo:1469; 4061661.51 north; 594262.46 east; 36.70 lat; -121.94 lon; 48.02 depth; utmZone: 10 2024-11-06T02:59:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861951.68 2024-11-06T02:59:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:13Z- waiting for 1730861955.68, time is 1730861952.89 2024-11-06T02:59:13Z- AHRS_M2 msg: 1730861953.29 epoch sec; seqNo:1373 2024-11-06T02:59:13Z- AHRS_M2 msg: 0.03 phi; -0.09 theta; 1.77 psi 2024-11-06T02:59:13Z- handleDepth()- Depth_Keller msg: 1730861953.30 epoch sec; seqNo:1470; depth 48.05 2024-11-06T02:59:13Z- handleDVL() - DVL msg epochMillisec: 1730861953.32 2024-11-06T02:59:13Z- handleDvl() - RDI_Pathfinder msg: 1730861953.32 sec; seqNo:454; #beams:4 2024-11-06T02:59:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.28 , 45.50 , 43.07 , 48.59 2024-11-06T02:59:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.03 , 0.08 2024-11-06T02:59:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861953.34 epoch sec; seqNo:1470; 4061661.51 north; 594262.46 east; 36.70 lat; -121.94 lon; 48.05 depth; utmZone: 10 2024-11-06T02:59:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861951.68 2024-11-06T02:59:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:13Z- waiting for 1730861955.68, time is 1730861953.34 2024-11-06T02:59:13Z- AHRS_M2 msg: 1730861953.69 epoch sec; seqNo:1374 2024-11-06T02:59:13Z- AHRS_M2 msg: 0.03 phi; -0.09 theta; 1.76 psi 2024-11-06T02:59:13Z- handleDepth()- Depth_Keller msg: 1730861953.70 epoch sec; seqNo:1471; depth 48.08 2024-11-06T02:59:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861953.71 epoch sec; seqNo:1471; 4061661.15 north; 594263.69 east; 36.70 lat; -121.94 lon; 48.08 depth; utmZone: 10 2024-11-06T02:59:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861951.68 2024-11-06T02:59:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:13Z- waiting for 1730861955.68, time is 1730861953.71 2024-11-06T02:59:14Z- handleDepth()- Depth_Keller msg: 1730861954.10 epoch sec; seqNo:1472; depth 48.12 2024-11-06T02:59:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861954.12 epoch sec; seqNo:1472; 4061661.15 north; 594263.69 east; 36.70 lat; -121.94 lon; 48.12 depth; utmZone: 10 2024-11-06T02:59:14Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:59:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861951.68 2024-11-06T02:59:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:14Z- waiting for 1730861955.68, time is 1730861954.12 2024-11-06T02:59:14Z- AHRS_M2 msg: 1730861954.49 epoch sec; seqNo:1375 2024-11-06T02:59:14Z- AHRS_M2 msg: 0.04 phi; -0.09 theta; 1.74 psi 2024-11-06T02:59:14Z- handleDepth()- Depth_Keller msg: 1730861954.50 epoch sec; seqNo:1473; depth 48.14 2024-11-06T02:59:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861954.51 epoch sec; seqNo:1473; 4061661.15 north; 594263.69 east; 36.70 lat; -121.94 lon; 48.14 depth; utmZone: 10 2024-11-06T02:59:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861951.68 2024-11-06T02:59:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:14Z- waiting for 1730861955.68, time is 1730861954.51 2024-11-06T02:59:14Z- AHRS_M2 msg: 1730861954.90 epoch sec; seqNo:1376 2024-11-06T02:59:14Z- AHRS_M2 msg: 0.04 phi; -0.10 theta; 1.74 psi 2024-11-06T02:59:14Z- handleDepth()- Depth_Keller msg: 1730861954.91 epoch sec; seqNo:1474; depth 48.17 2024-11-06T02:59:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861954.91 epoch sec; seqNo:1474; 4061661.15 north; 594263.69 east; 36.70 lat; -121.94 lon; 48.17 depth; utmZone: 10 2024-11-06T02:59:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861951.68 2024-11-06T02:59:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:15Z- waiting for 1730861955.68, time is 1730861954.91 2024-11-06T02:59:15Z- AHRS_M2 msg: 1730861955.30 epoch sec; seqNo:1377 2024-11-06T02:59:15Z- AHRS_M2 msg: 0.04 phi; -0.10 theta; 1.73 psi 2024-11-06T02:59:15Z- handleDepth()- Depth_Keller msg: 1730861955.32 epoch sec; seqNo:1475; depth 48.22 2024-11-06T02:59:15Z- handleDVL() - DVL msg epochMillisec: 1730861955.34 2024-11-06T02:59:15Z- handleDvl() - RDI_Pathfinder msg: 1730861955.34 sec; seqNo:455; #beams:4 2024-11-06T02:59:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.94 , 44.84 , 42.85 , 49.03 2024-11-06T02:59:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , 0.04 2024-11-06T02:59:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861955.35 epoch sec; seqNo:1475; 4061661.16 north; 594264.91 east; 36.70 lat; -121.94 lon; 48.22 depth; utmZone: 10 2024-11-06T02:59:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861951.68 2024-11-06T02:59:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:15Z- waiting for 1730861955.68, time is 1730861955.35 2024-11-06T02:59:15Z- AHRS_M2 msg: 1730861955.70 epoch sec; seqNo:1378 2024-11-06T02:59:15Z- AHRS_M2 msg: 0.05 phi; -0.10 theta; 1.72 psi 2024-11-06T02:59:15Z- handleDepth()- Depth_Keller msg: 1730861955.71 epoch sec; seqNo:1476; depth 48.22 2024-11-06T02:59:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861955.73 epoch sec; seqNo:1476; 4061661.16 north; 594264.91 east; 36.70 lat; -121.94 lon; 48.22 depth; utmZone: 10 2024-11-06T02:59:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861951.68 2024-11-06T02:59:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:15Z- waiting for 1730861955.68, time is 1730861955.73 2024-11-06T02:59:15Z- LcmTrn::updateTrn() >>>> pose time 1730861955.73, meas time 1730861955.34 <<<< 2024-11-06T02:59:15Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861955.73 2024-11-06T02:59:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861955.34 sec, ping # 0; waiting for INS data more recent than 1730861951.68... 2024-11-06T02:59:15Z- TerrainNav::motionUpdate() - currEstimate = (4061661, 594264.91, 48.22) 2024-11-06T02:59:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:59:15Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:59:15Z- TNavPF::Projecting Measurements 2024-11-06T02:59:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:59:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:59:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:59:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:59:15Z- TNavFilter::4 good beams. 2024-11-06T02:59:15Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:59:15Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:59:15Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:59:15Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:59:15Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:59:15Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:59:15Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:59:15Z- TerrainMapOctree: (vehN, vehE ) = (4061661.23, 594264.55). 2024-11-06T02:59:15Z- TerrainMapOctree: Min Distance = 89.95. 2024-11-06T02:59:15Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:59:15Z- Forcing Subcloud Comparison 2024-11-06T02:59:15Z- Weighting particles with subcloud comparison 2024-11-06T02:59:15Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:15Z- meanExpectedMeasDiff: -0.903648 delta_rms_squared: 2.430594 alpha: 0.991839 2024-11-06T02:59:15Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:16Z- meanExpectedMeasDiff: 0.672946 delta_rms_squared: 1.420487 alpha: 0.986116 2024-11-06T02:59:16Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:16Z- meanExpectedMeasDiff: 1.053811 delta_rms_squared: 1.212552 alpha: 0.983774 2024-11-06T02:59:16Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:16Z- meanExpectedMeasDiff: -0.172087 delta_rms_squared: 1.678009 alpha: 0.988221 2024-11-06T02:59:16Z- Subcloud NIS: 0.203044 2024-11-06T02:59:16Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:59:16Z- TNavPF:: sumWeights = 0.509362 2024-11-06T02:59:16Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:59:16Z- TNavPF::First Term of Map Covariance Matrix is 0.70 2024-11-06T02:59:16Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T02:59:16Z- TerrainNav::Resampling particle filter... 2024-11-06T02:59:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861955.34 sec. 2024-11-06T02:59:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:59:16Z- variances([0], [2], and psi_berg) = 0.342186 0.264416 0.000000 2024-11-06T02:59:16Z- publishEstimatesDecoder() - at 1730861955.73 sec 2024-11-06T02:59:16Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:59:16Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061693.13 594249.18 57.74 2024-11-06T02:59:16Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061693.52 594249.06 54.69 lat/lon: 36.70 -121.94 2024-11-06T02:59:16Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.34 0.26 0.76 0.00 2024-11-06T02:59:16Z- handleDepth()- Depth_Keller msg: 1730861956.13 epoch sec; seqNo:1477; depth 48.26 2024-11-06T02:59:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861956.15 epoch sec; seqNo:1477; 4061660.78 north; 594264.91 east; 36.70 lat; -121.94 lon; 48.26 depth; utmZone: 10 2024-11-06T02:59:16Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:59:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861955.73 2024-11-06T02:59:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:16Z- waiting for 1730861959.73, time is 1730861956.15 2024-11-06T02:59:16Z- AHRS_M2 msg: 1730861956.51 epoch sec; seqNo:1379 2024-11-06T02:59:16Z- AHRS_M2 msg: 0.05 phi; -0.10 theta; 1.71 psi 2024-11-06T02:59:16Z- handleDepth()- Depth_Keller msg: 1730861956.52 epoch sec; seqNo:1478; depth 48.30 2024-11-06T02:59:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861956.53 epoch sec; seqNo:1478; 4061660.80 north; 594266.13 east; 36.70 lat; -121.94 lon; 48.30 depth; utmZone: 10 2024-11-06T02:59:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861955.73 2024-11-06T02:59:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:16Z- waiting for 1730861959.73, time is 1730861956.53 2024-11-06T02:59:16Z- AHRS_M2 msg: 1730861956.91 epoch sec; seqNo:1380 2024-11-06T02:59:16Z- AHRS_M2 msg: 0.05 phi; -0.11 theta; 1.70 psi 2024-11-06T02:59:16Z- handleDepth()- Depth_Keller msg: 1730861956.92 epoch sec; seqNo:1479; depth 48.32 2024-11-06T02:59:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861956.93 epoch sec; seqNo:1479; 4061660.80 north; 594266.13 east; 36.70 lat; -121.94 lon; 48.32 depth; utmZone: 10 2024-11-06T02:59:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861955.73 2024-11-06T02:59:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:17Z- waiting for 1730861959.73, time is 1730861956.93 2024-11-06T02:59:17Z- handleDepth()- Depth_Keller msg: 1730861957.34 epoch sec; seqNo:1480; depth 48.35 2024-11-06T02:59:17Z- handleDVL() - DVL msg epochMillisec: 1730861957.36 2024-11-06T02:59:17Z- handleDvl() - RDI_Pathfinder msg: 1730861957.36 sec; seqNo:456; #beams:4 2024-11-06T02:59:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.94 , 44.40 , 42.63 , 48.81 2024-11-06T02:59:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , -0.02 2024-11-06T02:59:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861957.37 epoch sec; seqNo:1480; 4061660.80 north; 594266.13 east; 36.70 lat; -121.94 lon; 48.35 depth; utmZone: 10 2024-11-06T02:59:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861955.73 2024-11-06T02:59:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:17Z- waiting for 1730861959.73, time is 1730861957.37 2024-11-06T02:59:17Z- AHRS_M2 msg: 1730861957.73 epoch sec; seqNo:1381 2024-11-06T02:59:17Z- AHRS_M2 msg: 0.06 phi; -0.11 theta; 1.69 psi 2024-11-06T02:59:17Z- handleDepth()- Depth_Keller msg: 1730861957.74 epoch sec; seqNo:1481; depth 48.38 2024-11-06T02:59:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861957.76 epoch sec; seqNo:1481; 4061660.81 north; 594267.35 east; 36.70 lat; -121.94 lon; 48.38 depth; utmZone: 10 2024-11-06T02:59:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861955.73 2024-11-06T02:59:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:17Z- waiting for 1730861959.73, time is 1730861957.76 2024-11-06T02:59:18Z- AHRS_M2 msg: 1730861958.13 epoch sec; seqNo:1382 2024-11-06T02:59:18Z- AHRS_M2 msg: 0.06 phi; -0.11 theta; 1.68 psi 2024-11-06T02:59:18Z- handleDepth()- Depth_Keller msg: 1730861958.13 epoch sec; seqNo:1482; depth 48.41 2024-11-06T02:59:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861958.15 epoch sec; seqNo:1482; 4061660.81 north; 594267.35 east; 36.70 lat; -121.94 lon; 48.41 depth; utmZone: 10 2024-11-06T02:59:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861955.73 2024-11-06T02:59:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:18Z- waiting for 1730861959.73, time is 1730861958.15 2024-11-06T02:59:18Z- AHRS_M2 msg: 1730861958.53 epoch sec; seqNo:1383 2024-11-06T02:59:18Z- AHRS_M2 msg: 0.06 phi; -0.11 theta; 1.67 psi 2024-11-06T02:59:18Z- handleDepth()- Depth_Keller msg: 1730861958.54 epoch sec; seqNo:1483; depth 48.43 2024-11-06T02:59:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861958.54 epoch sec; seqNo:1483; 4061660.43 north; 594267.36 east; 36.70 lat; -121.94 lon; 48.43 depth; utmZone: 10 2024-11-06T02:59:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861955.73 2024-11-06T02:59:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:18Z- waiting for 1730861959.73, time is 1730861958.54 2024-11-06T02:59:18Z- handleDepth()- Depth_Keller msg: 1730861958.96 epoch sec; seqNo:1484; depth 48.45 2024-11-06T02:59:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861958.98 epoch sec; seqNo:1484; 4061660.43 north; 594267.36 east; 36.70 lat; -121.94 lon; 48.45 depth; utmZone: 10 2024-11-06T02:59:19Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:59:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861955.73 2024-11-06T02:59:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:19Z- waiting for 1730861959.73, time is 1730861958.98 2024-11-06T02:59:19Z- AHRS_M2 msg: 1730861959.34 epoch sec; seqNo:1384 2024-11-06T02:59:19Z- AHRS_M2 msg: 0.06 phi; -0.11 theta; 1.66 psi 2024-11-06T02:59:19Z- handleDepth()- Depth_Keller msg: 1730861959.35 epoch sec; seqNo:1485; depth 48.49 2024-11-06T02:59:19Z- handleDVL() - DVL msg epochMillisec: 1730861959.37 2024-11-06T02:59:19Z- handleDvl() - RDI_Pathfinder msg: 1730861959.37 sec; seqNo:457; #beams:4 2024-11-06T02:59:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.16 , 43.96 , 42.19 , 48.59 2024-11-06T02:59:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.05 , -0.03 2024-11-06T02:59:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861959.38 epoch sec; seqNo:1485; 4061660.44 north; 594268.58 east; 36.70 lat; -121.94 lon; 48.49 depth; utmZone: 10 2024-11-06T02:59:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861955.73 2024-11-06T02:59:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:19Z- waiting for 1730861959.73, time is 1730861959.38 2024-11-06T02:59:19Z- AHRS_M2 msg: 1730861959.74 epoch sec; seqNo:1385 2024-11-06T02:59:19Z- AHRS_M2 msg: 0.06 phi; -0.11 theta; 1.65 psi 2024-11-06T02:59:19Z- handleDepth()- Depth_Keller msg: 1730861959.75 epoch sec; seqNo:1486; depth 48.52 2024-11-06T02:59:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861959.77 epoch sec; seqNo:1486; 4061660.44 north; 594268.58 east; 36.70 lat; -121.94 lon; 48.52 depth; utmZone: 10 2024-11-06T02:59:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861955.73 2024-11-06T02:59:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:19Z- waiting for 1730861959.73, time is 1730861959.77 2024-11-06T02:59:19Z- LcmTrn::updateTrn() >>>> pose time 1730861959.77, meas time 1730861959.37 <<<< 2024-11-06T02:59:19Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861959.77 2024-11-06T02:59:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861959.37 sec, ping # 0; waiting for INS data more recent than 1730861955.73... 2024-11-06T02:59:19Z- TerrainNav::motionUpdate() - currEstimate = (4061660, 594268.58, 48.52) 2024-11-06T02:59:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:59:19Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:59:19Z- TNavPF::Projecting Measurements 2024-11-06T02:59:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:59:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:59:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:59:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:59:19Z- TNavFilter::4 good beams. 2024-11-06T02:59:19Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:59:19Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:59:19Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:59:19Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:59:19Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:59:19Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:59:19Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:59:19Z- TerrainMapOctree: (vehN, vehE ) = (4061660.52, 594268.21). 2024-11-06T02:59:19Z- TerrainMapOctree: Min Distance = 91.32. 2024-11-06T02:59:19Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:59:20Z- Forcing Subcloud Comparison 2024-11-06T02:59:20Z- Weighting particles with subcloud comparison 2024-11-06T02:59:20Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:20Z- meanExpectedMeasDiff: -0.632678 delta_rms_squared: 1.548005 alpha: 0.987245 2024-11-06T02:59:20Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:20Z- meanExpectedMeasDiff: 0.461907 delta_rms_squared: 1.275537 alpha: 0.984562 2024-11-06T02:59:20Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:20Z- meanExpectedMeasDiff: 0.864192 delta_rms_squared: 1.212824 alpha: 0.983777 2024-11-06T02:59:20Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:20Z- meanExpectedMeasDiff: -0.736982 delta_rms_squared: 1.561540 alpha: 0.987354 2024-11-06T02:59:20Z- Subcloud NIS: 0.201817 2024-11-06T02:59:20Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:59:20Z- TNavPF:: sumWeights = 0.573205 2024-11-06T02:59:20Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:59:20Z- TNavPF::First Term of Map Covariance Matrix is 0.45 2024-11-06T02:59:20Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.05 2024-11-06T02:59:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861959.37 sec. 2024-11-06T02:59:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:59:20Z- variances([0], [2], and psi_berg) = 0.344182 0.264502 0.000000 2024-11-06T02:59:20Z- publishEstimatesDecoder() - at 1730861959.77 sec 2024-11-06T02:59:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:59:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061691.58 594252.50 55.09 2024-11-06T02:59:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061692.80 594252.73 54.97 lat/lon: 36.70 -121.94 2024-11-06T02:59:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.34 0.26 0.72 0.00 2024-11-06T02:59:20Z- AHRS_M2 msg: 1730861960.16 epoch sec; seqNo:1386 2024-11-06T02:59:20Z- AHRS_M2 msg: 0.07 phi; -0.11 theta; 1.64 psi 2024-11-06T02:59:20Z- handleDepth()- Depth_Keller msg: 1730861960.18 epoch sec; seqNo:1487; depth 48.54 2024-11-06T02:59:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861960.20 epoch sec; seqNo:1487; 4061660.44 north; 594268.58 east; 36.70 lat; -121.94 lon; 48.54 depth; utmZone: 10 2024-11-06T02:59:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861959.77 2024-11-06T02:59:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:20Z- waiting for 1730861963.77, time is 1730861960.20 2024-11-06T02:59:20Z- AHRS_M2 msg: 1730861960.55 epoch sec; seqNo:1387 2024-11-06T02:59:20Z- AHRS_M2 msg: 0.07 phi; -0.11 theta; 1.64 psi 2024-11-06T02:59:20Z- handleDepth()- Depth_Keller msg: 1730861960.56 epoch sec; seqNo:1488; depth 48.55 2024-11-06T02:59:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861960.56 epoch sec; seqNo:1488; 4061660.46 north; 594269.80 east; 36.70 lat; -121.94 lon; 48.55 depth; utmZone: 10 2024-11-06T02:59:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861959.77 2024-11-06T02:59:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:20Z- waiting for 1730861963.77, time is 1730861960.56 2024-11-06T02:59:20Z- AHRS_M2 msg: 1730861960.95 epoch sec; seqNo:1388 2024-11-06T02:59:20Z- AHRS_M2 msg: 0.07 phi; -0.11 theta; 1.63 psi 2024-11-06T02:59:20Z- handleDepth()- Depth_Keller msg: 1730861960.97 epoch sec; seqNo:1489; depth 48.58 2024-11-06T02:59:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861960.98 epoch sec; seqNo:1489; 4061660.46 north; 594269.80 east; 36.70 lat; -121.94 lon; 48.58 depth; utmZone: 10 2024-11-06T02:59:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861959.77 2024-11-06T02:59:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:21Z- waiting for 1730861963.77, time is 1730861960.98 2024-11-06T02:59:21Z- AHRS_M2 msg: 1730861961.36 epoch sec; seqNo:1389 2024-11-06T02:59:21Z- AHRS_M2 msg: 0.07 phi; -0.11 theta; 1.63 psi 2024-11-06T02:59:21Z- handleDepth()- Depth_Keller msg: 1730861961.37 epoch sec; seqNo:1490; depth 48.61 2024-11-06T02:59:21Z- handleDVL() - DVL msg epochMillisec: 1730861961.39 2024-11-06T02:59:21Z- handleDvl() - RDI_Pathfinder msg: 1730861961.39 sec; seqNo:458; #beams:4 2024-11-06T02:59:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 43.43 , 42.00 , 48.53 2024-11-06T02:59:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.03 , -0.01 2024-11-06T02:59:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861961.40 epoch sec; seqNo:1490; 4061660.46 north; 594269.80 east; 36.70 lat; -121.94 lon; 48.61 depth; utmZone: 10 2024-11-06T02:59:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861959.77 2024-11-06T02:59:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:21Z- waiting for 1730861963.77, time is 1730861961.40 2024-11-06T02:59:21Z- AHRS_M2 msg: 1730861961.76 epoch sec; seqNo:1390 2024-11-06T02:59:21Z- AHRS_M2 msg: 0.07 phi; -0.11 theta; 1.62 psi 2024-11-06T02:59:21Z- handleDepth()- Depth_Keller msg: 1730861961.77 epoch sec; seqNo:1491; depth 48.65 2024-11-06T02:59:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861961.79 epoch sec; seqNo:1491; 4061660.46 north; 594269.80 east; 36.70 lat; -121.94 lon; 48.65 depth; utmZone: 10 2024-11-06T02:59:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861959.77 2024-11-06T02:59:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:21Z- waiting for 1730861963.77, time is 1730861961.79 2024-11-06T02:59:22Z- AHRS_M2 msg: 1730861962.17 epoch sec; seqNo:1391 2024-11-06T02:59:22Z- AHRS_M2 msg: 0.07 phi; -0.11 theta; 1.61 psi 2024-11-06T02:59:22Z- handleDepth()- Depth_Keller msg: 1730861962.18 epoch sec; seqNo:1492; depth 48.67 2024-11-06T02:59:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861962.19 epoch sec; seqNo:1492; 4061660.47 north; 594271.02 east; 36.70 lat; -121.94 lon; 48.67 depth; utmZone: 10 2024-11-06T02:59:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861959.77 2024-11-06T02:59:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:22Z- waiting for 1730861963.77, time is 1730861962.19 2024-11-06T02:59:22Z- AHRS_M2 msg: 1730861962.57 epoch sec; seqNo:1392 2024-11-06T02:59:22Z- AHRS_M2 msg: 0.07 phi; -0.11 theta; 1.61 psi 2024-11-06T02:59:22Z- handleDepth()- Depth_Keller msg: 1730861962.58 epoch sec; seqNo:1493; depth 48.69 2024-11-06T02:59:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861962.60 epoch sec; seqNo:1493; 4061660.09 north; 594271.02 east; 36.70 lat; -121.94 lon; 48.69 depth; utmZone: 10 2024-11-06T02:59:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861959.77 2024-11-06T02:59:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:22Z- waiting for 1730861963.77, time is 1730861962.60 2024-11-06T02:59:22Z- AHRS_M2 msg: 1730861962.97 epoch sec; seqNo:1393 2024-11-06T02:59:22Z- AHRS_M2 msg: 0.07 phi; -0.11 theta; 1.60 psi 2024-11-06T02:59:22Z- handleDepth()- Depth_Keller msg: 1730861962.99 epoch sec; seqNo:1494; depth 48.72 2024-11-06T02:59:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861963.00 epoch sec; seqNo:1494; 4061660.09 north; 594271.02 east; 36.70 lat; -121.94 lon; 48.72 depth; utmZone: 10 2024-11-06T02:59:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861959.77 2024-11-06T02:59:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:23Z- waiting for 1730861963.77, time is 1730861963.00 2024-11-06T02:59:23Z- AHRS_M2 msg: 1730861963.38 epoch sec; seqNo:1394 2024-11-06T02:59:23Z- AHRS_M2 msg: 0.08 phi; -0.11 theta; 1.60 psi 2024-11-06T02:59:23Z- handleDepth()- Depth_Keller msg: 1730861963.39 epoch sec; seqNo:1495; depth 48.75 2024-11-06T02:59:23Z- handleDVL() - DVL msg epochMillisec: 1730861963.40 2024-11-06T02:59:23Z- handleDvl() - RDI_Pathfinder msg: 1730861963.40 sec; seqNo:459; #beams:4 2024-11-06T02:59:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.08 , 43.02 , 41.80 , 48.32 2024-11-06T02:59:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.01 , -0.01 2024-11-06T02:59:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861963.43 epoch sec; seqNo:1495; 4061660.09 north; 594271.02 east; 36.70 lat; -121.94 lon; 48.75 depth; utmZone: 10 2024-11-06T02:59:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861959.77 2024-11-06T02:59:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:23Z- waiting for 1730861963.77, time is 1730861963.43 2024-11-06T02:59:23Z- AHRS_M2 msg: 1730861963.78 epoch sec; seqNo:1395 2024-11-06T02:59:23Z- AHRS_M2 msg: 0.08 phi; -0.11 theta; 1.59 psi 2024-11-06T02:59:23Z- handleDepth()- Depth_Keller msg: 1730861963.79 epoch sec; seqNo:1496; depth 48.78 2024-11-06T02:59:23Z- handleDVL() - DVL msg epochMillisec: 1730861963.80 2024-11-06T02:59:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:59:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:59:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:59:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:59:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:59:23Z- handleDvl() - RDI_Pathfinder msg: 1730861963.80 sec; seqNo:460; #beams:0 2024-11-06T02:59:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:59:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.01 , -0.01 2024-11-06T02:59:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861963.81 epoch sec; seqNo:1496; 4061660.11 north; 594272.24 east; 36.70 lat; -121.94 lon; 48.78 depth; utmZone: 10 2024-11-06T02:59:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861959.77 2024-11-06T02:59:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:23Z- waiting for 1730861963.77, time is 1730861963.81 2024-11-06T02:59:23Z- LcmTrn::updateTrn() >>>> pose time 1730861963.81, meas time 1730861963.80 <<<< 2024-11-06T02:59:23Z- LcmTrn::updateTrn() equating pose 1730861963.80 and meas times 1730861963.80 2024-11-06T02:59:23Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861963.80 2024-11-06T02:59:23Z- TerrainNav::motionUpdate() - currEstimate = (4061660, 594272.24, 48.78) 2024-11-06T02:59:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:59:23Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:59:23Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:59:23Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:59:23Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:59:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:59:23Z- variances([0], [2], and psi_berg) = 0.347996 0.266556 0.000000 2024-11-06T02:59:23Z- publishEstimatesDecoder() - at 1730861963.80 sec 2024-11-06T02:59:23Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:59:23Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061691.25 594256.12 55.60 2024-11-06T02:59:23Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061692.46 594256.40 55.23 lat/lon: 36.70 -121.94 2024-11-06T02:59:23Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.35 0.27 0.97 0.00 2024-11-06T02:59:24Z- AHRS_M2 msg: 1730861964.19 epoch sec; seqNo:1396 2024-11-06T02:59:24Z- AHRS_M2 msg: 0.08 phi; -0.11 theta; 1.59 psi 2024-11-06T02:59:24Z- handleDepth()- Depth_Keller msg: 1730861964.19 epoch sec; seqNo:1497; depth 48.81 2024-11-06T02:59:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861964.22 epoch sec; seqNo:1497; 4061660.11 north; 594272.24 east; 36.70 lat; -121.94 lon; 48.81 depth; utmZone: 10 2024-11-06T02:59:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861963.80 2024-11-06T02:59:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:24Z- waiting for 1730861967.80, time is 1730861964.22 2024-11-06T02:59:24Z- handleDepth()- Depth_Keller msg: 1730861964.61 epoch sec; seqNo:1498; depth 48.83 2024-11-06T02:59:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861964.62 epoch sec; seqNo:1498; 4061660.11 north; 594272.24 east; 36.70 lat; -121.94 lon; 48.83 depth; utmZone: 10 2024-11-06T02:59:24Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:59:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861963.80 2024-11-06T02:59:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:24Z- waiting for 1730861967.80, time is 1730861964.62 2024-11-06T02:59:25Z- AHRS_M2 msg: 1730861965.00 epoch sec; seqNo:1397 2024-11-06T02:59:25Z- AHRS_M2 msg: 0.07 phi; -0.11 theta; 1.57 psi 2024-11-06T02:59:25Z- handleDepth()- Depth_Keller msg: 1730861965.00 epoch sec; seqNo:1499; depth 48.86 2024-11-06T02:59:25Z- handleDVL() - DVL msg epochMillisec: 1730861965.01 2024-11-06T02:59:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:59:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:59:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:59:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:59:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:59:25Z- handleDvl() - RDI_Pathfinder msg: 1730861965.01 sec; seqNo:461; #beams:0 2024-11-06T02:59:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:59:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.01 , -0.01 2024-11-06T02:59:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861965.02 epoch sec; seqNo:1499; 4061660.11 north; 594272.24 east; 36.70 lat; -121.94 lon; 48.86 depth; utmZone: 10 2024-11-06T02:59:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861963.80 2024-11-06T02:59:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:25Z- waiting for 1730861967.80, time is 1730861965.02 2024-11-06T02:59:25Z- AHRS_M2 msg: 1730861965.40 epoch sec; seqNo:1398 2024-11-06T02:59:25Z- AHRS_M2 msg: 0.07 phi; -0.11 theta; 1.57 psi 2024-11-06T02:59:25Z- handleDepth()- Depth_Keller msg: 1730861965.41 epoch sec; seqNo:1500; depth 48.90 2024-11-06T02:59:25Z- handleDVL() - DVL msg epochMillisec: 1730861965.43 2024-11-06T02:59:25Z- handleDvl() - RDI_Pathfinder msg: 1730861965.43 sec; seqNo:462; #beams:4 2024-11-06T02:59:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.88 , 42.61 , 41.59 , 47.91 2024-11-06T02:59:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , -0.02 2024-11-06T02:59:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861965.45 epoch sec; seqNo:1500; 4061660.12 north; 594273.46 east; 36.70 lat; -121.94 lon; 48.90 depth; utmZone: 10 2024-11-06T02:59:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861963.80 2024-11-06T02:59:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:25Z- waiting for 1730861967.80, time is 1730861965.45 2024-11-06T02:59:25Z- AHRS_M2 msg: 1730861965.80 epoch sec; seqNo:1399 2024-11-06T02:59:25Z- AHRS_M2 msg: 0.07 phi; -0.11 theta; 1.56 psi 2024-11-06T02:59:25Z- handleDepth()- Depth_Keller msg: 1730861965.81 epoch sec; seqNo:1501; depth 48.93 2024-11-06T02:59:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861965.84 epoch sec; seqNo:1501; 4061660.12 north; 594273.46 east; 36.70 lat; -121.94 lon; 48.93 depth; utmZone: 10 2024-11-06T02:59:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861963.80 2024-11-06T02:59:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:25Z- waiting for 1730861967.80, time is 1730861965.84 2024-11-06T02:59:26Z- AHRS_M2 msg: 1730861966.21 epoch sec; seqNo:1400 2024-11-06T02:59:26Z- AHRS_M2 msg: 0.08 phi; -0.11 theta; 1.56 psi 2024-11-06T02:59:26Z- handleDepth()- Depth_Keller msg: 1730861966.22 epoch sec; seqNo:1502; depth 48.97 2024-11-06T02:59:26Z- handleDVL() - DVL msg epochMillisec: 1730861966.22 2024-11-06T02:59:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:59:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:59:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:59:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:59:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:59:26Z- handleDvl() - RDI_Pathfinder msg: 1730861966.22 sec; seqNo:463; #beams:0 2024-11-06T02:59:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:59:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , -0.02 2024-11-06T02:59:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861966.23 epoch sec; seqNo:1502; 4061660.12 north; 594273.46 east; 36.70 lat; -121.94 lon; 48.97 depth; utmZone: 10 2024-11-06T02:59:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861963.80 2024-11-06T02:59:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:26Z- waiting for 1730861967.80, time is 1730861966.23 2024-11-06T02:59:26Z- AHRS_M2 msg: 1730861966.61 epoch sec; seqNo:1401 2024-11-06T02:59:26Z- AHRS_M2 msg: 0.08 phi; -0.11 theta; 1.55 psi 2024-11-06T02:59:26Z- handleDepth()- Depth_Keller msg: 1730861966.62 epoch sec; seqNo:1503; depth 48.99 2024-11-06T02:59:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861966.63 epoch sec; seqNo:1503; 4061660.13 north; 594274.68 east; 36.70 lat; -121.94 lon; 48.99 depth; utmZone: 10 2024-11-06T02:59:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861963.80 2024-11-06T02:59:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:26Z- waiting for 1730861967.80, time is 1730861966.63 2024-11-06T02:59:27Z- AHRS_M2 msg: 1730861967.02 epoch sec; seqNo:1402 2024-11-06T02:59:27Z- AHRS_M2 msg: 0.08 phi; -0.11 theta; 1.55 psi 2024-11-06T02:59:27Z- handleDepth()- Depth_Keller msg: 1730861967.03 epoch sec; seqNo:1504; depth 49.04 2024-11-06T02:59:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861967.04 epoch sec; seqNo:1504; 4061660.13 north; 594274.68 east; 36.70 lat; -121.94 lon; 49.04 depth; utmZone: 10 2024-11-06T02:59:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861963.80 2024-11-06T02:59:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:27Z- waiting for 1730861967.80, time is 1730861967.04 2024-11-06T02:59:27Z- AHRS_M2 msg: 1730861967.42 epoch sec; seqNo:1403 2024-11-06T02:59:27Z- AHRS_M2 msg: 0.08 phi; -0.11 theta; 1.55 psi 2024-11-06T02:59:27Z- handleDepth()- Depth_Keller msg: 1730861967.43 epoch sec; seqNo:1505; depth 49.08 2024-11-06T02:59:27Z- handleDVL() - DVL msg epochMillisec: 1730861967.46 2024-11-06T02:59:27Z- handleDvl() - RDI_Pathfinder msg: 1730861967.46 sec; seqNo:464; #beams:4 2024-11-06T02:59:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 42.20 , 41.59 , 47.91 2024-11-06T02:59:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.03 , -0.00 2024-11-06T02:59:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861967.48 epoch sec; seqNo:1505; 4061660.13 north; 594274.68 east; 36.70 lat; -121.94 lon; 49.08 depth; utmZone: 10 2024-11-06T02:59:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861963.80 2024-11-06T02:59:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:27Z- waiting for 1730861967.80, time is 1730861967.48 2024-11-06T02:59:27Z- AHRS_M2 msg: 1730861967.83 epoch sec; seqNo:1404 2024-11-06T02:59:27Z- AHRS_M2 msg: 0.08 phi; -0.11 theta; 1.54 psi 2024-11-06T02:59:27Z- handleDepth()- Depth_Keller msg: 1730861967.84 epoch sec; seqNo:1506; depth 49.11 2024-11-06T02:59:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861967.85 epoch sec; seqNo:1506; 4061660.13 north; 594274.68 east; 36.70 lat; -121.94 lon; 49.11 depth; utmZone: 10 2024-11-06T02:59:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861963.80 2024-11-06T02:59:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:27Z- waiting for 1730861967.80, time is 1730861967.85 2024-11-06T02:59:27Z- LcmTrn::updateTrn() >>>> pose time 1730861967.85, meas time 1730861967.46 <<<< 2024-11-06T02:59:27Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861967.85 2024-11-06T02:59:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861967.46 sec, ping # 0; waiting for INS data more recent than 1730861963.80... 2024-11-06T02:59:27Z- TerrainNav::motionUpdate() - currEstimate = (4061660, 594274.68, 49.11) 2024-11-06T02:59:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:59:27Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:59:28Z- TNavPF::Projecting Measurements 2024-11-06T02:59:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:59:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:59:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:59:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:59:28Z- TNavFilter::4 good beams. 2024-11-06T02:59:28Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:59:28Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:59:28Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:59:28Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:59:28Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:59:28Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:59:28Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:59:28Z- TerrainMapOctree: (vehN, vehE ) = (4061660.13, 594274.45). 2024-11-06T02:59:28Z- TerrainMapOctree: Min Distance = 93.14. 2024-11-06T02:59:28Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:59:28Z- Forcing Subcloud Comparison 2024-11-06T02:59:28Z- Weighting particles with subcloud comparison 2024-11-06T02:59:28Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:28Z- meanExpectedMeasDiff: -1.072909 delta_rms_squared: 1.816416 alpha: 0.989109 2024-11-06T02:59:28Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:28Z- meanExpectedMeasDiff: -0.211247 delta_rms_squared: 1.493851 alpha: 0.986789 2024-11-06T02:59:28Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:28Z- meanExpectedMeasDiff: 2.715198 delta_rms_squared: 1.039447 alpha: 0.981122 2024-11-06T02:59:28Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:28Z- meanExpectedMeasDiff: -0.849974 delta_rms_squared: 1.878224 alpha: 0.989464 2024-11-06T02:59:28Z- Subcloud NIS: 0.211038 2024-11-06T02:59:28Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:59:28Z- TNavPF:: sumWeights = 0.251355 2024-11-06T02:59:28Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:59:28Z- TNavPF::First Term of Map Covariance Matrix is 0.37 2024-11-06T02:59:28Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T02:59:28Z- TerrainNav::Resampling particle filter... 2024-11-06T02:59:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861967.46 sec. 2024-11-06T02:59:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:59:28Z- variances([0], [2], and psi_berg) = 0.365284 0.282891 0.000000 2024-11-06T02:59:28Z- publishEstimatesDecoder() - at 1730861967.85 sec 2024-11-06T02:59:28Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:59:28Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061692.35 594258.37 54.72 2024-11-06T02:59:28Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061692.47 594258.73 55.58 lat/lon: 36.70 -121.94 2024-11-06T02:59:28Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.37 0.28 0.81 0.00 2024-11-06T02:59:28Z- AHRS_M2 msg: 1730861968.23 epoch sec; seqNo:1405 2024-11-06T02:59:28Z- AHRS_M2 msg: 0.09 phi; -0.11 theta; 1.53 psi 2024-11-06T02:59:28Z- handleDepth()- Depth_Keller msg: 1730861968.24 epoch sec; seqNo:1507; depth 49.16 2024-11-06T02:59:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861968.25 epoch sec; seqNo:1507; 4061660.53 north; 594275.90 east; 36.70 lat; -121.94 lon; 49.16 depth; utmZone: 10 2024-11-06T02:59:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861967.85 2024-11-06T02:59:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:28Z- waiting for 1730861971.85, time is 1730861968.25 2024-11-06T02:59:28Z- AHRS_M2 msg: 1730861968.63 epoch sec; seqNo:1406 2024-11-06T02:59:28Z- AHRS_M2 msg: 0.09 phi; -0.11 theta; 1.53 psi 2024-11-06T02:59:28Z- handleDepth()- Depth_Keller msg: 1730861968.65 epoch sec; seqNo:1508; depth 49.19 2024-11-06T02:59:28Z- handleDVL() - DVL msg epochMillisec: 1730861968.65 2024-11-06T02:59:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:59:28Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:59:28Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:59:28Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:59:28Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:59:28Z- handleDvl() - RDI_Pathfinder msg: 1730861968.65 sec; seqNo:465; #beams:0 2024-11-06T02:59:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:59:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.03 , -0.00 2024-11-06T02:59:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861968.66 epoch sec; seqNo:1508; 4061660.53 north; 594275.90 east; 36.70 lat; -121.94 lon; 49.19 depth; utmZone: 10 2024-11-06T02:59:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861967.85 2024-11-06T02:59:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:28Z- waiting for 1730861971.85, time is 1730861968.66 2024-11-06T02:59:29Z- AHRS_M2 msg: 1730861969.03 epoch sec; seqNo:1407 2024-11-06T02:59:29Z- AHRS_M2 msg: 0.09 phi; -0.11 theta; 1.52 psi 2024-11-06T02:59:29Z- handleDepth()- Depth_Keller msg: 1730861969.04 epoch sec; seqNo:1509; depth 49.24 2024-11-06T02:59:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861969.06 epoch sec; seqNo:1509; 4061660.53 north; 594275.90 east; 36.70 lat; -121.94 lon; 49.24 depth; utmZone: 10 2024-11-06T02:59:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861967.85 2024-11-06T02:59:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:29Z- waiting for 1730861971.85, time is 1730861969.06 2024-11-06T02:59:29Z- AHRS_M2 msg: 1730861969.44 epoch sec; seqNo:1408 2024-11-06T02:59:29Z- AHRS_M2 msg: 0.09 phi; -0.11 theta; 1.52 psi 2024-11-06T02:59:29Z- handleDepth()- Depth_Keller msg: 1730861969.45 epoch sec; seqNo:1510; depth 49.27 2024-11-06T02:59:29Z- handleDVL() - DVL msg epochMillisec: 1730861969.47 2024-11-06T02:59:29Z- handleDvl() - RDI_Pathfinder msg: 1730861969.47 sec; seqNo:466; #beams:4 2024-11-06T02:59:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.88 , 42.21 , 41.39 , 47.91 2024-11-06T02:59:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.01 , 0.03 2024-11-06T02:59:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861969.49 epoch sec; seqNo:1510; 4061660.53 north; 594275.90 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T02:59:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861967.85 2024-11-06T02:59:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:29Z- waiting for 1730861971.85, time is 1730861969.49 2024-11-06T02:59:29Z- AHRS_M2 msg: 1730861969.85 epoch sec; seqNo:1409 2024-11-06T02:59:29Z- AHRS_M2 msg: 0.10 phi; -0.11 theta; 1.52 psi 2024-11-06T02:59:29Z- handleDepth()- Depth_Keller msg: 1730861969.85 epoch sec; seqNo:1511; depth 49.31 2024-11-06T02:59:29Z- handleDVL() - DVL msg epochMillisec: 1730861969.86 2024-11-06T02:59:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:59:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:59:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:59:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:59:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:59:29Z- handleDvl() - RDI_Pathfinder msg: 1730861969.86 sec; seqNo:467; #beams:0 2024-11-06T02:59:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:59:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.01 , 0.03 2024-11-06T02:59:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861969.87 epoch sec; seqNo:1511; 4061660.54 north; 594277.12 east; 36.70 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T02:59:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861967.85 2024-11-06T02:59:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:30Z- waiting for 1730861971.85, time is 1730861969.87 2024-11-06T02:59:30Z- AHRS_M2 msg: 1730861970.25 epoch sec; seqNo:1410 2024-11-06T02:59:30Z- AHRS_M2 msg: 0.10 phi; -0.11 theta; 1.51 psi 2024-11-06T02:59:30Z- handleDepth()- Depth_Keller msg: 1730861970.26 epoch sec; seqNo:1512; depth 49.36 2024-11-06T02:59:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861970.27 epoch sec; seqNo:1512; 4061660.54 north; 594277.12 east; 36.70 lat; -121.94 lon; 49.36 depth; utmZone: 10 2024-11-06T02:59:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861967.85 2024-11-06T02:59:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:30Z- waiting for 1730861971.85, time is 1730861970.27 2024-11-06T02:59:30Z- AHRS_M2 msg: 1730861970.65 epoch sec; seqNo:1411 2024-11-06T02:59:30Z- AHRS_M2 msg: 0.11 phi; -0.12 theta; 1.51 psi 2024-11-06T02:59:30Z- handleDepth()- Depth_Keller msg: 1730861970.66 epoch sec; seqNo:1513; depth 49.39 2024-11-06T02:59:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861970.68 epoch sec; seqNo:1513; 4061660.54 north; 594277.12 east; 36.70 lat; -121.94 lon; 49.39 depth; utmZone: 10 2024-11-06T02:59:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861967.85 2024-11-06T02:59:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:30Z- waiting for 1730861971.85, time is 1730861970.68 2024-11-06T02:59:31Z- AHRS_M2 msg: 1730861971.06 epoch sec; seqNo:1412 2024-11-06T02:59:31Z- AHRS_M2 msg: 0.11 phi; -0.12 theta; 1.51 psi 2024-11-06T02:59:31Z- handleDepth()- Depth_Keller msg: 1730861971.06 epoch sec; seqNo:1514; depth 49.44 2024-11-06T02:59:31Z- handleDVL() - DVL msg epochMillisec: 1730861971.06 2024-11-06T02:59:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:59:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:59:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:59:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:59:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:59:31Z- handleDvl() - RDI_Pathfinder msg: 1730861971.06 sec; seqNo:468; #beams:0 2024-11-06T02:59:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:59:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.01 , 0.03 2024-11-06T02:59:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861971.07 epoch sec; seqNo:1514; 4061660.54 north; 594277.12 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T02:59:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861967.85 2024-11-06T02:59:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:31Z- waiting for 1730861971.85, time is 1730861971.07 2024-11-06T02:59:31Z- AHRS_M2 msg: 1730861971.46 epoch sec; seqNo:1413 2024-11-06T02:59:31Z- AHRS_M2 msg: 0.11 phi; -0.12 theta; 1.51 psi 2024-11-06T02:59:31Z- handleDepth()- Depth_Keller msg: 1730861971.46 epoch sec; seqNo:1515; depth 49.48 2024-11-06T02:59:31Z- handleDVL() - DVL msg epochMillisec: 1730861971.48 2024-11-06T02:59:31Z- handleDvl() - RDI_Pathfinder msg: 1730861971.48 sec; seqNo:469; #beams:4 2024-11-06T02:59:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.49 , 41.59 , 41.19 , 47.91 2024-11-06T02:59:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , 0.05 2024-11-06T02:59:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861971.52 epoch sec; seqNo:1515; 4061660.55 north; 594278.34 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T02:59:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861967.85 2024-11-06T02:59:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:31Z- waiting for 1730861971.85, time is 1730861971.52 2024-11-06T02:59:31Z- AHRS_M2 msg: 1730861971.86 epoch sec; seqNo:1414 2024-11-06T02:59:31Z- AHRS_M2 msg: 0.11 phi; -0.12 theta; 1.50 psi 2024-11-06T02:59:31Z- handleDepth()- Depth_Keller msg: 1730861971.87 epoch sec; seqNo:1516; depth 49.53 2024-11-06T02:59:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861971.89 epoch sec; seqNo:1516; 4061660.55 north; 594278.34 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T02:59:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861967.85 2024-11-06T02:59:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:32Z- waiting for 1730861971.85, time is 1730861971.89 2024-11-06T02:59:32Z- LcmTrn::updateTrn() >>>> pose time 1730861971.89, meas time 1730861971.48 <<<< 2024-11-06T02:59:32Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861971.89 2024-11-06T02:59:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861971.48 sec, ping # 0; waiting for INS data more recent than 1730861967.85... 2024-11-06T02:59:32Z- TerrainNav::motionUpdate() - currEstimate = (4061661, 594278.34, 49.53) 2024-11-06T02:59:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:59:32Z- TerrainNav:: SubcloudNIS is 0.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:59:32Z- TNavPF::Projecting Measurements 2024-11-06T02:59:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:59:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:59:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:59:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:59:32Z- TNavFilter::4 good beams. 2024-11-06T02:59:32Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:59:32Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:59:32Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:59:32Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:59:32Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:59:32Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:59:32Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:59:32Z- TerrainMapOctree: (vehN, vehE ) = (4061660.51, 594277.97). 2024-11-06T02:59:32Z- TerrainMapOctree: Min Distance = 93.76. 2024-11-06T02:59:32Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:59:32Z- Forcing Subcloud Comparison 2024-11-06T02:59:32Z- Weighting particles with subcloud comparison 2024-11-06T02:59:32Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:32Z- meanExpectedMeasDiff: -0.668305 delta_rms_squared: 1.605449 alpha: 0.987696 2024-11-06T02:59:32Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:32Z- meanExpectedMeasDiff: 0.115205 delta_rms_squared: 1.239920 alpha: 0.984126 2024-11-06T02:59:32Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:32Z- meanExpectedMeasDiff: 3.183107 delta_rms_squared: 1.125530 alpha: 0.982541 2024-11-06T02:59:32Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:32Z- meanExpectedMeasDiff: -0.587488 delta_rms_squared: 2.120621 alpha: 0.990657 2024-11-06T02:59:32Z- Subcloud NIS: 0.214645 2024-11-06T02:59:32Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:59:32Z- TNavPF:: sumWeights = 0.217884 2024-11-06T02:59:32Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:59:32Z- TNavPF::First Term of Map Covariance Matrix is 0.28 2024-11-06T02:59:32Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T02:59:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861971.48 sec. 2024-11-06T02:59:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:59:32Z- variances([0], [2], and psi_berg) = 0.366500 0.282058 0.000000 2024-11-06T02:59:32Z- publishEstimatesDecoder() - at 1730861971.89 sec 2024-11-06T02:59:32Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:59:32Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061691.58 594263.06 55.64 2024-11-06T02:59:32Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061692.88 594262.38 55.77 lat/lon: 36.70 -121.94 2024-11-06T02:59:32Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.37 0.28 0.71 0.00 2024-11-06T02:59:32Z- AHRS_M2 msg: 1730861972.28 epoch sec; seqNo:1415 2024-11-06T02:59:32Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 1.50 psi 2024-11-06T02:59:32Z- handleDepth()- Depth_Keller msg: 1730861972.30 epoch sec; seqNo:1517; depth 49.56 2024-11-06T02:59:32Z- handleDVL() - DVL msg epochMillisec: 1730861972.31 2024-11-06T02:59:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:59:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:59:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:59:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:59:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:59:32Z- handleDvl() - RDI_Pathfinder msg: 1730861972.31 sec; seqNo:470; #beams:0 2024-11-06T02:59:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:59:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , 0.05 2024-11-06T02:59:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861972.32 epoch sec; seqNo:1517; 4061660.55 north; 594278.34 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T02:59:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861971.89 2024-11-06T02:59:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:32Z- waiting for 1730861975.89, time is 1730861972.32 2024-11-06T02:59:32Z- AHRS_M2 msg: 1730861972.67 epoch sec; seqNo:1416 2024-11-06T02:59:32Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 1.50 psi 2024-11-06T02:59:32Z- handleDepth()- Depth_Keller msg: 1730861972.68 epoch sec; seqNo:1518; depth 49.60 2024-11-06T02:59:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861972.69 epoch sec; seqNo:1518; 4061660.57 north; 594279.56 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T02:59:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861971.89 2024-11-06T02:59:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:32Z- waiting for 1730861975.89, time is 1730861972.69 2024-11-06T02:59:33Z- AHRS_M2 msg: 1730861973.09 epoch sec; seqNo:1417 2024-11-06T02:59:33Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 1.50 psi 2024-11-06T02:59:33Z- handleDepth()- Depth_Keller msg: 1730861973.09 epoch sec; seqNo:1519; depth 49.64 2024-11-06T02:59:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861973.10 epoch sec; seqNo:1519; 4061660.57 north; 594279.56 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T02:59:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861971.89 2024-11-06T02:59:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:33Z- waiting for 1730861975.89, time is 1730861973.10 2024-11-06T02:59:33Z- AHRS_M2 msg: 1730861973.48 epoch sec; seqNo:1418 2024-11-06T02:59:33Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 1.50 psi 2024-11-06T02:59:33Z- handleDepth()- Depth_Keller msg: 1730861973.49 epoch sec; seqNo:1520; depth 49.67 2024-11-06T02:59:33Z- handleDVL() - DVL msg epochMillisec: 1730861973.51 2024-11-06T02:59:33Z- handleDvl() - RDI_Pathfinder msg: 1730861973.51 sec; seqNo:471; #beams:4 2024-11-06T02:59:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.69 , 41.19 , 40.57 , 48.12 2024-11-06T02:59:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , 0.05 2024-11-06T02:59:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861973.52 epoch sec; seqNo:1520; 4061660.57 north; 594279.56 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T02:59:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861971.89 2024-11-06T02:59:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:33Z- waiting for 1730861975.89, time is 1730861973.52 2024-11-06T02:59:33Z- AHRS_M2 msg: 1730861973.88 epoch sec; seqNo:1419 2024-11-06T02:59:33Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 1.50 psi 2024-11-06T02:59:33Z- handleDepth()- Depth_Keller msg: 1730861973.89 epoch sec; seqNo:1521; depth 49.71 2024-11-06T02:59:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861973.93 epoch sec; seqNo:1521; 4061660.57 north; 594279.56 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T02:59:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861971.89 2024-11-06T02:59:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:34Z- waiting for 1730861975.89, time is 1730861973.93 2024-11-06T02:59:34Z- AHRS_M2 msg: 1730861974.29 epoch sec; seqNo:1420 2024-11-06T02:59:34Z- AHRS_M2 msg: 0.13 phi; -0.11 theta; 1.50 psi 2024-11-06T02:59:34Z- handleDepth()- Depth_Keller msg: 1730861974.29 epoch sec; seqNo:1522; depth 49.73 2024-11-06T02:59:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861974.30 epoch sec; seqNo:1522; 4061660.58 north; 594280.78 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T02:59:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861971.89 2024-11-06T02:59:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:34Z- waiting for 1730861975.89, time is 1730861974.30 2024-11-06T02:59:34Z- AHRS_M2 msg: 1730861974.69 epoch sec; seqNo:1421 2024-11-06T02:59:34Z- AHRS_M2 msg: 0.13 phi; -0.11 theta; 1.50 psi 2024-11-06T02:59:34Z- handleDepth()- Depth_Keller msg: 1730861974.70 epoch sec; seqNo:1523; depth 49.77 2024-11-06T02:59:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861974.71 epoch sec; seqNo:1523; 4061660.58 north; 594280.78 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T02:59:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861971.89 2024-11-06T02:59:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:34Z- waiting for 1730861975.89, time is 1730861974.71 2024-11-06T02:59:35Z- AHRS_M2 msg: 1730861975.10 epoch sec; seqNo:1422 2024-11-06T02:59:35Z- AHRS_M2 msg: 0.13 phi; -0.11 theta; 1.50 psi 2024-11-06T02:59:35Z- handleDepth()- Depth_Keller msg: 1730861975.11 epoch sec; seqNo:1524; depth 49.81 2024-11-06T02:59:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861975.12 epoch sec; seqNo:1524; 4061660.58 north; 594280.78 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T02:59:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861971.89 2024-11-06T02:59:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:35Z- waiting for 1730861975.89, time is 1730861975.12 2024-11-06T02:59:35Z- AHRS_M2 msg: 1730861975.50 epoch sec; seqNo:1423 2024-11-06T02:59:35Z- AHRS_M2 msg: 0.13 phi; -0.11 theta; 1.50 psi 2024-11-06T02:59:35Z- handleDepth()- Depth_Keller msg: 1730861975.51 epoch sec; seqNo:1525; depth 49.83 2024-11-06T02:59:35Z- handleDVL() - DVL msg epochMillisec: 1730861975.53 2024-11-06T02:59:35Z- handleDvl() - RDI_Pathfinder msg: 1730861975.53 sec; seqNo:472; #beams:4 2024-11-06T02:59:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.69 , 40.98 , 40.57 , 47.51 2024-11-06T02:59:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , -0.00 2024-11-06T02:59:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861975.55 epoch sec; seqNo:1525; 4061660.59 north; 594282.00 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T02:59:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861971.89 2024-11-06T02:59:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:35Z- waiting for 1730861975.89, time is 1730861975.55 2024-11-06T02:59:35Z- AHRS_M2 msg: 1730861975.90 epoch sec; seqNo:1424 2024-11-06T02:59:35Z- AHRS_M2 msg: 0.13 phi; -0.10 theta; 1.50 psi 2024-11-06T02:59:35Z- handleDepth()- Depth_Keller msg: 1730861975.91 epoch sec; seqNo:1526; depth 49.87 2024-11-06T02:59:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861975.92 epoch sec; seqNo:1526; 4061660.59 north; 594282.00 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T02:59:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861971.89 2024-11-06T02:59:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:36Z- waiting for 1730861975.89, time is 1730861975.92 2024-11-06T02:59:36Z- LcmTrn::updateTrn() >>>> pose time 1730861975.92, meas time 1730861975.53 <<<< 2024-11-06T02:59:36Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861975.92 2024-11-06T02:59:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861975.53 sec, ping # 0; waiting for INS data more recent than 1730861971.89... 2024-11-06T02:59:36Z- TerrainNav::motionUpdate() - currEstimate = (4061661, 594282.00, 49.87) 2024-11-06T02:59:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:59:36Z- TerrainNav:: SubcloudNIS is 0.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:59:36Z- TNavPF::Projecting Measurements 2024-11-06T02:59:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:59:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:59:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:59:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:59:36Z- TNavFilter::4 good beams. 2024-11-06T02:59:36Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:59:36Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:59:36Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:59:36Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:59:36Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:59:36Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:59:36Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:59:36Z- TerrainMapOctree: (vehN, vehE ) = (4061660.59, 594281.64). 2024-11-06T02:59:36Z- TerrainMapOctree: Min Distance = 94.84. 2024-11-06T02:59:36Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:59:36Z- Forcing Subcloud Comparison 2024-11-06T02:59:36Z- Weighting particles with subcloud comparison 2024-11-06T02:59:36Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:36Z- meanExpectedMeasDiff: -0.881040 delta_rms_squared: 1.541754 alpha: 0.987194 2024-11-06T02:59:36Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:36Z- meanExpectedMeasDiff: 0.003157 delta_rms_squared: 1.160145 alpha: 0.983053 2024-11-06T02:59:36Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:36Z- meanExpectedMeasDiff: 3.687893 delta_rms_squared: 0.932529 alpha: 0.979003 2024-11-06T02:59:36Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:36Z- meanExpectedMeasDiff: 0.562362 delta_rms_squared: 1.513347 alpha: 0.986957 2024-11-06T02:59:36Z- Subcloud NIS: 0.202310 2024-11-06T02:59:36Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:59:36Z- TNavPF:: sumWeights = 0.160021 2024-11-06T02:59:36Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:59:36Z- TNavPF::First Term of Map Covariance Matrix is 0.30 2024-11-06T02:59:36Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T02:59:36Z- TerrainNav::Resampling particle filter... 2024-11-06T02:59:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861975.53 sec. 2024-11-06T02:59:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:59:36Z- variances([0], [2], and psi_berg) = 0.374058 0.250552 0.000000 2024-11-06T02:59:36Z- publishEstimatesDecoder() - at 1730861975.92 sec 2024-11-06T02:59:36Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:59:36Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061692.76 594265.81 55.48 2024-11-06T02:59:36Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061692.90 594265.96 55.79 lat/lon: 36.70 -121.94 2024-11-06T02:59:36Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.37 0.25 0.72 0.00 2024-11-06T02:59:36Z- AHRS_M2 msg: 1730861976.31 epoch sec; seqNo:1425 2024-11-06T02:59:36Z- AHRS_M2 msg: 0.13 phi; -0.10 theta; 1.50 psi 2024-11-06T02:59:36Z- handleDepth()- Depth_Keller msg: 1730861976.32 epoch sec; seqNo:1527; depth 49.89 2024-11-06T02:59:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861976.34 epoch sec; seqNo:1527; 4061660.59 north; 594282.00 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T02:59:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861975.92 2024-11-06T02:59:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:36Z- waiting for 1730861979.92, time is 1730861976.34 2024-11-06T02:59:36Z- AHRS_M2 msg: 1730861976.71 epoch sec; seqNo:1426 2024-11-06T02:59:36Z- AHRS_M2 msg: 0.13 phi; -0.10 theta; 1.50 psi 2024-11-06T02:59:36Z- handleDepth()- Depth_Keller msg: 1730861976.71 epoch sec; seqNo:1528; depth 49.93 2024-11-06T02:59:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861976.72 epoch sec; seqNo:1528; 4061660.59 north; 594282.00 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T02:59:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861975.92 2024-11-06T02:59:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:36Z- waiting for 1730861979.92, time is 1730861976.72 2024-11-06T02:59:37Z- AHRS_M2 msg: 1730861977.12 epoch sec; seqNo:1427 2024-11-06T02:59:37Z- AHRS_M2 msg: 0.13 phi; -0.10 theta; 1.49 psi 2024-11-06T02:59:37Z- handleDepth()- Depth_Keller msg: 1730861977.12 epoch sec; seqNo:1529; depth 49.94 2024-11-06T02:59:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861977.13 epoch sec; seqNo:1529; 4061660.98 north; 594283.22 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T02:59:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861975.92 2024-11-06T02:59:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:37Z- waiting for 1730861979.92, time is 1730861977.13 2024-11-06T02:59:37Z- AHRS_M2 msg: 1730861977.52 epoch sec; seqNo:1428 2024-11-06T02:59:37Z- AHRS_M2 msg: 0.13 phi; -0.10 theta; 1.49 psi 2024-11-06T02:59:37Z- handleDepth()- Depth_Keller msg: 1730861977.53 epoch sec; seqNo:1530; depth 49.96 2024-11-06T02:59:37Z- handleDVL() - DVL msg epochMillisec: 1730861977.55 2024-11-06T02:59:37Z- handleDvl() - RDI_Pathfinder msg: 1730861977.55 sec; seqNo:473; #beams:4 2024-11-06T02:59:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 40.37 , 40.78 , 46.89 2024-11-06T02:59:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , -0.04 2024-11-06T02:59:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861977.57 epoch sec; seqNo:1530; 4061660.98 north; 594283.22 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T02:59:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861975.92 2024-11-06T02:59:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:37Z- waiting for 1730861979.92, time is 1730861977.57 2024-11-06T02:59:37Z- AHRS_M2 msg: 1730861977.92 epoch sec; seqNo:1429 2024-11-06T02:59:37Z- AHRS_M2 msg: 0.13 phi; -0.10 theta; 1.49 psi 2024-11-06T02:59:37Z- handleDepth()- Depth_Keller msg: 1730861977.95 epoch sec; seqNo:1531; depth 50.00 2024-11-06T02:59:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861977.96 epoch sec; seqNo:1531; 4061660.98 north; 594283.22 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T02:59:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861975.92 2024-11-06T02:59:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:38Z- waiting for 1730861979.92, time is 1730861977.96 2024-11-06T02:59:38Z- AHRS_M2 msg: 1730861978.33 epoch sec; seqNo:1430 2024-11-06T02:59:38Z- AHRS_M2 msg: 0.13 phi; -0.10 theta; 1.49 psi 2024-11-06T02:59:38Z- handleDepth()- Depth_Keller msg: 1730861978.33 epoch sec; seqNo:1532; depth 50.03 2024-11-06T02:59:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861978.35 epoch sec; seqNo:1532; 4061661.00 north; 594284.44 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T02:59:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861975.92 2024-11-06T02:59:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:38Z- waiting for 1730861979.92, time is 1730861978.35 2024-11-06T02:59:38Z- AHRS_M2 msg: 1730861978.73 epoch sec; seqNo:1431 2024-11-06T02:59:38Z- AHRS_M2 msg: 0.13 phi; -0.10 theta; 1.49 psi 2024-11-06T02:59:38Z- handleDepth()- Depth_Keller msg: 1730861978.74 epoch sec; seqNo:1533; depth 50.05 2024-11-06T02:59:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861978.75 epoch sec; seqNo:1533; 4061661.00 north; 594284.44 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T02:59:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861975.92 2024-11-06T02:59:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:38Z- waiting for 1730861979.92, time is 1730861978.75 2024-11-06T02:59:39Z- AHRS_M2 msg: 1730861979.14 epoch sec; seqNo:1432 2024-11-06T02:59:39Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 1.49 psi 2024-11-06T02:59:39Z- handleDepth()- Depth_Keller msg: 1730861979.15 epoch sec; seqNo:1534; depth 50.07 2024-11-06T02:59:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861979.16 epoch sec; seqNo:1534; 4061661.00 north; 594284.44 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T02:59:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861975.92 2024-11-06T02:59:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:39Z- waiting for 1730861979.92, time is 1730861979.16 2024-11-06T02:59:39Z- AHRS_M2 msg: 1730861979.54 epoch sec; seqNo:1433 2024-11-06T02:59:39Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 1.49 psi 2024-11-06T02:59:39Z- handleDepth()- Depth_Keller msg: 1730861979.54 epoch sec; seqNo:1535; depth 50.09 2024-11-06T02:59:39Z- handleDVL() - DVL msg epochMillisec: 1730861979.57 2024-11-06T02:59:39Z- handleDvl() - RDI_Pathfinder msg: 1730861979.57 sec; seqNo:474; #beams:4 2024-11-06T02:59:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.08 , 40.17 , 40.57 , 45.88 2024-11-06T02:59:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.00 , -0.05 2024-11-06T02:59:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861979.58 epoch sec; seqNo:1535; 4061661.01 north; 594285.66 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T02:59:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861975.92 2024-11-06T02:59:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:39Z- waiting for 1730861979.92, time is 1730861979.58 2024-11-06T02:59:39Z- AHRS_M2 msg: 1730861979.95 epoch sec; seqNo:1434 2024-11-06T02:59:39Z- AHRS_M2 msg: 0.14 phi; -0.09 theta; 1.48 psi 2024-11-06T02:59:39Z- handleDepth()- Depth_Keller msg: 1730861979.96 epoch sec; seqNo:1536; depth 50.11 2024-11-06T02:59:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861979.97 epoch sec; seqNo:1536; 4061661.01 north; 594285.66 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T02:59:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861975.92 2024-11-06T02:59:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:40Z- waiting for 1730861979.92, time is 1730861979.97 2024-11-06T02:59:40Z- LcmTrn::updateTrn() >>>> pose time 1730861979.97, meas time 1730861979.57 <<<< 2024-11-06T02:59:40Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861979.97 2024-11-06T02:59:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861979.57 sec, ping # 0; waiting for INS data more recent than 1730861975.92... 2024-11-06T02:59:40Z- TerrainNav::motionUpdate() - currEstimate = (4061661, 594285.66, 50.11) 2024-11-06T02:59:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:59:40Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:59:40Z- TNavPF::Projecting Measurements 2024-11-06T02:59:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:59:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:59:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:59:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:59:40Z- TNavFilter::4 good beams. 2024-11-06T02:59:40Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:59:40Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:59:40Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:59:40Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:59:40Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:59:40Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:59:40Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:59:40Z- TerrainMapOctree: (vehN, vehE ) = (4061660.97, 594285.29). 2024-11-06T02:59:40Z- TerrainMapOctree: Min Distance = 95.77. 2024-11-06T02:59:40Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:59:40Z- Forcing Subcloud Comparison 2024-11-06T02:59:40Z- Weighting particles with subcloud comparison 2024-11-06T02:59:40Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:40Z- meanExpectedMeasDiff: -1.101959 delta_rms_squared: 2.822665 alpha: 0.992964 2024-11-06T02:59:40Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:40Z- meanExpectedMeasDiff: 0.352801 delta_rms_squared: 1.131165 alpha: 0.982626 2024-11-06T02:59:40Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:40Z- meanExpectedMeasDiff: 4.272917 delta_rms_squared: 1.184418 alpha: 0.983394 2024-11-06T02:59:40Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:40Z- meanExpectedMeasDiff: -0.718190 delta_rms_squared: 1.471080 alpha: 0.986587 2024-11-06T02:59:40Z- Subcloud NIS: 0.222918 2024-11-06T02:59:40Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:59:40Z- TNavPF:: sumWeights = 0.073857 2024-11-06T02:59:40Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:59:40Z- TNavPF::First Term of Map Covariance Matrix is 0.25 2024-11-06T02:59:40Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T02:59:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861979.57 sec. 2024-11-06T02:59:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:59:40Z- variances([0], [2], and psi_berg) = 0.393554 0.259488 0.000000 2024-11-06T02:59:40Z- publishEstimatesDecoder() - at 1730861979.97 sec 2024-11-06T02:59:40Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:59:40Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061692.50 594269.41 55.37 2024-11-06T02:59:40Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061693.28 594269.65 55.77 lat/lon: 36.70 -121.94 2024-11-06T02:59:40Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.39 0.26 0.69 0.00 2024-11-06T02:59:40Z- AHRS_M2 msg: 1730861980.35 epoch sec; seqNo:1435 2024-11-06T02:59:40Z- AHRS_M2 msg: 0.14 phi; -0.09 theta; 1.48 psi 2024-11-06T02:59:40Z- handleDepth()- Depth_Keller msg: 1730861980.35 epoch sec; seqNo:1537; depth 50.15 2024-11-06T02:59:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861980.36 epoch sec; seqNo:1537; 4061661.01 north; 594285.66 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T02:59:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861979.97 2024-11-06T02:59:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:40Z- waiting for 1730861983.97, time is 1730861980.36 2024-11-06T02:59:40Z- AHRS_M2 msg: 1730861980.75 epoch sec; seqNo:1436 2024-11-06T02:59:40Z- AHRS_M2 msg: 0.14 phi; -0.09 theta; 1.48 psi 2024-11-06T02:59:40Z- handleDepth()- Depth_Keller msg: 1730861980.77 epoch sec; seqNo:1538; depth 50.18 2024-11-06T02:59:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861980.78 epoch sec; seqNo:1538; 4061661.01 north; 594285.66 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T02:59:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861979.97 2024-11-06T02:59:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:40Z- waiting for 1730861983.97, time is 1730861980.78 2024-11-06T02:59:41Z- AHRS_M2 msg: 1730861981.16 epoch sec; seqNo:1437 2024-11-06T02:59:41Z- AHRS_M2 msg: 0.14 phi; -0.08 theta; 1.48 psi 2024-11-06T02:59:41Z- handleDepth()- Depth_Keller msg: 1730861981.16 epoch sec; seqNo:1539; depth 50.19 2024-11-06T02:59:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861981.17 epoch sec; seqNo:1539; 4061661.03 north; 594286.88 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T02:59:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861979.97 2024-11-06T02:59:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:41Z- waiting for 1730861983.97, time is 1730861981.17 2024-11-06T02:59:41Z- handleDepth()- Depth_Keller msg: 1730861981.58 epoch sec; seqNo:1540; depth 50.21 2024-11-06T02:59:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861981.60 epoch sec; seqNo:1540; 4061661.03 north; 594286.88 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T02:59:41Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T02:59:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861979.97 2024-11-06T02:59:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:41Z- waiting for 1730861983.97, time is 1730861981.60 2024-11-06T02:59:41Z- AHRS_M2 msg: 1730861981.97 epoch sec; seqNo:1438 2024-11-06T02:59:41Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 1.47 psi 2024-11-06T02:59:41Z- handleDepth()- Depth_Keller msg: 1730861981.97 epoch sec; seqNo:1541; depth 50.25 2024-11-06T02:59:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861981.98 epoch sec; seqNo:1541; 4061661.03 north; 594286.88 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T02:59:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861979.97 2024-11-06T02:59:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:42Z- waiting for 1730861983.97, time is 1730861981.98 2024-11-06T02:59:42Z- AHRS_M2 msg: 1730861982.37 epoch sec; seqNo:1439 2024-11-06T02:59:42Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 1.47 psi 2024-11-06T02:59:42Z- handleDepth()- Depth_Keller msg: 1730861982.37 epoch sec; seqNo:1542; depth 50.28 2024-11-06T02:59:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861982.38 epoch sec; seqNo:1542; 4061661.03 north; 594286.88 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T02:59:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861979.97 2024-11-06T02:59:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:42Z- waiting for 1730861983.97, time is 1730861982.38 2024-11-06T02:59:42Z- AHRS_M2 msg: 1730861982.77 epoch sec; seqNo:1440 2024-11-06T02:59:42Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 1.46 psi 2024-11-06T02:59:42Z- handleDepth()- Depth_Keller msg: 1730861982.79 epoch sec; seqNo:1543; depth 50.29 2024-11-06T02:59:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861982.80 epoch sec; seqNo:1543; 4061661.04 north; 594288.10 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T02:59:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861979.97 2024-11-06T02:59:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:42Z- waiting for 1730861983.97, time is 1730861982.80 2024-11-06T02:59:43Z- AHRS_M2 msg: 1730861983.18 epoch sec; seqNo:1441 2024-11-06T02:59:43Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 1.46 psi 2024-11-06T02:59:43Z- handleDepth()- Depth_Keller msg: 1730861983.19 epoch sec; seqNo:1544; depth 50.32 2024-11-06T02:59:43Z- handleDVL() - DVL msg epochMillisec: 1730861983.19 2024-11-06T02:59:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:59:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:59:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:59:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:59:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:59:43Z- handleDvl() - RDI_Pathfinder msg: 1730861983.19 sec; seqNo:475; #beams:0 2024-11-06T02:59:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:59:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.00 , -0.05 2024-11-06T02:59:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861983.21 epoch sec; seqNo:1544; 4061661.42 north; 594288.09 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T02:59:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861979.97 2024-11-06T02:59:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:43Z- waiting for 1730861983.97, time is 1730861983.21 2024-11-06T02:59:43Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T02:59:43Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T02:59:43Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T02:59:43Z- AHRS_M2 msg: 1730861983.57 epoch sec; seqNo:1442 2024-11-06T02:59:43Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 1.46 psi 2024-11-06T02:59:43Z- handleDepth()- Depth_Keller msg: 1730861983.61 epoch sec; seqNo:1545; depth 50.34 2024-11-06T02:59:43Z- handleDVL() - DVL msg epochMillisec: 1730861983.64 2024-11-06T02:59:43Z- handleDvl() - RDI_Pathfinder msg: 1730861983.64 sec; seqNo:476; #beams:4 2024-11-06T02:59:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 39.96 , 40.57 , 45.26 2024-11-06T02:59:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.00 , 0.03 2024-11-06T02:59:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861983.68 epoch sec; seqNo:1545; 4061661.42 north; 594288.09 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T02:59:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861979.97 2024-11-06T02:59:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:43Z- waiting for 1730861983.97, time is 1730861983.68 2024-11-06T02:59:44Z- AHRS_M2 msg: 1730861983.99 epoch sec; seqNo:1443 2024-11-06T02:59:44Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 1.46 psi 2024-11-06T02:59:44Z- handleDepth()- Depth_Keller msg: 1730861984.00 epoch sec; seqNo:1546; depth 50.39 2024-11-06T02:59:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861984.02 epoch sec; seqNo:1546; 4061661.42 north; 594288.09 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T02:59:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861979.97 2024-11-06T02:59:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:44Z- waiting for 1730861983.97, time is 1730861984.02 2024-11-06T02:59:44Z- LcmTrn::updateTrn() >>>> pose time 1730861984.02, meas time 1730861983.64 <<<< 2024-11-06T02:59:44Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861984.02 2024-11-06T02:59:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861983.64 sec, ping # 0; waiting for INS data more recent than 1730861979.97... 2024-11-06T02:59:44Z- TerrainNav::motionUpdate() - currEstimate = (4061661, 594288.09, 50.39) 2024-11-06T02:59:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:59:44Z- TerrainNav:: SubcloudNIS is 0.22, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:59:44Z- TNavPF::Projecting Measurements 2024-11-06T02:59:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:59:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:59:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:59:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:59:44Z- TNavFilter::4 good beams. 2024-11-06T02:59:44Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:59:44Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:59:44Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:59:44Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:59:44Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:59:44Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:59:44Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:59:44Z- TerrainMapOctree: (vehN, vehE ) = (4061661.38, 594287.86). 2024-11-06T02:59:44Z- TerrainMapOctree: Min Distance = 96.37. 2024-11-06T02:59:44Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:59:44Z- Forcing Subcloud Comparison 2024-11-06T02:59:44Z- Weighting particles with subcloud comparison 2024-11-06T02:59:44Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:44Z- meanExpectedMeasDiff: -0.536821 delta_rms_squared: 1.488100 alpha: 0.986738 2024-11-06T02:59:44Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:44Z- meanExpectedMeasDiff: 0.068490 delta_rms_squared: 1.106516 alpha: 0.982246 2024-11-06T02:59:44Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:44Z- meanExpectedMeasDiff: 4.609258 delta_rms_squared: 0.798423 alpha: 0.975563 2024-11-06T02:59:44Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:44Z- meanExpectedMeasDiff: -0.871173 delta_rms_squared: 1.404903 alpha: 0.985964 2024-11-06T02:59:44Z- Subcloud NIS: 0.240314 2024-11-06T02:59:44Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:59:44Z- TNavPF:: sumWeights = 0.068556 2024-11-06T02:59:44Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:59:44Z- TNavPF::First Term of Map Covariance Matrix is 0.09 2024-11-06T02:59:44Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.01 2024-11-06T02:59:44Z- TerrainNav::Resampling particle filter... 2024-11-06T02:59:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861983.64 sec. 2024-11-06T02:59:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:59:44Z- variances([0], [2], and psi_berg) = 0.382764 0.234573 0.000000 2024-11-06T02:59:44Z- publishEstimatesDecoder() - at 1730861984.02 sec 2024-11-06T02:59:44Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:59:44Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061693.62 594271.83 58.24 2024-11-06T02:59:44Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061693.62 594271.93 55.81 lat/lon: 36.70 -121.94 2024-11-06T02:59:44Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.38 0.23 0.72 0.00 2024-11-06T02:59:44Z- AHRS_M2 msg: 1730861984.39 epoch sec; seqNo:1444 2024-11-06T02:59:44Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 1.46 psi 2024-11-06T02:59:44Z- handleDepth()- Depth_Keller msg: 1730861984.40 epoch sec; seqNo:1547; depth 50.41 2024-11-06T02:59:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861984.42 epoch sec; seqNo:1547; 4061661.43 north; 594289.31 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T02:59:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861984.02 2024-11-06T02:59:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:44Z- waiting for 1730861988.02, time is 1730861984.42 2024-11-06T02:59:44Z- AHRS_M2 msg: 1730861984.79 epoch sec; seqNo:1445 2024-11-06T02:59:44Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 1.46 psi 2024-11-06T02:59:44Z- handleDepth()- Depth_Keller msg: 1730861984.80 epoch sec; seqNo:1548; depth 50.44 2024-11-06T02:59:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861984.82 epoch sec; seqNo:1548; 4061661.43 north; 594289.31 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T02:59:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861984.02 2024-11-06T02:59:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:44Z- waiting for 1730861988.02, time is 1730861984.82 2024-11-06T02:59:45Z- AHRS_M2 msg: 1730861985.21 epoch sec; seqNo:1446 2024-11-06T02:59:45Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 1.45 psi 2024-11-06T02:59:45Z- handleDepth()- Depth_Keller msg: 1730861985.21 epoch sec; seqNo:1549; depth 50.45 2024-11-06T02:59:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861985.27 epoch sec; seqNo:1549; 4061661.43 north; 594289.31 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T02:59:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861984.02 2024-11-06T02:59:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:45Z- waiting for 1730861988.02, time is 1730861985.27 2024-11-06T02:59:45Z- AHRS_M2 msg: 1730861985.61 epoch sec; seqNo:1447 2024-11-06T02:59:45Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.45 psi 2024-11-06T02:59:45Z- handleDepth()- Depth_Keller msg: 1730861985.61 epoch sec; seqNo:1550; depth 50.50 2024-11-06T02:59:45Z- handleDVL() - DVL msg epochMillisec: 1730861985.63 2024-11-06T02:59:45Z- handleDvl() - RDI_Pathfinder msg: 1730861985.63 sec; seqNo:477; #beams:4 2024-11-06T02:59:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 39.96 , 40.57 , 45.06 2024-11-06T02:59:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.00 , 0.05 2024-11-06T02:59:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861985.67 epoch sec; seqNo:1550; 4061661.43 north; 594289.31 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T02:59:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861984.02 2024-11-06T02:59:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:45Z- waiting for 1730861988.02, time is 1730861985.67 2024-11-06T02:59:46Z- AHRS_M2 msg: 1730861986.00 epoch sec; seqNo:1448 2024-11-06T02:59:46Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.45 psi 2024-11-06T02:59:46Z- handleDepth()- Depth_Keller msg: 1730861986.01 epoch sec; seqNo:1551; depth 50.52 2024-11-06T02:59:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861986.04 epoch sec; seqNo:1551; 4061661.44 north; 594290.53 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T02:59:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861984.02 2024-11-06T02:59:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:46Z- waiting for 1730861988.02, time is 1730861986.04 2024-11-06T02:59:46Z- AHRS_M2 msg: 1730861986.41 epoch sec; seqNo:1449 2024-11-06T02:59:46Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.45 psi 2024-11-06T02:59:46Z- handleDepth()- Depth_Keller msg: 1730861986.42 epoch sec; seqNo:1552; depth 50.55 2024-11-06T02:59:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861986.43 epoch sec; seqNo:1552; 4061661.44 north; 594290.53 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T02:59:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861984.02 2024-11-06T02:59:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:46Z- waiting for 1730861988.02, time is 1730861986.43 2024-11-06T02:59:46Z- AHRS_M2 msg: 1730861986.81 epoch sec; seqNo:1450 2024-11-06T02:59:46Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.45 psi 2024-11-06T02:59:46Z- handleDepth()- Depth_Keller msg: 1730861986.82 epoch sec; seqNo:1553; depth 50.59 2024-11-06T02:59:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861986.83 epoch sec; seqNo:1553; 4061661.44 north; 594290.53 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T02:59:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861984.02 2024-11-06T02:59:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:46Z- waiting for 1730861988.02, time is 1730861986.83 2024-11-06T02:59:47Z- AHRS_M2 msg: 1730861987.23 epoch sec; seqNo:1451 2024-11-06T02:59:47Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.45 psi 2024-11-06T02:59:47Z- handleDepth()- Depth_Keller msg: 1730861987.29 epoch sec; seqNo:1554; depth 50.60 2024-11-06T02:59:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861987.32 epoch sec; seqNo:1554; 4061661.84 north; 594291.75 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T02:59:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861984.02 2024-11-06T02:59:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:47Z- waiting for 1730861988.02, time is 1730861987.32 2024-11-06T02:59:47Z- AHRS_M2 msg: 1730861987.62 epoch sec; seqNo:1452 2024-11-06T02:59:47Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 1.45 psi 2024-11-06T02:59:47Z- handleDepth()- Depth_Keller msg: 1730861987.62 epoch sec; seqNo:1555; depth 50.64 2024-11-06T02:59:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861987.65 epoch sec; seqNo:1555; 4061661.84 north; 594291.75 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T02:59:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861984.02 2024-11-06T02:59:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:47Z- waiting for 1730861988.02, time is 1730861987.65 2024-11-06T02:59:48Z- AHRS_M2 msg: 1730861988.02 epoch sec; seqNo:1453 2024-11-06T02:59:48Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 1.45 psi 2024-11-06T02:59:48Z- handleDepth()- Depth_Keller msg: 1730861988.03 epoch sec; seqNo:1556; depth 50.65 2024-11-06T02:59:48Z- handleDVL() - DVL msg epochMillisec: 1730861988.03 2024-11-06T02:59:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T02:59:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T02:59:48Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T02:59:48Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T02:59:48Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T02:59:48Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T02:59:48Z- handleDvl() - RDI_Pathfinder msg: 1730861988.03 sec; seqNo:478; #beams:0 2024-11-06T02:59:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T02:59:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.00 , 0.05 2024-11-06T02:59:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861988.04 epoch sec; seqNo:1556; 4061661.84 north; 594291.75 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T02:59:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861984.02 2024-11-06T02:59:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:48Z- waiting for 1730861988.02, time is 1730861988.04 2024-11-06T02:59:48Z- LcmTrn::updateTrn() >>>> pose time 1730861988.04, meas time 1730861988.03 <<<< 2024-11-06T02:59:48Z- LcmTrn::updateTrn() equating pose 1730861988.03 and meas times 1730861988.03 2024-11-06T02:59:48Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730861988.03 2024-11-06T02:59:48Z- TerrainNav::motionUpdate() - currEstimate = (4061662, 594291.75, 50.65) 2024-11-06T02:59:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:59:48Z- TerrainNav:: SubcloudNIS is 0.24, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:59:48Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T02:59:48Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T02:59:48Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T02:59:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:59:48Z- variances([0], [2], and psi_berg) = 0.384556 0.235807 0.000000 2024-11-06T02:59:48Z- publishEstimatesDecoder() - at 1730861988.03 sec 2024-11-06T02:59:48Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:59:48Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061694.14 594275.53 57.71 2024-11-06T02:59:48Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061694.04 594275.58 56.08 lat/lon: 36.70 -121.94 2024-11-06T02:59:48Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.38 0.24 0.96 0.00 2024-11-06T02:59:48Z- AHRS_M2 msg: 1730861988.43 epoch sec; seqNo:1454 2024-11-06T02:59:48Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 1.45 psi 2024-11-06T02:59:48Z- handleDepth()- Depth_Keller msg: 1730861988.44 epoch sec; seqNo:1557; depth 50.68 2024-11-06T02:59:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861988.45 epoch sec; seqNo:1557; 4061661.84 north; 594291.75 east; 36.70 lat; -121.94 lon; 50.68 depth; utmZone: 10 2024-11-06T02:59:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861988.03 2024-11-06T02:59:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:48Z- waiting for 1730861992.03, time is 1730861988.45 2024-11-06T02:59:48Z- AHRS_M2 msg: 1730861988.83 epoch sec; seqNo:1455 2024-11-06T02:59:48Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 1.45 psi 2024-11-06T02:59:48Z- handleDepth()- Depth_Keller msg: 1730861988.84 epoch sec; seqNo:1558; depth 50.70 2024-11-06T02:59:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861988.86 epoch sec; seqNo:1558; 4061661.85 north; 594292.97 east; 36.70 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T02:59:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861988.03 2024-11-06T02:59:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:48Z- waiting for 1730861992.03, time is 1730861988.86 2024-11-06T02:59:49Z- AHRS_M2 msg: 1730861989.28 epoch sec; seqNo:1456 2024-11-06T02:59:49Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 1.45 psi 2024-11-06T02:59:49Z- handleDepth()- Depth_Keller msg: 1730861989.29 epoch sec; seqNo:1559; depth 50.72 2024-11-06T02:59:49Z- handleDVL() - DVL msg epochMillisec: 1730861989.32 2024-11-06T02:59:49Z- handleDvl() - RDI_Pathfinder msg: 1730861989.32 sec; seqNo:479; #beams:4 2024-11-06T02:59:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.47 , 39.55 , 40.17 , 44.24 2024-11-06T02:59:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , 0.02 2024-11-06T02:59:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861989.33 epoch sec; seqNo:1559; 4061661.85 north; 594292.97 east; 36.70 lat; -121.94 lon; 50.72 depth; utmZone: 10 2024-11-06T02:59:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861988.03 2024-11-06T02:59:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:49Z- waiting for 1730861992.03, time is 1730861989.33 2024-11-06T02:59:49Z- AHRS_M2 msg: 1730861989.64 epoch sec; seqNo:1457 2024-11-06T02:59:49Z- AHRS_M2 msg: 0.13 phi; -0.05 theta; 1.45 psi 2024-11-06T02:59:49Z- handleDepth()- Depth_Keller msg: 1730861989.65 epoch sec; seqNo:1560; depth 50.74 2024-11-06T02:59:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861989.68 epoch sec; seqNo:1560; 4061661.85 north; 594292.97 east; 36.70 lat; -121.94 lon; 50.74 depth; utmZone: 10 2024-11-06T02:59:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861988.03 2024-11-06T02:59:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:49Z- waiting for 1730861992.03, time is 1730861989.68 2024-11-06T02:59:50Z- AHRS_M2 msg: 1730861990.05 epoch sec; seqNo:1458 2024-11-06T02:59:50Z- AHRS_M2 msg: 0.13 phi; -0.05 theta; 1.45 psi 2024-11-06T02:59:50Z- handleDepth()- Depth_Keller msg: 1730861990.06 epoch sec; seqNo:1561; depth 50.75 2024-11-06T02:59:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861990.07 epoch sec; seqNo:1561; 4061661.85 north; 594292.97 east; 36.70 lat; -121.94 lon; 50.75 depth; utmZone: 10 2024-11-06T02:59:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861988.03 2024-11-06T02:59:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:50Z- waiting for 1730861992.03, time is 1730861990.07 2024-11-06T02:59:50Z- AHRS_M2 msg: 1730861990.45 epoch sec; seqNo:1459 2024-11-06T02:59:50Z- AHRS_M2 msg: 0.13 phi; -0.05 theta; 1.45 psi 2024-11-06T02:59:50Z- handleDepth()- Depth_Keller msg: 1730861990.46 epoch sec; seqNo:1562; depth 50.76 2024-11-06T02:59:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861990.47 epoch sec; seqNo:1562; 4061661.86 north; 594294.19 east; 36.70 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T02:59:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861988.03 2024-11-06T02:59:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:50Z- waiting for 1730861992.03, time is 1730861990.47 2024-11-06T02:59:50Z- AHRS_M2 msg: 1730861990.85 epoch sec; seqNo:1460 2024-11-06T02:59:50Z- AHRS_M2 msg: 0.12 phi; -0.05 theta; 1.44 psi 2024-11-06T02:59:50Z- handleDepth()- Depth_Keller msg: 1730861990.86 epoch sec; seqNo:1563; depth 50.77 2024-11-06T02:59:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861990.87 epoch sec; seqNo:1563; 4061661.86 north; 594294.19 east; 36.70 lat; -121.94 lon; 50.77 depth; utmZone: 10 2024-11-06T02:59:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861988.03 2024-11-06T02:59:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:51Z- waiting for 1730861992.03, time is 1730861990.87 2024-11-06T02:59:51Z- AHRS_M2 msg: 1730861991.29 epoch sec; seqNo:1461 2024-11-06T02:59:51Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 1.44 psi 2024-11-06T02:59:51Z- handleDepth()- Depth_Keller msg: 1730861991.31 epoch sec; seqNo:1564; depth 50.79 2024-11-06T02:59:51Z- handleDVL() - DVL msg epochMillisec: 1730861991.33 2024-11-06T02:59:51Z- handleDvl() - RDI_Pathfinder msg: 1730861991.33 sec; seqNo:480; #beams:4 2024-11-06T02:59:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.06 , 39.55 , 40.37 , 43.63 2024-11-06T02:59:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , -0.02 2024-11-06T02:59:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861991.34 epoch sec; seqNo:1564; 4061662.24 north; 594294.19 east; 36.70 lat; -121.94 lon; 50.79 depth; utmZone: 10 2024-11-06T02:59:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861988.03 2024-11-06T02:59:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:51Z- waiting for 1730861992.03, time is 1730861991.34 2024-11-06T02:59:51Z- AHRS_M2 msg: 1730861991.67 epoch sec; seqNo:1462 2024-11-06T02:59:51Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 1.44 psi 2024-11-06T02:59:51Z- handleDepth()- Depth_Keller msg: 1730861991.67 epoch sec; seqNo:1565; depth 50.79 2024-11-06T02:59:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861991.68 epoch sec; seqNo:1565; 4061662.26 north; 594295.41 east; 36.70 lat; -121.94 lon; 50.79 depth; utmZone: 10 2024-11-06T02:59:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861988.03 2024-11-06T02:59:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:51Z- waiting for 1730861992.03, time is 1730861991.68 2024-11-06T02:59:52Z- AHRS_M2 msg: 1730861992.06 epoch sec; seqNo:1463 2024-11-06T02:59:52Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 1.44 psi 2024-11-06T02:59:52Z- handleDepth()- Depth_Keller msg: 1730861992.07 epoch sec; seqNo:1566; depth 50.82 2024-11-06T02:59:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861992.08 epoch sec; seqNo:1566; 4061662.26 north; 594295.41 east; 36.70 lat; -121.94 lon; 50.82 depth; utmZone: 10 2024-11-06T02:59:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861988.03 2024-11-06T02:59:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:52Z- waiting for 1730861992.03, time is 1730861992.08 2024-11-06T02:59:52Z- LcmTrn::updateTrn() >>>> pose time 1730861992.08, meas time 1730861991.33 <<<< 2024-11-06T02:59:52Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861992.08 2024-11-06T02:59:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861991.33 sec, ping # 0; waiting for INS data more recent than 1730861988.03... 2024-11-06T02:59:52Z- TerrainNav::motionUpdate() - currEstimate = (4061662, 594295.41, 50.82) 2024-11-06T02:59:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:59:52Z- TerrainNav:: SubcloudNIS is 0.24, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:59:52Z- TNavPF::Projecting Measurements 2024-11-06T02:59:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:59:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:59:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:59:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:59:52Z- TNavFilter::4 good beams. 2024-11-06T02:59:52Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:59:52Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:59:52Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:59:52Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:59:52Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:59:52Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:59:52Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:59:52Z- TerrainMapOctree: (vehN, vehE ) = (4061662.18, 594294.73). 2024-11-06T02:59:52Z- TerrainMapOctree: Min Distance = 98.56. 2024-11-06T02:59:52Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:59:52Z- Forcing Subcloud Comparison 2024-11-06T02:59:52Z- Weighting particles with subcloud comparison 2024-11-06T02:59:52Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:52Z- meanExpectedMeasDiff: -0.077182 delta_rms_squared: 1.881241 alpha: 0.989481 2024-11-06T02:59:52Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:52Z- meanExpectedMeasDiff: 1.067179 delta_rms_squared: 1.429465 alpha: 0.986202 2024-11-06T02:59:52Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:52Z- meanExpectedMeasDiff: 4.771957 delta_rms_squared: 2.074241 alpha: 0.990450 2024-11-06T02:59:52Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:52Z- meanExpectedMeasDiff: -1.185097 delta_rms_squared: 1.739093 alpha: 0.988631 2024-11-06T02:59:52Z- Subcloud NIS: 0.261599 2024-11-06T02:59:52Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:59:52Z- TNavPF:: sumWeights = 0.058061 2024-11-06T02:59:52Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:59:52Z- TNavPF::First Term of Map Covariance Matrix is 0.07 2024-11-06T02:59:52Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T02:59:52Z- TerrainNav::Resampling particle filter... 2024-11-06T02:59:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861991.33 sec. 2024-11-06T02:59:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:59:52Z- variances([0], [2], and psi_berg) = 0.368785 0.232665 0.000000 2024-11-06T02:59:52Z- publishEstimatesDecoder() - at 1730861992.08 sec 2024-11-06T02:59:52Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:59:52Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061694.49 594279.14 56.78 2024-11-06T02:59:52Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061694.50 594279.28 55.62 lat/lon: 36.70 -121.94 2024-11-06T02:59:52Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.37 0.23 0.75 0.00 2024-11-06T02:59:52Z- AHRS_M2 msg: 1730861992.47 epoch sec; seqNo:1464 2024-11-06T02:59:52Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 1.44 psi 2024-11-06T02:59:52Z- handleDepth()- Depth_Keller msg: 1730861992.49 epoch sec; seqNo:1567; depth 50.81 2024-11-06T02:59:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861992.51 epoch sec; seqNo:1567; 4061662.26 north; 594295.41 east; 36.70 lat; -121.94 lon; 50.81 depth; utmZone: 10 2024-11-06T02:59:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861992.08 2024-11-06T02:59:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:52Z- waiting for 1730861996.08, time is 1730861992.51 2024-11-06T02:59:52Z- AHRS_M2 msg: 1730861992.87 epoch sec; seqNo:1465 2024-11-06T02:59:52Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 1.44 psi 2024-11-06T02:59:52Z- handleDepth()- Depth_Keller msg: 1730861992.88 epoch sec; seqNo:1568; depth 50.81 2024-11-06T02:59:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861992.88 epoch sec; seqNo:1568; 4061662.27 north; 594296.63 east; 36.70 lat; -121.94 lon; 50.81 depth; utmZone: 10 2024-11-06T02:59:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861992.08 2024-11-06T02:59:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:53Z- waiting for 1730861996.08, time is 1730861992.88 2024-11-06T02:59:53Z- AHRS_M2 msg: 1730861993.30 epoch sec; seqNo:1466 2024-11-06T02:59:53Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 1.44 psi 2024-11-06T02:59:53Z- handleDepth()- Depth_Keller msg: 1730861993.30 epoch sec; seqNo:1569; depth 50.82 2024-11-06T02:59:53Z- handleDVL() - DVL msg epochMillisec: 1730861993.32 2024-11-06T02:59:53Z- handleDvl() - RDI_Pathfinder msg: 1730861993.32 sec; seqNo:481; #beams:4 2024-11-06T02:59:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.85 , 39.35 , 40.37 , 43.22 2024-11-06T02:59:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.03 , -0.05 2024-11-06T02:59:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861993.33 epoch sec; seqNo:1569; 4061662.27 north; 594296.63 east; 36.70 lat; -121.94 lon; 50.82 depth; utmZone: 10 2024-11-06T02:59:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861992.08 2024-11-06T02:59:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:53Z- waiting for 1730861996.08, time is 1730861993.33 2024-11-06T02:59:53Z- AHRS_M2 msg: 1730861993.68 epoch sec; seqNo:1467 2024-11-06T02:59:53Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 1.44 psi 2024-11-06T02:59:53Z- handleDepth()- Depth_Keller msg: 1730861993.68 epoch sec; seqNo:1570; depth 50.82 2024-11-06T02:59:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861993.70 epoch sec; seqNo:1570; 4061662.27 north; 594296.63 east; 36.70 lat; -121.94 lon; 50.82 depth; utmZone: 10 2024-11-06T02:59:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861992.08 2024-11-06T02:59:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:53Z- waiting for 1730861996.08, time is 1730861993.70 2024-11-06T02:59:54Z- AHRS_M2 msg: 1730861994.08 epoch sec; seqNo:1468 2024-11-06T02:59:54Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.44 psi 2024-11-06T02:59:54Z- handleDepth()- Depth_Keller msg: 1730861994.09 epoch sec; seqNo:1571; depth 50.83 2024-11-06T02:59:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861994.11 epoch sec; seqNo:1571; 4061662.27 north; 594296.63 east; 36.70 lat; -121.94 lon; 50.83 depth; utmZone: 10 2024-11-06T02:59:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861992.08 2024-11-06T02:59:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:54Z- waiting for 1730861996.08, time is 1730861994.11 2024-11-06T02:59:54Z- AHRS_M2 msg: 1730861994.50 epoch sec; seqNo:1469 2024-11-06T02:59:54Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.44 psi 2024-11-06T02:59:54Z- handleDepth()- Depth_Keller msg: 1730861994.51 epoch sec; seqNo:1572; depth 50.82 2024-11-06T02:59:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861994.52 epoch sec; seqNo:1572; 4061662.28 north; 594297.85 east; 36.70 lat; -121.94 lon; 50.82 depth; utmZone: 10 2024-11-06T02:59:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861992.08 2024-11-06T02:59:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:54Z- waiting for 1730861996.08, time is 1730861994.52 2024-11-06T02:59:54Z- AHRS_M2 msg: 1730861994.89 epoch sec; seqNo:1470 2024-11-06T02:59:54Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.44 psi 2024-11-06T02:59:54Z- handleDepth()- Depth_Keller msg: 1730861994.89 epoch sec; seqNo:1573; depth 50.84 2024-11-06T02:59:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861994.91 epoch sec; seqNo:1573; 4061662.28 north; 594297.85 east; 36.70 lat; -121.94 lon; 50.84 depth; utmZone: 10 2024-11-06T02:59:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861992.08 2024-11-06T02:59:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:55Z- waiting for 1730861996.08, time is 1730861994.91 2024-11-06T02:59:55Z- AHRS_M2 msg: 1730861995.30 epoch sec; seqNo:1471 2024-11-06T02:59:55Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.44 psi 2024-11-06T02:59:55Z- handleDepth()- Depth_Keller msg: 1730861995.30 epoch sec; seqNo:1574; depth 50.84 2024-11-06T02:59:55Z- handleDVL() - DVL msg epochMillisec: 1730861995.34 2024-11-06T02:59:55Z- handleDvl() - RDI_Pathfinder msg: 1730861995.34 sec; seqNo:482; #beams:4 2024-11-06T02:59:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.26 , 39.15 , 40.17 , 43.02 2024-11-06T02:59:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.00 , -0.04 2024-11-06T02:59:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861995.36 epoch sec; seqNo:1574; 4061662.28 north; 594297.85 east; 36.70 lat; -121.94 lon; 50.84 depth; utmZone: 10 2024-11-06T02:59:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861992.08 2024-11-06T02:59:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:55Z- waiting for 1730861996.08, time is 1730861995.36 2024-11-06T02:59:55Z- AHRS_M2 msg: 1730861995.70 epoch sec; seqNo:1472 2024-11-06T02:59:55Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.44 psi 2024-11-06T02:59:55Z- handleDepth()- Depth_Keller msg: 1730861995.70 epoch sec; seqNo:1575; depth 50.83 2024-11-06T02:59:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861995.72 epoch sec; seqNo:1575; 4061662.28 north; 594297.85 east; 36.70 lat; -121.94 lon; 50.83 depth; utmZone: 10 2024-11-06T02:59:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861992.08 2024-11-06T02:59:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:55Z- waiting for 1730861996.08, time is 1730861995.72 2024-11-06T02:59:56Z- AHRS_M2 msg: 1730861996.10 epoch sec; seqNo:1473 2024-11-06T02:59:56Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.44 psi 2024-11-06T02:59:56Z- handleDepth()- Depth_Keller msg: 1730861996.11 epoch sec; seqNo:1576; depth 50.84 2024-11-06T02:59:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861996.13 epoch sec; seqNo:1576; 4061662.67 north; 594299.06 east; 36.70 lat; -121.94 lon; 50.84 depth; utmZone: 10 2024-11-06T02:59:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861992.08 2024-11-06T02:59:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:56Z- waiting for 1730861996.08, time is 1730861996.13 2024-11-06T02:59:56Z- LcmTrn::updateTrn() >>>> pose time 1730861996.13, meas time 1730861995.34 <<<< 2024-11-06T02:59:56Z- LcmTrn::updateTrn() measUpdate first last update time: 1730861996.13 2024-11-06T02:59:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861995.34 sec, ping # 0; waiting for INS data more recent than 1730861992.08... 2024-11-06T02:59:56Z- TerrainNav::motionUpdate() - currEstimate = (4061663, 594299.06, 50.84) 2024-11-06T02:59:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T02:59:56Z- TerrainNav:: SubcloudNIS is 0.26, in allowable region under 1.40. Keep on Trucking. 2024-11-06T02:59:56Z- TNavPF::Projecting Measurements 2024-11-06T02:59:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T02:59:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T02:59:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T02:59:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T02:59:56Z- TNavFilter::4 good beams. 2024-11-06T02:59:56Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T02:59:56Z- TNavPF:: beamIndex[0] = 0 2024-11-06T02:59:56Z- TNavPF:: beamIndex[1] = 1 2024-11-06T02:59:56Z- TNavPF:: beamIndex[2] = 2 2024-11-06T02:59:56Z- TNavPF:: beamIndex[3] = 3 2024-11-06T02:59:56Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T02:59:56Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T02:59:56Z- TerrainMapOctree: (vehN, vehE ) = (4061662.59, 594298.35). 2024-11-06T02:59:56Z- TerrainMapOctree: Min Distance = 99.89. 2024-11-06T02:59:56Z- TerrainMapOctree: Using tile 4. 2024-11-06T02:59:56Z- Forcing Subcloud Comparison 2024-11-06T02:59:56Z- Weighting particles with subcloud comparison 2024-11-06T02:59:56Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:56Z- meanExpectedMeasDiff: -0.267239 delta_rms_squared: 1.521952 alpha: 0.987029 2024-11-06T02:59:56Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:56Z- meanExpectedMeasDiff: 0.979633 delta_rms_squared: 1.678662 alpha: 0.988226 2024-11-06T02:59:56Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:56Z- meanExpectedMeasDiff: 3.443806 delta_rms_squared: 1.302253 alpha: 0.984874 2024-11-06T02:59:56Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T02:59:56Z- meanExpectedMeasDiff: -2.118283 delta_rms_squared: 1.397914 alpha: 0.985895 2024-11-06T02:59:56Z- Subcloud NIS: 0.275860 2024-11-06T02:59:56Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T02:59:56Z- TNavPF:: sumWeights = 0.100172 2024-11-06T02:59:56Z- TNavPF::Calculating NIS Matrices 2024-11-06T02:59:56Z- TNavPF::First Term of Map Covariance Matrix is 0.10 2024-11-06T02:59:56Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T02:59:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861995.34 sec. 2024-11-06T02:59:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T02:59:56Z- variances([0], [2], and psi_berg) = 0.380315 0.239486 0.000000 2024-11-06T02:59:56Z- publishEstimatesDecoder() - at 1730861996.13 sec 2024-11-06T02:59:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T02:59:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061696.10 594283.71 54.20 2024-11-06T02:59:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061694.91 594282.94 55.38 lat/lon: 36.70 -121.94 2024-11-06T02:59:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.38 0.24 0.72 0.00 2024-11-06T02:59:56Z- AHRS_M2 msg: 1730861996.51 epoch sec; seqNo:1474 2024-11-06T02:59:56Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.44 psi 2024-11-06T02:59:56Z- handleDepth()- Depth_Keller msg: 1730861996.52 epoch sec; seqNo:1577; depth 50.84 2024-11-06T02:59:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861996.54 epoch sec; seqNo:1577; 4061662.67 north; 594299.06 east; 36.70 lat; -121.94 lon; 50.84 depth; utmZone: 10 2024-11-06T02:59:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861996.13 2024-11-06T02:59:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:56Z- waiting for 1730862000.13, time is 1730861996.54 2024-11-06T02:59:56Z- AHRS_M2 msg: 1730861996.91 epoch sec; seqNo:1475 2024-11-06T02:59:56Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.44 psi 2024-11-06T02:59:56Z- handleDepth()- Depth_Keller msg: 1730861996.92 epoch sec; seqNo:1578; depth 50.86 2024-11-06T02:59:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861996.92 epoch sec; seqNo:1578; 4061662.67 north; 594299.06 east; 36.70 lat; -121.94 lon; 50.86 depth; utmZone: 10 2024-11-06T02:59:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861996.13 2024-11-06T02:59:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:57Z- waiting for 1730862000.13, time is 1730861996.92 2024-11-06T02:59:57Z- AHRS_M2 msg: 1730861997.32 epoch sec; seqNo:1476 2024-11-06T02:59:57Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.44 psi 2024-11-06T02:59:57Z- handleDepth()- Depth_Keller msg: 1730861997.33 epoch sec; seqNo:1579; depth 50.87 2024-11-06T02:59:57Z- handleDVL() - DVL msg epochMillisec: 1730861997.36 2024-11-06T02:59:57Z- handleDvl() - RDI_Pathfinder msg: 1730861997.36 sec; seqNo:483; #beams:4 2024-11-06T02:59:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.85 , 39.15 , 40.78 , 42.61 2024-11-06T02:59:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.01 , -0.02 2024-11-06T02:59:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861997.38 epoch sec; seqNo:1579; 4061662.67 north; 594299.06 east; 36.70 lat; -121.94 lon; 50.87 depth; utmZone: 10 2024-11-06T02:59:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861996.13 2024-11-06T02:59:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:57Z- waiting for 1730862000.13, time is 1730861997.38 2024-11-06T02:59:57Z- AHRS_M2 msg: 1730861997.72 epoch sec; seqNo:1477 2024-11-06T02:59:57Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.43 psi 2024-11-06T02:59:57Z- handleDepth()- Depth_Keller msg: 1730861997.73 epoch sec; seqNo:1580; depth 50.87 2024-11-06T02:59:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861997.75 epoch sec; seqNo:1580; 4061662.69 north; 594300.28 east; 36.70 lat; -121.94 lon; 50.87 depth; utmZone: 10 2024-11-06T02:59:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861996.13 2024-11-06T02:59:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:57Z- waiting for 1730862000.13, time is 1730861997.75 2024-11-06T02:59:58Z- AHRS_M2 msg: 1730861998.12 epoch sec; seqNo:1478 2024-11-06T02:59:58Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.43 psi 2024-11-06T02:59:58Z- handleDepth()- Depth_Keller msg: 1730861998.13 epoch sec; seqNo:1581; depth 50.87 2024-11-06T02:59:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861998.15 epoch sec; seqNo:1581; 4061662.69 north; 594300.28 east; 36.70 lat; -121.94 lon; 50.87 depth; utmZone: 10 2024-11-06T02:59:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861996.13 2024-11-06T02:59:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:58Z- waiting for 1730862000.13, time is 1730861998.15 2024-11-06T02:59:58Z- AHRS_M2 msg: 1730861998.53 epoch sec; seqNo:1479 2024-11-06T02:59:58Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.43 psi 2024-11-06T02:59:58Z- handleDepth()- Depth_Keller msg: 1730861998.54 epoch sec; seqNo:1582; depth 50.87 2024-11-06T02:59:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861998.55 epoch sec; seqNo:1582; 4061662.69 north; 594300.28 east; 36.70 lat; -121.94 lon; 50.87 depth; utmZone: 10 2024-11-06T02:59:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861996.13 2024-11-06T02:59:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:58Z- waiting for 1730862000.13, time is 1730861998.55 2024-11-06T02:59:58Z- AHRS_M2 msg: 1730861998.93 epoch sec; seqNo:1480 2024-11-06T02:59:58Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 1.42 psi 2024-11-06T02:59:58Z- handleDepth()- Depth_Keller msg: 1730861998.94 epoch sec; seqNo:1583; depth 50.90 2024-11-06T02:59:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861998.95 epoch sec; seqNo:1583; 4061662.69 north; 594300.28 east; 36.70 lat; -121.94 lon; 50.90 depth; utmZone: 10 2024-11-06T02:59:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861996.13 2024-11-06T02:59:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:59Z- waiting for 1730862000.13, time is 1730861998.95 2024-11-06T02:59:59Z- AHRS_M2 msg: 1730861999.33 epoch sec; seqNo:1481 2024-11-06T02:59:59Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 1.42 psi 2024-11-06T02:59:59Z- handleDepth()- Depth_Keller msg: 1730861999.34 epoch sec; seqNo:1584; depth 50.89 2024-11-06T02:59:59Z- handleDVL() - DVL msg epochMillisec: 1730861999.37 2024-11-06T02:59:59Z- handleDvl() - RDI_Pathfinder msg: 1730861999.37 sec; seqNo:484; #beams:4 2024-11-06T02:59:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.65 , 39.15 , 40.57 , 42.41 2024-11-06T02:59:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.03 , 0.01 2024-11-06T02:59:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861999.39 epoch sec; seqNo:1584; 4061662.70 north; 594301.50 east; 36.70 lat; -121.94 lon; 50.89 depth; utmZone: 10 2024-11-06T02:59:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T02:59:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861996.13 2024-11-06T02:59:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:59Z- waiting for 1730862000.13, time is 1730861999.39 2024-11-06T02:59:59Z- AHRS_M2 msg: 1730861999.74 epoch sec; seqNo:1482 2024-11-06T02:59:59Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 1.42 psi 2024-11-06T02:59:59Z- handleDepth()- Depth_Keller msg: 1730861999.75 epoch sec; seqNo:1585; depth 50.92 2024-11-06T02:59:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730861999.76 epoch sec; seqNo:1585; 4061663.08 north; 594301.50 east; 36.70 lat; -121.94 lon; 50.92 depth; utmZone: 10 2024-11-06T02:59:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T02:59:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861996.13 2024-11-06T02:59:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T02:59:59Z- waiting for 1730862000.13, time is 1730861999.76 2024-11-06T03:00:00Z- AHRS_M2 msg: 1730862000.15 epoch sec; seqNo:1483 2024-11-06T03:00:00Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.42 psi 2024-11-06T03:00:00Z- handleDepth()- Depth_Keller msg: 1730862000.15 epoch sec; seqNo:1586; depth 50.92 2024-11-06T03:00:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862000.16 epoch sec; seqNo:1586; 4061663.08 north; 594301.50 east; 36.70 lat; -121.94 lon; 50.92 depth; utmZone: 10 2024-11-06T03:00:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730861996.13 2024-11-06T03:00:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:00Z- waiting for 1730862000.13, time is 1730862000.16 2024-11-06T03:00:00Z- LcmTrn::updateTrn() >>>> pose time 1730862000.16, meas time 1730861999.37 <<<< 2024-11-06T03:00:00Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862000.16 2024-11-06T03:00:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730861999.37 sec, ping # 0; waiting for INS data more recent than 1730861996.13... 2024-11-06T03:00:00Z- TerrainNav::motionUpdate() - currEstimate = (4061663, 594301.50, 50.92) 2024-11-06T03:00:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:00:00Z- TerrainNav:: SubcloudNIS is 0.28, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:00:00Z- TNavPF::Projecting Measurements 2024-11-06T03:00:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:00:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:00:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:00:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:00:00Z- TNavFilter::4 good beams. 2024-11-06T03:00:00Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:00:00Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:00:00Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:00:00Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:00:00Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:00:00Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:00:00Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:00:00Z- TerrainMapOctree: (vehN, vehE ) = (4061663.00, 594301.02). 2024-11-06T03:00:00Z- TerrainMapOctree: Min Distance = 100.86. 2024-11-06T03:00:00Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:00:00Z- Forcing Subcloud Comparison 2024-11-06T03:00:00Z- Weighting particles with subcloud comparison 2024-11-06T03:00:00Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:00Z- meanExpectedMeasDiff: 0.366361 delta_rms_squared: 1.550097 alpha: 0.987262 2024-11-06T03:00:00Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:00Z- meanExpectedMeasDiff: 1.436629 delta_rms_squared: 1.766246 alpha: 0.988803 2024-11-06T03:00:00Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:00Z- meanExpectedMeasDiff: 3.341390 delta_rms_squared: 1.306818 alpha: 0.984926 2024-11-06T03:00:00Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:00Z- meanExpectedMeasDiff: -2.439768 delta_rms_squared: 1.348347 alpha: 0.985384 2024-11-06T03:00:00Z- Subcloud NIS: 0.302897 2024-11-06T03:00:00Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:00:00Z- TNavPF:: sumWeights = 0.086181 2024-11-06T03:00:00Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:00:00Z- TNavPF::First Term of Map Covariance Matrix is 0.06 2024-11-06T03:00:00Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T03:00:00Z- TerrainNav::Resampling particle filter... 2024-11-06T03:00:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730861999.37 sec. 2024-11-06T03:00:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:00:00Z- variances([0], [2], and psi_berg) = 0.375893 0.231190 0.000000 2024-11-06T03:00:00Z- publishEstimatesDecoder() - at 1730862000.16 sec 2024-11-06T03:00:00Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:00:00Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061695.41 594285.24 54.73 2024-11-06T03:00:00Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061695.28 594285.35 55.15 lat/lon: 36.70 -121.94 2024-11-06T03:00:00Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.38 0.23 0.71 0.00 2024-11-06T03:00:00Z- AHRS_M2 msg: 1730862000.55 epoch sec; seqNo:1484 2024-11-06T03:00:00Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.42 psi 2024-11-06T03:00:00Z- handleDepth()- Depth_Keller msg: 1730862000.57 epoch sec; seqNo:1587; depth 50.93 2024-11-06T03:00:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862000.59 epoch sec; seqNo:1587; 4061663.08 north; 594301.50 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T03:00:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862000.16 2024-11-06T03:00:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:00Z- waiting for 1730862004.16, time is 1730862000.59 2024-11-06T03:00:00Z- AHRS_M2 msg: 1730862000.95 epoch sec; seqNo:1485 2024-11-06T03:00:00Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.42 psi 2024-11-06T03:00:00Z- handleDepth()- Depth_Keller msg: 1730862000.96 epoch sec; seqNo:1588; depth 50.95 2024-11-06T03:00:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862000.97 epoch sec; seqNo:1588; 4061663.09 north; 594302.72 east; 36.70 lat; -121.94 lon; 50.95 depth; utmZone: 10 2024-11-06T03:00:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862000.16 2024-11-06T03:00:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:01Z- waiting for 1730862004.16, time is 1730862000.97 2024-11-06T03:00:01Z- AHRS_M2 msg: 1730862001.35 epoch sec; seqNo:1486 2024-11-06T03:00:01Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.42 psi 2024-11-06T03:00:01Z- handleDepth()- Depth_Keller msg: 1730862001.38 epoch sec; seqNo:1589; depth 50.96 2024-11-06T03:00:01Z- handleDVL() - DVL msg epochMillisec: 1730862001.40 2024-11-06T03:00:01Z- handleDvl() - RDI_Pathfinder msg: 1730862001.40 sec; seqNo:485; #beams:4 2024-11-06T03:00:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.65 , 39.15 , 40.37 , 42.41 2024-11-06T03:00:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , 0.02 2024-11-06T03:00:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862001.42 epoch sec; seqNo:1589; 4061663.09 north; 594302.72 east; 36.70 lat; -121.94 lon; 50.96 depth; utmZone: 10 2024-11-06T03:00:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862000.16 2024-11-06T03:00:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:01Z- waiting for 1730862004.16, time is 1730862001.42 2024-11-06T03:00:01Z- AHRS_M2 msg: 1730862001.76 epoch sec; seqNo:1487 2024-11-06T03:00:01Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.42 psi 2024-11-06T03:00:01Z- handleDepth()- Depth_Keller msg: 1730862001.77 epoch sec; seqNo:1590; depth 50.97 2024-11-06T03:00:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862001.80 epoch sec; seqNo:1590; 4061663.09 north; 594302.72 east; 36.70 lat; -121.94 lon; 50.97 depth; utmZone: 10 2024-11-06T03:00:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862000.16 2024-11-06T03:00:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:01Z- waiting for 1730862004.16, time is 1730862001.80 2024-11-06T03:00:02Z- AHRS_M2 msg: 1730862002.16 epoch sec; seqNo:1488 2024-11-06T03:00:02Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.41 psi 2024-11-06T03:00:02Z- handleDepth()- Depth_Keller msg: 1730862002.17 epoch sec; seqNo:1591; depth 50.97 2024-11-06T03:00:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862002.18 epoch sec; seqNo:1591; 4061663.49 north; 594303.94 east; 36.70 lat; -121.94 lon; 50.97 depth; utmZone: 10 2024-11-06T03:00:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862000.16 2024-11-06T03:00:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:02Z- waiting for 1730862004.16, time is 1730862002.18 2024-11-06T03:00:02Z- AHRS_M2 msg: 1730862002.58 epoch sec; seqNo:1489 2024-11-06T03:00:02Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.41 psi 2024-11-06T03:00:02Z- handleDepth()- Depth_Keller msg: 1730862002.58 epoch sec; seqNo:1592; depth 50.99 2024-11-06T03:00:02Z- handleDVL() - DVL msg epochMillisec: 1730862002.59 2024-11-06T03:00:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:00:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:00:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:00:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:00:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:00:02Z- handleDvl() - RDI_Pathfinder msg: 1730862002.59 sec; seqNo:486; #beams:0 2024-11-06T03:00:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:00:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , 0.02 2024-11-06T03:00:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862002.60 epoch sec; seqNo:1592; 4061663.49 north; 594303.94 east; 36.70 lat; -121.94 lon; 50.99 depth; utmZone: 10 2024-11-06T03:00:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862000.16 2024-11-06T03:00:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:02Z- waiting for 1730862004.16, time is 1730862002.60 2024-11-06T03:00:02Z- AHRS_M2 msg: 1730862002.97 epoch sec; seqNo:1490 2024-11-06T03:00:02Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.41 psi 2024-11-06T03:00:02Z- handleDepth()- Depth_Keller msg: 1730862002.98 epoch sec; seqNo:1593; depth 51.00 2024-11-06T03:00:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862002.99 epoch sec; seqNo:1593; 4061663.49 north; 594303.94 east; 36.70 lat; -121.94 lon; 51.00 depth; utmZone: 10 2024-11-06T03:00:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862000.16 2024-11-06T03:00:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:03Z- waiting for 1730862004.16, time is 1730862002.99 2024-11-06T03:00:03Z- AHRS_M2 msg: 1730862003.37 epoch sec; seqNo:1491 2024-11-06T03:00:03Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.41 psi 2024-11-06T03:00:03Z- handleDepth()- Depth_Keller msg: 1730862003.38 epoch sec; seqNo:1594; depth 50.99 2024-11-06T03:00:03Z- handleDVL() - DVL msg epochMillisec: 1730862003.41 2024-11-06T03:00:03Z- handleDvl() - RDI_Pathfinder msg: 1730862003.41 sec; seqNo:487; #beams:4 2024-11-06T03:00:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.67 , 38.87 , 39.81 , 42.05 2024-11-06T03:00:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , 0.04 2024-11-06T03:00:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862003.42 epoch sec; seqNo:1594; 4061663.49 north; 594303.94 east; 36.70 lat; -121.94 lon; 50.99 depth; utmZone: 10 2024-11-06T03:00:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862000.16 2024-11-06T03:00:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:03Z- waiting for 1730862004.16, time is 1730862003.42 2024-11-06T03:00:03Z- AHRS_M2 msg: 1730862003.78 epoch sec; seqNo:1492 2024-11-06T03:00:03Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.41 psi 2024-11-06T03:00:03Z- handleDepth()- Depth_Keller msg: 1730862003.79 epoch sec; seqNo:1595; depth 51.01 2024-11-06T03:00:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862003.80 epoch sec; seqNo:1595; 4061663.50 north; 594305.16 east; 36.70 lat; -121.94 lon; 51.01 depth; utmZone: 10 2024-11-06T03:00:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862000.16 2024-11-06T03:00:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:03Z- waiting for 1730862004.16, time is 1730862003.80 2024-11-06T03:00:04Z- AHRS_M2 msg: 1730862004.18 epoch sec; seqNo:1493 2024-11-06T03:00:04Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.41 psi 2024-11-06T03:00:04Z- handleDepth()- Depth_Keller msg: 1730862004.19 epoch sec; seqNo:1596; depth 51.01 2024-11-06T03:00:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862004.20 epoch sec; seqNo:1596; 4061663.50 north; 594305.16 east; 36.70 lat; -121.94 lon; 51.01 depth; utmZone: 10 2024-11-06T03:00:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862000.16 2024-11-06T03:00:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:04Z- waiting for 1730862004.16, time is 1730862004.20 2024-11-06T03:00:04Z- LcmTrn::updateTrn() >>>> pose time 1730862004.20, meas time 1730862003.41 <<<< 2024-11-06T03:00:04Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862004.20 2024-11-06T03:00:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862003.41 sec, ping # 0; waiting for INS data more recent than 1730862000.16... 2024-11-06T03:00:04Z- TerrainNav::motionUpdate() - currEstimate = (4061663, 594305.16, 51.01) 2024-11-06T03:00:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:00:04Z- TerrainNav:: SubcloudNIS is 0.30, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:00:04Z- TNavPF::Projecting Measurements 2024-11-06T03:00:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:00:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:00:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:00:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:00:04Z- TNavFilter::4 good beams. 2024-11-06T03:00:04Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:00:04Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:00:04Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:00:04Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:00:04Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:00:04Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:00:04Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:00:04Z- TerrainMapOctree: (vehN, vehE ) = (4061663.42, 594304.44). 2024-11-06T03:00:04Z- TerrainMapOctree: Min Distance = 102.27. 2024-11-06T03:00:04Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:00:04Z- Forcing Subcloud Comparison 2024-11-06T03:00:04Z- Weighting particles with subcloud comparison 2024-11-06T03:00:04Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:04Z- meanExpectedMeasDiff: -0.064595 delta_rms_squared: 1.498419 alpha: 0.986828 2024-11-06T03:00:04Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:04Z- meanExpectedMeasDiff: 2.360402 delta_rms_squared: 1.601819 alpha: 0.987668 2024-11-06T03:00:04Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:04Z- meanExpectedMeasDiff: 2.192081 delta_rms_squared: 0.966620 alpha: 0.979729 2024-11-06T03:00:04Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:04Z- meanExpectedMeasDiff: -1.678737 delta_rms_squared: 1.714109 alpha: 0.988467 2024-11-06T03:00:04Z- Subcloud NIS: 0.321800 2024-11-06T03:00:04Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:00:04Z- TNavPF:: sumWeights = 0.164637 2024-11-06T03:00:04Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:00:04Z- TNavPF::First Term of Map Covariance Matrix is 0.13 2024-11-06T03:00:04Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T03:00:04Z- TerrainNav::Resampling particle filter... 2024-11-06T03:00:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862003.41 sec. 2024-11-06T03:00:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:00:04Z- variances([0], [2], and psi_berg) = 0.363412 0.225002 0.000000 2024-11-06T03:00:04Z- publishEstimatesDecoder() - at 1730862004.20 sec 2024-11-06T03:00:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:00:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061695.86 594288.88 54.95 2024-11-06T03:00:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061695.61 594288.97 54.97 lat/lon: 36.70 -121.94 2024-11-06T03:00:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.36 0.23 0.69 0.00 2024-11-06T03:00:04Z- AHRS_M2 msg: 1730862004.59 epoch sec; seqNo:1494 2024-11-06T03:00:04Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.41 psi 2024-11-06T03:00:04Z- handleDepth()- Depth_Keller msg: 1730862004.60 epoch sec; seqNo:1597; depth 51.01 2024-11-06T03:00:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862004.63 epoch sec; seqNo:1597; 4061663.50 north; 594305.16 east; 36.70 lat; -121.94 lon; 51.01 depth; utmZone: 10 2024-11-06T03:00:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862004.20 2024-11-06T03:00:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:04Z- waiting for 1730862008.20, time is 1730862004.63 2024-11-06T03:00:04Z- AHRS_M2 msg: 1730862004.99 epoch sec; seqNo:1495 2024-11-06T03:00:04Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.41 psi 2024-11-06T03:00:05Z- handleDepth()- Depth_Keller msg: 1730862005.00 epoch sec; seqNo:1598; depth 51.03 2024-11-06T03:00:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862005.01 epoch sec; seqNo:1598; 4061663.88 north; 594305.15 east; 36.70 lat; -121.94 lon; 51.03 depth; utmZone: 10 2024-11-06T03:00:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862004.20 2024-11-06T03:00:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:05Z- waiting for 1730862008.20, time is 1730862005.01 2024-11-06T03:00:05Z- AHRS_M2 msg: 1730862005.39 epoch sec; seqNo:1496 2024-11-06T03:00:05Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.41 psi 2024-11-06T03:00:05Z- handleDepth()- Depth_Keller msg: 1730862005.40 epoch sec; seqNo:1599; depth 51.01 2024-11-06T03:00:05Z- handleDVL() - DVL msg epochMillisec: 1730862005.42 2024-11-06T03:00:05Z- handleDvl() - RDI_Pathfinder msg: 1730862005.42 sec; seqNo:488; #beams:4 2024-11-06T03:00:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.04 , 38.87 , 40.74 , 41.87 2024-11-06T03:00:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , 0.03 2024-11-06T03:00:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862005.45 epoch sec; seqNo:1599; 4061663.89 north; 594306.37 east; 36.70 lat; -121.94 lon; 51.01 depth; utmZone: 10 2024-11-06T03:00:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862004.20 2024-11-06T03:00:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:05Z- waiting for 1730862008.20, time is 1730862005.45 2024-11-06T03:00:05Z- AHRS_M2 msg: 1730862005.80 epoch sec; seqNo:1497 2024-11-06T03:00:05Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.41 psi 2024-11-06T03:00:05Z- handleDepth()- Depth_Keller msg: 1730862005.80 epoch sec; seqNo:1600; depth 51.01 2024-11-06T03:00:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862005.82 epoch sec; seqNo:1600; 4061663.89 north; 594306.37 east; 36.70 lat; -121.94 lon; 51.01 depth; utmZone: 10 2024-11-06T03:00:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862004.20 2024-11-06T03:00:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:05Z- waiting for 1730862008.20, time is 1730862005.82 2024-11-06T03:00:06Z- AHRS_M2 msg: 1730862006.20 epoch sec; seqNo:1498 2024-11-06T03:00:06Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.41 psi 2024-11-06T03:00:06Z- handleDepth()- Depth_Keller msg: 1730862006.21 epoch sec; seqNo:1601; depth 51.00 2024-11-06T03:00:06Z- handleDVL() - DVL msg epochMillisec: 1730862006.22 2024-11-06T03:00:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:00:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:00:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:00:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:00:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:00:06Z- handleDvl() - RDI_Pathfinder msg: 1730862006.22 sec; seqNo:489; #beams:0 2024-11-06T03:00:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:00:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , 0.03 2024-11-06T03:00:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862006.23 epoch sec; seqNo:1601; 4061663.89 north; 594306.37 east; 36.70 lat; -121.94 lon; 51.00 depth; utmZone: 10 2024-11-06T03:00:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862004.20 2024-11-06T03:00:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:06Z- waiting for 1730862008.20, time is 1730862006.23 2024-11-06T03:00:06Z- AHRS_M2 msg: 1730862006.61 epoch sec; seqNo:1499 2024-11-06T03:00:06Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.41 psi 2024-11-06T03:00:06Z- handleDepth()- Depth_Keller msg: 1730862006.62 epoch sec; seqNo:1602; depth 51.00 2024-11-06T03:00:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862006.63 epoch sec; seqNo:1602; 4061663.91 north; 594307.59 east; 36.70 lat; -121.94 lon; 51.00 depth; utmZone: 10 2024-11-06T03:00:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862004.20 2024-11-06T03:00:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:06Z- waiting for 1730862008.20, time is 1730862006.63 2024-11-06T03:00:07Z- AHRS_M2 msg: 1730862007.01 epoch sec; seqNo:1500 2024-11-06T03:00:07Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.41 psi 2024-11-06T03:00:07Z- handleDepth()- Depth_Keller msg: 1730862007.02 epoch sec; seqNo:1603; depth 51.01 2024-11-06T03:00:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862007.03 epoch sec; seqNo:1603; 4061663.91 north; 594307.59 east; 36.70 lat; -121.94 lon; 51.01 depth; utmZone: 10 2024-11-06T03:00:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862004.20 2024-11-06T03:00:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:07Z- waiting for 1730862008.20, time is 1730862007.03 2024-11-06T03:00:07Z- handleDepth()- Depth_Keller msg: 1730862007.44 epoch sec; seqNo:1604; depth 50.98 2024-11-06T03:00:07Z- handleDVL() - DVL msg epochMillisec: 1730862007.46 2024-11-06T03:00:07Z- handleDvl() - RDI_Pathfinder msg: 1730862007.46 sec; seqNo:490; #beams:4 2024-11-06T03:00:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.85 , 39.06 , 40.56 , 41.87 2024-11-06T03:00:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , -0.03 2024-11-06T03:00:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862007.47 epoch sec; seqNo:1604; 4061663.91 north; 594307.59 east; 36.70 lat; -121.94 lon; 50.98 depth; utmZone: 10 2024-11-06T03:00:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862004.20 2024-11-06T03:00:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:07Z- waiting for 1730862008.20, time is 1730862007.47 2024-11-06T03:00:07Z- AHRS_M2 msg: 1730862007.82 epoch sec; seqNo:1501 2024-11-06T03:00:07Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.41 psi 2024-11-06T03:00:07Z- handleDepth()- Depth_Keller msg: 1730862007.82 epoch sec; seqNo:1605; depth 50.98 2024-11-06T03:00:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862007.85 epoch sec; seqNo:1605; 4061663.91 north; 594307.59 east; 36.70 lat; -121.94 lon; 50.98 depth; utmZone: 10 2024-11-06T03:00:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862004.20 2024-11-06T03:00:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:07Z- waiting for 1730862008.20, time is 1730862007.85 2024-11-06T03:00:08Z- AHRS_M2 msg: 1730862008.22 epoch sec; seqNo:1502 2024-11-06T03:00:08Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.41 psi 2024-11-06T03:00:08Z- handleDepth()- Depth_Keller msg: 1730862008.23 epoch sec; seqNo:1606; depth 50.97 2024-11-06T03:00:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862008.24 epoch sec; seqNo:1606; 4061663.92 north; 594308.81 east; 36.70 lat; -121.94 lon; 50.97 depth; utmZone: 10 2024-11-06T03:00:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862004.20 2024-11-06T03:00:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:08Z- waiting for 1730862008.20, time is 1730862008.24 2024-11-06T03:00:08Z- LcmTrn::updateTrn() >>>> pose time 1730862008.24, meas time 1730862007.46 <<<< 2024-11-06T03:00:08Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862008.24 2024-11-06T03:00:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862007.46 sec, ping # 0; waiting for INS data more recent than 1730862004.20... 2024-11-06T03:00:08Z- TerrainNav::motionUpdate() - currEstimate = (4061664, 594308.81, 50.97) 2024-11-06T03:00:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:00:08Z- TerrainNav:: SubcloudNIS is 0.32, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:00:08Z- TNavPF::Projecting Measurements 2024-11-06T03:00:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:00:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:00:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:00:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:00:08Z- TNavFilter::4 good beams. 2024-11-06T03:00:08Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:00:08Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:00:08Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:00:08Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:00:08Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:00:08Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:00:08Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:00:08Z- TerrainMapOctree: (vehN, vehE ) = (4061663.84, 594308.10). 2024-11-06T03:00:08Z- TerrainMapOctree: Min Distance = 103.92. 2024-11-06T03:00:08Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:00:08Z- Forcing Subcloud Comparison 2024-11-06T03:00:08Z- Weighting particles with subcloud comparison 2024-11-06T03:00:08Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:08Z- meanExpectedMeasDiff: 0.266003 delta_rms_squared: 2.372519 alpha: 0.991641 2024-11-06T03:00:08Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:08Z- meanExpectedMeasDiff: 4.511197 delta_rms_squared: 1.230255 alpha: 0.984003 2024-11-06T03:00:08Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:08Z- meanExpectedMeasDiff: 3.502642 delta_rms_squared: 1.114842 alpha: 0.982376 2024-11-06T03:00:08Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:08Z- meanExpectedMeasDiff: -1.837820 delta_rms_squared: 1.652423 alpha: 0.988041 2024-11-06T03:00:08Z- Subcloud NIS: 0.358225 2024-11-06T03:00:08Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:00:08Z- TNavPF:: sumWeights = 0.017940 2024-11-06T03:00:08Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:00:08Z- TNavPF::First Term of Map Covariance Matrix is 0.03 2024-11-06T03:00:08Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:00:08Z- TerrainNav::Resampling particle filter... 2024-11-06T03:00:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862007.46 sec. 2024-11-06T03:00:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:00:08Z- variances([0], [2], and psi_berg) = 0.351422 0.229029 0.000000 2024-11-06T03:00:08Z- publishEstimatesDecoder() - at 1730862008.24 sec 2024-11-06T03:00:08Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:00:08Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061696.35 594292.58 55.07 2024-11-06T03:00:08Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061696.03 594292.50 54.32 lat/lon: 36.70 -121.94 2024-11-06T03:00:08Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.35 0.23 0.70 0.00 2024-11-06T03:00:08Z- AHRS_M2 msg: 1730862008.63 epoch sec; seqNo:1503 2024-11-06T03:00:08Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.41 psi 2024-11-06T03:00:08Z- handleDepth()- Depth_Keller msg: 1730862008.65 epoch sec; seqNo:1607; depth 50.97 2024-11-06T03:00:08Z- handleDVL() - DVL msg epochMillisec: 1730862008.65 2024-11-06T03:00:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:00:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:00:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:00:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:00:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:00:08Z- handleDvl() - RDI_Pathfinder msg: 1730862008.65 sec; seqNo:491; #beams:0 2024-11-06T03:00:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:00:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , -0.03 2024-11-06T03:00:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862008.67 epoch sec; seqNo:1607; 4061664.30 north; 594308.81 east; 36.70 lat; -121.94 lon; 50.97 depth; utmZone: 10 2024-11-06T03:00:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862008.24 2024-11-06T03:00:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:08Z- waiting for 1730862012.24, time is 1730862008.67 2024-11-06T03:00:09Z- AHRS_M2 msg: 1730862009.03 epoch sec; seqNo:1504 2024-11-06T03:00:09Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.41 psi 2024-11-06T03:00:09Z- handleDepth()- Depth_Keller msg: 1730862009.04 epoch sec; seqNo:1608; depth 50.96 2024-11-06T03:00:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862009.05 epoch sec; seqNo:1608; 4061664.30 north; 594308.81 east; 36.70 lat; -121.94 lon; 50.96 depth; utmZone: 10 2024-11-06T03:00:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862008.24 2024-11-06T03:00:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:09Z- waiting for 1730862012.24, time is 1730862009.05 2024-11-06T03:00:09Z- AHRS_M2 msg: 1730862009.44 epoch sec; seqNo:1505 2024-11-06T03:00:09Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.41 psi 2024-11-06T03:00:09Z- handleDepth()- Depth_Keller msg: 1730862009.44 epoch sec; seqNo:1609; depth 50.95 2024-11-06T03:00:09Z- handleDVL() - DVL msg epochMillisec: 1730862009.46 2024-11-06T03:00:09Z- handleDvl() - RDI_Pathfinder msg: 1730862009.46 sec; seqNo:492; #beams:4 2024-11-06T03:00:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.55 , 39.06 , 40.74 , 41.68 2024-11-06T03:00:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , -0.06 2024-11-06T03:00:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862009.47 epoch sec; seqNo:1609; 4061664.31 north; 594310.03 east; 36.70 lat; -121.94 lon; 50.95 depth; utmZone: 10 2024-11-06T03:00:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862008.24 2024-11-06T03:00:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:09Z- waiting for 1730862012.24, time is 1730862009.47 2024-11-06T03:00:09Z- AHRS_M2 msg: 1730862009.84 epoch sec; seqNo:1506 2024-11-06T03:00:09Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.41 psi 2024-11-06T03:00:09Z- handleDepth()- Depth_Keller msg: 1730862009.85 epoch sec; seqNo:1610; depth 50.94 2024-11-06T03:00:09Z- handleDVL() - DVL msg epochMillisec: 1730862009.85 2024-11-06T03:00:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:00:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:00:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:00:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:00:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:00:09Z- handleDvl() - RDI_Pathfinder msg: 1730862009.85 sec; seqNo:493; #beams:0 2024-11-06T03:00:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:00:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , -0.06 2024-11-06T03:00:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862009.87 epoch sec; seqNo:1610; 4061664.31 north; 594310.03 east; 36.70 lat; -121.94 lon; 50.94 depth; utmZone: 10 2024-11-06T03:00:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862008.24 2024-11-06T03:00:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:10Z- waiting for 1730862012.24, time is 1730862009.87 2024-11-06T03:00:10Z- AHRS_M2 msg: 1730862010.24 epoch sec; seqNo:1507 2024-11-06T03:00:10Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.41 psi 2024-11-06T03:00:10Z- handleDepth()- Depth_Keller msg: 1730862010.26 epoch sec; seqNo:1611; depth 50.94 2024-11-06T03:00:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862010.29 epoch sec; seqNo:1611; 4061664.31 north; 594310.03 east; 36.70 lat; -121.94 lon; 50.94 depth; utmZone: 10 2024-11-06T03:00:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862008.24 2024-11-06T03:00:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:10Z- waiting for 1730862012.24, time is 1730862010.29 2024-11-06T03:00:10Z- AHRS_M2 msg: 1730862010.66 epoch sec; seqNo:1508 2024-11-06T03:00:10Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.41 psi 2024-11-06T03:00:10Z- handleDepth()- Depth_Keller msg: 1730862010.66 epoch sec; seqNo:1612; depth 50.94 2024-11-06T03:00:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862010.67 epoch sec; seqNo:1612; 4061664.31 north; 594310.03 east; 36.70 lat; -121.94 lon; 50.94 depth; utmZone: 10 2024-11-06T03:00:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862008.24 2024-11-06T03:00:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:10Z- waiting for 1730862012.24, time is 1730862010.67 2024-11-06T03:00:11Z- AHRS_M2 msg: 1730862011.05 epoch sec; seqNo:1509 2024-11-06T03:00:11Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.41 psi 2024-11-06T03:00:11Z- handleDepth()- Depth_Keller msg: 1730862011.06 epoch sec; seqNo:1613; depth 50.91 2024-11-06T03:00:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862011.07 epoch sec; seqNo:1613; 4061664.32 north; 594311.25 east; 36.70 lat; -121.94 lon; 50.91 depth; utmZone: 10 2024-11-06T03:00:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862008.24 2024-11-06T03:00:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:11Z- waiting for 1730862012.24, time is 1730862011.07 2024-11-06T03:00:11Z- AHRS_M2 msg: 1730862011.45 epoch sec; seqNo:1510 2024-11-06T03:00:11Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.41 psi 2024-11-06T03:00:11Z- handleDepth()- Depth_Keller msg: 1730862011.47 epoch sec; seqNo:1614; depth 50.89 2024-11-06T03:00:11Z- handleDVL() - DVL msg epochMillisec: 1730862011.49 2024-11-06T03:00:11Z- handleDvl() - RDI_Pathfinder msg: 1730862011.49 sec; seqNo:494; #beams:4 2024-11-06T03:00:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.29 , 38.68 , 40.93 , 41.30 2024-11-06T03:00:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.02 , -0.06 2024-11-06T03:00:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862011.50 epoch sec; seqNo:1614; 4061664.32 north; 594311.25 east; 36.70 lat; -121.94 lon; 50.89 depth; utmZone: 10 2024-11-06T03:00:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862008.24 2024-11-06T03:00:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:11Z- waiting for 1730862012.24, time is 1730862011.50 2024-11-06T03:00:11Z- AHRS_M2 msg: 1730862011.86 epoch sec; seqNo:1511 2024-11-06T03:00:11Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.41 psi 2024-11-06T03:00:11Z- handleDepth()- Depth_Keller msg: 1730862011.86 epoch sec; seqNo:1615; depth 50.90 2024-11-06T03:00:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862011.88 epoch sec; seqNo:1615; 4061664.32 north; 594311.25 east; 36.70 lat; -121.94 lon; 50.90 depth; utmZone: 10 2024-11-06T03:00:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862008.24 2024-11-06T03:00:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:12Z- waiting for 1730862012.24, time is 1730862011.88 2024-11-06T03:00:12Z- AHRS_M2 msg: 1730862012.26 epoch sec; seqNo:1512 2024-11-06T03:00:12Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.41 psi 2024-11-06T03:00:12Z- handleDepth()- Depth_Keller msg: 1730862012.27 epoch sec; seqNo:1616; depth 50.89 2024-11-06T03:00:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862012.30 epoch sec; seqNo:1616; 4061664.72 north; 594312.47 east; 36.70 lat; -121.94 lon; 50.89 depth; utmZone: 10 2024-11-06T03:00:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862008.24 2024-11-06T03:00:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:12Z- waiting for 1730862012.24, time is 1730862012.30 2024-11-06T03:00:12Z- LcmTrn::updateTrn() >>>> pose time 1730862012.30, meas time 1730862011.49 <<<< 2024-11-06T03:00:12Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862012.30 2024-11-06T03:00:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862011.49 sec, ping # 0; waiting for INS data more recent than 1730862008.24... 2024-11-06T03:00:12Z- TerrainNav::motionUpdate() - currEstimate = (4061665, 594312.47, 50.89) 2024-11-06T03:00:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:00:12Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:00:12Z- TNavPF::Projecting Measurements 2024-11-06T03:00:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:00:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:00:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:00:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:00:12Z- TNavFilter::4 good beams. 2024-11-06T03:00:12Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:00:12Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:00:12Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:00:12Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:00:12Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:00:12Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:00:12Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:00:12Z- TerrainMapOctree: (vehN, vehE ) = (4061664.56, 594311.73). 2024-11-06T03:00:12Z- TerrainMapOctree: Min Distance = 105.41. 2024-11-06T03:00:12Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:00:12Z- Forcing Subcloud Comparison 2024-11-06T03:00:12Z- Weighting particles with subcloud comparison 2024-11-06T03:00:12Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:12Z- meanExpectedMeasDiff: -0.725860 delta_rms_squared: 1.500098 alpha: 0.986843 2024-11-06T03:00:12Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:12Z- meanExpectedMeasDiff: 4.111248 delta_rms_squared: 1.082292 alpha: 0.981856 2024-11-06T03:00:12Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:12Z- meanExpectedMeasDiff: 3.862995 delta_rms_squared: 1.192237 alpha: 0.983502 2024-11-06T03:00:12Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:12Z- meanExpectedMeasDiff: -0.996939 delta_rms_squared: 1.989457 alpha: 0.990047 2024-11-06T03:00:12Z- Subcloud NIS: 0.383541 2024-11-06T03:00:12Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:00:12Z- TNavPF:: sumWeights = 0.023960 2024-11-06T03:00:12Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:00:12Z- TNavPF::First Term of Map Covariance Matrix is 0.07 2024-11-06T03:00:12Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:00:12Z- TerrainNav::Resampling particle filter... 2024-11-06T03:00:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862011.49 sec. 2024-11-06T03:00:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:00:12Z- variances([0], [2], and psi_berg) = 0.300520 0.221559 0.000000 2024-11-06T03:00:12Z- publishEstimatesDecoder() - at 1730862012.30 sec 2024-11-06T03:00:12Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:00:12Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061697.03 594296.35 54.49 2024-11-06T03:00:12Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061696.88 594296.36 53.65 lat/lon: 36.70 -121.94 2024-11-06T03:00:12Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.30 0.22 0.66 0.00 2024-11-06T03:00:12Z- AHRS_M2 msg: 1730862012.70 epoch sec; seqNo:1513 2024-11-06T03:00:12Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.40 psi 2024-11-06T03:00:12Z- handleDepth()- Depth_Keller msg: 1730862012.71 epoch sec; seqNo:1617; depth 50.88 2024-11-06T03:00:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862012.73 epoch sec; seqNo:1617; 4061664.72 north; 594312.47 east; 36.70 lat; -121.94 lon; 50.88 depth; utmZone: 10 2024-11-06T03:00:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862012.30 2024-11-06T03:00:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:12Z- waiting for 1730862016.30, time is 1730862012.73 2024-11-06T03:00:13Z- AHRS_M2 msg: 1730862013.07 epoch sec; seqNo:1514 2024-11-06T03:00:13Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.40 psi 2024-11-06T03:00:13Z- handleDepth()- Depth_Keller msg: 1730862013.08 epoch sec; seqNo:1618; depth 50.88 2024-11-06T03:00:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862013.09 epoch sec; seqNo:1618; 4061664.72 north; 594312.47 east; 36.70 lat; -121.94 lon; 50.88 depth; utmZone: 10 2024-11-06T03:00:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862012.30 2024-11-06T03:00:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:13Z- waiting for 1730862016.30, time is 1730862013.09 2024-11-06T03:00:13Z- AHRS_M2 msg: 1730862013.47 epoch sec; seqNo:1515 2024-11-06T03:00:13Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.40 psi 2024-11-06T03:00:13Z- handleDepth()- Depth_Keller msg: 1730862013.48 epoch sec; seqNo:1619; depth 50.88 2024-11-06T03:00:13Z- handleDVL() - DVL msg epochMillisec: 1730862013.50 2024-11-06T03:00:13Z- handleDvl() - RDI_Pathfinder msg: 1730862013.50 sec; seqNo:495; #beams:4 2024-11-06T03:00:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.23 , 38.50 , 41.12 , 41.30 2024-11-06T03:00:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.05 , -0.02 2024-11-06T03:00:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862013.53 epoch sec; seqNo:1619; 4061664.72 north; 594312.47 east; 36.70 lat; -121.94 lon; 50.88 depth; utmZone: 10 2024-11-06T03:00:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862012.30 2024-11-06T03:00:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:13Z- waiting for 1730862016.30, time is 1730862013.53 2024-11-06T03:00:13Z- AHRS_M2 msg: 1730862013.88 epoch sec; seqNo:1516 2024-11-06T03:00:13Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.40 psi 2024-11-06T03:00:13Z- handleDepth()- Depth_Keller msg: 1730862013.89 epoch sec; seqNo:1620; depth 50.87 2024-11-06T03:00:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862013.90 epoch sec; seqNo:1620; 4061664.73 north; 594313.69 east; 36.70 lat; -121.94 lon; 50.87 depth; utmZone: 10 2024-11-06T03:00:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862012.30 2024-11-06T03:00:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:14Z- waiting for 1730862016.30, time is 1730862013.90 2024-11-06T03:00:14Z- AHRS_M2 msg: 1730862014.28 epoch sec; seqNo:1517 2024-11-06T03:00:14Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.40 psi 2024-11-06T03:00:14Z- handleDepth()- Depth_Keller msg: 1730862014.30 epoch sec; seqNo:1621; depth 50.86 2024-11-06T03:00:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862014.31 epoch sec; seqNo:1621; 4061664.73 north; 594313.69 east; 36.70 lat; -121.94 lon; 50.86 depth; utmZone: 10 2024-11-06T03:00:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862012.30 2024-11-06T03:00:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:14Z- waiting for 1730862016.30, time is 1730862014.31 2024-11-06T03:00:14Z- AHRS_M2 msg: 1730862014.69 epoch sec; seqNo:1518 2024-11-06T03:00:14Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.40 psi 2024-11-06T03:00:14Z- handleDepth()- Depth_Keller msg: 1730862014.70 epoch sec; seqNo:1622; depth 50.86 2024-11-06T03:00:14Z- handleDVL() - DVL msg epochMillisec: 1730862014.70 2024-11-06T03:00:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:00:14Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:00:14Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:00:14Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:00:14Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:00:14Z- handleDvl() - RDI_Pathfinder msg: 1730862014.70 sec; seqNo:496; #beams:0 2024-11-06T03:00:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:00:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.05 , -0.02 2024-11-06T03:00:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862014.72 epoch sec; seqNo:1622; 4061665.11 north; 594313.68 east; 36.70 lat; -121.94 lon; 50.86 depth; utmZone: 10 2024-11-06T03:00:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862012.30 2024-11-06T03:00:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:14Z- waiting for 1730862016.30, time is 1730862014.72 2024-11-06T03:00:15Z- AHRS_M2 msg: 1730862015.09 epoch sec; seqNo:1519 2024-11-06T03:00:15Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.39 psi 2024-11-06T03:00:15Z- handleDepth()- Depth_Keller msg: 1730862015.10 epoch sec; seqNo:1623; depth 50.85 2024-11-06T03:00:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862015.11 epoch sec; seqNo:1623; 4061665.11 north; 594313.68 east; 36.70 lat; -121.94 lon; 50.85 depth; utmZone: 10 2024-11-06T03:00:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862012.30 2024-11-06T03:00:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:15Z- waiting for 1730862016.30, time is 1730862015.11 2024-11-06T03:00:15Z- AHRS_M2 msg: 1730862015.50 epoch sec; seqNo:1520 2024-11-06T03:00:15Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.39 psi 2024-11-06T03:00:15Z- handleDepth()- Depth_Keller msg: 1730862015.50 epoch sec; seqNo:1624; depth 50.86 2024-11-06T03:00:15Z- handleDVL() - DVL msg epochMillisec: 1730862015.52 2024-11-06T03:00:15Z- handleDvl() - RDI_Pathfinder msg: 1730862015.52 sec; seqNo:497; #beams:4 2024-11-06T03:00:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.67 , 38.69 , 40.93 , 41.12 2024-11-06T03:00:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.03 , 0.04 2024-11-06T03:00:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862015.53 epoch sec; seqNo:1624; 4061665.11 north; 594313.68 east; 36.70 lat; -121.94 lon; 50.86 depth; utmZone: 10 2024-11-06T03:00:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862012.30 2024-11-06T03:00:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:15Z- waiting for 1730862016.30, time is 1730862015.53 2024-11-06T03:00:15Z- AHRS_M2 msg: 1730862015.90 epoch sec; seqNo:1521 2024-11-06T03:00:15Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.39 psi 2024-11-06T03:00:15Z- handleDepth()- Depth_Keller msg: 1730862015.92 epoch sec; seqNo:1625; depth 50.86 2024-11-06T03:00:15Z- handleDVL() - DVL msg epochMillisec: 1730862015.92 2024-11-06T03:00:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:00:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:00:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:00:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:00:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:00:15Z- handleDvl() - RDI_Pathfinder msg: 1730862015.92 sec; seqNo:498; #beams:0 2024-11-06T03:00:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:00:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.03 , 0.04 2024-11-06T03:00:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862015.94 epoch sec; seqNo:1625; 4061665.12 north; 594314.90 east; 36.70 lat; -121.94 lon; 50.86 depth; utmZone: 10 2024-11-06T03:00:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862012.30 2024-11-06T03:00:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:16Z- waiting for 1730862016.30, time is 1730862015.94 2024-11-06T03:00:16Z- AHRS_M2 msg: 1730862016.30 epoch sec; seqNo:1522 2024-11-06T03:00:16Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 1.39 psi 2024-11-06T03:00:16Z- handleDepth()- Depth_Keller msg: 1730862016.31 epoch sec; seqNo:1626; depth 50.85 2024-11-06T03:00:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862016.32 epoch sec; seqNo:1626; 4061665.12 north; 594314.90 east; 36.70 lat; -121.94 lon; 50.85 depth; utmZone: 10 2024-11-06T03:00:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862012.30 2024-11-06T03:00:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:16Z- waiting for 1730862016.30, time is 1730862016.32 2024-11-06T03:00:16Z- LcmTrn::updateTrn() >>>> pose time 1730862016.32, meas time 1730862015.92 <<<< 2024-11-06T03:00:16Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862016.32 2024-11-06T03:00:16Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:00:16Z- TerrainNav::motionUpdate() - currEstimate = (4061665, 594314.90, 50.85) 2024-11-06T03:00:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:00:16Z- TerrainNav:: SubcloudNIS is 0.38, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:00:16Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:00:16Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:00:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:00:16Z- variances([0], [2], and psi_berg) = 0.301504 0.222156 0.000000 2024-11-06T03:00:16Z- publishEstimatesDecoder() - at 1730862016.32 sec 2024-11-06T03:00:16Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:00:16Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061697.48 594298.80 54.58 2024-11-06T03:00:16Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061697.29 594298.80 53.62 lat/lon: 36.70 -121.94 2024-11-06T03:00:16Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.30 0.22 0.91 0.00 2024-11-06T03:00:16Z- AHRS_M2 msg: 1730862016.71 epoch sec; seqNo:1523 2024-11-06T03:00:16Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 1.38 psi 2024-11-06T03:00:16Z- handleDepth()- Depth_Keller msg: 1730862016.72 epoch sec; seqNo:1627; depth 50.85 2024-11-06T03:00:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862016.73 epoch sec; seqNo:1627; 4061665.50 north; 594314.90 east; 36.70 lat; -121.94 lon; 50.85 depth; utmZone: 10 2024-11-06T03:00:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862016.32 2024-11-06T03:00:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:16Z- waiting for 1730862020.32, time is 1730862016.73 2024-11-06T03:00:17Z- AHRS_M2 msg: 1730862017.11 epoch sec; seqNo:1524 2024-11-06T03:00:17Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 1.38 psi 2024-11-06T03:00:17Z- handleDepth()- Depth_Keller msg: 1730862017.13 epoch sec; seqNo:1628; depth 50.86 2024-11-06T03:00:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862017.14 epoch sec; seqNo:1628; 4061665.50 north; 594314.90 east; 36.70 lat; -121.94 lon; 50.86 depth; utmZone: 10 2024-11-06T03:00:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862016.32 2024-11-06T03:00:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:17Z- waiting for 1730862020.32, time is 1730862017.14 2024-11-06T03:00:17Z- AHRS_M2 msg: 1730862017.52 epoch sec; seqNo:1525 2024-11-06T03:00:17Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 1.38 psi 2024-11-06T03:00:17Z- handleDepth()- Depth_Keller msg: 1730862017.52 epoch sec; seqNo:1629; depth 50.83 2024-11-06T03:00:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862017.55 epoch sec; seqNo:1629; 4061665.51 north; 594316.12 east; 36.70 lat; -121.94 lon; 50.83 depth; utmZone: 10 2024-11-06T03:00:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862016.32 2024-11-06T03:00:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:17Z- waiting for 1730862020.32, time is 1730862017.55 2024-11-06T03:00:17Z- AHRS_M2 msg: 1730862017.92 epoch sec; seqNo:1526 2024-11-06T03:00:17Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 1.38 psi 2024-11-06T03:00:17Z- handleDepth()- Depth_Keller msg: 1730862017.92 epoch sec; seqNo:1630; depth 50.84 2024-11-06T03:00:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862017.93 epoch sec; seqNo:1630; 4061665.51 north; 594316.12 east; 36.70 lat; -121.94 lon; 50.84 depth; utmZone: 10 2024-11-06T03:00:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862016.32 2024-11-06T03:00:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:18Z- waiting for 1730862020.32, time is 1730862017.93 2024-11-06T03:00:18Z- handleDepth()- Depth_Keller msg: 1730862018.35 epoch sec; seqNo:1631; depth 50.83 2024-11-06T03:00:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862018.36 epoch sec; seqNo:1631; 4061665.51 north; 594316.12 east; 36.70 lat; -121.94 lon; 50.83 depth; utmZone: 10 2024-11-06T03:00:18Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:00:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862016.32 2024-11-06T03:00:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:18Z- waiting for 1730862020.32, time is 1730862018.36 2024-11-06T03:00:18Z- AHRS_M2 msg: 1730862018.73 epoch sec; seqNo:1527 2024-11-06T03:00:18Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 1.38 psi 2024-11-06T03:00:18Z- handleDepth()- Depth_Keller msg: 1730862018.74 epoch sec; seqNo:1632; depth 50.82 2024-11-06T03:00:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862018.76 epoch sec; seqNo:1632; 4061665.91 north; 594317.33 east; 36.70 lat; -121.94 lon; 50.82 depth; utmZone: 10 2024-11-06T03:00:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862016.32 2024-11-06T03:00:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:18Z- waiting for 1730862020.32, time is 1730862018.76 2024-11-06T03:00:19Z- AHRS_M2 msg: 1730862019.13 epoch sec; seqNo:1528 2024-11-06T03:00:19Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 1.38 psi 2024-11-06T03:00:19Z- handleDepth()- Depth_Keller msg: 1730862019.13 epoch sec; seqNo:1633; depth 50.81 2024-11-06T03:00:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862019.17 epoch sec; seqNo:1633; 4061665.91 north; 594317.33 east; 36.70 lat; -121.94 lon; 50.81 depth; utmZone: 10 2024-11-06T03:00:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862016.32 2024-11-06T03:00:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:19Z- waiting for 1730862020.32, time is 1730862019.17 2024-11-06T03:00:19Z- AHRS_M2 msg: 1730862019.54 epoch sec; seqNo:1529 2024-11-06T03:00:19Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 1.39 psi 2024-11-06T03:00:19Z- handleDepth()- Depth_Keller msg: 1730862019.55 epoch sec; seqNo:1634; depth 50.80 2024-11-06T03:00:19Z- handleDVL() - DVL msg epochMillisec: 1730862019.57 2024-11-06T03:00:19Z- handleDvl() - RDI_Pathfinder msg: 1730862019.57 sec; seqNo:499; #beams:4 2024-11-06T03:00:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.85 , 38.87 , 41.87 , 40.74 2024-11-06T03:00:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , 0.01 2024-11-06T03:00:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862019.59 epoch sec; seqNo:1634; 4061665.91 north; 594317.33 east; 36.70 lat; -121.94 lon; 50.80 depth; utmZone: 10 2024-11-06T03:00:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862016.32 2024-11-06T03:00:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:19Z- waiting for 1730862020.32, time is 1730862019.59 2024-11-06T03:00:19Z- AHRS_M2 msg: 1730862019.94 epoch sec; seqNo:1530 2024-11-06T03:00:19Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.39 psi 2024-11-06T03:00:19Z- handleDepth()- Depth_Keller msg: 1730862019.95 epoch sec; seqNo:1635; depth 50.78 2024-11-06T03:00:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862019.96 epoch sec; seqNo:1635; 4061665.91 north; 594317.33 east; 36.70 lat; -121.94 lon; 50.78 depth; utmZone: 10 2024-11-06T03:00:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862016.32 2024-11-06T03:00:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:20Z- waiting for 1730862020.32, time is 1730862019.96 2024-11-06T03:00:20Z- AHRS_M2 msg: 1730862020.34 epoch sec; seqNo:1531 2024-11-06T03:00:20Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.39 psi 2024-11-06T03:00:20Z- handleDepth()- Depth_Keller msg: 1730862020.35 epoch sec; seqNo:1636; depth 50.77 2024-11-06T03:00:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862020.36 epoch sec; seqNo:1636; 4061665.92 north; 594318.55 east; 36.70 lat; -121.94 lon; 50.77 depth; utmZone: 10 2024-11-06T03:00:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862016.32 2024-11-06T03:00:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:20Z- waiting for 1730862020.32, time is 1730862020.36 2024-11-06T03:00:20Z- LcmTrn::updateTrn() >>>> pose time 1730862020.36, meas time 1730862019.57 <<<< 2024-11-06T03:00:20Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862020.36 2024-11-06T03:00:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862019.57 sec, ping # 0; waiting for INS data more recent than 1730862016.32... 2024-11-06T03:00:20Z- TerrainNav::motionUpdate() - currEstimate = (4061666, 594318.55, 50.77) 2024-11-06T03:00:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:00:20Z- TerrainNav:: SubcloudNIS is 0.38, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:00:20Z- TNavPF::Projecting Measurements 2024-11-06T03:00:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:00:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:00:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:00:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:00:20Z- TNavFilter::4 good beams. 2024-11-06T03:00:20Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:00:20Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:00:20Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:00:20Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:00:20Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:00:20Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:00:20Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:00:20Z- TerrainMapOctree: (vehN, vehE ) = (4061665.77, 594317.85). 2024-11-06T03:00:20Z- TerrainMapOctree: Min Distance = 108.16. 2024-11-06T03:00:20Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:00:20Z- Forcing Subcloud Comparison 2024-11-06T03:00:20Z- Weighting particles with subcloud comparison 2024-11-06T03:00:20Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:20Z- meanExpectedMeasDiff: 0.389998 delta_rms_squared: 2.438038 alpha: 0.991863 2024-11-06T03:00:20Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:20Z- meanExpectedMeasDiff: 1.052916 delta_rms_squared: 1.288663 alpha: 0.984717 2024-11-06T03:00:20Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:20Z- meanExpectedMeasDiff: 5.363667 delta_rms_squared: 1.550067 alpha: 0.987262 2024-11-06T03:00:20Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:20Z- meanExpectedMeasDiff: 0.934487 delta_rms_squared: 1.539168 alpha: 0.987173 2024-11-06T03:00:20Z- Subcloud NIS: 0.413605 2024-11-06T03:00:20Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:00:20Z- TNavPF:: sumWeights = 0.053799 2024-11-06T03:00:20Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:00:20Z- TNavPF::First Term of Map Covariance Matrix is 0.10 2024-11-06T03:00:20Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:00:20Z- TerrainNav::Resampling particle filter... 2024-11-06T03:00:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862019.57 sec. 2024-11-06T03:00:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:00:20Z- variances([0], [2], and psi_berg) = 0.351100 0.232959 0.000000 2024-11-06T03:00:20Z- publishEstimatesDecoder() - at 1730862020.36 sec 2024-11-06T03:00:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:00:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061698.38 594302.79 53.42 2024-11-06T03:00:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061697.98 594302.53 52.58 lat/lon: 36.70 -121.94 2024-11-06T03:00:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.35 0.23 0.75 0.00 2024-11-06T03:00:20Z- AHRS_M2 msg: 1730862020.76 epoch sec; seqNo:1532 2024-11-06T03:00:20Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.39 psi 2024-11-06T03:00:20Z- handleDepth()- Depth_Keller msg: 1730862020.77 epoch sec; seqNo:1637; depth 50.76 2024-11-06T03:00:20Z- handleDVL() - DVL msg epochMillisec: 1730862020.78 2024-11-06T03:00:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:00:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:00:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:00:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:00:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:00:20Z- handleDvl() - RDI_Pathfinder msg: 1730862020.78 sec; seqNo:500; #beams:0 2024-11-06T03:00:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:00:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , 0.01 2024-11-06T03:00:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862020.80 epoch sec; seqNo:1637; 4061665.92 north; 594318.55 east; 36.70 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T03:00:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862020.36 2024-11-06T03:00:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:20Z- waiting for 1730862024.36, time is 1730862020.80 2024-11-06T03:00:21Z- AHRS_M2 msg: 1730862021.16 epoch sec; seqNo:1533 2024-11-06T03:00:21Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.39 psi 2024-11-06T03:00:21Z- handleDepth()- Depth_Keller msg: 1730862021.17 epoch sec; seqNo:1638; depth 50.75 2024-11-06T03:00:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862021.19 epoch sec; seqNo:1638; 4061665.92 north; 594318.55 east; 36.70 lat; -121.94 lon; 50.75 depth; utmZone: 10 2024-11-06T03:00:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862020.36 2024-11-06T03:00:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:21Z- waiting for 1730862024.36, time is 1730862021.19 2024-11-06T03:00:21Z- AHRS_M2 msg: 1730862021.56 epoch sec; seqNo:1534 2024-11-06T03:00:21Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.40 psi 2024-11-06T03:00:21Z- handleDepth()- Depth_Keller msg: 1730862021.57 epoch sec; seqNo:1639; depth 50.73 2024-11-06T03:00:21Z- handleDVL() - DVL msg epochMillisec: 1730862021.59 2024-11-06T03:00:21Z- handleDvl() - RDI_Pathfinder msg: 1730862021.59 sec; seqNo:501; #beams:4 2024-11-06T03:00:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.04 , 38.69 , 41.68 , 41.12 2024-11-06T03:00:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.05 , -0.03 2024-11-06T03:00:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862021.61 epoch sec; seqNo:1639; 4061665.92 north; 594318.55 east; 36.70 lat; -121.94 lon; 50.73 depth; utmZone: 10 2024-11-06T03:00:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862020.36 2024-11-06T03:00:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:21Z- waiting for 1730862024.36, time is 1730862021.61 2024-11-06T03:00:21Z- AHRS_M2 msg: 1730862021.96 epoch sec; seqNo:1535 2024-11-06T03:00:21Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.40 psi 2024-11-06T03:00:21Z- handleDepth()- Depth_Keller msg: 1730862021.97 epoch sec; seqNo:1640; depth 50.71 2024-11-06T03:00:21Z- handleDVL() - DVL msg epochMillisec: 1730862021.97 2024-11-06T03:00:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:00:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:00:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:00:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:00:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:00:21Z- handleDvl() - RDI_Pathfinder msg: 1730862021.97 sec; seqNo:502; #beams:0 2024-11-06T03:00:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:00:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.05 , -0.03 2024-11-06T03:00:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862022.00 epoch sec; seqNo:1640; 4061666.31 north; 594319.77 east; 36.70 lat; -121.94 lon; 50.71 depth; utmZone: 10 2024-11-06T03:00:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862020.36 2024-11-06T03:00:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:22Z- waiting for 1730862024.36, time is 1730862022.00 2024-11-06T03:00:22Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:00:22Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:00:22Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:00:22Z- AHRS_M2 msg: 1730862022.35 epoch sec; seqNo:1536 2024-11-06T03:00:22Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.40 psi 2024-11-06T03:00:22Z- handleDepth()- Depth_Keller msg: 1730862022.39 epoch sec; seqNo:1641; depth 50.70 2024-11-06T03:00:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862022.40 epoch sec; seqNo:1641; 4061666.31 north; 594319.77 east; 36.70 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T03:00:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862020.36 2024-11-06T03:00:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:22Z- waiting for 1730862024.36, time is 1730862022.40 2024-11-06T03:00:22Z- AHRS_M2 msg: 1730862022.77 epoch sec; seqNo:1537 2024-11-06T03:00:22Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.40 psi 2024-11-06T03:00:22Z- handleDepth()- Depth_Keller msg: 1730862022.77 epoch sec; seqNo:1642; depth 50.68 2024-11-06T03:00:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862022.79 epoch sec; seqNo:1642; 4061666.31 north; 594319.77 east; 36.70 lat; -121.94 lon; 50.68 depth; utmZone: 10 2024-11-06T03:00:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862020.36 2024-11-06T03:00:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:22Z- waiting for 1730862024.36, time is 1730862022.79 2024-11-06T03:00:23Z- AHRS_M2 msg: 1730862023.17 epoch sec; seqNo:1538 2024-11-06T03:00:23Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.40 psi 2024-11-06T03:00:23Z- handleDepth()- Depth_Keller msg: 1730862023.18 epoch sec; seqNo:1643; depth 50.65 2024-11-06T03:00:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862023.19 epoch sec; seqNo:1643; 4061666.33 north; 594320.99 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:00:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862020.36 2024-11-06T03:00:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:23Z- waiting for 1730862024.36, time is 1730862023.19 2024-11-06T03:00:23Z- AHRS_M2 msg: 1730862023.58 epoch sec; seqNo:1539 2024-11-06T03:00:23Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.40 psi 2024-11-06T03:00:23Z- handleDepth()- Depth_Keller msg: 1730862023.58 epoch sec; seqNo:1644; depth 50.64 2024-11-06T03:00:23Z- handleDVL() - DVL msg epochMillisec: 1730862023.60 2024-11-06T03:00:23Z- handleDvl() - RDI_Pathfinder msg: 1730862023.60 sec; seqNo:503; #beams:4 2024-11-06T03:00:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.66 , 38.50 , 41.67 , 40.74 2024-11-06T03:00:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.01 , -0.04 2024-11-06T03:00:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862023.61 epoch sec; seqNo:1644; 4061666.33 north; 594320.99 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:00:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862020.36 2024-11-06T03:00:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:23Z- waiting for 1730862024.36, time is 1730862023.61 2024-11-06T03:00:23Z- AHRS_M2 msg: 1730862023.98 epoch sec; seqNo:1540 2024-11-06T03:00:23Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.40 psi 2024-11-06T03:00:23Z- handleDepth()- Depth_Keller msg: 1730862023.99 epoch sec; seqNo:1645; depth 50.61 2024-11-06T03:00:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862024.01 epoch sec; seqNo:1645; 4061666.33 north; 594320.99 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:00:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862020.36 2024-11-06T03:00:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:24Z- waiting for 1730862024.36, time is 1730862024.01 2024-11-06T03:00:24Z- AHRS_M2 msg: 1730862024.38 epoch sec; seqNo:1541 2024-11-06T03:00:24Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.40 psi 2024-11-06T03:00:24Z- handleDepth()- Depth_Keller msg: 1730862024.39 epoch sec; seqNo:1646; depth 50.59 2024-11-06T03:00:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862024.40 epoch sec; seqNo:1646; 4061666.33 north; 594320.99 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:00:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862020.36 2024-11-06T03:00:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:24Z- waiting for 1730862024.36, time is 1730862024.40 2024-11-06T03:00:24Z- LcmTrn::updateTrn() >>>> pose time 1730862024.40, meas time 1730862023.60 <<<< 2024-11-06T03:00:24Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862024.40 2024-11-06T03:00:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862023.60 sec, ping # 0; waiting for INS data more recent than 1730862020.36... 2024-11-06T03:00:24Z- TerrainNav::motionUpdate() - currEstimate = (4061666, 594320.99, 50.59) 2024-11-06T03:00:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:00:24Z- TerrainNav:: SubcloudNIS is 0.41, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:00:24Z- TNavPF::Projecting Measurements 2024-11-06T03:00:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:00:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:00:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:00:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:00:24Z- TNavFilter::4 good beams. 2024-11-06T03:00:24Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:00:24Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:00:24Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:00:24Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:00:24Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:00:24Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:00:24Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:00:24Z- TerrainMapOctree: (vehN, vehE ) = (4061666.25, 594320.51). 2024-11-06T03:00:24Z- TerrainMapOctree: Min Distance = 109.48. 2024-11-06T03:00:24Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:00:24Z- Forcing Subcloud Comparison 2024-11-06T03:00:24Z- Weighting particles with subcloud comparison 2024-11-06T03:00:24Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:24Z- meanExpectedMeasDiff: -0.105767 delta_rms_squared: 2.316393 alpha: 0.991440 2024-11-06T03:00:24Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:24Z- meanExpectedMeasDiff: 0.017573 delta_rms_squared: 1.229372 alpha: 0.983992 2024-11-06T03:00:24Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:24Z- meanExpectedMeasDiff: 3.936851 delta_rms_squared: 1.119693 alpha: 0.982451 2024-11-06T03:00:24Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:24Z- meanExpectedMeasDiff: 0.125490 delta_rms_squared: 1.996234 alpha: 0.990081 2024-11-06T03:00:24Z- Subcloud NIS: 0.427436 2024-11-06T03:00:24Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:00:24Z- TNavPF:: sumWeights = 0.140206 2024-11-06T03:00:24Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:00:24Z- TNavPF::First Term of Map Covariance Matrix is 0.21 2024-11-06T03:00:24Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.02 2024-11-06T03:00:24Z- TerrainNav::Resampling particle filter... 2024-11-06T03:00:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862023.60 sec. 2024-11-06T03:00:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:00:24Z- variances([0], [2], and psi_berg) = 0.340894 0.214688 0.000000 2024-11-06T03:00:24Z- publishEstimatesDecoder() - at 1730862024.40 sec 2024-11-06T03:00:24Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:00:24Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061698.95 594305.07 51.40 2024-11-06T03:00:24Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061698.38 594304.92 52.02 lat/lon: 36.70 -121.94 2024-11-06T03:00:24Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.34 0.21 0.66 0.00 2024-11-06T03:00:24Z- AHRS_M2 msg: 1730862024.80 epoch sec; seqNo:1542 2024-11-06T03:00:24Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.40 psi 2024-11-06T03:00:24Z- handleDepth()- Depth_Keller msg: 1730862024.81 epoch sec; seqNo:1647; depth 50.57 2024-11-06T03:00:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862024.84 epoch sec; seqNo:1647; 4061666.34 north; 594322.21 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T03:00:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862024.40 2024-11-06T03:00:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:24Z- waiting for 1730862028.40, time is 1730862024.84 2024-11-06T03:00:25Z- AHRS_M2 msg: 1730862025.20 epoch sec; seqNo:1543 2024-11-06T03:00:25Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.40 psi 2024-11-06T03:00:25Z- handleDepth()- Depth_Keller msg: 1730862025.21 epoch sec; seqNo:1648; depth 50.52 2024-11-06T03:00:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862025.22 epoch sec; seqNo:1648; 4061666.72 north; 594322.21 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:00:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862024.40 2024-11-06T03:00:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:25Z- waiting for 1730862028.40, time is 1730862025.22 2024-11-06T03:00:25Z- AHRS_M2 msg: 1730862025.59 epoch sec; seqNo:1544 2024-11-06T03:00:25Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.40 psi 2024-11-06T03:00:25Z- handleDepth()- Depth_Keller msg: 1730862025.61 epoch sec; seqNo:1649; depth 50.51 2024-11-06T03:00:25Z- handleDVL() - DVL msg epochMillisec: 1730862025.64 2024-11-06T03:00:25Z- handleDvl() - RDI_Pathfinder msg: 1730862025.64 sec; seqNo:504; #beams:4 2024-11-06T03:00:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.48 , 38.50 , 42.05 , 40.74 2024-11-06T03:00:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , -0.02 2024-11-06T03:00:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862025.65 epoch sec; seqNo:1649; 4061666.72 north; 594322.21 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:00:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862024.40 2024-11-06T03:00:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:25Z- waiting for 1730862028.40, time is 1730862025.65 2024-11-06T03:00:26Z- AHRS_M2 msg: 1730862026.00 epoch sec; seqNo:1545 2024-11-06T03:00:26Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.40 psi 2024-11-06T03:00:26Z- handleDepth()- Depth_Keller msg: 1730862026.00 epoch sec; seqNo:1650; depth 50.50 2024-11-06T03:00:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862026.02 epoch sec; seqNo:1650; 4061666.73 north; 594323.43 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:00:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862024.40 2024-11-06T03:00:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:26Z- waiting for 1730862028.40, time is 1730862026.02 2024-11-06T03:00:26Z- AHRS_M2 msg: 1730862026.40 epoch sec; seqNo:1546 2024-11-06T03:00:26Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.40 psi 2024-11-06T03:00:26Z- handleDepth()- Depth_Keller msg: 1730862026.41 epoch sec; seqNo:1651; depth 50.47 2024-11-06T03:00:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862026.42 epoch sec; seqNo:1651; 4061666.73 north; 594323.43 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:00:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862024.40 2024-11-06T03:00:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:26Z- waiting for 1730862028.40, time is 1730862026.42 2024-11-06T03:00:26Z- AHRS_M2 msg: 1730862026.81 epoch sec; seqNo:1547 2024-11-06T03:00:26Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.40 psi 2024-11-06T03:00:26Z- handleDepth()- Depth_Keller msg: 1730862026.82 epoch sec; seqNo:1652; depth 50.45 2024-11-06T03:00:26Z- handleDVL() - DVL msg epochMillisec: 1730862026.82 2024-11-06T03:00:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:00:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:00:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:00:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:00:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:00:26Z- handleDvl() - RDI_Pathfinder msg: 1730862026.82 sec; seqNo:505; #beams:0 2024-11-06T03:00:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:00:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , -0.02 2024-11-06T03:00:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862026.83 epoch sec; seqNo:1652; 4061666.73 north; 594323.43 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:00:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862024.40 2024-11-06T03:00:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:26Z- waiting for 1730862028.40, time is 1730862026.83 2024-11-06T03:00:27Z- AHRS_M2 msg: 1730862027.21 epoch sec; seqNo:1548 2024-11-06T03:00:27Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.41 psi 2024-11-06T03:00:27Z- handleDepth()- Depth_Keller msg: 1730862027.26 epoch sec; seqNo:1653; depth 50.41 2024-11-06T03:00:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862027.34 epoch sec; seqNo:1653; 4061666.73 north; 594323.43 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:00:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862024.40 2024-11-06T03:00:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:27Z- waiting for 1730862028.40, time is 1730862027.34 2024-11-06T03:00:27Z- AHRS_M2 msg: 1730862027.62 epoch sec; seqNo:1549 2024-11-06T03:00:27Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.41 psi 2024-11-06T03:00:27Z- handleDepth()- Depth_Keller msg: 1730862027.63 epoch sec; seqNo:1654; depth 50.39 2024-11-06T03:00:27Z- handleDVL() - DVL msg epochMillisec: 1730862027.66 2024-11-06T03:00:27Z- handleDvl() - RDI_Pathfinder msg: 1730862027.66 sec; seqNo:506; #beams:4 2024-11-06T03:00:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.48 , 38.50 , 41.67 , 40.74 2024-11-06T03:00:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.00 , -0.03 2024-11-06T03:00:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862027.67 epoch sec; seqNo:1654; 4061666.75 north; 594324.65 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:00:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862024.40 2024-11-06T03:00:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:27Z- waiting for 1730862028.40, time is 1730862027.67 2024-11-06T03:00:28Z- AHRS_M2 msg: 1730862028.02 epoch sec; seqNo:1550 2024-11-06T03:00:28Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.41 psi 2024-11-06T03:00:28Z- handleDepth()- Depth_Keller msg: 1730862028.03 epoch sec; seqNo:1655; depth 50.38 2024-11-06T03:00:28Z- handleDVL() - DVL msg epochMillisec: 1730862028.03 2024-11-06T03:00:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:00:28Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:00:28Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:00:28Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:00:28Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:00:28Z- handleDvl() - RDI_Pathfinder msg: 1730862028.03 sec; seqNo:507; #beams:0 2024-11-06T03:00:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:00:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.00 , -0.03 2024-11-06T03:00:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862028.05 epoch sec; seqNo:1655; 4061667.12 north; 594324.64 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:00:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862024.40 2024-11-06T03:00:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:28Z- waiting for 1730862028.40, time is 1730862028.05 2024-11-06T03:00:28Z- AHRS_M2 msg: 1730862028.42 epoch sec; seqNo:1551 2024-11-06T03:00:28Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.41 psi 2024-11-06T03:00:28Z- handleDepth()- Depth_Keller msg: 1730862028.43 epoch sec; seqNo:1656; depth 50.35 2024-11-06T03:00:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862028.44 epoch sec; seqNo:1656; 4061667.12 north; 594324.64 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:00:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862024.40 2024-11-06T03:00:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:28Z- waiting for 1730862028.40, time is 1730862028.44 2024-11-06T03:00:28Z- LcmTrn::updateTrn() >>>> pose time 1730862028.44, meas time 1730862028.03 <<<< 2024-11-06T03:00:28Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862028.44 2024-11-06T03:00:28Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:00:28Z- TerrainNav::motionUpdate() - currEstimate = (4061667, 594324.64, 50.35) 2024-11-06T03:00:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:00:28Z- TerrainNav:: SubcloudNIS is 0.43, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:00:28Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:00:28Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:00:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:00:28Z- variances([0], [2], and psi_berg) = 0.342936 0.217120 0.000000 2024-11-06T03:00:28Z- publishEstimatesDecoder() - at 1730862028.44 sec 2024-11-06T03:00:28Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:00:28Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061699.83 594308.68 51.44 2024-11-06T03:00:28Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061699.18 594308.58 51.77 lat/lon: 36.70 -121.94 2024-11-06T03:00:28Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.34 0.22 0.91 0.00 2024-11-06T03:00:28Z- AHRS_M2 msg: 1730862028.83 epoch sec; seqNo:1552 2024-11-06T03:00:28Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.41 psi 2024-11-06T03:00:28Z- handleDepth()- Depth_Keller msg: 1730862028.84 epoch sec; seqNo:1657; depth 50.33 2024-11-06T03:00:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862028.86 epoch sec; seqNo:1657; 4061667.12 north; 594324.64 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:00:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862028.44 2024-11-06T03:00:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:28Z- waiting for 1730862032.44, time is 1730862028.86 2024-11-06T03:00:29Z- AHRS_M2 msg: 1730862029.28 epoch sec; seqNo:1553 2024-11-06T03:00:29Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.41 psi 2024-11-06T03:00:29Z- handleDepth()- Depth_Keller msg: 1730862029.29 epoch sec; seqNo:1658; depth 50.32 2024-11-06T03:00:29Z- handleDVL() - DVL msg epochMillisec: 1730862029.32 2024-11-06T03:00:29Z- handleDvl() - RDI_Pathfinder msg: 1730862029.32 sec; seqNo:508; #beams:4 2024-11-06T03:00:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.48 , 38.31 , 41.87 , 40.56 2024-11-06T03:00:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.01 , -0.03 2024-11-06T03:00:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862029.33 epoch sec; seqNo:1658; 4061667.14 north; 594325.86 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:00:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862028.44 2024-11-06T03:00:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:29Z- waiting for 1730862032.44, time is 1730862029.33 2024-11-06T03:00:29Z- AHRS_M2 msg: 1730862029.64 epoch sec; seqNo:1554 2024-11-06T03:00:29Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.41 psi 2024-11-06T03:00:29Z- handleDepth()- Depth_Keller msg: 1730862029.65 epoch sec; seqNo:1659; depth 50.30 2024-11-06T03:00:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862029.68 epoch sec; seqNo:1659; 4061667.14 north; 594325.86 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:00:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862028.44 2024-11-06T03:00:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:29Z- waiting for 1730862032.44, time is 1730862029.68 2024-11-06T03:00:30Z- AHRS_M2 msg: 1730862030.04 epoch sec; seqNo:1555 2024-11-06T03:00:30Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 1.41 psi 2024-11-06T03:00:30Z- handleDepth()- Depth_Keller msg: 1730862030.05 epoch sec; seqNo:1660; depth 50.28 2024-11-06T03:00:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862030.06 epoch sec; seqNo:1660; 4061667.14 north; 594325.86 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:00:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862028.44 2024-11-06T03:00:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:30Z- waiting for 1730862032.44, time is 1730862030.06 2024-11-06T03:00:30Z- AHRS_M2 msg: 1730862030.44 epoch sec; seqNo:1556 2024-11-06T03:00:30Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 1.41 psi 2024-11-06T03:00:30Z- handleDepth()- Depth_Keller msg: 1730862030.45 epoch sec; seqNo:1661; depth 50.26 2024-11-06T03:00:30Z- handleDVL() - DVL msg epochMillisec: 1730862030.46 2024-11-06T03:00:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:00:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:00:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:00:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:00:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:00:30Z- handleDvl() - RDI_Pathfinder msg: 1730862030.46 sec; seqNo:509; #beams:0 2024-11-06T03:00:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:00:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.01 , -0.03 2024-11-06T03:00:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862030.46 epoch sec; seqNo:1661; 4061667.14 north; 594325.86 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:00:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862028.44 2024-11-06T03:00:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:30Z- waiting for 1730862032.44, time is 1730862030.46 2024-11-06T03:00:30Z- AHRS_M2 msg: 1730862030.85 epoch sec; seqNo:1557 2024-11-06T03:00:30Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 1.41 psi 2024-11-06T03:00:30Z- handleDepth()- Depth_Keller msg: 1730862030.85 epoch sec; seqNo:1662; depth 50.25 2024-11-06T03:00:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862030.86 epoch sec; seqNo:1662; 4061667.15 north; 594327.08 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:00:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862028.44 2024-11-06T03:00:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:31Z- waiting for 1730862032.44, time is 1730862030.86 2024-11-06T03:00:31Z- AHRS_M2 msg: 1730862031.29 epoch sec; seqNo:1558 2024-11-06T03:00:31Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 1.41 psi 2024-11-06T03:00:31Z- handleDepth()- Depth_Keller msg: 1730862031.30 epoch sec; seqNo:1663; depth 50.23 2024-11-06T03:00:31Z- handleDVL() - DVL msg epochMillisec: 1730862031.32 2024-11-06T03:00:31Z- handleDvl() - RDI_Pathfinder msg: 1730862031.32 sec; seqNo:510; #beams:4 2024-11-06T03:00:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.29 , 38.31 , 41.49 , 40.93 2024-11-06T03:00:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.01 , -0.01 2024-11-06T03:00:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862031.33 epoch sec; seqNo:1663; 4061667.53 north; 594327.08 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:00:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862028.44 2024-11-06T03:00:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:31Z- waiting for 1730862032.44, time is 1730862031.33 2024-11-06T03:00:31Z- AHRS_M2 msg: 1730862031.66 epoch sec; seqNo:1559 2024-11-06T03:00:31Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 1.41 psi 2024-11-06T03:00:31Z- handleDepth()- Depth_Keller msg: 1730862031.66 epoch sec; seqNo:1664; depth 50.22 2024-11-06T03:00:31Z- handleDVL() - DVL msg epochMillisec: 1730862031.67 2024-11-06T03:00:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:00:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:00:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:00:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:00:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:00:31Z- handleDvl() - RDI_Pathfinder msg: 1730862031.67 sec; seqNo:511; #beams:0 2024-11-06T03:00:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:00:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.01 , -0.01 2024-11-06T03:00:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862031.69 epoch sec; seqNo:1664; 4061667.53 north; 594327.08 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:00:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862028.44 2024-11-06T03:00:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:31Z- waiting for 1730862032.44, time is 1730862031.69 2024-11-06T03:00:32Z- AHRS_M2 msg: 1730862032.06 epoch sec; seqNo:1560 2024-11-06T03:00:32Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 1.41 psi 2024-11-06T03:00:32Z- handleDepth()- Depth_Keller msg: 1730862032.07 epoch sec; seqNo:1665; depth 50.22 2024-11-06T03:00:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862032.08 epoch sec; seqNo:1665; 4061667.53 north; 594327.08 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:00:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862028.44 2024-11-06T03:00:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:32Z- waiting for 1730862032.44, time is 1730862032.08 2024-11-06T03:00:32Z- AHRS_M2 msg: 1730862032.46 epoch sec; seqNo:1561 2024-11-06T03:00:32Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 1.40 psi 2024-11-06T03:00:32Z- handleDepth()- Depth_Keller msg: 1730862032.47 epoch sec; seqNo:1666; depth 50.20 2024-11-06T03:00:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862032.48 epoch sec; seqNo:1666; 4061667.54 north; 594328.30 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:00:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862028.44 2024-11-06T03:00:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:32Z- waiting for 1730862032.44, time is 1730862032.48 2024-11-06T03:00:32Z- LcmTrn::updateTrn() >>>> pose time 1730862032.48, meas time 1730862031.67 <<<< 2024-11-06T03:00:32Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862032.48 2024-11-06T03:00:32Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:00:32Z- TerrainNav::motionUpdate() - currEstimate = (4061668, 594328.30, 50.20) 2024-11-06T03:00:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:00:32Z- TerrainNav:: SubcloudNIS is 0.43, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:00:32Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:00:32Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:00:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:00:32Z- variances([0], [2], and psi_berg) = 0.344716 0.219860 0.000000 2024-11-06T03:00:32Z- publishEstimatesDecoder() - at 1730862032.48 sec 2024-11-06T03:00:32Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:00:32Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061700.20 594312.34 51.65 2024-11-06T03:00:32Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061699.60 594312.23 51.62 lat/lon: 36.70 -121.94 2024-11-06T03:00:32Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.34 0.22 1.15 0.00 2024-11-06T03:00:32Z- AHRS_M2 msg: 1730862032.87 epoch sec; seqNo:1562 2024-11-06T03:00:32Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 1.40 psi 2024-11-06T03:00:32Z- handleDepth()- Depth_Keller msg: 1730862032.88 epoch sec; seqNo:1667; depth 50.19 2024-11-06T03:00:32Z- handleDVL() - DVL msg epochMillisec: 1730862032.88 2024-11-06T03:00:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:00:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:00:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:00:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:00:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:00:32Z- handleDvl() - RDI_Pathfinder msg: 1730862032.88 sec; seqNo:512; #beams:0 2024-11-06T03:00:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:00:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.01 , -0.01 2024-11-06T03:00:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862032.89 epoch sec; seqNo:1667; 4061667.54 north; 594328.30 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:00:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862032.48 2024-11-06T03:00:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:33Z- waiting for 1730862036.48, time is 1730862032.89 2024-11-06T03:00:33Z- AHRS_M2 msg: 1730862033.29 epoch sec; seqNo:1563 2024-11-06T03:00:33Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.40 psi 2024-11-06T03:00:33Z- handleDepth()- Depth_Keller msg: 1730862033.30 epoch sec; seqNo:1668; depth 50.18 2024-11-06T03:00:33Z- handleDVL() - DVL msg epochMillisec: 1730862033.32 2024-11-06T03:00:33Z- handleDvl() - RDI_Pathfinder msg: 1730862033.32 sec; seqNo:513; #beams:4 2024-11-06T03:00:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.73 , 38.69 , 41.30 , 40.93 2024-11-06T03:00:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.01 , 0.03 2024-11-06T03:00:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862033.34 epoch sec; seqNo:1668; 4061667.54 north; 594328.30 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:00:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862032.48 2024-11-06T03:00:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:33Z- waiting for 1730862036.48, time is 1730862033.34 2024-11-06T03:00:33Z- AHRS_M2 msg: 1730862033.67 epoch sec; seqNo:1564 2024-11-06T03:00:33Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.40 psi 2024-11-06T03:00:33Z- handleDepth()- Depth_Keller msg: 1730862033.68 epoch sec; seqNo:1669; depth 50.17 2024-11-06T03:00:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862033.70 epoch sec; seqNo:1669; 4061667.54 north; 594328.30 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:00:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862032.48 2024-11-06T03:00:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:33Z- waiting for 1730862036.48, time is 1730862033.70 2024-11-06T03:00:34Z- AHRS_M2 msg: 1730862034.08 epoch sec; seqNo:1565 2024-11-06T03:00:34Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.40 psi 2024-11-06T03:00:34Z- handleDepth()- Depth_Keller msg: 1730862034.08 epoch sec; seqNo:1670; depth 50.16 2024-11-06T03:00:34Z- handleDVL() - DVL msg epochMillisec: 1730862034.09 2024-11-06T03:00:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:00:34Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:00:34Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:00:34Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:00:34Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:00:34Z- handleDvl() - RDI_Pathfinder msg: 1730862034.09 sec; seqNo:514; #beams:0 2024-11-06T03:00:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:00:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.01 , 0.03 2024-11-06T03:00:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862034.11 epoch sec; seqNo:1670; 4061667.56 north; 594329.52 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:00:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862032.48 2024-11-06T03:00:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:34Z- waiting for 1730862036.48, time is 1730862034.11 2024-11-06T03:00:34Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:00:34Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:00:34Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:00:34Z- AHRS_M2 msg: 1730862034.47 epoch sec; seqNo:1566 2024-11-06T03:00:34Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.40 psi 2024-11-06T03:00:34Z- handleDepth()- Depth_Keller msg: 1730862034.51 epoch sec; seqNo:1671; depth 50.16 2024-11-06T03:00:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862034.52 epoch sec; seqNo:1671; 4061667.94 north; 594329.52 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:00:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862032.48 2024-11-06T03:00:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:34Z- waiting for 1730862036.48, time is 1730862034.52 2024-11-06T03:00:34Z- AHRS_M2 msg: 1730862034.89 epoch sec; seqNo:1567 2024-11-06T03:00:34Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.40 psi 2024-11-06T03:00:34Z- handleDepth()- Depth_Keller msg: 1730862034.90 epoch sec; seqNo:1672; depth 50.14 2024-11-06T03:00:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862034.90 epoch sec; seqNo:1672; 4061667.94 north; 594329.52 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:00:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862032.48 2024-11-06T03:00:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:35Z- waiting for 1730862036.48, time is 1730862034.90 2024-11-06T03:00:35Z- AHRS_M2 msg: 1730862035.30 epoch sec; seqNo:1568 2024-11-06T03:00:35Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.40 psi 2024-11-06T03:00:35Z- handleDepth()- Depth_Keller msg: 1730862035.30 epoch sec; seqNo:1673; depth 50.14 2024-11-06T03:00:35Z- handleDVL() - DVL msg epochMillisec: 1730862035.33 2024-11-06T03:00:35Z- handleDvl() - RDI_Pathfinder msg: 1730862035.33 sec; seqNo:515; #beams:4 2024-11-06T03:00:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.55 , 38.69 , 41.12 , 41.12 2024-11-06T03:00:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , 0.03 2024-11-06T03:00:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862035.34 epoch sec; seqNo:1673; 4061667.94 north; 594329.52 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:00:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862032.48 2024-11-06T03:00:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:35Z- waiting for 1730862036.48, time is 1730862035.34 2024-11-06T03:00:35Z- AHRS_M2 msg: 1730862035.70 epoch sec; seqNo:1569 2024-11-06T03:00:35Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.40 psi 2024-11-06T03:00:35Z- handleDepth()- Depth_Keller msg: 1730862035.70 epoch sec; seqNo:1674; depth 50.14 2024-11-06T03:00:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862035.73 epoch sec; seqNo:1674; 4061667.95 north; 594330.74 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:00:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862032.48 2024-11-06T03:00:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:35Z- waiting for 1730862036.48, time is 1730862035.73 2024-11-06T03:00:36Z- AHRS_M2 msg: 1730862036.10 epoch sec; seqNo:1570 2024-11-06T03:00:36Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.40 psi 2024-11-06T03:00:36Z- handleDepth()- Depth_Keller msg: 1730862036.11 epoch sec; seqNo:1675; depth 50.13 2024-11-06T03:00:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862036.12 epoch sec; seqNo:1675; 4061667.95 north; 594330.74 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:00:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862032.48 2024-11-06T03:00:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:36Z- waiting for 1730862036.48, time is 1730862036.12 2024-11-06T03:00:36Z- AHRS_M2 msg: 1730862036.50 epoch sec; seqNo:1571 2024-11-06T03:00:36Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.40 psi 2024-11-06T03:00:36Z- handleDepth()- Depth_Keller msg: 1730862036.51 epoch sec; seqNo:1676; depth 50.12 2024-11-06T03:00:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862036.52 epoch sec; seqNo:1676; 4061667.95 north; 594330.74 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:00:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862032.48 2024-11-06T03:00:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:36Z- waiting for 1730862036.48, time is 1730862036.52 2024-11-06T03:00:36Z- LcmTrn::updateTrn() >>>> pose time 1730862036.52, meas time 1730862035.33 <<<< 2024-11-06T03:00:36Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862036.52 2024-11-06T03:00:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862035.33 sec, ping # 0; waiting for INS data more recent than 1730862032.48... 2024-11-06T03:00:36Z- TerrainNav::motionUpdate() - currEstimate = (4061668, 594330.74, 50.12) 2024-11-06T03:00:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:00:36Z- TerrainNav:: SubcloudNIS is 0.43, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:00:36Z- TNavPF::Projecting Measurements 2024-11-06T03:00:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:00:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:00:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:00:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:00:36Z- TNavFilter::4 good beams. 2024-11-06T03:00:36Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:00:36Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:00:36Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:00:36Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:00:36Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:00:36Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:00:36Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:00:36Z- TerrainMapOctree: (vehN, vehE ) = (4061667.83, 594330.02). 2024-11-06T03:00:36Z- TerrainMapOctree: Min Distance = 114.69. 2024-11-06T03:00:36Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:00:36Z- Forcing Subcloud Comparison 2024-11-06T03:00:36Z- Weighting particles with subcloud comparison 2024-11-06T03:00:36Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:36Z- meanExpectedMeasDiff: 2.282234 delta_rms_squared: 2.647114 alpha: 0.992501 2024-11-06T03:00:36Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:36Z- meanExpectedMeasDiff: -0.396037 delta_rms_squared: 1.611121 alpha: 0.987738 2024-11-06T03:00:36Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:36Z- meanExpectedMeasDiff: 5.601817 delta_rms_squared: 1.402500 alpha: 0.985940 2024-11-06T03:00:36Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:36Z- meanExpectedMeasDiff: -0.119299 delta_rms_squared: 1.768759 alpha: 0.988819 2024-11-06T03:00:36Z- Subcloud NIS: 0.458065 2024-11-06T03:00:36Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:00:36Z- TNavPF:: sumWeights = 0.025486 2024-11-06T03:00:36Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:00:36Z- TNavPF::First Term of Map Covariance Matrix is 0.05 2024-11-06T03:00:36Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:00:36Z- TerrainNav::Resampling particle filter... 2024-11-06T03:00:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862035.33 sec. 2024-11-06T03:00:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:00:36Z- variances([0], [2], and psi_berg) = 0.402033 0.197307 0.000000 2024-11-06T03:00:36Z- publishEstimatesDecoder() - at 1730862036.52 sec 2024-11-06T03:00:36Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:00:36Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061700.54 594314.81 51.86 2024-11-06T03:00:36Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061700.02 594314.51 50.70 lat/lon: 36.70 -121.94 2024-11-06T03:00:36Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.40 0.20 0.79 0.00 2024-11-06T03:00:36Z- AHRS_M2 msg: 1730862036.92 epoch sec; seqNo:1572 2024-11-06T03:00:36Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.40 psi 2024-11-06T03:00:36Z- handleDepth()- Depth_Keller msg: 1730862036.93 epoch sec; seqNo:1677; depth 50.10 2024-11-06T03:00:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862036.94 epoch sec; seqNo:1677; 4061668.34 north; 594331.95 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:00:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862036.52 2024-11-06T03:00:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:37Z- waiting for 1730862040.52, time is 1730862036.94 2024-11-06T03:00:37Z- AHRS_M2 msg: 1730862037.33 epoch sec; seqNo:1573 2024-11-06T03:00:37Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.41 psi 2024-11-06T03:00:37Z- handleDepth()- Depth_Keller msg: 1730862037.35 epoch sec; seqNo:1678; depth 50.11 2024-11-06T03:00:37Z- handleDVL() - DVL msg epochMillisec: 1730862037.38 2024-11-06T03:00:37Z- handleDvl() - RDI_Pathfinder msg: 1730862037.38 sec; seqNo:516; #beams:4 2024-11-06T03:00:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.05 , 38.87 , 40.93 , 41.49 2024-11-06T03:00:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.00 2024-11-06T03:00:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862037.39 epoch sec; seqNo:1678; 4061668.34 north; 594331.95 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:00:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862036.52 2024-11-06T03:00:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:37Z- waiting for 1730862040.52, time is 1730862037.39 2024-11-06T03:00:37Z- AHRS_M2 msg: 1730862037.71 epoch sec; seqNo:1574 2024-11-06T03:00:37Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.41 psi 2024-11-06T03:00:37Z- handleDepth()- Depth_Keller msg: 1730862037.72 epoch sec; seqNo:1679; depth 50.08 2024-11-06T03:00:37Z- handleDVL() - DVL msg epochMillisec: 1730862037.72 2024-11-06T03:00:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:00:37Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:00:37Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:00:37Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:00:37Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:00:37Z- handleDvl() - RDI_Pathfinder msg: 1730862037.72 sec; seqNo:517; #beams:0 2024-11-06T03:00:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:00:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.00 2024-11-06T03:00:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862037.74 epoch sec; seqNo:1679; 4061668.34 north; 594331.95 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:00:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862036.52 2024-11-06T03:00:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:37Z- waiting for 1730862040.52, time is 1730862037.74 2024-11-06T03:00:38Z- AHRS_M2 msg: 1730862038.12 epoch sec; seqNo:1575 2024-11-06T03:00:38Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.41 psi 2024-11-06T03:00:38Z- handleDepth()- Depth_Keller msg: 1730862038.12 epoch sec; seqNo:1680; depth 50.09 2024-11-06T03:00:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862038.13 epoch sec; seqNo:1680; 4061668.34 north; 594331.95 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:00:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862036.52 2024-11-06T03:00:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:38Z- waiting for 1730862040.52, time is 1730862038.13 2024-11-06T03:00:38Z- AHRS_M2 msg: 1730862038.52 epoch sec; seqNo:1576 2024-11-06T03:00:38Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.41 psi 2024-11-06T03:00:38Z- handleDepth()- Depth_Keller msg: 1730862038.54 epoch sec; seqNo:1681; depth 50.10 2024-11-06T03:00:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862038.55 epoch sec; seqNo:1681; 4061668.36 north; 594333.17 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:00:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862036.52 2024-11-06T03:00:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:38Z- waiting for 1730862040.52, time is 1730862038.55 2024-11-06T03:00:38Z- AHRS_M2 msg: 1730862038.93 epoch sec; seqNo:1577 2024-11-06T03:00:38Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.41 psi 2024-11-06T03:00:38Z- handleDepth()- Depth_Keller msg: 1730862038.93 epoch sec; seqNo:1682; depth 50.09 2024-11-06T03:00:38Z- handleDVL() - DVL msg epochMillisec: 1730862038.94 2024-11-06T03:00:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:00:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:00:38Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:00:38Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:00:38Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:00:38Z- handleDvl() - RDI_Pathfinder msg: 1730862038.94 sec; seqNo:518; #beams:0 2024-11-06T03:00:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:00:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.00 2024-11-06T03:00:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862038.96 epoch sec; seqNo:1682; 4061668.36 north; 594333.17 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:00:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862036.52 2024-11-06T03:00:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:39Z- waiting for 1730862040.52, time is 1730862038.96 2024-11-06T03:00:39Z- AHRS_M2 msg: 1730862039.33 epoch sec; seqNo:1578 2024-11-06T03:00:39Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.41 psi 2024-11-06T03:00:39Z- handleDepth()- Depth_Keller msg: 1730862039.34 epoch sec; seqNo:1683; depth 50.07 2024-11-06T03:00:39Z- handleDVL() - DVL msg epochMillisec: 1730862039.36 2024-11-06T03:00:39Z- handleDvl() - RDI_Pathfinder msg: 1730862039.36 sec; seqNo:519; #beams:4 2024-11-06T03:00:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.12 , 38.87 , 40.37 , 41.49 2024-11-06T03:00:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , -0.05 2024-11-06T03:00:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862039.38 epoch sec; seqNo:1683; 4061668.36 north; 594333.17 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:00:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862036.52 2024-11-06T03:00:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:39Z- waiting for 1730862040.52, time is 1730862039.38 2024-11-06T03:00:39Z- AHRS_M2 msg: 1730862039.74 epoch sec; seqNo:1579 2024-11-06T03:00:39Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.41 psi 2024-11-06T03:00:39Z- handleDepth()- Depth_Keller msg: 1730862039.74 epoch sec; seqNo:1684; depth 50.06 2024-11-06T03:00:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862039.75 epoch sec; seqNo:1684; 4061668.37 north; 594334.39 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:00:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862036.52 2024-11-06T03:00:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:39Z- waiting for 1730862040.52, time is 1730862039.75 2024-11-06T03:00:40Z- AHRS_M2 msg: 1730862040.14 epoch sec; seqNo:1580 2024-11-06T03:00:40Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.41 psi 2024-11-06T03:00:40Z- handleDepth()- Depth_Keller msg: 1730862040.14 epoch sec; seqNo:1685; depth 50.06 2024-11-06T03:00:40Z- handleDVL() - DVL msg epochMillisec: 1730862040.15 2024-11-06T03:00:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:00:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:00:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:00:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:00:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:00:40Z- handleDvl() - RDI_Pathfinder msg: 1730862040.15 sec; seqNo:520; #beams:0 2024-11-06T03:00:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:00:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , -0.05 2024-11-06T03:00:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862040.16 epoch sec; seqNo:1685; 4061668.75 north; 594334.39 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:00:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862036.52 2024-11-06T03:00:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:40Z- waiting for 1730862040.52, time is 1730862040.16 2024-11-06T03:00:40Z- AHRS_M2 msg: 1730862040.54 epoch sec; seqNo:1581 2024-11-06T03:00:40Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.41 psi 2024-11-06T03:00:40Z- handleDepth()- Depth_Keller msg: 1730862040.55 epoch sec; seqNo:1686; depth 50.04 2024-11-06T03:00:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862040.56 epoch sec; seqNo:1686; 4061668.75 north; 594334.39 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:00:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862036.52 2024-11-06T03:00:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:40Z- waiting for 1730862040.52, time is 1730862040.56 2024-11-06T03:00:40Z- LcmTrn::updateTrn() >>>> pose time 1730862040.56, meas time 1730862040.15 <<<< 2024-11-06T03:00:40Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862040.56 2024-11-06T03:00:40Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:00:40Z- TerrainNav::motionUpdate() - currEstimate = (4061669, 594334.39, 50.04) 2024-11-06T03:00:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:00:40Z- TerrainNav:: SubcloudNIS is 0.46, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:00:40Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:00:40Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:00:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:00:40Z- variances([0], [2], and psi_berg) = 0.405148 0.199120 0.000000 2024-11-06T03:00:40Z- publishEstimatesDecoder() - at 1730862040.56 sec 2024-11-06T03:00:40Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:00:40Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061701.32 594318.38 51.24 2024-11-06T03:00:40Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061700.82 594318.16 50.63 lat/lon: 36.70 -121.94 2024-11-06T03:00:40Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.41 0.20 1.04 0.00 2024-11-06T03:00:40Z- AHRS_M2 msg: 1730862040.95 epoch sec; seqNo:1582 2024-11-06T03:00:40Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:00:40Z- handleDepth()- Depth_Keller msg: 1730862040.96 epoch sec; seqNo:1687; depth 50.04 2024-11-06T03:00:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862040.97 epoch sec; seqNo:1687; 4061668.75 north; 594334.39 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:00:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862040.56 2024-11-06T03:00:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:41Z- waiting for 1730862044.56, time is 1730862040.97 2024-11-06T03:00:41Z- handleDepth()- Depth_Keller msg: 1730862041.37 epoch sec; seqNo:1688; depth 50.04 2024-11-06T03:00:41Z- handleDVL() - DVL msg epochMillisec: 1730862041.39 2024-11-06T03:00:41Z- handleDvl() - RDI_Pathfinder msg: 1730862041.39 sec; seqNo:521; #beams:4 2024-11-06T03:00:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.93 , 38.50 , 40.18 , 41.30 2024-11-06T03:00:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.00 , -0.07 2024-11-06T03:00:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862041.40 epoch sec; seqNo:1688; 4061668.76 north; 594335.61 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:00:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862040.56 2024-11-06T03:00:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:41Z- waiting for 1730862044.56, time is 1730862041.40 2024-11-06T03:00:41Z- AHRS_M2 msg: 1730862041.76 epoch sec; seqNo:1583 2024-11-06T03:00:41Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:00:41Z- handleDepth()- Depth_Keller msg: 1730862041.76 epoch sec; seqNo:1689; depth 50.02 2024-11-06T03:00:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862041.78 epoch sec; seqNo:1689; 4061668.76 north; 594335.61 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:00:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862040.56 2024-11-06T03:00:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:41Z- waiting for 1730862044.56, time is 1730862041.78 2024-11-06T03:00:42Z- AHRS_M2 msg: 1730862042.17 epoch sec; seqNo:1584 2024-11-06T03:00:42Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:00:42Z- handleDepth()- Depth_Keller msg: 1730862042.17 epoch sec; seqNo:1690; depth 50.02 2024-11-06T03:00:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862042.18 epoch sec; seqNo:1690; 4061668.76 north; 594335.61 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:00:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862040.56 2024-11-06T03:00:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:42Z- waiting for 1730862044.56, time is 1730862042.18 2024-11-06T03:00:42Z- AHRS_M2 msg: 1730862042.56 epoch sec; seqNo:1585 2024-11-06T03:00:42Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.40 psi 2024-11-06T03:00:42Z- handleDepth()- Depth_Keller msg: 1730862042.57 epoch sec; seqNo:1691; depth 50.02 2024-11-06T03:00:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862042.59 epoch sec; seqNo:1691; 4061668.76 north; 594335.61 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:00:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862040.56 2024-11-06T03:00:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:42Z- waiting for 1730862044.56, time is 1730862042.59 2024-11-06T03:00:42Z- AHRS_M2 msg: 1730862042.97 epoch sec; seqNo:1586 2024-11-06T03:00:42Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.40 psi 2024-11-06T03:00:42Z- handleDepth()- Depth_Keller msg: 1730862042.98 epoch sec; seqNo:1692; depth 50.01 2024-11-06T03:00:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862042.99 epoch sec; seqNo:1692; 4061668.77 north; 594336.83 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:00:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862040.56 2024-11-06T03:00:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:43Z- waiting for 1730862044.56, time is 1730862042.99 2024-11-06T03:00:43Z- handleDepth()- Depth_Keller msg: 1730862043.39 epoch sec; seqNo:1693; depth 50.01 2024-11-06T03:00:43Z- handleDVL() - DVL msg epochMillisec: 1730862043.41 2024-11-06T03:00:43Z- handleDvl() - RDI_Pathfinder msg: 1730862043.41 sec; seqNo:522; #beams:4 2024-11-06T03:00:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.42 , 38.50 , 39.44 , 41.12 2024-11-06T03:00:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.00 , -0.03 2024-11-06T03:00:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862043.45 epoch sec; seqNo:1693; 4061669.15 north; 594336.83 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:00:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862040.56 2024-11-06T03:00:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:43Z- waiting for 1730862044.56, time is 1730862043.45 2024-11-06T03:00:43Z- AHRS_M2 msg: 1730862043.77 epoch sec; seqNo:1587 2024-11-06T03:00:43Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.40 psi 2024-11-06T03:00:43Z- handleDepth()- Depth_Keller msg: 1730862043.79 epoch sec; seqNo:1694; depth 50.01 2024-11-06T03:00:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862043.81 epoch sec; seqNo:1694; 4061669.15 north; 594336.83 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:00:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862040.56 2024-11-06T03:00:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:43Z- waiting for 1730862044.56, time is 1730862043.81 2024-11-06T03:00:44Z- AHRS_M2 msg: 1730862044.18 epoch sec; seqNo:1588 2024-11-06T03:00:44Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.40 psi 2024-11-06T03:00:44Z- handleDepth()- Depth_Keller msg: 1730862044.19 epoch sec; seqNo:1695; depth 50.00 2024-11-06T03:00:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862044.20 epoch sec; seqNo:1695; 4061669.15 north; 594336.83 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:00:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862040.56 2024-11-06T03:00:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:44Z- waiting for 1730862044.56, time is 1730862044.20 2024-11-06T03:00:44Z- AHRS_M2 msg: 1730862044.59 epoch sec; seqNo:1589 2024-11-06T03:00:44Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.40 psi 2024-11-06T03:00:44Z- handleDepth()- Depth_Keller msg: 1730862044.60 epoch sec; seqNo:1696; depth 50.00 2024-11-06T03:00:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862044.60 epoch sec; seqNo:1696; 4061669.17 north; 594338.05 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:00:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862040.56 2024-11-06T03:00:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:44Z- waiting for 1730862044.56, time is 1730862044.60 2024-11-06T03:00:44Z- LcmTrn::updateTrn() >>>> pose time 1730862044.60, meas time 1730862043.41 <<<< 2024-11-06T03:00:44Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862044.60 2024-11-06T03:00:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862043.41 sec, ping # 0; waiting for INS data more recent than 1730862040.56... 2024-11-06T03:00:44Z- TerrainNav::motionUpdate() - currEstimate = (4061669, 594338.05, 50.00) 2024-11-06T03:00:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:00:44Z- TerrainNav:: SubcloudNIS is 0.46, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:00:44Z- TNavPF::Projecting Measurements 2024-11-06T03:00:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:00:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:00:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:00:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:00:44Z- TNavFilter::4 good beams. 2024-11-06T03:00:44Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:00:44Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:00:44Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:00:44Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:00:44Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:00:44Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:00:44Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:00:44Z- TerrainMapOctree: (vehN, vehE ) = (4061669.04, 594336.97). 2024-11-06T03:00:44Z- TerrainMapOctree: Min Distance = 118.82. 2024-11-06T03:00:44Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:00:44Z- Forcing Subcloud Comparison 2024-11-06T03:00:44Z- Weighting particles with subcloud comparison 2024-11-06T03:00:44Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:44Z- meanExpectedMeasDiff: 1.861849 delta_rms_squared: 2.123217 alpha: 0.990668 2024-11-06T03:00:44Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:44Z- meanExpectedMeasDiff: -0.204803 delta_rms_squared: 2.212514 alpha: 0.991041 2024-11-06T03:00:44Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:44Z- meanExpectedMeasDiff: 6.097974 delta_rms_squared: 1.368572 alpha: 0.985597 2024-11-06T03:00:44Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:44Z- meanExpectedMeasDiff: -1.842148 delta_rms_squared: 1.712416 alpha: 0.988455 2024-11-06T03:00:44Z- Subcloud NIS: 0.487705 2024-11-06T03:00:44Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:00:44Z- TNavPF:: sumWeights = 0.006497 2024-11-06T03:00:44Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:00:44Z- TNavPF::First Term of Map Covariance Matrix is 0.01 2024-11-06T03:00:44Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:00:44Z- TerrainNav::Resampling particle filter... 2024-11-06T03:00:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862043.41 sec. 2024-11-06T03:00:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:00:44Z- variances([0], [2], and psi_berg) = 0.577901 0.218120 0.000000 2024-11-06T03:00:44Z- publishEstimatesDecoder() - at 1730862044.60 sec 2024-11-06T03:00:44Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:00:44Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061701.62 594322.23 50.41 2024-11-06T03:00:44Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061700.87 594321.85 49.94 lat/lon: 36.70 -121.94 2024-11-06T03:00:44Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.58 0.22 0.85 0.00 2024-11-06T03:00:45Z- handleDepth()- Depth_Keller msg: 1730862045.01 epoch sec; seqNo:1697; depth 50.00 2024-11-06T03:00:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862045.02 epoch sec; seqNo:1697; 4061669.17 north; 594338.05 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:00:45Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:00:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862044.60 2024-11-06T03:00:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:45Z- waiting for 1730862048.60, time is 1730862045.02 2024-11-06T03:00:45Z- AHRS_M2 msg: 1730862045.39 epoch sec; seqNo:1590 2024-11-06T03:00:45Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.40 psi 2024-11-06T03:00:45Z- handleDepth()- Depth_Keller msg: 1730862045.39 epoch sec; seqNo:1698; depth 50.01 2024-11-06T03:00:45Z- handleDVL() - DVL msg epochMillisec: 1730862045.42 2024-11-06T03:00:45Z- handleDvl() - RDI_Pathfinder msg: 1730862045.42 sec; seqNo:523; #beams:4 2024-11-06T03:00:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.55 , 38.31 , 39.06 , 41.12 2024-11-06T03:00:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.01 , -0.01 2024-11-06T03:00:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862045.44 epoch sec; seqNo:1698; 4061669.17 north; 594338.05 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:00:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862044.60 2024-11-06T03:00:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:45Z- waiting for 1730862048.60, time is 1730862045.44 2024-11-06T03:00:45Z- AHRS_M2 msg: 1730862045.80 epoch sec; seqNo:1591 2024-11-06T03:00:45Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.40 psi 2024-11-06T03:00:45Z- handleDepth()- Depth_Keller msg: 1730862045.81 epoch sec; seqNo:1699; depth 50.00 2024-11-06T03:00:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862045.82 epoch sec; seqNo:1699; 4061669.17 north; 594338.05 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:00:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862044.60 2024-11-06T03:00:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:45Z- waiting for 1730862048.60, time is 1730862045.82 2024-11-06T03:00:46Z- AHRS_M2 msg: 1730862046.20 epoch sec; seqNo:1592 2024-11-06T03:00:46Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.40 psi 2024-11-06T03:00:46Z- handleDepth()- Depth_Keller msg: 1730862046.21 epoch sec; seqNo:1700; depth 50.01 2024-11-06T03:00:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862046.22 epoch sec; seqNo:1700; 4061669.56 north; 594339.26 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:00:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862044.60 2024-11-06T03:00:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:46Z- waiting for 1730862048.60, time is 1730862046.22 2024-11-06T03:00:46Z- AHRS_M2 msg: 1730862046.60 epoch sec; seqNo:1593 2024-11-06T03:00:46Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.40 psi 2024-11-06T03:00:46Z- handleDepth()- Depth_Keller msg: 1730862046.61 epoch sec; seqNo:1701; depth 50.02 2024-11-06T03:00:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862046.63 epoch sec; seqNo:1701; 4061669.56 north; 594339.26 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:00:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862044.60 2024-11-06T03:00:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:46Z- waiting for 1730862048.60, time is 1730862046.63 2024-11-06T03:00:47Z- AHRS_M2 msg: 1730862047.01 epoch sec; seqNo:1594 2024-11-06T03:00:47Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.40 psi 2024-11-06T03:00:47Z- handleDepth()- Depth_Keller msg: 1730862047.01 epoch sec; seqNo:1702; depth 50.04 2024-11-06T03:00:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862047.02 epoch sec; seqNo:1702; 4061669.56 north; 594339.26 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:00:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862044.60 2024-11-06T03:00:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:47Z- waiting for 1730862048.60, time is 1730862047.02 2024-11-06T03:00:47Z- AHRS_M2 msg: 1730862047.41 epoch sec; seqNo:1595 2024-11-06T03:00:47Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.40 psi 2024-11-06T03:00:47Z- handleDepth()- Depth_Keller msg: 1730862047.42 epoch sec; seqNo:1703; depth 50.05 2024-11-06T03:00:47Z- handleDVL() - DVL msg epochMillisec: 1730862047.44 2024-11-06T03:00:47Z- handleDvl() - RDI_Pathfinder msg: 1730862047.44 sec; seqNo:524; #beams:4 2024-11-06T03:00:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.80 , 38.31 , 38.87 , 41.49 2024-11-06T03:00:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.01 , -0.01 2024-11-06T03:00:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862047.45 epoch sec; seqNo:1703; 4061669.57 north; 594340.48 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:00:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862044.60 2024-11-06T03:00:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:47Z- waiting for 1730862048.60, time is 1730862047.45 2024-11-06T03:00:47Z- AHRS_M2 msg: 1730862047.82 epoch sec; seqNo:1596 2024-11-06T03:00:47Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.40 psi 2024-11-06T03:00:47Z- handleDepth()- Depth_Keller msg: 1730862047.82 epoch sec; seqNo:1704; depth 50.05 2024-11-06T03:00:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862047.84 epoch sec; seqNo:1704; 4061669.57 north; 594340.48 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:00:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862044.60 2024-11-06T03:00:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:47Z- waiting for 1730862048.60, time is 1730862047.84 2024-11-06T03:00:48Z- AHRS_M2 msg: 1730862048.22 epoch sec; seqNo:1597 2024-11-06T03:00:48Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.40 psi 2024-11-06T03:00:48Z- handleDepth()- Depth_Keller msg: 1730862048.22 epoch sec; seqNo:1705; depth 50.06 2024-11-06T03:00:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862048.23 epoch sec; seqNo:1705; 4061669.57 north; 594340.48 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:00:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862044.60 2024-11-06T03:00:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:48Z- waiting for 1730862048.60, time is 1730862048.23 2024-11-06T03:00:48Z- AHRS_M2 msg: 1730862048.63 epoch sec; seqNo:1598 2024-11-06T03:00:48Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.40 psi 2024-11-06T03:00:48Z- handleDepth()- Depth_Keller msg: 1730862048.63 epoch sec; seqNo:1706; depth 50.07 2024-11-06T03:00:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862048.65 epoch sec; seqNo:1706; 4061669.95 north; 594340.48 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:00:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862044.60 2024-11-06T03:00:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:48Z- waiting for 1730862048.60, time is 1730862048.65 2024-11-06T03:00:48Z- LcmTrn::updateTrn() >>>> pose time 1730862048.65, meas time 1730862047.44 <<<< 2024-11-06T03:00:48Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862048.65 2024-11-06T03:00:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862047.44 sec, ping # 0; waiting for INS data more recent than 1730862044.60... 2024-11-06T03:00:48Z- TerrainNav::motionUpdate() - currEstimate = (4061670, 594340.48, 50.07) 2024-11-06T03:00:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:00:48Z- TerrainNav:: SubcloudNIS is 0.49, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:00:48Z- TNavPF::Projecting Measurements 2024-11-06T03:00:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:00:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:00:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:00:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:00:48Z- TNavFilter::4 good beams. 2024-11-06T03:00:48Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:00:48Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:00:48Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:00:48Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:00:48Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:00:48Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:00:48Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:00:48Z- TerrainMapOctree: (vehN, vehE ) = (4061669.72, 594339.75). 2024-11-06T03:00:48Z- TerrainMapOctree: Min Distance = 120.42. 2024-11-06T03:00:48Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:00:48Z- Forcing Subcloud Comparison 2024-11-06T03:00:48Z- Weighting particles with subcloud comparison 2024-11-06T03:00:48Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:48Z- meanExpectedMeasDiff: 2.636647 delta_rms_squared: 2.038442 alpha: 0.990284 2024-11-06T03:00:48Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:48Z- meanExpectedMeasDiff: -1.068296 delta_rms_squared: 1.631632 alpha: 0.987891 2024-11-06T03:00:48Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:48Z- meanExpectedMeasDiff: 5.253280 delta_rms_squared: 1.273124 alpha: 0.984534 2024-11-06T03:00:48Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:48Z- meanExpectedMeasDiff: -2.374809 delta_rms_squared: 1.484542 alpha: 0.986707 2024-11-06T03:00:48Z- Subcloud NIS: 0.546654 2024-11-06T03:00:48Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:00:48Z- TNavPF:: sumWeights = 0.007367 2024-11-06T03:00:48Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:00:48Z- TNavPF::First Term of Map Covariance Matrix is 0.04 2024-11-06T03:00:48Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:00:48Z- TerrainNav::Resampling particle filter... 2024-11-06T03:00:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862047.44 sec. 2024-11-06T03:00:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:00:48Z- variances([0], [2], and psi_berg) = 0.669367 0.229930 0.000000 2024-11-06T03:00:48Z- publishEstimatesDecoder() - at 1730862048.65 sec 2024-11-06T03:00:48Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:00:48Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061702.48 594324.58 50.41 2024-11-06T03:00:48Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061701.63 594324.29 49.51 lat/lon: 36.70 -121.94 2024-11-06T03:00:48Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.67 0.23 0.77 0.00 2024-11-06T03:00:49Z- AHRS_M2 msg: 1730862049.03 epoch sec; seqNo:1599 2024-11-06T03:00:49Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.40 psi 2024-11-06T03:00:49Z- handleDepth()- Depth_Keller msg: 1730862049.05 epoch sec; seqNo:1707; depth 50.09 2024-11-06T03:00:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862049.06 epoch sec; seqNo:1707; 4061669.96 north; 594341.70 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:00:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862048.65 2024-11-06T03:00:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:49Z- waiting for 1730862052.65, time is 1730862049.06 2024-11-06T03:00:49Z- AHRS_M2 msg: 1730862049.43 epoch sec; seqNo:1600 2024-11-06T03:00:49Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.40 psi 2024-11-06T03:00:49Z- handleDepth()- Depth_Keller msg: 1730862049.44 epoch sec; seqNo:1708; depth 50.10 2024-11-06T03:00:49Z- handleDVL() - DVL msg epochMillisec: 1730862049.46 2024-11-06T03:00:49Z- handleDvl() - RDI_Pathfinder msg: 1730862049.46 sec; seqNo:525; #beams:4 2024-11-06T03:00:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.79 , 38.50 , 38.50 , 41.49 2024-11-06T03:00:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , -0.01 2024-11-06T03:00:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862049.48 epoch sec; seqNo:1708; 4061669.96 north; 594341.70 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:00:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862048.65 2024-11-06T03:00:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:49Z- waiting for 1730862052.65, time is 1730862049.48 2024-11-06T03:00:49Z- AHRS_M2 msg: 1730862049.84 epoch sec; seqNo:1601 2024-11-06T03:00:49Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.40 psi 2024-11-06T03:00:49Z- handleDepth()- Depth_Keller msg: 1730862049.85 epoch sec; seqNo:1709; depth 50.12 2024-11-06T03:00:49Z- handleDVL() - DVL msg epochMillisec: 1730862049.85 2024-11-06T03:00:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:00:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:00:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:00:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:00:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:00:49Z- handleDvl() - RDI_Pathfinder msg: 1730862049.85 sec; seqNo:526; #beams:0 2024-11-06T03:00:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:00:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , -0.01 2024-11-06T03:00:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862049.86 epoch sec; seqNo:1709; 4061669.96 north; 594341.70 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:00:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862048.65 2024-11-06T03:00:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:49Z- waiting for 1730862052.65, time is 1730862049.86 2024-11-06T03:00:50Z- AHRS_M2 msg: 1730862050.25 epoch sec; seqNo:1602 2024-11-06T03:00:50Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.40 psi 2024-11-06T03:00:50Z- handleDepth()- Depth_Keller msg: 1730862050.26 epoch sec; seqNo:1710; depth 50.13 2024-11-06T03:00:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862050.27 epoch sec; seqNo:1710; 4061669.96 north; 594341.70 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:00:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862048.65 2024-11-06T03:00:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:50Z- waiting for 1730862052.65, time is 1730862050.27 2024-11-06T03:00:50Z- AHRS_M2 msg: 1730862050.65 epoch sec; seqNo:1603 2024-11-06T03:00:50Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.40 psi 2024-11-06T03:00:50Z- handleDepth()- Depth_Keller msg: 1730862050.66 epoch sec; seqNo:1711; depth 50.13 2024-11-06T03:00:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862050.67 epoch sec; seqNo:1711; 4061669.98 north; 594342.92 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:00:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862048.65 2024-11-06T03:00:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:50Z- waiting for 1730862052.65, time is 1730862050.67 2024-11-06T03:00:51Z- AHRS_M2 msg: 1730862051.05 epoch sec; seqNo:1604 2024-11-06T03:00:51Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.40 psi 2024-11-06T03:00:51Z- handleDepth()- Depth_Keller msg: 1730862051.05 epoch sec; seqNo:1712; depth 50.14 2024-11-06T03:00:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862051.06 epoch sec; seqNo:1712; 4061669.98 north; 594342.92 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:00:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862048.65 2024-11-06T03:00:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:51Z- waiting for 1730862052.65, time is 1730862051.06 2024-11-06T03:00:51Z- AHRS_M2 msg: 1730862051.45 epoch sec; seqNo:1605 2024-11-06T03:00:51Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.40 psi 2024-11-06T03:00:51Z- handleDepth()- Depth_Keller msg: 1730862051.46 epoch sec; seqNo:1713; depth 50.15 2024-11-06T03:00:51Z- handleDVL() - DVL msg epochMillisec: 1730862051.48 2024-11-06T03:00:51Z- handleDvl() - RDI_Pathfinder msg: 1730862051.48 sec; seqNo:527; #beams:4 2024-11-06T03:00:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.49 , 38.31 , 38.68 , 41.67 2024-11-06T03:00:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.02 2024-11-06T03:00:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862051.51 epoch sec; seqNo:1713; 4061670.36 north; 594342.92 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:00:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862048.65 2024-11-06T03:00:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:51Z- waiting for 1730862052.65, time is 1730862051.51 2024-11-06T03:00:51Z- AHRS_M2 msg: 1730862051.86 epoch sec; seqNo:1606 2024-11-06T03:00:51Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.40 psi 2024-11-06T03:00:51Z- handleDepth()- Depth_Keller msg: 1730862051.86 epoch sec; seqNo:1714; depth 50.16 2024-11-06T03:00:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862051.88 epoch sec; seqNo:1714; 4061670.36 north; 594342.92 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:00:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862048.65 2024-11-06T03:00:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:52Z- waiting for 1730862052.65, time is 1730862051.88 2024-11-06T03:00:52Z- AHRS_M2 msg: 1730862052.26 epoch sec; seqNo:1607 2024-11-06T03:00:52Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.41 psi 2024-11-06T03:00:52Z- handleDepth()- Depth_Keller msg: 1730862052.27 epoch sec; seqNo:1715; depth 50.19 2024-11-06T03:00:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862052.28 epoch sec; seqNo:1715; 4061670.37 north; 594344.14 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:00:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862048.65 2024-11-06T03:00:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:52Z- waiting for 1730862052.65, time is 1730862052.28 2024-11-06T03:00:52Z- AHRS_M2 msg: 1730862052.67 epoch sec; seqNo:1608 2024-11-06T03:00:52Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.41 psi 2024-11-06T03:00:52Z- handleDepth()- Depth_Keller msg: 1730862052.68 epoch sec; seqNo:1716; depth 50.19 2024-11-06T03:00:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862052.70 epoch sec; seqNo:1716; 4061670.37 north; 594344.14 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:00:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862048.65 2024-11-06T03:00:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:52Z- waiting for 1730862052.65, time is 1730862052.70 2024-11-06T03:00:52Z- LcmTrn::updateTrn() >>>> pose time 1730862052.70, meas time 1730862051.48 <<<< 2024-11-06T03:00:52Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862052.70 2024-11-06T03:00:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862051.48 sec, ping # 0; waiting for INS data more recent than 1730862048.65... 2024-11-06T03:00:52Z- TerrainNav::motionUpdate() - currEstimate = (4061670, 594344.14, 50.19) 2024-11-06T03:00:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:00:52Z- TerrainNav:: SubcloudNIS is 0.55, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:00:52Z- TNavPF::Projecting Measurements 2024-11-06T03:00:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:00:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:00:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:00:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:00:52Z- TNavFilter::4 good beams. 2024-11-06T03:00:52Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:00:52Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:00:52Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:00:52Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:00:52Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:00:52Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:00:52Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:00:52Z- TerrainMapOctree: (vehN, vehE ) = (4061670.24, 594343.04). 2024-11-06T03:00:52Z- TerrainMapOctree: Min Distance = 122.54. 2024-11-06T03:00:52Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:00:52Z- Forcing Subcloud Comparison 2024-11-06T03:00:52Z- Weighting particles with subcloud comparison 2024-11-06T03:00:52Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:52Z- meanExpectedMeasDiff: 2.200383 delta_rms_squared: 1.586863 alpha: 0.987553 2024-11-06T03:00:52Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:52Z- meanExpectedMeasDiff: 0.122816 delta_rms_squared: 1.480640 alpha: 0.986672 2024-11-06T03:00:52Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:52Z- meanExpectedMeasDiff: 4.818697 delta_rms_squared: 1.362149 alpha: 0.985530 2024-11-06T03:00:52Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:00:52Z- meanExpectedMeasDiff: -2.678991 delta_rms_squared: 1.488489 alpha: 0.986742 2024-11-06T03:00:52Z- Subcloud NIS: 0.590234 2024-11-06T03:00:52Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:00:52Z- TNavPF:: sumWeights = 0.015295 2024-11-06T03:00:52Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:00:52Z- TNavPF::First Term of Map Covariance Matrix is 0.04 2024-11-06T03:00:52Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:00:52Z- TerrainNav::Resampling particle filter... 2024-11-06T03:00:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862051.48 sec. 2024-11-06T03:00:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:00:52Z- variances([0], [2], and psi_berg) = 0.658566 0.237644 0.000000 2024-11-06T03:00:52Z- publishEstimatesDecoder() - at 1730862052.70 sec 2024-11-06T03:00:52Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:00:52Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061702.77 594328.10 48.23 2024-11-06T03:00:52Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061702.16 594327.99 49.07 lat/lon: 36.70 -121.94 2024-11-06T03:00:52Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.66 0.24 0.69 0.00 2024-11-06T03:00:53Z- AHRS_M2 msg: 1730862053.07 epoch sec; seqNo:1609 2024-11-06T03:00:53Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.41 psi 2024-11-06T03:00:53Z- handleDepth()- Depth_Keller msg: 1730862053.08 epoch sec; seqNo:1717; depth 50.20 2024-11-06T03:00:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862053.09 epoch sec; seqNo:1717; 4061670.37 north; 594344.14 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:00:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862052.70 2024-11-06T03:00:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:53Z- waiting for 1730862056.70, time is 1730862053.09 2024-11-06T03:00:53Z- handleDepth()- Depth_Keller msg: 1730862053.51 epoch sec; seqNo:1718; depth 50.20 2024-11-06T03:00:53Z- handleDVL() - DVL msg epochMillisec: 1730862053.54 2024-11-06T03:00:53Z- handleDvl() - RDI_Pathfinder msg: 1730862053.54 sec; seqNo:528; #beams:4 2024-11-06T03:00:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.05 , 37.94 , 37.94 , 41.68 2024-11-06T03:00:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , -0.02 2024-11-06T03:00:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862053.55 epoch sec; seqNo:1718; 4061670.38 north; 594345.36 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:00:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862052.70 2024-11-06T03:00:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:53Z- waiting for 1730862056.70, time is 1730862053.55 2024-11-06T03:00:53Z- AHRS_M2 msg: 1730862053.88 epoch sec; seqNo:1610 2024-11-06T03:00:53Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.41 psi 2024-11-06T03:00:53Z- handleDepth()- Depth_Keller msg: 1730862053.88 epoch sec; seqNo:1719; depth 50.22 2024-11-06T03:00:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862053.91 epoch sec; seqNo:1719; 4061670.76 north; 594345.35 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:00:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862052.70 2024-11-06T03:00:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:54Z- waiting for 1730862056.70, time is 1730862053.91 2024-11-06T03:00:54Z- AHRS_M2 msg: 1730862054.28 epoch sec; seqNo:1611 2024-11-06T03:00:54Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.41 psi 2024-11-06T03:00:54Z- handleDepth()- Depth_Keller msg: 1730862054.29 epoch sec; seqNo:1720; depth 50.22 2024-11-06T03:00:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862054.31 epoch sec; seqNo:1720; 4061670.76 north; 594345.35 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:00:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862052.70 2024-11-06T03:00:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:54Z- waiting for 1730862056.70, time is 1730862054.31 2024-11-06T03:00:54Z- AHRS_M2 msg: 1730862054.68 epoch sec; seqNo:1612 2024-11-06T03:00:54Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.41 psi 2024-11-06T03:00:54Z- handleDepth()- Depth_Keller msg: 1730862054.69 epoch sec; seqNo:1721; depth 50.22 2024-11-06T03:00:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862054.70 epoch sec; seqNo:1721; 4061670.76 north; 594345.35 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:00:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862052.70 2024-11-06T03:00:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:54Z- waiting for 1730862056.70, time is 1730862054.70 2024-11-06T03:00:55Z- AHRS_M2 msg: 1730862055.10 epoch sec; seqNo:1613 2024-11-06T03:00:55Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.41 psi 2024-11-06T03:00:55Z- handleDepth()- Depth_Keller msg: 1730862055.13 epoch sec; seqNo:1722; depth 50.24 2024-11-06T03:00:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862055.14 epoch sec; seqNo:1722; 4061670.78 north; 594346.57 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:00:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862052.70 2024-11-06T03:00:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:55Z- waiting for 1730862056.70, time is 1730862055.14 2024-11-06T03:00:55Z- handleDepth()- Depth_Keller msg: 1730862055.52 epoch sec; seqNo:1723; depth 50.25 2024-11-06T03:00:55Z- handleDVL() - DVL msg epochMillisec: 1730862055.54 2024-11-06T03:00:55Z- handleDvl() - RDI_Pathfinder msg: 1730862055.54 sec; seqNo:529; #beams:4 2024-11-06T03:00:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.86 , 37.94 , 37.75 , 41.11 2024-11-06T03:00:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , -0.04 2024-11-06T03:00:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862055.58 epoch sec; seqNo:1723; 4061670.78 north; 594346.57 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:00:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862052.70 2024-11-06T03:00:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:55Z- waiting for 1730862056.70, time is 1730862055.58 2024-11-06T03:00:55Z- AHRS_M2 msg: 1730862055.89 epoch sec; seqNo:1614 2024-11-06T03:00:55Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.42 psi 2024-11-06T03:00:55Z- handleDepth()- Depth_Keller msg: 1730862055.90 epoch sec; seqNo:1724; depth 50.25 2024-11-06T03:00:55Z- handleDVL() - DVL msg epochMillisec: 1730862055.90 2024-11-06T03:00:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:00:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:00:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:00:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:00:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:00:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:00:55Z- handleDvl() - RDI_Pathfinder msg: 1730862055.90 sec; seqNo:530; #beams:0 2024-11-06T03:00:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:00:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , -0.04 2024-11-06T03:00:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862055.92 epoch sec; seqNo:1724; 4061670.78 north; 594346.57 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:00:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862052.70 2024-11-06T03:00:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:56Z- waiting for 1730862056.70, time is 1730862055.92 2024-11-06T03:00:56Z- AHRS_M2 msg: 1730862056.30 epoch sec; seqNo:1615 2024-11-06T03:00:56Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.42 psi 2024-11-06T03:00:56Z- handleDepth()- Depth_Keller msg: 1730862056.31 epoch sec; seqNo:1725; depth 50.26 2024-11-06T03:00:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862056.32 epoch sec; seqNo:1725; 4061670.79 north; 594347.79 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:00:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862052.70 2024-11-06T03:00:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:56Z- waiting for 1730862056.70, time is 1730862056.32 2024-11-06T03:00:56Z- AHRS_M2 msg: 1730862056.70 epoch sec; seqNo:1616 2024-11-06T03:00:56Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.42 psi 2024-11-06T03:00:56Z- handleDepth()- Depth_Keller msg: 1730862056.71 epoch sec; seqNo:1726; depth 50.24 2024-11-06T03:00:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862056.72 epoch sec; seqNo:1726; 4061670.79 north; 594347.79 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:00:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862052.70 2024-11-06T03:00:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:56Z- waiting for 1730862056.70, time is 1730862056.72 2024-11-06T03:00:56Z- LcmTrn::updateTrn() >>>> pose time 1730862056.72, meas time 1730862055.90 <<<< 2024-11-06T03:00:56Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862056.72 2024-11-06T03:00:56Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:00:56Z- TerrainNav::motionUpdate() - currEstimate = (4061671, 594347.79, 50.24) 2024-11-06T03:00:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:00:56Z- TerrainNav:: SubcloudNIS is 0.59, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:00:56Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:00:56Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:00:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:00:56Z- variances([0], [2], and psi_berg) = 0.661557 0.240144 0.000000 2024-11-06T03:00:56Z- publishEstimatesDecoder() - at 1730862056.72 sec 2024-11-06T03:00:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:00:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061703.21 594331.74 47.77 2024-11-06T03:00:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061702.58 594331.65 49.12 lat/lon: 36.70 -121.94 2024-11-06T03:00:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.66 0.24 0.93 0.00 2024-11-06T03:00:57Z- AHRS_M2 msg: 1730862057.11 epoch sec; seqNo:1617 2024-11-06T03:00:57Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.42 psi 2024-11-06T03:00:57Z- handleDepth()- Depth_Keller msg: 1730862057.12 epoch sec; seqNo:1727; depth 50.25 2024-11-06T03:00:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862057.14 epoch sec; seqNo:1727; 4061671.17 north; 594347.79 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:00:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862056.72 2024-11-06T03:00:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:57Z- waiting for 1730862060.72, time is 1730862057.14 2024-11-06T03:00:57Z- AHRS_M2 msg: 1730862057.51 epoch sec; seqNo:1618 2024-11-06T03:00:57Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.42 psi 2024-11-06T03:00:57Z- handleDepth()- Depth_Keller msg: 1730862057.52 epoch sec; seqNo:1728; depth 50.27 2024-11-06T03:00:57Z- handleDVL() - DVL msg epochMillisec: 1730862057.55 2024-11-06T03:00:57Z- handleDvl() - RDI_Pathfinder msg: 1730862057.55 sec; seqNo:531; #beams:4 2024-11-06T03:00:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.23 , 37.75 , 37.56 , 40.93 2024-11-06T03:00:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.02 , -0.05 2024-11-06T03:00:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862057.57 epoch sec; seqNo:1728; 4061671.17 north; 594347.79 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:00:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862056.72 2024-11-06T03:00:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:57Z- waiting for 1730862060.72, time is 1730862057.57 2024-11-06T03:00:57Z- AHRS_M2 msg: 1730862057.92 epoch sec; seqNo:1619 2024-11-06T03:00:57Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.42 psi 2024-11-06T03:00:57Z- handleDepth()- Depth_Keller msg: 1730862057.92 epoch sec; seqNo:1729; depth 50.27 2024-11-06T03:00:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862057.94 epoch sec; seqNo:1729; 4061671.18 north; 594349.01 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:00:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862056.72 2024-11-06T03:00:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:58Z- waiting for 1730862060.72, time is 1730862057.94 2024-11-06T03:00:58Z- AHRS_M2 msg: 1730862058.32 epoch sec; seqNo:1620 2024-11-06T03:00:58Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.42 psi 2024-11-06T03:00:58Z- handleDepth()- Depth_Keller msg: 1730862058.34 epoch sec; seqNo:1730; depth 50.28 2024-11-06T03:00:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862058.35 epoch sec; seqNo:1730; 4061671.18 north; 594349.01 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:00:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862056.72 2024-11-06T03:00:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:58Z- waiting for 1730862060.72, time is 1730862058.35 2024-11-06T03:00:58Z- AHRS_M2 msg: 1730862058.72 epoch sec; seqNo:1621 2024-11-06T03:00:58Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.41 psi 2024-11-06T03:00:58Z- handleDepth()- Depth_Keller msg: 1730862058.73 epoch sec; seqNo:1731; depth 50.27 2024-11-06T03:00:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862058.76 epoch sec; seqNo:1731; 4061671.18 north; 594349.01 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:00:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862056.72 2024-11-06T03:00:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:58Z- waiting for 1730862060.72, time is 1730862058.76 2024-11-06T03:00:59Z- AHRS_M2 msg: 1730862059.13 epoch sec; seqNo:1622 2024-11-06T03:00:59Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:00:59Z- handleDepth()- Depth_Keller msg: 1730862059.13 epoch sec; seqNo:1732; depth 50.28 2024-11-06T03:00:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862059.14 epoch sec; seqNo:1732; 4061671.18 north; 594349.01 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:00:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:00:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862056.72 2024-11-06T03:00:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:59Z- waiting for 1730862060.72, time is 1730862059.14 2024-11-06T03:00:59Z- AHRS_M2 msg: 1730862059.53 epoch sec; seqNo:1623 2024-11-06T03:00:59Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:00:59Z- handleDepth()- Depth_Keller msg: 1730862059.54 epoch sec; seqNo:1733; depth 50.29 2024-11-06T03:00:59Z- handleDVL() - DVL msg epochMillisec: 1730862059.56 2024-11-06T03:00:59Z- handleDvl() - RDI_Pathfinder msg: 1730862059.56 sec; seqNo:532; #beams:4 2024-11-06T03:00:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.24 , 37.38 , 37.01 , 40.37 2024-11-06T03:00:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.00 , -0.06 2024-11-06T03:00:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862059.60 epoch sec; seqNo:1733; 4061671.20 north; 594350.23 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:00:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:00:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862056.72 2024-11-06T03:00:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:00:59Z- waiting for 1730862060.72, time is 1730862059.60 2024-11-06T03:00:59Z- AHRS_M2 msg: 1730862059.94 epoch sec; seqNo:1624 2024-11-06T03:00:59Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:00:59Z- handleDepth()- Depth_Keller msg: 1730862059.95 epoch sec; seqNo:1734; depth 50.30 2024-11-06T03:00:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862059.96 epoch sec; seqNo:1734; 4061671.20 north; 594350.23 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:01:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862056.72 2024-11-06T03:01:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:00Z- waiting for 1730862060.72, time is 1730862059.96 2024-11-06T03:01:00Z- AHRS_M2 msg: 1730862060.34 epoch sec; seqNo:1625 2024-11-06T03:01:00Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:01:00Z- handleDepth()- Depth_Keller msg: 1730862060.35 epoch sec; seqNo:1735; depth 50.30 2024-11-06T03:01:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862060.38 epoch sec; seqNo:1735; 4061671.20 north; 594350.23 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:01:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862056.72 2024-11-06T03:01:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:00Z- waiting for 1730862060.72, time is 1730862060.38 2024-11-06T03:01:00Z- AHRS_M2 msg: 1730862060.74 epoch sec; seqNo:1626 2024-11-06T03:01:00Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:01:00Z- handleDepth()- Depth_Keller msg: 1730862060.75 epoch sec; seqNo:1736; depth 50.31 2024-11-06T03:01:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862060.76 epoch sec; seqNo:1736; 4061671.59 north; 594351.45 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:01:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862056.72 2024-11-06T03:01:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:00Z- waiting for 1730862060.72, time is 1730862060.76 2024-11-06T03:01:00Z- LcmTrn::updateTrn() >>>> pose time 1730862060.76, meas time 1730862059.56 <<<< 2024-11-06T03:01:00Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862060.76 2024-11-06T03:01:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862059.56 sec, ping # 0; waiting for INS data more recent than 1730862056.72... 2024-11-06T03:01:00Z- TerrainNav::motionUpdate() - currEstimate = (4061672, 594351.45, 50.31) 2024-11-06T03:01:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:01:00Z- TerrainNav:: SubcloudNIS is 0.59, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:01:00Z- TNavPF::Projecting Measurements 2024-11-06T03:01:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:01:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:01:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:01:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:01:00Z- TNavFilter::4 good beams. 2024-11-06T03:01:00Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:01:00Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:01:00Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:01:00Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:01:00Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:01:00Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:01:00Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:01:00Z- TerrainMapOctree: (vehN, vehE ) = (4061671.35, 594350.37). 2024-11-06T03:01:00Z- TerrainMapOctree: Min Distance = 127.51. 2024-11-06T03:01:00Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:01:01Z- Forcing Subcloud Comparison 2024-11-06T03:01:01Z- Weighting particles with subcloud comparison 2024-11-06T03:01:01Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:01Z- meanExpectedMeasDiff: 4.013399 delta_rms_squared: 3.644563 alpha: 0.994542 2024-11-06T03:01:01Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:01Z- meanExpectedMeasDiff: 0.912267 delta_rms_squared: 1.846965 alpha: 0.989287 2024-11-06T03:01:01Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:01Z- meanExpectedMeasDiff: 0.402947 delta_rms_squared: 4.786848 alpha: 0.995839 2024-11-06T03:01:01Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:01Z- meanExpectedMeasDiff: -1.846787 delta_rms_squared: 2.034528 alpha: 0.990265 2024-11-06T03:01:01Z- Subcloud NIS: 0.619931 2024-11-06T03:01:01Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:01:01Z- TNavPF:: sumWeights = 0.070618 2024-11-06T03:01:01Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:01:01Z- TNavPF::First Term of Map Covariance Matrix is 0.56 2024-11-06T03:01:01Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T03:01:01Z- TerrainNav::Resampling particle filter... 2024-11-06T03:01:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862059.56 sec. 2024-11-06T03:01:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:01:01Z- variances([0], [2], and psi_berg) = 0.735742 0.149784 0.000000 2024-11-06T03:01:01Z- publishEstimatesDecoder() - at 1730862060.76 sec 2024-11-06T03:01:01Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:01:01Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061703.90 594335.38 48.27 2024-11-06T03:01:01Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061703.48 594335.07 48.85 lat/lon: 36.70 -121.94 2024-11-06T03:01:01Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.74 0.15 0.63 0.00 2024-11-06T03:01:01Z- AHRS_M2 msg: 1730862061.15 epoch sec; seqNo:1627 2024-11-06T03:01:01Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:01:01Z- handleDepth()- Depth_Keller msg: 1730862061.16 epoch sec; seqNo:1737; depth 50.32 2024-11-06T03:01:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862061.17 epoch sec; seqNo:1737; 4061671.59 north; 594351.45 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:01:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862060.76 2024-11-06T03:01:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:01Z- waiting for 1730862064.76, time is 1730862061.17 2024-11-06T03:01:01Z- AHRS_M2 msg: 1730862061.55 epoch sec; seqNo:1628 2024-11-06T03:01:01Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:01:01Z- handleDepth()- Depth_Keller msg: 1730862061.56 epoch sec; seqNo:1738; depth 50.33 2024-11-06T03:01:01Z- handleDVL() - DVL msg epochMillisec: 1730862061.58 2024-11-06T03:01:01Z- handleDvl() - RDI_Pathfinder msg: 1730862061.58 sec; seqNo:533; #beams:4 2024-11-06T03:01:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.86 , 37.19 , 36.81 , 40.74 2024-11-06T03:01:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.04 , -0.05 2024-11-06T03:01:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862061.62 epoch sec; seqNo:1738; 4061671.59 north; 594351.45 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:01:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862060.76 2024-11-06T03:01:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:01Z- waiting for 1730862064.76, time is 1730862061.62 2024-11-06T03:01:01Z- AHRS_M2 msg: 1730862061.96 epoch sec; seqNo:1629 2024-11-06T03:01:01Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:01:01Z- handleDepth()- Depth_Keller msg: 1730862061.96 epoch sec; seqNo:1739; depth 50.33 2024-11-06T03:01:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862061.97 epoch sec; seqNo:1739; 4061671.59 north; 594351.45 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:01:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862060.76 2024-11-06T03:01:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:02Z- waiting for 1730862064.76, time is 1730862061.97 2024-11-06T03:01:02Z- AHRS_M2 msg: 1730862062.36 epoch sec; seqNo:1630 2024-11-06T03:01:02Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:01:02Z- handleDepth()- Depth_Keller msg: 1730862062.39 epoch sec; seqNo:1740; depth 50.35 2024-11-06T03:01:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862062.41 epoch sec; seqNo:1740; 4061671.60 north; 594352.67 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:01:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862060.76 2024-11-06T03:01:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:02Z- waiting for 1730862064.76, time is 1730862062.41 2024-11-06T03:01:02Z- AHRS_M2 msg: 1730862062.76 epoch sec; seqNo:1631 2024-11-06T03:01:02Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:01:02Z- handleDepth()- Depth_Keller msg: 1730862062.77 epoch sec; seqNo:1741; depth 50.36 2024-11-06T03:01:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862062.79 epoch sec; seqNo:1741; 4061671.60 north; 594352.67 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:01:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862060.76 2024-11-06T03:01:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:02Z- waiting for 1730862064.76, time is 1730862062.79 2024-11-06T03:01:03Z- AHRS_M2 msg: 1730862063.17 epoch sec; seqNo:1632 2024-11-06T03:01:03Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:01:03Z- handleDepth()- Depth_Keller msg: 1730862063.18 epoch sec; seqNo:1742; depth 50.36 2024-11-06T03:01:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862063.19 epoch sec; seqNo:1742; 4061671.98 north; 594352.66 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:01:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862060.76 2024-11-06T03:01:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:03Z- waiting for 1730862064.76, time is 1730862063.19 2024-11-06T03:01:03Z- AHRS_M2 msg: 1730862063.57 epoch sec; seqNo:1633 2024-11-06T03:01:03Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:01:03Z- handleDepth()- Depth_Keller msg: 1730862063.59 epoch sec; seqNo:1743; depth 50.38 2024-11-06T03:01:03Z- handleDVL() - DVL msg epochMillisec: 1730862063.61 2024-11-06T03:01:03Z- handleDvl() - RDI_Pathfinder msg: 1730862063.61 sec; seqNo:534; #beams:4 2024-11-06T03:01:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.67 , 37.38 , 36.44 , 40.18 2024-11-06T03:01:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , -0.02 2024-11-06T03:01:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862063.63 epoch sec; seqNo:1743; 4061671.98 north; 594352.66 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:01:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862060.76 2024-11-06T03:01:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:03Z- waiting for 1730862064.76, time is 1730862063.63 2024-11-06T03:01:03Z- AHRS_M2 msg: 1730862063.97 epoch sec; seqNo:1634 2024-11-06T03:01:03Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:01:03Z- handleDepth()- Depth_Keller msg: 1730862063.98 epoch sec; seqNo:1744; depth 50.39 2024-11-06T03:01:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862064.00 epoch sec; seqNo:1744; 4061671.99 north; 594353.88 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:01:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862060.76 2024-11-06T03:01:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:04Z- waiting for 1730862064.76, time is 1730862064.00 2024-11-06T03:01:04Z- AHRS_M2 msg: 1730862064.39 epoch sec; seqNo:1635 2024-11-06T03:01:04Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:01:04Z- handleDepth()- Depth_Keller msg: 1730862064.40 epoch sec; seqNo:1745; depth 50.40 2024-11-06T03:01:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862064.41 epoch sec; seqNo:1745; 4061671.99 north; 594353.88 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:01:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862060.76 2024-11-06T03:01:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:04Z- waiting for 1730862064.76, time is 1730862064.41 2024-11-06T03:01:04Z- AHRS_M2 msg: 1730862064.78 epoch sec; seqNo:1636 2024-11-06T03:01:04Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:01:04Z- handleDepth()- Depth_Keller msg: 1730862064.79 epoch sec; seqNo:1746; depth 50.40 2024-11-06T03:01:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862064.80 epoch sec; seqNo:1746; 4061671.99 north; 594353.88 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:01:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862060.76 2024-11-06T03:01:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:04Z- waiting for 1730862064.76, time is 1730862064.80 2024-11-06T03:01:04Z- LcmTrn::updateTrn() >>>> pose time 1730862064.80, meas time 1730862063.61 <<<< 2024-11-06T03:01:04Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862064.80 2024-11-06T03:01:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862063.61 sec, ping # 0; waiting for INS data more recent than 1730862060.76... 2024-11-06T03:01:04Z- TerrainNav::motionUpdate() - currEstimate = (4061672, 594353.88, 50.40) 2024-11-06T03:01:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:01:04Z- TerrainNav:: SubcloudNIS is 0.62, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:01:04Z- TNavPF::Projecting Measurements 2024-11-06T03:01:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:01:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:01:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:01:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:01:04Z- TNavFilter::4 good beams. 2024-11-06T03:01:04Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:01:04Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:01:04Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:01:04Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:01:04Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:01:04Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:01:04Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:01:04Z- TerrainMapOctree: (vehN, vehE ) = (4061671.87, 594353.17). 2024-11-06T03:01:04Z- TerrainMapOctree: Min Distance = 129.41. 2024-11-06T03:01:04Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:01:05Z- Forcing Subcloud Comparison 2024-11-06T03:01:05Z- Weighting particles with subcloud comparison 2024-11-06T03:01:05Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:05Z- meanExpectedMeasDiff: 5.263894 delta_rms_squared: 1.399810 alpha: 0.985914 2024-11-06T03:01:05Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:05Z- meanExpectedMeasDiff: 2.055249 delta_rms_squared: 1.100684 alpha: 0.982154 2024-11-06T03:01:05Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:05Z- meanExpectedMeasDiff: -3.880761 delta_rms_squared: 2.074196 alpha: 0.990450 2024-11-06T03:01:05Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:05Z- meanExpectedMeasDiff: -1.719769 delta_rms_squared: 1.529059 alpha: 0.987089 2024-11-06T03:01:05Z- Subcloud NIS: 0.641358 2024-11-06T03:01:05Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:01:05Z- TNavPF:: sumWeights = 0.001916 2024-11-06T03:01:05Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:01:05Z- TNavPF::First Term of Map Covariance Matrix is 0.05 2024-11-06T03:01:05Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:01:05Z- TerrainNav::Resampling particle filter... 2024-11-06T03:01:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862063.61 sec. 2024-11-06T03:01:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:01:05Z- variances([0], [2], and psi_berg) = 0.793336 0.130654 0.000000 2024-11-06T03:01:05Z- publishEstimatesDecoder() - at 1730862064.80 sec 2024-11-06T03:01:05Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:01:05Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061704.29 594337.78 49.10 2024-11-06T03:01:05Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061703.70 594337.49 48.85 lat/lon: 36.70 -121.94 2024-11-06T03:01:05Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.79 0.13 0.66 0.00 2024-11-06T03:01:05Z- AHRS_M2 msg: 1730862065.19 epoch sec; seqNo:1637 2024-11-06T03:01:05Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.40 psi 2024-11-06T03:01:05Z- handleDepth()- Depth_Keller msg: 1730862065.21 epoch sec; seqNo:1747; depth 50.43 2024-11-06T03:01:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862065.26 epoch sec; seqNo:1747; 4061671.99 north; 594353.88 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:01:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862064.80 2024-11-06T03:01:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:05Z- waiting for 1730862068.80, time is 1730862065.26 2024-11-06T03:01:05Z- AHRS_M2 msg: 1730862065.60 epoch sec; seqNo:1638 2024-11-06T03:01:05Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.40 psi 2024-11-06T03:01:05Z- handleDepth()- Depth_Keller msg: 1730862065.60 epoch sec; seqNo:1748; depth 50.43 2024-11-06T03:01:05Z- handleDVL() - DVL msg epochMillisec: 1730862065.62 2024-11-06T03:01:05Z- handleDvl() - RDI_Pathfinder msg: 1730862065.62 sec; seqNo:535; #beams:4 2024-11-06T03:01:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.74 , 37.00 , 36.07 , 40.37 2024-11-06T03:01:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.00 2024-11-06T03:01:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862065.68 epoch sec; seqNo:1748; 4061672.39 north; 594355.10 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:01:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862064.80 2024-11-06T03:01:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:05Z- waiting for 1730862068.80, time is 1730862065.68 2024-11-06T03:01:06Z- AHRS_M2 msg: 1730862066.00 epoch sec; seqNo:1639 2024-11-06T03:01:06Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.40 psi 2024-11-06T03:01:06Z- handleDepth()- Depth_Keller msg: 1730862066.01 epoch sec; seqNo:1749; depth 50.46 2024-11-06T03:01:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862066.02 epoch sec; seqNo:1749; 4061672.39 north; 594355.10 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:01:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862064.80 2024-11-06T03:01:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:06Z- waiting for 1730862068.80, time is 1730862066.02 2024-11-06T03:01:06Z- AHRS_M2 msg: 1730862066.40 epoch sec; seqNo:1640 2024-11-06T03:01:06Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.40 psi 2024-11-06T03:01:06Z- handleDepth()- Depth_Keller msg: 1730862066.40 epoch sec; seqNo:1750; depth 50.46 2024-11-06T03:01:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862066.42 epoch sec; seqNo:1750; 4061672.39 north; 594355.10 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:01:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862064.80 2024-11-06T03:01:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:06Z- waiting for 1730862068.80, time is 1730862066.42 2024-11-06T03:01:06Z- AHRS_M2 msg: 1730862066.80 epoch sec; seqNo:1641 2024-11-06T03:01:06Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.40 psi 2024-11-06T03:01:06Z- handleDepth()- Depth_Keller msg: 1730862066.81 epoch sec; seqNo:1751; depth 50.46 2024-11-06T03:01:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862066.83 epoch sec; seqNo:1751; 4061672.40 north; 594356.32 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:01:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862064.80 2024-11-06T03:01:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:06Z- waiting for 1730862068.80, time is 1730862066.83 2024-11-06T03:01:07Z- AHRS_M2 msg: 1730862067.21 epoch sec; seqNo:1642 2024-11-06T03:01:07Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:01:07Z- handleDepth()- Depth_Keller msg: 1730862067.22 epoch sec; seqNo:1752; depth 50.48 2024-11-06T03:01:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862067.28 epoch sec; seqNo:1752; 4061672.40 north; 594356.32 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:01:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862064.80 2024-11-06T03:01:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:07Z- waiting for 1730862068.80, time is 1730862067.28 2024-11-06T03:01:07Z- AHRS_M2 msg: 1730862067.61 epoch sec; seqNo:1643 2024-11-06T03:01:07Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:01:07Z- handleDepth()- Depth_Keller msg: 1730862067.62 epoch sec; seqNo:1753; depth 50.48 2024-11-06T03:01:07Z- handleDVL() - DVL msg epochMillisec: 1730862067.65 2024-11-06T03:01:07Z- handleDvl() - RDI_Pathfinder msg: 1730862067.65 sec; seqNo:536; #beams:4 2024-11-06T03:01:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.99 , 37.19 , 35.69 , 40.55 2024-11-06T03:01:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , 0.01 2024-11-06T03:01:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862067.69 epoch sec; seqNo:1753; 4061672.40 north; 594356.32 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:01:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862064.80 2024-11-06T03:01:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:07Z- waiting for 1730862068.80, time is 1730862067.69 2024-11-06T03:01:08Z- AHRS_M2 msg: 1730862068.01 epoch sec; seqNo:1644 2024-11-06T03:01:08Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.41 psi 2024-11-06T03:01:08Z- handleDepth()- Depth_Keller msg: 1730862068.02 epoch sec; seqNo:1754; depth 50.48 2024-11-06T03:01:08Z- handleDVL() - DVL msg epochMillisec: 1730862068.02 2024-11-06T03:01:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:01:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:01:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:01:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:01:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:01:08Z- handleDvl() - RDI_Pathfinder msg: 1730862068.02 sec; seqNo:537; #beams:0 2024-11-06T03:01:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:01:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , 0.01 2024-11-06T03:01:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862068.05 epoch sec; seqNo:1754; 4061672.78 north; 594356.31 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:01:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862064.80 2024-11-06T03:01:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:08Z- waiting for 1730862068.80, time is 1730862068.05 2024-11-06T03:01:08Z- AHRS_M2 msg: 1730862068.42 epoch sec; seqNo:1645 2024-11-06T03:01:08Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.41 psi 2024-11-06T03:01:08Z- handleDepth()- Depth_Keller msg: 1730862068.43 epoch sec; seqNo:1755; depth 50.49 2024-11-06T03:01:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862068.44 epoch sec; seqNo:1755; 4061672.79 north; 594357.53 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:01:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862064.80 2024-11-06T03:01:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:08Z- waiting for 1730862068.80, time is 1730862068.44 2024-11-06T03:01:08Z- AHRS_M2 msg: 1730862068.82 epoch sec; seqNo:1646 2024-11-06T03:01:08Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.41 psi 2024-11-06T03:01:08Z- handleDepth()- Depth_Keller msg: 1730862068.83 epoch sec; seqNo:1756; depth 50.52 2024-11-06T03:01:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862068.84 epoch sec; seqNo:1756; 4061672.79 north; 594357.53 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:01:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862064.80 2024-11-06T03:01:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:08Z- waiting for 1730862068.80, time is 1730862068.84 2024-11-06T03:01:08Z- LcmTrn::updateTrn() >>>> pose time 1730862068.84, meas time 1730862068.02 <<<< 2024-11-06T03:01:08Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862068.84 2024-11-06T03:01:08Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:01:08Z- TerrainNav::motionUpdate() - currEstimate = (4061673, 594357.53, 50.52) 2024-11-06T03:01:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:01:08Z- TerrainNav:: SubcloudNIS is 0.64, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:01:08Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:01:08Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:01:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:01:09Z- variances([0], [2], and psi_berg) = 0.795340 0.132395 0.000000 2024-11-06T03:01:09Z- publishEstimatesDecoder() - at 1730862068.84 sec 2024-11-06T03:01:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:01:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061705.12 594341.37 49.00 2024-11-06T03:01:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061704.50 594341.14 48.96 lat/lon: 36.70 -121.94 2024-11-06T03:01:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.80 0.13 0.90 0.00 2024-11-06T03:01:09Z- AHRS_M2 msg: 1730862069.26 epoch sec; seqNo:1647 2024-11-06T03:01:09Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.41 psi 2024-11-06T03:01:09Z- handleDepth()- Depth_Keller msg: 1730862069.28 epoch sec; seqNo:1757; depth 50.51 2024-11-06T03:01:09Z- handleDVL() - DVL msg epochMillisec: 1730862069.31 2024-11-06T03:01:09Z- handleDvl() - RDI_Pathfinder msg: 1730862069.31 sec; seqNo:538; #beams:4 2024-11-06T03:01:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.56 , 36.82 , 34.95 , 40.74 2024-11-06T03:01:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , -0.03 2024-11-06T03:01:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862069.32 epoch sec; seqNo:1757; 4061672.79 north; 594357.53 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:01:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862068.84 2024-11-06T03:01:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:09Z- waiting for 1730862072.84, time is 1730862069.32 2024-11-06T03:01:09Z- handleDepth()- Depth_Keller msg: 1730862069.66 epoch sec; seqNo:1758; depth 50.52 2024-11-06T03:01:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862069.71 epoch sec; seqNo:1758; 4061672.80 north; 594358.75 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:01:09Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:01:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862068.84 2024-11-06T03:01:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:09Z- waiting for 1730862072.84, time is 1730862069.71 2024-11-06T03:01:10Z- handleDepth()- Depth_Keller msg: 1730862070.06 epoch sec; seqNo:1759; depth 50.53 2024-11-06T03:01:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862070.07 epoch sec; seqNo:1759; 4061672.80 north; 594358.75 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:01:10Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:01:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862068.84 2024-11-06T03:01:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:10Z- waiting for 1730862072.84, time is 1730862070.07 2024-11-06T03:01:10Z- AHRS_M2 msg: 1730862070.44 epoch sec; seqNo:1648 2024-11-06T03:01:10Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.42 psi 2024-11-06T03:01:10Z- handleDepth()- Depth_Keller msg: 1730862070.46 epoch sec; seqNo:1760; depth 50.54 2024-11-06T03:01:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862070.47 epoch sec; seqNo:1760; 4061672.80 north; 594358.75 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:01:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862068.84 2024-11-06T03:01:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:10Z- waiting for 1730862072.84, time is 1730862070.47 2024-11-06T03:01:10Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:01:10Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:01:10Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:01:10Z- AHRS_M2 msg: 1730862070.84 epoch sec; seqNo:1649 2024-11-06T03:01:10Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.42 psi 2024-11-06T03:01:10Z- handleDepth()- Depth_Keller msg: 1730862070.86 epoch sec; seqNo:1761; depth 50.54 2024-11-06T03:01:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862070.87 epoch sec; seqNo:1761; 4061672.80 north; 594358.75 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:01:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862068.84 2024-11-06T03:01:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:11Z- waiting for 1730862072.84, time is 1730862070.87 2024-11-06T03:01:11Z- AHRS_M2 msg: 1730862071.29 epoch sec; seqNo:1650 2024-11-06T03:01:11Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.42 psi 2024-11-06T03:01:11Z- handleDepth()- Depth_Keller msg: 1730862071.29 epoch sec; seqNo:1762; depth 50.54 2024-11-06T03:01:11Z- handleDVL() - DVL msg epochMillisec: 1730862071.31 2024-11-06T03:01:11Z- handleDvl() - RDI_Pathfinder msg: 1730862071.31 sec; seqNo:539; #beams:4 2024-11-06T03:01:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.37 , 36.81 , 34.20 , 40.37 2024-11-06T03:01:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , -0.05 2024-11-06T03:01:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862071.33 epoch sec; seqNo:1762; 4061673.20 north; 594359.97 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:01:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862068.84 2024-11-06T03:01:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:11Z- waiting for 1730862072.84, time is 1730862071.33 2024-11-06T03:01:11Z- AHRS_M2 msg: 1730862071.67 epoch sec; seqNo:1651 2024-11-06T03:01:11Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.42 psi 2024-11-06T03:01:11Z- handleDepth()- Depth_Keller msg: 1730862071.67 epoch sec; seqNo:1763; depth 50.55 2024-11-06T03:01:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862071.68 epoch sec; seqNo:1763; 4061673.20 north; 594359.97 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:01:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862068.84 2024-11-06T03:01:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:11Z- waiting for 1730862072.84, time is 1730862071.68 2024-11-06T03:01:12Z- AHRS_M2 msg: 1730862072.06 epoch sec; seqNo:1652 2024-11-06T03:01:12Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.42 psi 2024-11-06T03:01:12Z- handleDepth()- Depth_Keller msg: 1730862072.07 epoch sec; seqNo:1764; depth 50.55 2024-11-06T03:01:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862072.08 epoch sec; seqNo:1764; 4061673.20 north; 594359.97 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:01:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862068.84 2024-11-06T03:01:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:12Z- waiting for 1730862072.84, time is 1730862072.08 2024-11-06T03:01:12Z- AHRS_M2 msg: 1730862072.46 epoch sec; seqNo:1653 2024-11-06T03:01:12Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.42 psi 2024-11-06T03:01:12Z- handleDepth()- Depth_Keller msg: 1730862072.47 epoch sec; seqNo:1765; depth 50.54 2024-11-06T03:01:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862072.48 epoch sec; seqNo:1765; 4061673.21 north; 594361.19 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:01:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862068.84 2024-11-06T03:01:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:12Z- waiting for 1730862072.84, time is 1730862072.48 2024-11-06T03:01:12Z- AHRS_M2 msg: 1730862072.86 epoch sec; seqNo:1654 2024-11-06T03:01:12Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.43 psi 2024-11-06T03:01:12Z- handleDepth()- Depth_Keller msg: 1730862072.87 epoch sec; seqNo:1766; depth 50.55 2024-11-06T03:01:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862072.88 epoch sec; seqNo:1766; 4061673.21 north; 594361.19 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:01:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862068.84 2024-11-06T03:01:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:13Z- waiting for 1730862072.84, time is 1730862072.88 2024-11-06T03:01:13Z- LcmTrn::updateTrn() >>>> pose time 1730862072.88, meas time 1730862071.31 <<<< 2024-11-06T03:01:13Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862072.88 2024-11-06T03:01:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862071.31 sec, ping # 0; waiting for INS data more recent than 1730862068.84... 2024-11-06T03:01:13Z- TerrainNav::motionUpdate() - currEstimate = (4061673, 594361.19, 50.55) 2024-11-06T03:01:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:01:13Z- TerrainNav:: SubcloudNIS is 0.64, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:01:13Z- TNavPF::Projecting Measurements 2024-11-06T03:01:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:01:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:01:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:01:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:01:13Z- TNavFilter::4 good beams. 2024-11-06T03:01:13Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:01:13Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:01:13Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:01:13Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:01:13Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:01:13Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:01:13Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:01:13Z- TerrainMapOctree: (vehN, vehE ) = (4061673.05, 594359.77). 2024-11-06T03:01:13Z- TerrainMapOctree: Min Distance = 134.06. 2024-11-06T03:01:13Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:01:13Z- Forcing Subcloud Comparison 2024-11-06T03:01:13Z- Weighting particles with subcloud comparison 2024-11-06T03:01:13Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:13Z- meanExpectedMeasDiff: -0.093018 delta_rms_squared: 1.897745 alpha: 0.989571 2024-11-06T03:01:13Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:13Z- meanExpectedMeasDiff: 3.836000 delta_rms_squared: 1.397467 alpha: 0.985890 2024-11-06T03:01:13Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:13Z- meanExpectedMeasDiff: -6.843560 delta_rms_squared: 1.601872 alpha: 0.987669 2024-11-06T03:01:13Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:13Z- meanExpectedMeasDiff: 0.514525 delta_rms_squared: 2.205445 alpha: 0.991013 2024-11-06T03:01:13Z- Subcloud NIS: 0.709016 2024-11-06T03:01:13Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:01:13Z- TNavPF:: sumWeights = 0.000191 2024-11-06T03:01:13Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:01:13Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:01:13Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:01:13Z- TerrainNav::Resampling particle filter... 2024-11-06T03:01:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862071.31 sec. 2024-11-06T03:01:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:01:13Z- variances([0], [2], and psi_berg) = 0.781939 0.131499 0.000000 2024-11-06T03:01:13Z- publishEstimatesDecoder() - at 1730862072.88 sec 2024-11-06T03:01:13Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:01:13Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061705.43 594345.05 48.76 2024-11-06T03:01:13Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061704.88 594344.77 49.41 lat/lon: 36.70 -121.94 2024-11-06T03:01:13Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.78 0.13 0.66 0.00 2024-11-06T03:01:13Z- AHRS_M2 msg: 1730862073.30 epoch sec; seqNo:1655 2024-11-06T03:01:13Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.43 psi 2024-11-06T03:01:13Z- handleDepth()- Depth_Keller msg: 1730862073.33 epoch sec; seqNo:1767; depth 50.55 2024-11-06T03:01:13Z- handleDVL() - DVL msg epochMillisec: 1730862073.36 2024-11-06T03:01:13Z- handleDvl() - RDI_Pathfinder msg: 1730862073.36 sec; seqNo:540; #beams:4 2024-11-06T03:01:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.10 , 36.53 , 33.81 , 39.59 2024-11-06T03:01:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , -0.06 2024-11-06T03:01:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862073.38 epoch sec; seqNo:1767; 4061673.21 north; 594361.19 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:01:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862072.88 2024-11-06T03:01:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:13Z- waiting for 1730862076.88, time is 1730862073.38 2024-11-06T03:01:13Z- AHRS_M2 msg: 1730862073.67 epoch sec; seqNo:1656 2024-11-06T03:01:13Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.43 psi 2024-11-06T03:01:13Z- handleDepth()- Depth_Keller msg: 1730862073.68 epoch sec; seqNo:1768; depth 50.54 2024-11-06T03:01:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862073.71 epoch sec; seqNo:1768; 4061673.21 north; 594361.19 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:01:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862072.88 2024-11-06T03:01:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:13Z- waiting for 1730862076.88, time is 1730862073.71 2024-11-06T03:01:14Z- AHRS_M2 msg: 1730862074.07 epoch sec; seqNo:1657 2024-11-06T03:01:14Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.43 psi 2024-11-06T03:01:14Z- handleDepth()- Depth_Keller msg: 1730862074.09 epoch sec; seqNo:1769; depth 50.55 2024-11-06T03:01:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862074.10 epoch sec; seqNo:1769; 4061673.22 north; 594362.41 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:01:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862072.88 2024-11-06T03:01:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:14Z- waiting for 1730862076.88, time is 1730862074.10 2024-11-06T03:01:14Z- AHRS_M2 msg: 1730862074.48 epoch sec; seqNo:1658 2024-11-06T03:01:14Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.43 psi 2024-11-06T03:01:14Z- handleDepth()- Depth_Keller msg: 1730862074.49 epoch sec; seqNo:1770; depth 50.55 2024-11-06T03:01:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862074.50 epoch sec; seqNo:1770; 4061673.22 north; 594362.41 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:01:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862072.88 2024-11-06T03:01:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:14Z- waiting for 1730862076.88, time is 1730862074.50 2024-11-06T03:01:14Z- AHRS_M2 msg: 1730862074.88 epoch sec; seqNo:1659 2024-11-06T03:01:14Z- AHRS_M2 msg: 0.15 phi; 0.05 theta; 1.43 psi 2024-11-06T03:01:14Z- handleDepth()- Depth_Keller msg: 1730862074.89 epoch sec; seqNo:1771; depth 50.54 2024-11-06T03:01:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862074.90 epoch sec; seqNo:1771; 4061673.60 north; 594362.41 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:01:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862072.88 2024-11-06T03:01:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:15Z- waiting for 1730862076.88, time is 1730862074.90 2024-11-06T03:01:15Z- AHRS_M2 msg: 1730862075.30 epoch sec; seqNo:1660 2024-11-06T03:01:15Z- AHRS_M2 msg: 0.15 phi; 0.05 theta; 1.43 psi 2024-11-06T03:01:15Z- handleDepth()- Depth_Keller msg: 1730862075.30 epoch sec; seqNo:1772; depth 50.53 2024-11-06T03:01:15Z- handleDVL() - DVL msg epochMillisec: 1730862075.33 2024-11-06T03:01:15Z- handleDvl() - RDI_Pathfinder msg: 1730862075.33 sec; seqNo:541; #beams:4 2024-11-06T03:01:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.58 , 36.53 , 33.13 , 39.41 2024-11-06T03:01:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.01 , -0.05 2024-11-06T03:01:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862075.34 epoch sec; seqNo:1772; 4061673.62 north; 594363.63 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:01:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862072.88 2024-11-06T03:01:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:15Z- waiting for 1730862076.88, time is 1730862075.34 2024-11-06T03:01:15Z- AHRS_M2 msg: 1730862075.69 epoch sec; seqNo:1661 2024-11-06T03:01:15Z- AHRS_M2 msg: 0.15 phi; 0.05 theta; 1.43 psi 2024-11-06T03:01:15Z- handleDepth()- Depth_Keller msg: 1730862075.70 epoch sec; seqNo:1773; depth 50.53 2024-11-06T03:01:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862075.72 epoch sec; seqNo:1773; 4061673.62 north; 594363.63 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:01:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862072.88 2024-11-06T03:01:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:15Z- waiting for 1730862076.88, time is 1730862075.72 2024-11-06T03:01:16Z- AHRS_M2 msg: 1730862076.10 epoch sec; seqNo:1662 2024-11-06T03:01:16Z- AHRS_M2 msg: 0.15 phi; 0.05 theta; 1.43 psi 2024-11-06T03:01:16Z- handleDepth()- Depth_Keller msg: 1730862076.10 epoch sec; seqNo:1774; depth 50.53 2024-11-06T03:01:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862076.11 epoch sec; seqNo:1774; 4061673.62 north; 594363.63 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:01:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862072.88 2024-11-06T03:01:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:16Z- waiting for 1730862076.88, time is 1730862076.11 2024-11-06T03:01:16Z- AHRS_M2 msg: 1730862076.50 epoch sec; seqNo:1663 2024-11-06T03:01:16Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.43 psi 2024-11-06T03:01:16Z- handleDepth()- Depth_Keller msg: 1730862076.51 epoch sec; seqNo:1775; depth 50.52 2024-11-06T03:01:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862076.52 epoch sec; seqNo:1775; 4061673.62 north; 594363.63 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:01:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862072.88 2024-11-06T03:01:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:16Z- waiting for 1730862076.88, time is 1730862076.52 2024-11-06T03:01:16Z- AHRS_M2 msg: 1730862076.90 epoch sec; seqNo:1664 2024-11-06T03:01:16Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.43 psi 2024-11-06T03:01:16Z- handleDepth()- Depth_Keller msg: 1730862076.91 epoch sec; seqNo:1776; depth 50.51 2024-11-06T03:01:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862076.93 epoch sec; seqNo:1776; 4061673.63 north; 594364.85 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:01:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862072.88 2024-11-06T03:01:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:17Z- waiting for 1730862076.88, time is 1730862076.93 2024-11-06T03:01:17Z- LcmTrn::updateTrn() >>>> pose time 1730862076.93, meas time 1730862075.33 <<<< 2024-11-06T03:01:17Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862076.93 2024-11-06T03:01:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862075.33 sec, ping # 0; waiting for INS data more recent than 1730862072.88... 2024-11-06T03:01:17Z- TerrainNav::motionUpdate() - currEstimate = (4061674, 594364.85, 50.51) 2024-11-06T03:01:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:01:17Z- TerrainNav:: SubcloudNIS is 0.71, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:01:17Z- TNavPF::Projecting Measurements 2024-11-06T03:01:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:01:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:01:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:01:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:01:17Z- TNavFilter::4 good beams. 2024-11-06T03:01:17Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:01:17Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:01:17Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:01:17Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:01:17Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:01:17Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:01:17Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:01:17Z- TerrainMapOctree: (vehN, vehE ) = (4061673.46, 594363.40). 2024-11-06T03:01:17Z- TerrainMapOctree: Min Distance = 136.81. 2024-11-06T03:01:17Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:01:17Z- Forcing Subcloud Comparison 2024-11-06T03:01:17Z- Weighting particles with subcloud comparison 2024-11-06T03:01:17Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:17Z- meanExpectedMeasDiff: -0.440538 delta_rms_squared: 1.320018 alpha: 0.985075 2024-11-06T03:01:17Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:17Z- meanExpectedMeasDiff: 5.851351 delta_rms_squared: 1.917555 alpha: 0.989678 2024-11-06T03:01:17Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:17Z- meanExpectedMeasDiff: -8.198145 delta_rms_squared: 1.108582 alpha: 0.982279 2024-11-06T03:01:17Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:17Z- meanExpectedMeasDiff: 2.248634 delta_rms_squared: 1.964781 alpha: 0.989923 2024-11-06T03:01:17Z- Subcloud NIS: 0.773101 2024-11-06T03:01:17Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:01:17Z- TNavPF:: sumWeights = 0.000000 2024-11-06T03:01:17Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:01:17Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:01:17Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:01:17Z- TerrainNav::Resampling particle filter... 2024-11-06T03:01:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862075.33 sec. 2024-11-06T03:01:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:01:17Z- variances([0], [2], and psi_berg) = 0.809051 0.130180 0.000000 2024-11-06T03:01:17Z- publishEstimatesDecoder() - at 1730862076.93 sec 2024-11-06T03:01:17Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:01:17Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061705.93 594348.64 49.12 2024-11-06T03:01:17Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061704.75 594348.53 49.32 lat/lon: 36.70 -121.94 2024-11-06T03:01:17Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.81 0.13 0.61 0.00 2024-11-06T03:01:17Z- AHRS_M2 msg: 1730862077.31 epoch sec; seqNo:1665 2024-11-06T03:01:17Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.44 psi 2024-11-06T03:01:17Z- handleDepth()- Depth_Keller msg: 1730862077.31 epoch sec; seqNo:1777; depth 50.51 2024-11-06T03:01:17Z- handleDVL() - DVL msg epochMillisec: 1730862077.37 2024-11-06T03:01:17Z- handleDvl() - RDI_Pathfinder msg: 1730862077.37 sec; seqNo:542; #beams:4 2024-11-06T03:01:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.41 , 36.02 , 32.79 , 39.07 2024-11-06T03:01:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.01 , -0.04 2024-11-06T03:01:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862077.41 epoch sec; seqNo:1777; 4061673.63 north; 594364.85 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:01:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862076.93 2024-11-06T03:01:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:17Z- waiting for 1730862080.93, time is 1730862077.41 2024-11-06T03:01:17Z- AHRS_M2 msg: 1730862077.71 epoch sec; seqNo:1666 2024-11-06T03:01:17Z- AHRS_M2 msg: 0.15 phi; 0.07 theta; 1.44 psi 2024-11-06T03:01:17Z- handleDepth()- Depth_Keller msg: 1730862077.72 epoch sec; seqNo:1778; depth 50.50 2024-11-06T03:01:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862077.73 epoch sec; seqNo:1778; 4061673.63 north; 594364.85 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:01:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862076.93 2024-11-06T03:01:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:17Z- waiting for 1730862080.93, time is 1730862077.73 2024-11-06T03:01:18Z- AHRS_M2 msg: 1730862078.12 epoch sec; seqNo:1667 2024-11-06T03:01:18Z- AHRS_M2 msg: 0.15 phi; 0.07 theta; 1.44 psi 2024-11-06T03:01:18Z- handleDepth()- Depth_Keller msg: 1730862078.12 epoch sec; seqNo:1779; depth 50.48 2024-11-06T03:01:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862078.14 epoch sec; seqNo:1779; 4061673.63 north; 594364.85 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:01:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862076.93 2024-11-06T03:01:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:18Z- waiting for 1730862080.93, time is 1730862078.14 2024-11-06T03:01:18Z- AHRS_M2 msg: 1730862078.52 epoch sec; seqNo:1668 2024-11-06T03:01:18Z- AHRS_M2 msg: 0.15 phi; 0.07 theta; 1.44 psi 2024-11-06T03:01:18Z- handleDepth()- Depth_Keller msg: 1730862078.53 epoch sec; seqNo:1780; depth 50.49 2024-11-06T03:01:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862078.55 epoch sec; seqNo:1780; 4061674.02 north; 594366.06 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:01:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862076.93 2024-11-06T03:01:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:18Z- waiting for 1730862080.93, time is 1730862078.55 2024-11-06T03:01:18Z- AHRS_M2 msg: 1730862078.92 epoch sec; seqNo:1669 2024-11-06T03:01:18Z- AHRS_M2 msg: 0.15 phi; 0.07 theta; 1.44 psi 2024-11-06T03:01:18Z- handleDepth()- Depth_Keller msg: 1730862078.93 epoch sec; seqNo:1781; depth 50.48 2024-11-06T03:01:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862078.94 epoch sec; seqNo:1781; 4061674.02 north; 594366.06 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:01:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862076.93 2024-11-06T03:01:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:19Z- waiting for 1730862080.93, time is 1730862078.94 2024-11-06T03:01:19Z- AHRS_M2 msg: 1730862079.33 epoch sec; seqNo:1670 2024-11-06T03:01:19Z- AHRS_M2 msg: 0.15 phi; 0.08 theta; 1.44 psi 2024-11-06T03:01:19Z- handleDepth()- Depth_Keller msg: 1730862079.33 epoch sec; seqNo:1782; depth 50.47 2024-11-06T03:01:19Z- handleDVL() - DVL msg epochMillisec: 1730862079.35 2024-11-06T03:01:19Z- handleDvl() - RDI_Pathfinder msg: 1730862079.35 sec; seqNo:543; #beams:4 2024-11-06T03:01:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.74 , 35.68 , 31.94 , 39.08 2024-11-06T03:01:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.00 , -0.03 2024-11-06T03:01:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862079.38 epoch sec; seqNo:1782; 4061674.02 north; 594366.06 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:01:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862076.93 2024-11-06T03:01:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:19Z- waiting for 1730862080.93, time is 1730862079.38 2024-11-06T03:01:19Z- AHRS_M2 msg: 1730862079.73 epoch sec; seqNo:1671 2024-11-06T03:01:19Z- AHRS_M2 msg: 0.15 phi; 0.08 theta; 1.44 psi 2024-11-06T03:01:19Z- handleDepth()- Depth_Keller msg: 1730862079.75 epoch sec; seqNo:1783; depth 50.47 2024-11-06T03:01:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862079.76 epoch sec; seqNo:1783; 4061674.02 north; 594366.06 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:01:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862076.93 2024-11-06T03:01:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:19Z- waiting for 1730862080.93, time is 1730862079.76 2024-11-06T03:01:20Z- AHRS_M2 msg: 1730862080.14 epoch sec; seqNo:1672 2024-11-06T03:01:20Z- AHRS_M2 msg: 0.15 phi; 0.08 theta; 1.44 psi 2024-11-06T03:01:20Z- handleDepth()- Depth_Keller msg: 1730862080.15 epoch sec; seqNo:1784; depth 50.45 2024-11-06T03:01:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862080.17 epoch sec; seqNo:1784; 4061674.04 north; 594367.28 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:01:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862076.93 2024-11-06T03:01:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:20Z- waiting for 1730862080.93, time is 1730862080.17 2024-11-06T03:01:20Z- AHRS_M2 msg: 1730862080.54 epoch sec; seqNo:1673 2024-11-06T03:01:20Z- AHRS_M2 msg: 0.15 phi; 0.08 theta; 1.44 psi 2024-11-06T03:01:20Z- handleDepth()- Depth_Keller msg: 1730862080.55 epoch sec; seqNo:1785; depth 50.44 2024-11-06T03:01:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862080.56 epoch sec; seqNo:1785; 4061674.04 north; 594367.28 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:01:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862076.93 2024-11-06T03:01:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:20Z- waiting for 1730862080.93, time is 1730862080.56 2024-11-06T03:01:20Z- AHRS_M2 msg: 1730862080.94 epoch sec; seqNo:1674 2024-11-06T03:01:20Z- AHRS_M2 msg: 0.15 phi; 0.08 theta; 1.44 psi 2024-11-06T03:01:20Z- handleDepth()- Depth_Keller msg: 1730862080.95 epoch sec; seqNo:1786; depth 50.44 2024-11-06T03:01:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862080.96 epoch sec; seqNo:1786; 4061674.04 north; 594367.28 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:01:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862076.93 2024-11-06T03:01:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:21Z- waiting for 1730862080.93, time is 1730862080.96 2024-11-06T03:01:21Z- LcmTrn::updateTrn() >>>> pose time 1730862080.96, meas time 1730862079.35 <<<< 2024-11-06T03:01:21Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862080.96 2024-11-06T03:01:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862079.35 sec, ping # 0; waiting for INS data more recent than 1730862076.93... 2024-11-06T03:01:21Z- TerrainNav::motionUpdate() - currEstimate = (4061674, 594367.28, 50.44) 2024-11-06T03:01:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:01:21Z- TerrainNav:: SubcloudNIS is 0.77, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:01:21Z- TNavPF::Projecting Measurements 2024-11-06T03:01:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:01:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:01:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:01:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:01:21Z- TNavFilter::4 good beams. 2024-11-06T03:01:21Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:01:21Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:01:21Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:01:21Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:01:21Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:01:21Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:01:21Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:01:21Z- TerrainMapOctree: (vehN, vehE ) = (4061673.87, 594366.31). 2024-11-06T03:01:21Z- TerrainMapOctree: Min Distance = 139.01. 2024-11-06T03:01:21Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:01:21Z- Forcing Subcloud Comparison 2024-11-06T03:01:21Z- Weighting particles with subcloud comparison 2024-11-06T03:01:21Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:21Z- meanExpectedMeasDiff: -2.617056 delta_rms_squared: 1.724696 alpha: 0.988537 2024-11-06T03:01:21Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:21Z- meanExpectedMeasDiff: 2.981616 delta_rms_squared: 1.035520 alpha: 0.981052 2024-11-06T03:01:21Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:21Z- meanExpectedMeasDiff: -11.178451 delta_rms_squared: 1.998000 alpha: 0.990089 2024-11-06T03:01:21Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:21Z- meanExpectedMeasDiff: 4.257122 delta_rms_squared: 2.298544 alpha: 0.991374 2024-11-06T03:01:21Z- Subcloud NIS: 0.947577 2024-11-06T03:01:21Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:01:21Z- TNavPF:: sumWeights = 0.000000 2024-11-06T03:01:21Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:01:21Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:01:21Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:01:21Z- TerrainNav::Resampling particle filter... 2024-11-06T03:01:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862079.35 sec. 2024-11-06T03:01:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:01:21Z- variances([0], [2], and psi_berg) = 0.379649 0.102240 0.000000 2024-11-06T03:01:21Z- publishEstimatesDecoder() - at 1730862080.96 sec 2024-11-06T03:01:21Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:01:21Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061706.13 594351.09 51.83 2024-11-06T03:01:21Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061704.86 594350.80 50.59 lat/lon: 36.70 -121.94 2024-11-06T03:01:21Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.38 0.10 0.47 0.00 2024-11-06T03:01:21Z- AHRS_M2 msg: 1730862081.35 epoch sec; seqNo:1675 2024-11-06T03:01:21Z- AHRS_M2 msg: 0.15 phi; 0.08 theta; 1.44 psi 2024-11-06T03:01:21Z- handleDepth()- Depth_Keller msg: 1730862081.35 epoch sec; seqNo:1787; depth 50.42 2024-11-06T03:01:21Z- handleDVL() - DVL msg epochMillisec: 1730862081.39 2024-11-06T03:01:21Z- handleDvl() - RDI_Pathfinder msg: 1730862081.39 sec; seqNo:544; #beams:4 2024-11-06T03:01:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.89 , 35.85 , 31.77 , 37.89 2024-11-06T03:01:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.00 , -0.01 2024-11-06T03:01:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862081.40 epoch sec; seqNo:1787; 4061674.05 north; 594368.50 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:01:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862080.96 2024-11-06T03:01:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:21Z- waiting for 1730862084.96, time is 1730862081.40 2024-11-06T03:01:21Z- AHRS_M2 msg: 1730862081.76 epoch sec; seqNo:1676 2024-11-06T03:01:21Z- AHRS_M2 msg: 0.15 phi; 0.08 theta; 1.44 psi 2024-11-06T03:01:21Z- handleDepth()- Depth_Keller msg: 1730862081.76 epoch sec; seqNo:1788; depth 50.42 2024-11-06T03:01:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862081.78 epoch sec; seqNo:1788; 4061674.05 north; 594368.50 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:01:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862080.96 2024-11-06T03:01:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:21Z- waiting for 1730862084.96, time is 1730862081.78 2024-11-06T03:01:22Z- AHRS_M2 msg: 1730862082.15 epoch sec; seqNo:1677 2024-11-06T03:01:22Z- AHRS_M2 msg: 0.14 phi; 0.09 theta; 1.44 psi 2024-11-06T03:01:22Z- handleDepth()- Depth_Keller msg: 1730862082.16 epoch sec; seqNo:1789; depth 50.40 2024-11-06T03:01:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862082.17 epoch sec; seqNo:1789; 4061674.05 north; 594368.50 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:01:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862080.96 2024-11-06T03:01:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:22Z- waiting for 1730862084.96, time is 1730862082.17 2024-11-06T03:01:22Z- AHRS_M2 msg: 1730862082.56 epoch sec; seqNo:1678 2024-11-06T03:01:22Z- AHRS_M2 msg: 0.14 phi; 0.09 theta; 1.44 psi 2024-11-06T03:01:22Z- handleDepth()- Depth_Keller msg: 1730862082.57 epoch sec; seqNo:1790; depth 50.39 2024-11-06T03:01:22Z- handleDVL() - DVL msg epochMillisec: 1730862082.57 2024-11-06T03:01:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:01:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:01:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:01:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:01:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:01:22Z- handleDvl() - RDI_Pathfinder msg: 1730862082.57 sec; seqNo:545; #beams:0 2024-11-06T03:01:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:01:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.00 , -0.01 2024-11-06T03:01:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862082.58 epoch sec; seqNo:1790; 4061674.43 north; 594368.50 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:01:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862080.96 2024-11-06T03:01:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:22Z- waiting for 1730862084.96, time is 1730862082.58 2024-11-06T03:01:22Z- AHRS_M2 msg: 1730862082.97 epoch sec; seqNo:1679 2024-11-06T03:01:22Z- AHRS_M2 msg: 0.14 phi; 0.09 theta; 1.44 psi 2024-11-06T03:01:22Z- handleDepth()- Depth_Keller msg: 1730862082.97 epoch sec; seqNo:1791; depth 50.38 2024-11-06T03:01:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862082.99 epoch sec; seqNo:1791; 4061674.44 north; 594369.72 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:01:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862080.96 2024-11-06T03:01:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:23Z- waiting for 1730862084.96, time is 1730862082.99 2024-11-06T03:01:23Z- AHRS_M2 msg: 1730862083.37 epoch sec; seqNo:1680 2024-11-06T03:01:23Z- AHRS_M2 msg: 0.14 phi; 0.09 theta; 1.43 psi 2024-11-06T03:01:23Z- handleDepth()- Depth_Keller msg: 1730862083.38 epoch sec; seqNo:1792; depth 50.36 2024-11-06T03:01:23Z- handleDVL() - DVL msg epochMillisec: 1730862083.40 2024-11-06T03:01:23Z- handleDvl() - RDI_Pathfinder msg: 1730862083.40 sec; seqNo:546; #beams:4 2024-11-06T03:01:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.04 , 36.02 , 31.77 , 38.06 2024-11-06T03:01:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.03 2024-11-06T03:01:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862083.42 epoch sec; seqNo:1792; 4061674.44 north; 594369.72 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:01:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862080.96 2024-11-06T03:01:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:23Z- waiting for 1730862084.96, time is 1730862083.42 2024-11-06T03:01:23Z- handleDepth()- Depth_Keller msg: 1730862083.80 epoch sec; seqNo:1793; depth 50.34 2024-11-06T03:01:23Z- handleDVL() - DVL msg epochMillisec: 1730862083.80 2024-11-06T03:01:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:01:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:01:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:01:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:01:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:01:23Z- handleDvl() - RDI_Pathfinder msg: 1730862083.80 sec; seqNo:547; #beams:0 2024-11-06T03:01:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:01:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.03 2024-11-06T03:01:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862083.81 epoch sec; seqNo:1793; 4061674.44 north; 594369.72 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:01:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862080.96 2024-11-06T03:01:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:23Z- waiting for 1730862084.96, time is 1730862083.81 2024-11-06T03:01:24Z- AHRS_M2 msg: 1730862084.17 epoch sec; seqNo:1681 2024-11-06T03:01:24Z- AHRS_M2 msg: 0.14 phi; 0.09 theta; 1.43 psi 2024-11-06T03:01:24Z- handleDepth()- Depth_Keller msg: 1730862084.18 epoch sec; seqNo:1794; depth 50.32 2024-11-06T03:01:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862084.20 epoch sec; seqNo:1794; 4061674.44 north; 594369.72 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:01:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862080.96 2024-11-06T03:01:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:24Z- waiting for 1730862084.96, time is 1730862084.20 2024-11-06T03:01:24Z- AHRS_M2 msg: 1730862084.58 epoch sec; seqNo:1682 2024-11-06T03:01:24Z- AHRS_M2 msg: 0.14 phi; 0.09 theta; 1.43 psi 2024-11-06T03:01:24Z- handleDepth()- Depth_Keller msg: 1730862084.59 epoch sec; seqNo:1795; depth 50.30 2024-11-06T03:01:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862084.60 epoch sec; seqNo:1795; 4061674.45 north; 594370.94 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:01:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862080.96 2024-11-06T03:01:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:24Z- waiting for 1730862084.96, time is 1730862084.60 2024-11-06T03:01:24Z- AHRS_M2 msg: 1730862084.99 epoch sec; seqNo:1683 2024-11-06T03:01:25Z- AHRS_M2 msg: 0.14 phi; 0.09 theta; 1.44 psi 2024-11-06T03:01:25Z- handleDepth()- Depth_Keller msg: 1730862084.99 epoch sec; seqNo:1796; depth 50.28 2024-11-06T03:01:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862085.00 epoch sec; seqNo:1796; 4061674.45 north; 594370.94 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:01:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862080.96 2024-11-06T03:01:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:25Z- waiting for 1730862084.96, time is 1730862085.00 2024-11-06T03:01:25Z- LcmTrn::updateTrn() >>>> pose time 1730862085.00, meas time 1730862083.80 <<<< 2024-11-06T03:01:25Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862085.00 2024-11-06T03:01:25Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:01:25Z- TerrainNav::motionUpdate() - currEstimate = (4061674, 594370.94, 50.28) 2024-11-06T03:01:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:01:25Z- TerrainNav:: SubcloudNIS is 0.95, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:01:25Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:01:25Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:01:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:01:25Z- variances([0], [2], and psi_berg) = 0.381652 0.104146 0.000000 2024-11-06T03:01:25Z- publishEstimatesDecoder() - at 1730862085.00 sec 2024-11-06T03:01:25Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:01:25Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061706.53 594354.69 51.75 2024-11-06T03:01:25Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061705.28 594354.45 50.43 lat/lon: 36.70 -121.94 2024-11-06T03:01:25Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.38 0.10 0.72 0.00 2024-11-06T03:01:25Z- handleDepth()- Depth_Keller msg: 1730862085.41 epoch sec; seqNo:1797; depth 50.25 2024-11-06T03:01:25Z- handleDVL() - DVL msg epochMillisec: 1730862085.43 2024-11-06T03:01:25Z- handleDvl() - RDI_Pathfinder msg: 1730862085.43 sec; seqNo:548; #beams:4 2024-11-06T03:01:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.70 , 36.02 , 32.62 , 38.06 2024-11-06T03:01:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , 0.02 2024-11-06T03:01:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862085.44 epoch sec; seqNo:1797; 4061674.45 north; 594370.94 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:01:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862085.00 2024-11-06T03:01:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:25Z- waiting for 1730862089.00, time is 1730862085.44 2024-11-06T03:01:25Z- AHRS_M2 msg: 1730862085.79 epoch sec; seqNo:1684 2024-11-06T03:01:25Z- AHRS_M2 msg: 0.14 phi; 0.09 theta; 1.44 psi 2024-11-06T03:01:25Z- handleDepth()- Depth_Keller msg: 1730862085.80 epoch sec; seqNo:1798; depth 50.24 2024-11-06T03:01:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862085.84 epoch sec; seqNo:1798; 4061674.85 north; 594372.16 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:01:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862085.00 2024-11-06T03:01:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:25Z- waiting for 1730862089.00, time is 1730862085.84 2024-11-06T03:01:26Z- AHRS_M2 msg: 1730862086.20 epoch sec; seqNo:1685 2024-11-06T03:01:26Z- AHRS_M2 msg: 0.14 phi; 0.09 theta; 1.44 psi 2024-11-06T03:01:26Z- handleDepth()- Depth_Keller msg: 1730862086.21 epoch sec; seqNo:1799; depth 50.22 2024-11-06T03:01:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862086.21 epoch sec; seqNo:1799; 4061674.85 north; 594372.16 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:01:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862085.00 2024-11-06T03:01:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:26Z- waiting for 1730862089.00, time is 1730862086.21 2024-11-06T03:01:26Z- AHRS_M2 msg: 1730862086.70 epoch sec; seqNo:1686 2024-11-06T03:01:26Z- AHRS_M2 msg: 0.14 phi; 0.09 theta; 1.44 psi 2024-11-06T03:01:26Z- handleDepth()- Depth_Keller msg: 1730862086.73 epoch sec; seqNo:1800; depth 50.17 2024-11-06T03:01:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862086.83 epoch sec; seqNo:1800; 4061674.85 north; 594372.16 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:01:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862085.00 2024-11-06T03:01:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:26Z- waiting for 1730862089.00, time is 1730862086.83 2024-11-06T03:01:27Z- handleDepth()- Depth_Keller msg: 1730862087.03 epoch sec; seqNo:1801; depth 50.14 2024-11-06T03:01:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862087.04 epoch sec; seqNo:1801; 4061674.85 north; 594372.16 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:01:27Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:01:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862085.00 2024-11-06T03:01:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:27Z- waiting for 1730862089.00, time is 1730862087.04 2024-11-06T03:01:27Z- AHRS_M2 msg: 1730862087.41 epoch sec; seqNo:1687 2024-11-06T03:01:27Z- AHRS_M2 msg: 0.13 phi; 0.09 theta; 1.44 psi 2024-11-06T03:01:27Z- handleDepth()- Depth_Keller msg: 1730862087.42 epoch sec; seqNo:1802; depth 50.10 2024-11-06T03:01:27Z- handleDVL() - DVL msg epochMillisec: 1730862087.44 2024-11-06T03:01:27Z- handleDvl() - RDI_Pathfinder msg: 1730862087.44 sec; seqNo:549; #beams:4 2024-11-06T03:01:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.85 , 36.02 , 33.64 , 38.40 2024-11-06T03:01:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.05 , -0.05 2024-11-06T03:01:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862087.46 epoch sec; seqNo:1802; 4061674.86 north; 594373.38 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:01:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862085.00 2024-11-06T03:01:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:27Z- waiting for 1730862089.00, time is 1730862087.46 2024-11-06T03:01:27Z- AHRS_M2 msg: 1730862087.81 epoch sec; seqNo:1688 2024-11-06T03:01:27Z- AHRS_M2 msg: 0.13 phi; 0.08 theta; 1.44 psi 2024-11-06T03:01:27Z- handleDepth()- Depth_Keller msg: 1730862087.82 epoch sec; seqNo:1803; depth 50.09 2024-11-06T03:01:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862087.84 epoch sec; seqNo:1803; 4061674.86 north; 594373.38 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:01:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862085.00 2024-11-06T03:01:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:27Z- waiting for 1730862089.00, time is 1730862087.84 2024-11-06T03:01:28Z- AHRS_M2 msg: 1730862088.22 epoch sec; seqNo:1689 2024-11-06T03:01:28Z- AHRS_M2 msg: 0.13 phi; 0.08 theta; 1.45 psi 2024-11-06T03:01:28Z- handleDepth()- Depth_Keller msg: 1730862088.22 epoch sec; seqNo:1804; depth 50.05 2024-11-06T03:01:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862088.23 epoch sec; seqNo:1804; 4061674.86 north; 594373.38 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:01:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862085.00 2024-11-06T03:01:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:28Z- waiting for 1730862089.00, time is 1730862088.23 2024-11-06T03:01:28Z- AHRS_M2 msg: 1730862088.63 epoch sec; seqNo:1690 2024-11-06T03:01:28Z- AHRS_M2 msg: 0.13 phi; 0.08 theta; 1.45 psi 2024-11-06T03:01:28Z- handleDepth()- Depth_Keller msg: 1730862088.63 epoch sec; seqNo:1805; depth 50.02 2024-11-06T03:01:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862088.65 epoch sec; seqNo:1805; 4061674.87 north; 594374.60 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:01:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862085.00 2024-11-06T03:01:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:28Z- waiting for 1730862089.00, time is 1730862088.65 2024-11-06T03:01:29Z- AHRS_M2 msg: 1730862089.02 epoch sec; seqNo:1691 2024-11-06T03:01:29Z- AHRS_M2 msg: 0.13 phi; 0.08 theta; 1.45 psi 2024-11-06T03:01:29Z- handleDepth()- Depth_Keller msg: 1730862089.04 epoch sec; seqNo:1806; depth 49.99 2024-11-06T03:01:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862089.05 epoch sec; seqNo:1806; 4061674.87 north; 594374.60 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:01:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862085.00 2024-11-06T03:01:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:29Z- waiting for 1730862089.00, time is 1730862089.05 2024-11-06T03:01:29Z- LcmTrn::updateTrn() >>>> pose time 1730862089.05, meas time 1730862087.44 <<<< 2024-11-06T03:01:29Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862089.05 2024-11-06T03:01:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862087.44 sec, ping # 0; waiting for INS data more recent than 1730862085.00... 2024-11-06T03:01:29Z- TerrainNav::motionUpdate() - currEstimate = (4061675, 594374.60, 49.99) 2024-11-06T03:01:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:01:29Z- TerrainNav:: SubcloudNIS is 0.95, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:01:29Z- TNavPF::Projecting Measurements 2024-11-06T03:01:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:01:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:01:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:01:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:01:29Z- TNavFilter::4 good beams. 2024-11-06T03:01:29Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:01:29Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:01:29Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:01:29Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:01:29Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:01:29Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:01:29Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:01:29Z- TerrainMapOctree: (vehN, vehE ) = (4061674.71, 594373.15). 2024-11-06T03:01:29Z- TerrainMapOctree: Min Distance = 144.34. 2024-11-06T03:01:29Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:01:29Z- Forcing Subcloud Comparison 2024-11-06T03:01:29Z- Weighting particles with subcloud comparison 2024-11-06T03:01:29Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:29Z- meanExpectedMeasDiff: -4.704491 delta_rms_squared: 0.840419 alpha: 0.976756 2024-11-06T03:01:29Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:29Z- meanExpectedMeasDiff: 3.023302 delta_rms_squared: 1.117572 alpha: 0.982419 2024-11-06T03:01:29Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:29Z- meanExpectedMeasDiff: -8.627332 delta_rms_squared: 1.435327 alpha: 0.986257 2024-11-06T03:01:29Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:29Z- meanExpectedMeasDiff: 6.693041 delta_rms_squared: 1.182767 alpha: 0.983372 2024-11-06T03:01:29Z- Subcloud NIS: 1.058834 2024-11-06T03:01:29Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:01:29Z- TNavPF:: sumWeights = 0.000000 2024-11-06T03:01:29Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:01:29Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:01:29Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:01:29Z- TerrainNav::Resampling particle filter... 2024-11-06T03:01:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862087.44 sec. 2024-11-06T03:01:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:01:29Z- variances([0], [2], and psi_berg) = 0.495830 0.110798 0.000000 2024-11-06T03:01:29Z- publishEstimatesDecoder() - at 1730862089.05 sec 2024-11-06T03:01:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:01:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061707.01 594358.29 50.88 2024-11-06T03:01:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061705.82 594358.14 50.72 lat/lon: 36.70 -121.94 2024-11-06T03:01:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.50 0.11 0.77 0.00 2024-11-06T03:01:29Z- AHRS_M2 msg: 1730862089.43 epoch sec; seqNo:1692 2024-11-06T03:01:29Z- AHRS_M2 msg: 0.13 phi; 0.08 theta; 1.45 psi 2024-11-06T03:01:29Z- handleDepth()- Depth_Keller msg: 1730862089.43 epoch sec; seqNo:1807; depth 49.95 2024-11-06T03:01:29Z- handleDVL() - DVL msg epochMillisec: 1730862089.47 2024-11-06T03:01:29Z- handleDvl() - RDI_Pathfinder msg: 1730862089.47 sec; seqNo:550; #beams:4 2024-11-06T03:01:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.68 , 35.51 , 32.62 , 38.06 2024-11-06T03:01:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.03 , -0.10 2024-11-06T03:01:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862089.49 epoch sec; seqNo:1807; 4061674.87 north; 594374.60 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:01:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862089.05 2024-11-06T03:01:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:29Z- waiting for 1730862093.05, time is 1730862089.49 2024-11-06T03:01:29Z- AHRS_M2 msg: 1730862089.83 epoch sec; seqNo:1693 2024-11-06T03:01:29Z- AHRS_M2 msg: 0.13 phi; 0.07 theta; 1.45 psi 2024-11-06T03:01:29Z- handleDepth()- Depth_Keller msg: 1730862089.84 epoch sec; seqNo:1808; depth 49.92 2024-11-06T03:01:29Z- handleDVL() - DVL msg epochMillisec: 1730862089.84 2024-11-06T03:01:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:01:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:01:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:01:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:01:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:01:29Z- handleDvl() - RDI_Pathfinder msg: 1730862089.84 sec; seqNo:551; #beams:0 2024-11-06T03:01:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:01:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.03 , -0.10 2024-11-06T03:01:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862089.85 epoch sec; seqNo:1808; 4061674.87 north; 594374.60 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:01:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862089.05 2024-11-06T03:01:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:29Z- waiting for 1730862093.05, time is 1730862089.85 2024-11-06T03:01:30Z- AHRS_M2 msg: 1730862090.23 epoch sec; seqNo:1694 2024-11-06T03:01:30Z- AHRS_M2 msg: 0.13 phi; 0.07 theta; 1.45 psi 2024-11-06T03:01:30Z- handleDepth()- Depth_Keller msg: 1730862090.24 epoch sec; seqNo:1809; depth 49.89 2024-11-06T03:01:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862090.26 epoch sec; seqNo:1809; 4061674.89 north; 594375.82 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:01:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862089.05 2024-11-06T03:01:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:30Z- waiting for 1730862093.05, time is 1730862090.26 2024-11-06T03:01:30Z- AHRS_M2 msg: 1730862090.64 epoch sec; seqNo:1695 2024-11-06T03:01:30Z- AHRS_M2 msg: 0.13 phi; 0.07 theta; 1.45 psi 2024-11-06T03:01:30Z- handleDepth()- Depth_Keller msg: 1730862090.65 epoch sec; seqNo:1810; depth 49.86 2024-11-06T03:01:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862090.67 epoch sec; seqNo:1810; 4061674.89 north; 594375.82 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:01:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862089.05 2024-11-06T03:01:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:30Z- waiting for 1730862093.05, time is 1730862090.67 2024-11-06T03:01:31Z- AHRS_M2 msg: 1730862091.05 epoch sec; seqNo:1696 2024-11-06T03:01:31Z- AHRS_M2 msg: 0.13 phi; 0.07 theta; 1.45 psi 2024-11-06T03:01:31Z- handleDepth()- Depth_Keller msg: 1730862091.05 epoch sec; seqNo:1811; depth 49.83 2024-11-06T03:01:31Z- handleDVL() - DVL msg epochMillisec: 1730862091.06 2024-11-06T03:01:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:01:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:01:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:01:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:01:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:01:31Z- handleDvl() - RDI_Pathfinder msg: 1730862091.06 sec; seqNo:552; #beams:0 2024-11-06T03:01:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:01:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.03 , -0.10 2024-11-06T03:01:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862091.07 epoch sec; seqNo:1811; 4061674.89 north; 594375.82 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:01:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862089.05 2024-11-06T03:01:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:31Z- waiting for 1730862093.05, time is 1730862091.07 2024-11-06T03:01:31Z- AHRS_M2 msg: 1730862091.46 epoch sec; seqNo:1697 2024-11-06T03:01:31Z- AHRS_M2 msg: 0.14 phi; 0.07 theta; 1.45 psi 2024-11-06T03:01:31Z- handleDepth()- Depth_Keller msg: 1730862091.47 epoch sec; seqNo:1812; depth 49.82 2024-11-06T03:01:31Z- handleDVL() - DVL msg epochMillisec: 1730862091.49 2024-11-06T03:01:31Z- handleDvl() - RDI_Pathfinder msg: 1730862091.49 sec; seqNo:553; #beams:4 2024-11-06T03:01:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.64 , 35.51 , 33.30 , 38.06 2024-11-06T03:01:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.02 , -0.08 2024-11-06T03:01:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862091.52 epoch sec; seqNo:1812; 4061675.28 north; 594377.03 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:01:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862089.05 2024-11-06T03:01:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:31Z- waiting for 1730862093.05, time is 1730862091.52 2024-11-06T03:01:31Z- AHRS_M2 msg: 1730862091.85 epoch sec; seqNo:1698 2024-11-06T03:01:31Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 1.45 psi 2024-11-06T03:01:31Z- handleDepth()- Depth_Keller msg: 1730862091.87 epoch sec; seqNo:1813; depth 49.79 2024-11-06T03:01:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862091.89 epoch sec; seqNo:1813; 4061675.28 north; 594377.03 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:01:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862089.05 2024-11-06T03:01:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:32Z- waiting for 1730862093.05, time is 1730862091.89 2024-11-06T03:01:32Z- AHRS_M2 msg: 1730862092.25 epoch sec; seqNo:1699 2024-11-06T03:01:32Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 1.45 psi 2024-11-06T03:01:32Z- handleDepth()- Depth_Keller msg: 1730862092.26 epoch sec; seqNo:1814; depth 49.76 2024-11-06T03:01:32Z- handleDVL() - DVL msg epochMillisec: 1730862092.26 2024-11-06T03:01:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:01:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:01:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:01:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:01:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:01:32Z- handleDvl() - RDI_Pathfinder msg: 1730862092.26 sec; seqNo:554; #beams:0 2024-11-06T03:01:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:01:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.02 , -0.08 2024-11-06T03:01:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862092.27 epoch sec; seqNo:1814; 4061675.28 north; 594377.03 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:01:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862089.05 2024-11-06T03:01:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:32Z- waiting for 1730862093.05, time is 1730862092.27 2024-11-06T03:01:32Z- AHRS_M2 msg: 1730862092.67 epoch sec; seqNo:1700 2024-11-06T03:01:32Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 1.45 psi 2024-11-06T03:01:32Z- handleDepth()- Depth_Keller msg: 1730862092.67 epoch sec; seqNo:1815; depth 49.74 2024-11-06T03:01:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862092.68 epoch sec; seqNo:1815; 4061675.28 north; 594377.03 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:01:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862089.05 2024-11-06T03:01:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:32Z- waiting for 1730862093.05, time is 1730862092.68 2024-11-06T03:01:33Z- AHRS_M2 msg: 1730862093.07 epoch sec; seqNo:1701 2024-11-06T03:01:33Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 1.45 psi 2024-11-06T03:01:33Z- handleDepth()- Depth_Keller msg: 1730862093.07 epoch sec; seqNo:1816; depth 49.75 2024-11-06T03:01:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862093.08 epoch sec; seqNo:1816; 4061675.29 north; 594378.25 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:01:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862089.05 2024-11-06T03:01:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:33Z- waiting for 1730862093.05, time is 1730862093.08 2024-11-06T03:01:33Z- LcmTrn::updateTrn() >>>> pose time 1730862093.08, meas time 1730862092.26 <<<< 2024-11-06T03:01:33Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862093.08 2024-11-06T03:01:33Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:01:33Z- TerrainNav::motionUpdate() - currEstimate = (4061675, 594378.25, 49.75) 2024-11-06T03:01:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:01:33Z- TerrainNav:: SubcloudNIS is 1.06, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:01:33Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:01:33Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:01:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:01:33Z- variances([0], [2], and psi_berg) = 0.497733 0.113311 0.000000 2024-11-06T03:01:33Z- publishEstimatesDecoder() - at 1730862093.08 sec 2024-11-06T03:01:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:01:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061707.51 594361.92 51.78 2024-11-06T03:01:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061706.24 594361.80 50.47 lat/lon: 36.70 -121.94 2024-11-06T03:01:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.50 0.11 1.02 0.00 2024-11-06T03:01:33Z- AHRS_M2 msg: 1730862093.47 epoch sec; seqNo:1702 2024-11-06T03:01:33Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 1.44 psi 2024-11-06T03:01:33Z- handleDepth()- Depth_Keller msg: 1730862093.49 epoch sec; seqNo:1817; depth 49.73 2024-11-06T03:01:33Z- handleDVL() - DVL msg epochMillisec: 1730862093.51 2024-11-06T03:01:33Z- handleDvl() - RDI_Pathfinder msg: 1730862093.51 sec; seqNo:555; #beams:4 2024-11-06T03:01:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.81 , 35.34 , 33.13 , 38.06 2024-11-06T03:01:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.04 , -0.00 2024-11-06T03:01:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862093.52 epoch sec; seqNo:1817; 4061675.29 north; 594378.25 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:01:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862093.08 2024-11-06T03:01:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:33Z- waiting for 1730862097.08, time is 1730862093.52 2024-11-06T03:01:33Z- AHRS_M2 msg: 1730862093.87 epoch sec; seqNo:1703 2024-11-06T03:01:33Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 1.44 psi 2024-11-06T03:01:33Z- handleDepth()- Depth_Keller msg: 1730862093.88 epoch sec; seqNo:1818; depth 49.70 2024-11-06T03:01:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862093.89 epoch sec; seqNo:1818; 4061675.29 north; 594378.25 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:01:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862093.08 2024-11-06T03:01:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:34Z- waiting for 1730862097.08, time is 1730862093.89 2024-11-06T03:01:34Z- AHRS_M2 msg: 1730862094.27 epoch sec; seqNo:1704 2024-11-06T03:01:34Z- AHRS_M2 msg: 0.12 phi; 0.06 theta; 1.44 psi 2024-11-06T03:01:34Z- handleDepth()- Depth_Keller msg: 1730862094.29 epoch sec; seqNo:1819; depth 49.69 2024-11-06T03:01:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862094.30 epoch sec; seqNo:1819; 4061675.29 north; 594378.25 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:01:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862093.08 2024-11-06T03:01:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:34Z- waiting for 1730862097.08, time is 1730862094.30 2024-11-06T03:01:34Z- AHRS_M2 msg: 1730862094.68 epoch sec; seqNo:1705 2024-11-06T03:01:34Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 1.44 psi 2024-11-06T03:01:34Z- handleDepth()- Depth_Keller msg: 1730862094.69 epoch sec; seqNo:1820; depth 49.68 2024-11-06T03:01:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862094.70 epoch sec; seqNo:1820; 4061675.67 north; 594378.25 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:01:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862093.08 2024-11-06T03:01:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:34Z- waiting for 1730862097.08, time is 1730862094.70 2024-11-06T03:01:35Z- AHRS_M2 msg: 1730862095.08 epoch sec; seqNo:1706 2024-11-06T03:01:35Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 1.44 psi 2024-11-06T03:01:35Z- handleDepth()- Depth_Keller msg: 1730862095.09 epoch sec; seqNo:1821; depth 49.67 2024-11-06T03:01:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862095.10 epoch sec; seqNo:1821; 4061675.69 north; 594379.47 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:01:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862093.08 2024-11-06T03:01:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:35Z- waiting for 1730862097.08, time is 1730862095.10 2024-11-06T03:01:35Z- AHRS_M2 msg: 1730862095.49 epoch sec; seqNo:1707 2024-11-06T03:01:35Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 1.43 psi 2024-11-06T03:01:35Z- handleDepth()- Depth_Keller msg: 1730862095.49 epoch sec; seqNo:1822; depth 49.68 2024-11-06T03:01:35Z- handleDVL() - DVL msg epochMillisec: 1730862095.52 2024-11-06T03:01:35Z- handleDvl() - RDI_Pathfinder msg: 1730862095.52 sec; seqNo:556; #beams:4 2024-11-06T03:01:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.64 , 35.34 , 32.79 , 38.56 2024-11-06T03:01:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.02 , 0.04 2024-11-06T03:01:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862095.54 epoch sec; seqNo:1822; 4061675.69 north; 594379.47 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:01:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862093.08 2024-11-06T03:01:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:35Z- waiting for 1730862097.08, time is 1730862095.54 2024-11-06T03:01:35Z- AHRS_M2 msg: 1730862095.89 epoch sec; seqNo:1708 2024-11-06T03:01:35Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 1.43 psi 2024-11-06T03:01:35Z- handleDepth()- Depth_Keller msg: 1730862095.90 epoch sec; seqNo:1823; depth 49.67 2024-11-06T03:01:35Z- handleDVL() - DVL msg epochMillisec: 1730862095.90 2024-11-06T03:01:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:01:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:01:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:01:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:01:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:01:35Z- handleDvl() - RDI_Pathfinder msg: 1730862095.90 sec; seqNo:557; #beams:0 2024-11-06T03:01:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:01:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.02 , 0.04 2024-11-06T03:01:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862095.92 epoch sec; seqNo:1823; 4061675.69 north; 594379.47 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:01:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862093.08 2024-11-06T03:01:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:36Z- waiting for 1730862097.08, time is 1730862095.92 2024-11-06T03:01:36Z- AHRS_M2 msg: 1730862096.30 epoch sec; seqNo:1709 2024-11-06T03:01:36Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 1.43 psi 2024-11-06T03:01:36Z- handleDepth()- Depth_Keller msg: 1730862096.30 epoch sec; seqNo:1824; depth 49.67 2024-11-06T03:01:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862096.31 epoch sec; seqNo:1824; 4061675.70 north; 594380.69 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:01:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862093.08 2024-11-06T03:01:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:36Z- waiting for 1730862097.08, time is 1730862096.31 2024-11-06T03:01:36Z- AHRS_M2 msg: 1730862096.70 epoch sec; seqNo:1710 2024-11-06T03:01:36Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 1.43 psi 2024-11-06T03:01:36Z- handleDepth()- Depth_Keller msg: 1730862096.70 epoch sec; seqNo:1825; depth 49.67 2024-11-06T03:01:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862096.72 epoch sec; seqNo:1825; 4061675.70 north; 594380.69 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:01:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862093.08 2024-11-06T03:01:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:36Z- waiting for 1730862097.08, time is 1730862096.72 2024-11-06T03:01:37Z- AHRS_M2 msg: 1730862097.11 epoch sec; seqNo:1711 2024-11-06T03:01:37Z- AHRS_M2 msg: 0.11 phi; 0.05 theta; 1.43 psi 2024-11-06T03:01:37Z- handleDepth()- Depth_Keller msg: 1730862097.11 epoch sec; seqNo:1826; depth 49.67 2024-11-06T03:01:37Z- handleDVL() - DVL msg epochMillisec: 1730862097.11 2024-11-06T03:01:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:01:37Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:01:37Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:01:37Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:01:37Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:01:37Z- handleDvl() - RDI_Pathfinder msg: 1730862097.11 sec; seqNo:558; #beams:0 2024-11-06T03:01:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:01:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.02 , 0.04 2024-11-06T03:01:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862097.12 epoch sec; seqNo:1826; 4061675.70 north; 594380.69 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:01:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862093.08 2024-11-06T03:01:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:37Z- waiting for 1730862097.08, time is 1730862097.12 2024-11-06T03:01:37Z- LcmTrn::updateTrn() >>>> pose time 1730862097.12, meas time 1730862097.11 <<<< 2024-11-06T03:01:37Z- LcmTrn::updateTrn() equating pose 1730862097.11 and meas times 1730862097.11 2024-11-06T03:01:37Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862097.11 2024-11-06T03:01:37Z- TerrainNav::motionUpdate() - currEstimate = (4061676, 594380.69, 49.67) 2024-11-06T03:01:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:01:37Z- TerrainNav:: SubcloudNIS is 1.06, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:01:37Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:01:37Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:01:37Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:01:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:01:37Z- variances([0], [2], and psi_berg) = 0.499022 0.114921 0.000000 2024-11-06T03:01:37Z- publishEstimatesDecoder() - at 1730862097.11 sec 2024-11-06T03:01:37Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:01:37Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061707.92 594364.36 51.33 2024-11-06T03:01:37Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061706.65 594364.23 50.39 lat/lon: 36.70 -121.94 2024-11-06T03:01:37Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.50 0.11 1.27 0.00 2024-11-06T03:01:37Z- AHRS_M2 msg: 1730862097.51 epoch sec; seqNo:1712 2024-11-06T03:01:37Z- AHRS_M2 msg: 0.11 phi; 0.05 theta; 1.43 psi 2024-11-06T03:01:37Z- handleDepth()- Depth_Keller msg: 1730862097.51 epoch sec; seqNo:1827; depth 49.66 2024-11-06T03:01:37Z- handleDVL() - DVL msg epochMillisec: 1730862097.53 2024-11-06T03:01:37Z- handleDvl() - RDI_Pathfinder msg: 1730862097.53 sec; seqNo:559; #beams:4 2024-11-06T03:01:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.64 , 35.68 , 32.62 , 38.40 2024-11-06T03:01:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , 0.03 2024-11-06T03:01:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862097.56 epoch sec; seqNo:1827; 4061676.08 north; 594380.69 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:01:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862097.11 2024-11-06T03:01:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:37Z- waiting for 1730862101.11, time is 1730862097.56 2024-11-06T03:01:37Z- AHRS_M2 msg: 1730862097.91 epoch sec; seqNo:1713 2024-11-06T03:01:37Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 1.43 psi 2024-11-06T03:01:37Z- handleDepth()- Depth_Keller msg: 1730862097.92 epoch sec; seqNo:1828; depth 49.67 2024-11-06T03:01:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862097.93 epoch sec; seqNo:1828; 4061676.09 north; 594381.91 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:01:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862097.11 2024-11-06T03:01:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:38Z- waiting for 1730862101.11, time is 1730862097.93 2024-11-06T03:01:38Z- AHRS_M2 msg: 1730862098.31 epoch sec; seqNo:1714 2024-11-06T03:01:38Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 1.43 psi 2024-11-06T03:01:38Z- handleDepth()- Depth_Keller msg: 1730862098.32 epoch sec; seqNo:1829; depth 49.67 2024-11-06T03:01:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862098.33 epoch sec; seqNo:1829; 4061676.09 north; 594381.91 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:01:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862097.11 2024-11-06T03:01:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:38Z- waiting for 1730862101.11, time is 1730862098.33 2024-11-06T03:01:38Z- AHRS_M2 msg: 1730862098.72 epoch sec; seqNo:1715 2024-11-06T03:01:38Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.43 psi 2024-11-06T03:01:38Z- handleDepth()- Depth_Keller msg: 1730862098.72 epoch sec; seqNo:1830; depth 49.66 2024-11-06T03:01:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862098.73 epoch sec; seqNo:1830; 4061676.09 north; 594381.91 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:01:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862097.11 2024-11-06T03:01:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:38Z- waiting for 1730862101.11, time is 1730862098.73 2024-11-06T03:01:39Z- AHRS_M2 msg: 1730862099.13 epoch sec; seqNo:1716 2024-11-06T03:01:39Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.43 psi 2024-11-06T03:01:39Z- handleDepth()- Depth_Keller msg: 1730862099.13 epoch sec; seqNo:1831; depth 49.66 2024-11-06T03:01:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862099.14 epoch sec; seqNo:1831; 4061676.09 north; 594381.91 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:01:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862097.11 2024-11-06T03:01:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:39Z- waiting for 1730862101.11, time is 1730862099.14 2024-11-06T03:01:39Z- AHRS_M2 msg: 1730862099.53 epoch sec; seqNo:1717 2024-11-06T03:01:39Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.44 psi 2024-11-06T03:01:39Z- handleDepth()- Depth_Keller msg: 1730862099.54 epoch sec; seqNo:1832; depth 49.65 2024-11-06T03:01:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862099.56 epoch sec; seqNo:1832; 4061676.10 north; 594383.13 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:01:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862097.11 2024-11-06T03:01:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:39Z- waiting for 1730862101.11, time is 1730862099.56 2024-11-06T03:01:39Z- AHRS_M2 msg: 1730862099.93 epoch sec; seqNo:1718 2024-11-06T03:01:39Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.44 psi 2024-11-06T03:01:39Z- handleDepth()- Depth_Keller msg: 1730862099.94 epoch sec; seqNo:1833; depth 49.65 2024-11-06T03:01:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862099.96 epoch sec; seqNo:1833; 4061676.10 north; 594383.13 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:01:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862097.11 2024-11-06T03:01:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:40Z- waiting for 1730862101.11, time is 1730862099.96 2024-11-06T03:01:40Z- AHRS_M2 msg: 1730862100.34 epoch sec; seqNo:1719 2024-11-06T03:01:40Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.44 psi 2024-11-06T03:01:40Z- handleDepth()- Depth_Keller msg: 1730862100.34 epoch sec; seqNo:1834; depth 49.65 2024-11-06T03:01:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862100.36 epoch sec; seqNo:1834; 4061676.10 north; 594383.13 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:01:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862097.11 2024-11-06T03:01:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:40Z- waiting for 1730862101.11, time is 1730862100.36 2024-11-06T03:01:40Z- AHRS_M2 msg: 1730862100.74 epoch sec; seqNo:1720 2024-11-06T03:01:40Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.44 psi 2024-11-06T03:01:40Z- handleDepth()- Depth_Keller msg: 1730862100.75 epoch sec; seqNo:1835; depth 49.64 2024-11-06T03:01:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862100.76 epoch sec; seqNo:1835; 4061676.50 north; 594384.34 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:01:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862097.11 2024-11-06T03:01:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:40Z- waiting for 1730862101.11, time is 1730862100.76 2024-11-06T03:01:41Z- AHRS_M2 msg: 1730862101.14 epoch sec; seqNo:1721 2024-11-06T03:01:41Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.44 psi 2024-11-06T03:01:41Z- handleDepth()- Depth_Keller msg: 1730862101.15 epoch sec; seqNo:1836; depth 49.63 2024-11-06T03:01:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862101.17 epoch sec; seqNo:1836; 4061676.50 north; 594384.34 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:01:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862097.11 2024-11-06T03:01:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:41Z- waiting for 1730862101.11, time is 1730862101.17 2024-11-06T03:01:41Z- LcmTrn::updateTrn() >>>> pose time 1730862101.17, meas time 1730862097.53 <<<< 2024-11-06T03:01:41Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862101.17 2024-11-06T03:01:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862097.53 sec, ping # 0; waiting for INS data more recent than 1730862097.11... 2024-11-06T03:01:41Z- TerrainNav::motionUpdate() - currEstimate = (4061676, 594384.34, 49.63) 2024-11-06T03:01:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:01:41Z- TerrainNav:: SubcloudNIS is 1.06, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:01:41Z- TNavPF::Projecting Measurements 2024-11-06T03:01:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:01:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:01:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:01:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:01:41Z- TNavFilter::4 good beams. 2024-11-06T03:01:41Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:01:41Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:01:41Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:01:41Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:01:41Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:01:41Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:01:41Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:01:41Z- TerrainMapOctree: (vehN, vehE ) = (4061675.78, 594381.07). 2024-11-06T03:01:41Z- TerrainMapOctree: Min Distance = 150.62. 2024-11-06T03:01:41Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:01:41Z- Forcing Subcloud Comparison 2024-11-06T03:01:41Z- Weighting particles with subcloud comparison 2024-11-06T03:01:41Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:41Z- meanExpectedMeasDiff: -6.373722 delta_rms_squared: 2.076117 alpha: 0.990459 2024-11-06T03:01:41Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:41Z- meanExpectedMeasDiff: 1.573914 delta_rms_squared: 1.792026 alpha: 0.988963 2024-11-06T03:01:41Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:41Z- meanExpectedMeasDiff: -8.193773 delta_rms_squared: 1.777229 alpha: 0.988872 2024-11-06T03:01:41Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:41Z- meanExpectedMeasDiff: 5.210281 delta_rms_squared: 1.858097 alpha: 0.989351 2024-11-06T03:01:41Z- Subcloud NIS: 1.124062 2024-11-06T03:01:41Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:01:41Z- TNavPF:: sumWeights = 0.000000 2024-11-06T03:01:41Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:01:41Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:01:41Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:01:41Z- TerrainNav::Resampling particle filter... 2024-11-06T03:01:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862097.53 sec. 2024-11-06T03:01:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:01:41Z- variances([0], [2], and psi_berg) = 1.485750 0.121545 0.000000 2024-11-06T03:01:41Z- publishEstimatesDecoder() - at 1730862101.17 sec 2024-11-06T03:01:41Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:01:41Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061708.72 594368.05 51.62 2024-11-06T03:01:41Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061708.03 594367.89 51.69 lat/lon: 36.70 -121.94 2024-11-06T03:01:41Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.49 0.12 0.91 0.00 2024-11-06T03:01:41Z- AHRS_M2 msg: 1730862101.55 epoch sec; seqNo:1722 2024-11-06T03:01:41Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.43 psi 2024-11-06T03:01:41Z- handleDepth()- Depth_Keller msg: 1730862101.56 epoch sec; seqNo:1837; depth 49.64 2024-11-06T03:01:41Z- handleDVL() - DVL msg epochMillisec: 1730862101.58 2024-11-06T03:01:41Z- handleDvl() - RDI_Pathfinder msg: 1730862101.58 sec; seqNo:560; #beams:4 2024-11-06T03:01:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.81 , 34.15 , 32.28 , 38.90 2024-11-06T03:01:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , -0.06 2024-11-06T03:01:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862101.62 epoch sec; seqNo:1837; 4061676.50 north; 594384.34 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:01:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862101.17 2024-11-06T03:01:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:41Z- waiting for 1730862105.17, time is 1730862101.62 2024-11-06T03:01:41Z- AHRS_M2 msg: 1730862101.95 epoch sec; seqNo:1723 2024-11-06T03:01:41Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.43 psi 2024-11-06T03:01:41Z- handleDepth()- Depth_Keller msg: 1730862101.95 epoch sec; seqNo:1838; depth 49.62 2024-11-06T03:01:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862101.97 epoch sec; seqNo:1838; 4061676.51 north; 594385.56 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:01:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862101.17 2024-11-06T03:01:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:42Z- waiting for 1730862105.17, time is 1730862101.97 2024-11-06T03:01:42Z- AHRS_M2 msg: 1730862102.38 epoch sec; seqNo:1724 2024-11-06T03:01:42Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.43 psi 2024-11-06T03:01:42Z- handleDepth()- Depth_Keller msg: 1730862102.38 epoch sec; seqNo:1839; depth 49.63 2024-11-06T03:01:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862102.40 epoch sec; seqNo:1839; 4061676.51 north; 594385.56 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:01:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862101.17 2024-11-06T03:01:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:42Z- waiting for 1730862105.17, time is 1730862102.40 2024-11-06T03:01:42Z- AHRS_M2 msg: 1730862102.76 epoch sec; seqNo:1725 2024-11-06T03:01:42Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.43 psi 2024-11-06T03:01:42Z- handleDepth()- Depth_Keller msg: 1730862102.77 epoch sec; seqNo:1840; depth 49.63 2024-11-06T03:01:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862102.78 epoch sec; seqNo:1840; 4061676.51 north; 594385.56 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:01:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862101.17 2024-11-06T03:01:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:42Z- waiting for 1730862105.17, time is 1730862102.78 2024-11-06T03:01:43Z- AHRS_M2 msg: 1730862103.16 epoch sec; seqNo:1726 2024-11-06T03:01:43Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.43 psi 2024-11-06T03:01:43Z- handleDepth()- Depth_Keller msg: 1730862103.18 epoch sec; seqNo:1841; depth 49.62 2024-11-06T03:01:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862103.19 epoch sec; seqNo:1841; 4061676.51 north; 594385.56 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:01:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862101.17 2024-11-06T03:01:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:43Z- waiting for 1730862105.17, time is 1730862103.19 2024-11-06T03:01:43Z- AHRS_M2 msg: 1730862103.57 epoch sec; seqNo:1727 2024-11-06T03:01:43Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.43 psi 2024-11-06T03:01:43Z- handleDepth()- Depth_Keller msg: 1730862103.57 epoch sec; seqNo:1842; depth 49.60 2024-11-06T03:01:43Z- handleDVL() - DVL msg epochMillisec: 1730862103.60 2024-11-06T03:01:43Z- handleDvl() - RDI_Pathfinder msg: 1730862103.60 sec; seqNo:561; #beams:4 2024-11-06T03:01:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.13 , 34.15 , 32.28 , 38.56 2024-11-06T03:01:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , -0.08 2024-11-06T03:01:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862103.62 epoch sec; seqNo:1842; 4061676.52 north; 594386.78 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:01:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862101.17 2024-11-06T03:01:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:43Z- waiting for 1730862105.17, time is 1730862103.62 2024-11-06T03:01:43Z- AHRS_M2 msg: 1730862103.97 epoch sec; seqNo:1728 2024-11-06T03:01:43Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.43 psi 2024-11-06T03:01:43Z- handleDepth()- Depth_Keller msg: 1730862103.98 epoch sec; seqNo:1843; depth 49.62 2024-11-06T03:01:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862104.01 epoch sec; seqNo:1843; 4061676.52 north; 594386.78 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:01:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862101.17 2024-11-06T03:01:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:44Z- waiting for 1730862105.17, time is 1730862104.01 2024-11-06T03:01:44Z- AHRS_M2 msg: 1730862104.38 epoch sec; seqNo:1729 2024-11-06T03:01:44Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.43 psi 2024-11-06T03:01:44Z- handleDepth()- Depth_Keller msg: 1730862104.38 epoch sec; seqNo:1844; depth 49.60 2024-11-06T03:01:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862104.39 epoch sec; seqNo:1844; 4061676.90 north; 594386.78 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:01:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862101.17 2024-11-06T03:01:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:44Z- waiting for 1730862105.17, time is 1730862104.39 2024-11-06T03:01:44Z- handleDepth()- Depth_Keller msg: 1730862104.80 epoch sec; seqNo:1845; depth 49.60 2024-11-06T03:01:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862104.81 epoch sec; seqNo:1845; 4061676.92 north; 594388.00 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:01:44Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:01:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862101.17 2024-11-06T03:01:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:44Z- waiting for 1730862105.17, time is 1730862104.81 2024-11-06T03:01:45Z- AHRS_M2 msg: 1730862105.18 epoch sec; seqNo:1730 2024-11-06T03:01:45Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.43 psi 2024-11-06T03:01:45Z- handleDepth()- Depth_Keller msg: 1730862105.19 epoch sec; seqNo:1846; depth 49.59 2024-11-06T03:01:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862105.23 epoch sec; seqNo:1846; 4061676.92 north; 594388.00 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:01:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862101.17 2024-11-06T03:01:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:45Z- waiting for 1730862105.17, time is 1730862105.23 2024-11-06T03:01:45Z- LcmTrn::updateTrn() >>>> pose time 1730862105.23, meas time 1730862103.60 <<<< 2024-11-06T03:01:45Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862105.23 2024-11-06T03:01:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862103.60 sec, ping # 0; waiting for INS data more recent than 1730862101.17... 2024-11-06T03:01:45Z- TerrainNav::motionUpdate() - currEstimate = (4061677, 594388.00, 49.59) 2024-11-06T03:01:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:01:45Z- TerrainNav:: SubcloudNIS is 1.12, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:01:45Z- TNavPF::Projecting Measurements 2024-11-06T03:01:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:01:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:01:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:01:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:01:45Z- TNavFilter::4 good beams. 2024-11-06T03:01:45Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:01:45Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:01:45Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:01:45Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:01:45Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:01:45Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:01:45Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:01:45Z- TerrainMapOctree: (vehN, vehE ) = (4061676.75, 594386.53). 2024-11-06T03:01:45Z- TerrainMapOctree: Min Distance = 154.94. 2024-11-06T03:01:45Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:01:45Z- Forcing Subcloud Comparison 2024-11-06T03:01:45Z- Weighting particles with subcloud comparison 2024-11-06T03:01:45Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:45Z- meanExpectedMeasDiff: -6.118084 delta_rms_squared: 1.222025 alpha: 0.983897 2024-11-06T03:01:45Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:45Z- meanExpectedMeasDiff: 1.209925 delta_rms_squared: 1.934395 alpha: 0.989767 2024-11-06T03:01:45Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:45Z- meanExpectedMeasDiff: -5.598050 delta_rms_squared: 1.613483 alpha: 0.987756 2024-11-06T03:01:45Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:45Z- meanExpectedMeasDiff: 4.152796 delta_rms_squared: 1.237292 alpha: 0.984093 2024-11-06T03:01:45Z- Subcloud NIS: 1.125933 2024-11-06T03:01:45Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:01:45Z- TNavPF:: sumWeights = 0.000018 2024-11-06T03:01:45Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:01:45Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:01:45Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:01:45Z- TerrainNav::Resampling particle filter... 2024-11-06T03:01:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862103.60 sec. 2024-11-06T03:01:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:01:45Z- variances([0], [2], and psi_berg) = 1.263811 0.101269 0.000000 2024-11-06T03:01:45Z- publishEstimatesDecoder() - at 1730862105.23 sec 2024-11-06T03:01:45Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:01:45Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061709.18 594371.71 50.84 2024-11-06T03:01:45Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061708.82 594371.58 52.98 lat/lon: 36.70 -121.94 2024-11-06T03:01:45Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.26 0.10 0.87 0.00 2024-11-06T03:01:45Z- AHRS_M2 msg: 1730862105.60 epoch sec; seqNo:1731 2024-11-06T03:01:45Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.44 psi 2024-11-06T03:01:45Z- handleDepth()- Depth_Keller msg: 1730862105.60 epoch sec; seqNo:1847; depth 49.61 2024-11-06T03:01:45Z- handleDVL() - DVL msg epochMillisec: 1730862105.62 2024-11-06T03:01:45Z- handleDvl() - RDI_Pathfinder msg: 1730862105.62 sec; seqNo:562; #beams:4 2024-11-06T03:01:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.79 , 34.15 , 32.11 , 38.22 2024-11-06T03:01:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.00 , -0.09 2024-11-06T03:01:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862105.65 epoch sec; seqNo:1847; 4061676.92 north; 594388.00 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:01:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862105.23 2024-11-06T03:01:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:45Z- waiting for 1730862109.23, time is 1730862105.65 2024-11-06T03:01:45Z- AHRS_M2 msg: 1730862105.99 epoch sec; seqNo:1732 2024-11-06T03:01:46Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.44 psi 2024-11-06T03:01:46Z- handleDepth()- Depth_Keller msg: 1730862106.00 epoch sec; seqNo:1848; depth 49.59 2024-11-06T03:01:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862106.02 epoch sec; seqNo:1848; 4061676.92 north; 594388.00 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:01:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862105.23 2024-11-06T03:01:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:46Z- waiting for 1730862109.23, time is 1730862106.02 2024-11-06T03:01:46Z- AHRS_M2 msg: 1730862106.40 epoch sec; seqNo:1733 2024-11-06T03:01:46Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.43 psi 2024-11-06T03:01:46Z- handleDepth()- Depth_Keller msg: 1730862106.41 epoch sec; seqNo:1849; depth 49.58 2024-11-06T03:01:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862106.41 epoch sec; seqNo:1849; 4061676.93 north; 594389.22 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:01:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862105.23 2024-11-06T03:01:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:46Z- waiting for 1730862109.23, time is 1730862106.41 2024-11-06T03:01:46Z- AHRS_M2 msg: 1730862106.80 epoch sec; seqNo:1734 2024-11-06T03:01:46Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.43 psi 2024-11-06T03:01:46Z- handleDepth()- Depth_Keller msg: 1730862106.81 epoch sec; seqNo:1850; depth 49.60 2024-11-06T03:01:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862106.82 epoch sec; seqNo:1850; 4061676.93 north; 594389.22 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:01:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862105.23 2024-11-06T03:01:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:46Z- waiting for 1730862109.23, time is 1730862106.82 2024-11-06T03:01:47Z- AHRS_M2 msg: 1730862107.21 epoch sec; seqNo:1735 2024-11-06T03:01:47Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.43 psi 2024-11-06T03:01:47Z- handleDepth()- Depth_Keller msg: 1730862107.22 epoch sec; seqNo:1851; depth 49.60 2024-11-06T03:01:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862107.28 epoch sec; seqNo:1851; 4061676.93 north; 594389.22 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:01:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862105.23 2024-11-06T03:01:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:47Z- waiting for 1730862109.23, time is 1730862107.28 2024-11-06T03:01:47Z- AHRS_M2 msg: 1730862107.61 epoch sec; seqNo:1736 2024-11-06T03:01:47Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.43 psi 2024-11-06T03:01:47Z- handleDepth()- Depth_Keller msg: 1730862107.62 epoch sec; seqNo:1852; depth 49.59 2024-11-06T03:01:47Z- handleDVL() - DVL msg epochMillisec: 1730862107.64 2024-11-06T03:01:47Z- handleDvl() - RDI_Pathfinder msg: 1730862107.64 sec; seqNo:563; #beams:4 2024-11-06T03:01:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.49 , 33.64 , 32.11 , 37.31 2024-11-06T03:01:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.03 , -0.04 2024-11-06T03:01:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862107.66 epoch sec; seqNo:1852; 4061676.93 north; 594389.22 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:01:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862105.23 2024-11-06T03:01:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:47Z- waiting for 1730862109.23, time is 1730862107.66 2024-11-06T03:01:48Z- AHRS_M2 msg: 1730862108.01 epoch sec; seqNo:1737 2024-11-06T03:01:48Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.43 psi 2024-11-06T03:01:48Z- handleDepth()- Depth_Keller msg: 1730862108.02 epoch sec; seqNo:1853; depth 49.61 2024-11-06T03:01:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862108.04 epoch sec; seqNo:1853; 4061677.32 north; 594390.44 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:01:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862105.23 2024-11-06T03:01:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:48Z- waiting for 1730862109.23, time is 1730862108.04 2024-11-06T03:01:48Z- AHRS_M2 msg: 1730862108.41 epoch sec; seqNo:1738 2024-11-06T03:01:48Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.43 psi 2024-11-06T03:01:48Z- handleDepth()- Depth_Keller msg: 1730862108.42 epoch sec; seqNo:1854; depth 49.61 2024-11-06T03:01:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862108.44 epoch sec; seqNo:1854; 4061677.32 north; 594390.44 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:01:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862105.23 2024-11-06T03:01:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:48Z- waiting for 1730862109.23, time is 1730862108.44 2024-11-06T03:01:48Z- AHRS_M2 msg: 1730862108.82 epoch sec; seqNo:1739 2024-11-06T03:01:48Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.42 psi 2024-11-06T03:01:48Z- handleDepth()- Depth_Keller msg: 1730862108.83 epoch sec; seqNo:1855; depth 49.61 2024-11-06T03:01:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862108.84 epoch sec; seqNo:1855; 4061677.32 north; 594390.44 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:01:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862105.23 2024-11-06T03:01:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:48Z- waiting for 1730862109.23, time is 1730862108.84 2024-11-06T03:01:49Z- AHRS_M2 msg: 1730862109.27 epoch sec; seqNo:1740 2024-11-06T03:01:49Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.42 psi 2024-11-06T03:01:49Z- handleDepth()- Depth_Keller msg: 1730862109.28 epoch sec; seqNo:1856; depth 49.60 2024-11-06T03:01:49Z- handleDVL() - DVL msg epochMillisec: 1730862109.31 2024-11-06T03:01:49Z- handleDvl() - RDI_Pathfinder msg: 1730862109.31 sec; seqNo:564; #beams:4 2024-11-06T03:01:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.64 , 33.33 , 32.42 , 36.70 2024-11-06T03:01:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.04 , 0.02 2024-11-06T03:01:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862109.33 epoch sec; seqNo:1856; 4061677.32 north; 594390.44 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:01:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862105.23 2024-11-06T03:01:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:49Z- waiting for 1730862109.23, time is 1730862109.33 2024-11-06T03:01:49Z- LcmTrn::updateTrn() >>>> pose time 1730862109.33, meas time 1730862109.31 <<<< 2024-11-06T03:01:49Z- LcmTrn::updateTrn() equating pose 1730862109.31 and meas times 1730862109.31 2024-11-06T03:01:49Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862109.31 2024-11-06T03:01:49Z- TerrainNav::motionUpdate() - currEstimate = (4061677, 594390.44, 49.60) 2024-11-06T03:01:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:01:49Z- TerrainNav:: SubcloudNIS is 1.13, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:01:49Z- TNavPF::Projecting Measurements 2024-11-06T03:01:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:01:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:01:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:01:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:01:49Z- TNavFilter::4 good beams. 2024-11-06T03:01:49Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:01:49Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:01:49Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:01:49Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:01:49Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:01:49Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:01:49Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:01:49Z- TerrainMapOctree: (vehN, vehE ) = (4061677.32, 594390.44). 2024-11-06T03:01:49Z- TerrainMapOctree: Min Distance = 158.13. 2024-11-06T03:01:49Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:01:49Z- Forcing Subcloud Comparison 2024-11-06T03:01:49Z- Weighting particles with subcloud comparison 2024-11-06T03:01:49Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:49Z- meanExpectedMeasDiff: -4.107132 delta_rms_squared: 1.571142 alpha: 0.987430 2024-11-06T03:01:49Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:49Z- meanExpectedMeasDiff: -0.110145 delta_rms_squared: 1.370127 alpha: 0.985613 2024-11-06T03:01:49Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:49Z- meanExpectedMeasDiff: -4.360684 delta_rms_squared: 1.799624 alpha: 0.989009 2024-11-06T03:01:49Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:49Z- meanExpectedMeasDiff: 2.188177 delta_rms_squared: 1.449354 alpha: 0.986389 2024-11-06T03:01:49Z- Subcloud NIS: 1.040172 2024-11-06T03:01:49Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:01:49Z- TNavPF:: sumWeights = 0.007716 2024-11-06T03:01:49Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:01:49Z- TNavPF::First Term of Map Covariance Matrix is 0.06 2024-11-06T03:01:49Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:01:49Z- TerrainNav::Resampling particle filter... 2024-11-06T03:01:49Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862109.31 sec, ping # 0. 2024-11-06T03:01:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:01:49Z- variances([0], [2], and psi_berg) = 0.676601 0.067079 0.000000 2024-11-06T03:01:49Z- publishEstimatesDecoder() - at 1730862109.31 sec 2024-11-06T03:01:49Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:01:49Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061709.69 594373.94 53.17 2024-11-06T03:01:49Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061709.79 594374.00 54.08 lat/lon: 36.70 -121.94 2024-11-06T03:01:49Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.68 0.07 0.44 0.00 2024-11-06T03:01:49Z- AHRS_M2 msg: 1730862109.65 epoch sec; seqNo:1741 2024-11-06T03:01:49Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.42 psi 2024-11-06T03:01:49Z- handleDepth()- Depth_Keller msg: 1730862109.66 epoch sec; seqNo:1857; depth 49.61 2024-11-06T03:01:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862109.68 epoch sec; seqNo:1857; 4061677.34 north; 594391.66 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:01:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862109.31 2024-11-06T03:01:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:49Z- waiting for 1730862113.31, time is 1730862109.68 2024-11-06T03:01:50Z- AHRS_M2 msg: 1730862110.03 epoch sec; seqNo:1742 2024-11-06T03:01:50Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.41 psi 2024-11-06T03:01:50Z- handleDepth()- Depth_Keller msg: 1730862110.04 epoch sec; seqNo:1858; depth 49.61 2024-11-06T03:01:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862110.05 epoch sec; seqNo:1858; 4061677.34 north; 594391.66 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:01:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862109.31 2024-11-06T03:01:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:50Z- waiting for 1730862113.31, time is 1730862110.05 2024-11-06T03:01:50Z- AHRS_M2 msg: 1730862110.44 epoch sec; seqNo:1743 2024-11-06T03:01:50Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.41 psi 2024-11-06T03:01:50Z- handleDepth()- Depth_Keller msg: 1730862110.44 epoch sec; seqNo:1859; depth 49.61 2024-11-06T03:01:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862110.45 epoch sec; seqNo:1859; 4061677.71 north; 594391.65 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:01:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862109.31 2024-11-06T03:01:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:50Z- waiting for 1730862113.31, time is 1730862110.45 2024-11-06T03:01:50Z- AHRS_M2 msg: 1730862110.84 epoch sec; seqNo:1744 2024-11-06T03:01:50Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.41 psi 2024-11-06T03:01:50Z- handleDepth()- Depth_Keller msg: 1730862110.85 epoch sec; seqNo:1860; depth 49.61 2024-11-06T03:01:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862110.85 epoch sec; seqNo:1860; 4061677.71 north; 594391.65 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:01:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862109.31 2024-11-06T03:01:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:51Z- waiting for 1730862113.31, time is 1730862110.85 2024-11-06T03:01:51Z- handleDepth()- Depth_Keller msg: 1730862111.30 epoch sec; seqNo:1861; depth 49.61 2024-11-06T03:01:51Z- handleDVL() - DVL msg epochMillisec: 1730862111.33 2024-11-06T03:01:51Z- handleDvl() - RDI_Pathfinder msg: 1730862111.33 sec; seqNo:565; #beams:4 2024-11-06T03:01:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.79 , 33.18 , 33.03 , 36.24 2024-11-06T03:01:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , 0.03 2024-11-06T03:01:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862111.35 epoch sec; seqNo:1861; 4061677.73 north; 594392.87 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:01:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862109.31 2024-11-06T03:01:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:51Z- waiting for 1730862113.31, time is 1730862111.35 2024-11-06T03:01:51Z- AHRS_M2 msg: 1730862111.65 epoch sec; seqNo:1745 2024-11-06T03:01:51Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.41 psi 2024-11-06T03:01:51Z- handleDepth()- Depth_Keller msg: 1730862111.66 epoch sec; seqNo:1862; depth 49.61 2024-11-06T03:01:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862111.68 epoch sec; seqNo:1862; 4061677.73 north; 594392.87 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:01:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862109.31 2024-11-06T03:01:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:51Z- waiting for 1730862113.31, time is 1730862111.68 2024-11-06T03:01:52Z- AHRS_M2 msg: 1730862112.05 epoch sec; seqNo:1746 2024-11-06T03:01:52Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.41 psi 2024-11-06T03:01:52Z- handleDepth()- Depth_Keller msg: 1730862112.06 epoch sec; seqNo:1863; depth 49.61 2024-11-06T03:01:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862112.08 epoch sec; seqNo:1863; 4061677.73 north; 594392.87 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:01:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862109.31 2024-11-06T03:01:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:52Z- waiting for 1730862113.31, time is 1730862112.08 2024-11-06T03:01:52Z- AHRS_M2 msg: 1730862112.45 epoch sec; seqNo:1747 2024-11-06T03:01:52Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.41 psi 2024-11-06T03:01:52Z- handleDepth()- Depth_Keller msg: 1730862112.46 epoch sec; seqNo:1864; depth 49.59 2024-11-06T03:01:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862112.47 epoch sec; seqNo:1864; 4061677.74 north; 594394.09 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:01:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862109.31 2024-11-06T03:01:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:52Z- waiting for 1730862113.31, time is 1730862112.47 2024-11-06T03:01:52Z- AHRS_M2 msg: 1730862112.87 epoch sec; seqNo:1748 2024-11-06T03:01:52Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.41 psi 2024-11-06T03:01:52Z- handleDepth()- Depth_Keller msg: 1730862112.88 epoch sec; seqNo:1865; depth 49.60 2024-11-06T03:01:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862112.89 epoch sec; seqNo:1865; 4061678.12 north; 594394.09 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:01:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862109.31 2024-11-06T03:01:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:53Z- waiting for 1730862113.31, time is 1730862112.89 2024-11-06T03:01:53Z- AHRS_M2 msg: 1730862113.29 epoch sec; seqNo:1749 2024-11-06T03:01:53Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.41 psi 2024-11-06T03:01:53Z- handleDepth()- Depth_Keller msg: 1730862113.30 epoch sec; seqNo:1866; depth 49.59 2024-11-06T03:01:53Z- handleDVL() - DVL msg epochMillisec: 1730862113.32 2024-11-06T03:01:53Z- handleDvl() - RDI_Pathfinder msg: 1730862113.32 sec; seqNo:566; #beams:4 2024-11-06T03:01:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.94 , 33.03 , 32.57 , 36.39 2024-11-06T03:01:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , -0.01 2024-11-06T03:01:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862113.34 epoch sec; seqNo:1866; 4061678.12 north; 594394.09 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:01:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862109.31 2024-11-06T03:01:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:53Z- waiting for 1730862113.31, time is 1730862113.34 2024-11-06T03:01:53Z- LcmTrn::updateTrn() >>>> pose time 1730862113.34, meas time 1730862113.32 <<<< 2024-11-06T03:01:53Z- LcmTrn::updateTrn() equating pose 1730862113.32 and meas times 1730862113.32 2024-11-06T03:01:53Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862113.32 2024-11-06T03:01:53Z- TerrainNav::motionUpdate() - currEstimate = (4061678, 594394.09, 49.59) 2024-11-06T03:01:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:01:53Z- TerrainNav:: SubcloudNIS is 1.04, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:01:53Z- TNavPF::Projecting Measurements 2024-11-06T03:01:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:01:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:01:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:01:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:01:53Z- TNavFilter::4 good beams. 2024-11-06T03:01:53Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:01:53Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:01:53Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:01:53Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:01:53Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:01:53Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:01:53Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:01:53Z- TerrainMapOctree: (vehN, vehE ) = (4061678.12, 594394.09). 2024-11-06T03:01:53Z- TerrainMapOctree: Min Distance = 161.02. 2024-11-06T03:01:53Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:01:53Z- Forcing Subcloud Comparison 2024-11-06T03:01:53Z- Weighting particles with subcloud comparison 2024-11-06T03:01:53Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:53Z- meanExpectedMeasDiff: -2.149434 delta_rms_squared: 0.836125 alpha: 0.976639 2024-11-06T03:01:53Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:53Z- meanExpectedMeasDiff: 0.006014 delta_rms_squared: 0.757141 alpha: 0.974265 2024-11-06T03:01:53Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:53Z- meanExpectedMeasDiff: -3.347213 delta_rms_squared: 1.855783 alpha: 0.989338 2024-11-06T03:01:53Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:53Z- meanExpectedMeasDiff: 2.244298 delta_rms_squared: 0.909836 alpha: 0.978491 2024-11-06T03:01:53Z- Subcloud NIS: 0.942243 2024-11-06T03:01:53Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:01:53Z- TNavPF:: sumWeights = 0.053876 2024-11-06T03:01:53Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:01:53Z- TNavPF::First Term of Map Covariance Matrix is 0.16 2024-11-06T03:01:53Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T03:01:53Z- TerrainNav::Resampling particle filter... 2024-11-06T03:01:53Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862113.32 sec, ping # 0. 2024-11-06T03:01:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:01:53Z- variances([0], [2], and psi_berg) = 0.600829 0.073437 0.000000 2024-11-06T03:01:53Z- publishEstimatesDecoder() - at 1730862113.32 sec 2024-11-06T03:01:53Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:01:53Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061710.32 594377.74 53.55 2024-11-06T03:01:53Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061710.52 594377.63 54.52 lat/lon: 36.70 -121.94 2024-11-06T03:01:53Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.60 0.07 0.34 0.00 2024-11-06T03:01:53Z- AHRS_M2 msg: 1730862113.67 epoch sec; seqNo:1750 2024-11-06T03:01:53Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.42 psi 2024-11-06T03:01:53Z- handleDepth()- Depth_Keller msg: 1730862113.68 epoch sec; seqNo:1867; depth 49.60 2024-11-06T03:01:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862113.71 epoch sec; seqNo:1867; 4061678.12 north; 594394.09 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:01:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862113.32 2024-11-06T03:01:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:53Z- waiting for 1730862117.32, time is 1730862113.71 2024-11-06T03:01:54Z- AHRS_M2 msg: 1730862114.07 epoch sec; seqNo:1751 2024-11-06T03:01:54Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.42 psi 2024-11-06T03:01:54Z- handleDepth()- Depth_Keller msg: 1730862114.08 epoch sec; seqNo:1868; depth 49.58 2024-11-06T03:01:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862114.09 epoch sec; seqNo:1868; 4061678.13 north; 594395.31 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:01:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862113.32 2024-11-06T03:01:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:54Z- waiting for 1730862117.32, time is 1730862114.09 2024-11-06T03:01:54Z- AHRS_M2 msg: 1730862114.47 epoch sec; seqNo:1752 2024-11-06T03:01:54Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.42 psi 2024-11-06T03:01:54Z- handleDepth()- Depth_Keller msg: 1730862114.48 epoch sec; seqNo:1869; depth 49.60 2024-11-06T03:01:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862114.50 epoch sec; seqNo:1869; 4061678.13 north; 594395.31 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:01:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862113.32 2024-11-06T03:01:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:54Z- waiting for 1730862117.32, time is 1730862114.50 2024-11-06T03:01:54Z- AHRS_M2 msg: 1730862114.88 epoch sec; seqNo:1753 2024-11-06T03:01:54Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.42 psi 2024-11-06T03:01:54Z- handleDepth()- Depth_Keller msg: 1730862114.88 epoch sec; seqNo:1870; depth 49.58 2024-11-06T03:01:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862114.89 epoch sec; seqNo:1870; 4061678.13 north; 594395.31 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:01:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862113.32 2024-11-06T03:01:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:55Z- waiting for 1730862117.32, time is 1730862114.89 2024-11-06T03:01:55Z- AHRS_M2 msg: 1730862115.30 epoch sec; seqNo:1754 2024-11-06T03:01:55Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.43 psi 2024-11-06T03:01:55Z- handleDepth()- Depth_Keller msg: 1730862115.30 epoch sec; seqNo:1871; depth 49.58 2024-11-06T03:01:55Z- handleDVL() - DVL msg epochMillisec: 1730862115.32 2024-11-06T03:01:55Z- handleDvl() - RDI_Pathfinder msg: 1730862115.32 sec; seqNo:567; #beams:4 2024-11-06T03:01:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.94 , 32.42 , 32.26 , 36.54 2024-11-06T03:01:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , -0.06 2024-11-06T03:01:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862115.33 epoch sec; seqNo:1871; 4061678.15 north; 594396.53 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:01:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862113.32 2024-11-06T03:01:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:55Z- waiting for 1730862117.32, time is 1730862115.33 2024-11-06T03:01:55Z- AHRS_M2 msg: 1730862115.69 epoch sec; seqNo:1755 2024-11-06T03:01:55Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 1.43 psi 2024-11-06T03:01:55Z- handleDepth()- Depth_Keller msg: 1730862115.69 epoch sec; seqNo:1872; depth 49.58 2024-11-06T03:01:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862115.71 epoch sec; seqNo:1872; 4061678.53 north; 594396.53 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:01:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862113.32 2024-11-06T03:01:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:55Z- waiting for 1730862117.32, time is 1730862115.71 2024-11-06T03:01:56Z- AHRS_M2 msg: 1730862116.09 epoch sec; seqNo:1756 2024-11-06T03:01:56Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 1.44 psi 2024-11-06T03:01:56Z- handleDepth()- Depth_Keller msg: 1730862116.10 epoch sec; seqNo:1873; depth 49.58 2024-11-06T03:01:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862116.11 epoch sec; seqNo:1873; 4061678.53 north; 594396.53 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:01:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862113.32 2024-11-06T03:01:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:56Z- waiting for 1730862117.32, time is 1730862116.11 2024-11-06T03:01:56Z- AHRS_M2 msg: 1730862116.50 epoch sec; seqNo:1757 2024-11-06T03:01:56Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 1.44 psi 2024-11-06T03:01:56Z- handleDepth()- Depth_Keller msg: 1730862116.50 epoch sec; seqNo:1874; depth 49.58 2024-11-06T03:01:56Z- handleDVL() - DVL msg epochMillisec: 1730862116.51 2024-11-06T03:01:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:01:56Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:01:56Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:01:56Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:01:56Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:01:56Z- handleDvl() - RDI_Pathfinder msg: 1730862116.51 sec; seqNo:568; #beams:0 2024-11-06T03:01:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:01:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , -0.06 2024-11-06T03:01:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862116.52 epoch sec; seqNo:1874; 4061678.54 north; 594397.75 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:01:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862113.32 2024-11-06T03:01:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:56Z- waiting for 1730862117.32, time is 1730862116.52 2024-11-06T03:01:56Z- AHRS_M2 msg: 1730862116.90 epoch sec; seqNo:1758 2024-11-06T03:01:56Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.44 psi 2024-11-06T03:01:56Z- handleDepth()- Depth_Keller msg: 1730862116.90 epoch sec; seqNo:1875; depth 49.59 2024-11-06T03:01:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862116.91 epoch sec; seqNo:1875; 4061678.54 north; 594397.75 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:01:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862113.32 2024-11-06T03:01:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:57Z- waiting for 1730862117.32, time is 1730862116.91 2024-11-06T03:01:57Z- AHRS_M2 msg: 1730862117.30 epoch sec; seqNo:1759 2024-11-06T03:01:57Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.44 psi 2024-11-06T03:01:57Z- handleDepth()- Depth_Keller msg: 1730862117.31 epoch sec; seqNo:1876; depth 49.57 2024-11-06T03:01:57Z- handleDVL() - DVL msg epochMillisec: 1730862117.33 2024-11-06T03:01:57Z- handleDvl() - RDI_Pathfinder msg: 1730862117.33 sec; seqNo:569; #beams:4 2024-11-06T03:01:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.64 , 31.65 , 31.96 , 36.39 2024-11-06T03:01:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.08 2024-11-06T03:01:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862117.40 epoch sec; seqNo:1876; 4061678.54 north; 594397.75 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:01:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862113.32 2024-11-06T03:01:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:57Z- waiting for 1730862117.32, time is 1730862117.40 2024-11-06T03:01:57Z- LcmTrn::updateTrn() >>>> pose time 1730862117.40, meas time 1730862117.33 <<<< 2024-11-06T03:01:57Z- LcmTrn::updateTrn() equating pose 1730862117.33 and meas times 1730862117.33 2024-11-06T03:01:57Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862117.33 2024-11-06T03:01:57Z- TerrainNav::motionUpdate() - currEstimate = (4061679, 594397.75, 49.57) 2024-11-06T03:01:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:01:57Z- TerrainNav:: SubcloudNIS is 0.94, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:01:57Z- TNavPF::Projecting Measurements 2024-11-06T03:01:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:01:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:01:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:01:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:01:57Z- TNavFilter::4 good beams. 2024-11-06T03:01:57Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:01:57Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:01:57Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:01:57Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:01:57Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:01:57Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:01:57Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:01:57Z- TerrainMapOctree: (vehN, vehE ) = (4061678.54, 594397.75). 2024-11-06T03:01:57Z- TerrainMapOctree: Min Distance = 164.12. 2024-11-06T03:01:57Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:01:57Z- Forcing Subcloud Comparison 2024-11-06T03:01:57Z- Weighting particles with subcloud comparison 2024-11-06T03:01:57Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:57Z- meanExpectedMeasDiff: -2.691215 delta_rms_squared: 0.929425 alpha: 0.978935 2024-11-06T03:01:57Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:57Z- meanExpectedMeasDiff: -0.688878 delta_rms_squared: 0.672685 alpha: 0.971127 2024-11-06T03:01:57Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:57Z- meanExpectedMeasDiff: -3.682294 delta_rms_squared: 1.380068 alpha: 0.985715 2024-11-06T03:01:57Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:01:57Z- meanExpectedMeasDiff: 1.870497 delta_rms_squared: 0.822072 alpha: 0.976249 2024-11-06T03:01:57Z- Subcloud NIS: 0.979484 2024-11-06T03:01:57Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:01:57Z- TNavPF:: sumWeights = 0.041760 2024-11-06T03:01:57Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:01:57Z- TNavPF::First Term of Map Covariance Matrix is 0.17 2024-11-06T03:01:57Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:01:57Z- TerrainNav::Resampling particle filter... 2024-11-06T03:01:57Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862117.33 sec, ping # 0. 2024-11-06T03:01:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:01:57Z- variances([0], [2], and psi_berg) = 0.555267 0.082252 0.000000 2024-11-06T03:01:57Z- publishEstimatesDecoder() - at 1730862117.33 sec 2024-11-06T03:01:57Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:01:57Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061710.77 594381.39 54.78 2024-11-06T03:01:57Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061710.80 594381.25 55.09 lat/lon: 36.70 -121.94 2024-11-06T03:01:57Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.56 0.08 0.32 0.00 2024-11-06T03:01:57Z- AHRS_M2 msg: 1730862117.71 epoch sec; seqNo:1760 2024-11-06T03:01:57Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.44 psi 2024-11-06T03:01:57Z- handleDepth()- Depth_Keller msg: 1730862117.72 epoch sec; seqNo:1877; depth 49.59 2024-11-06T03:01:57Z- handleDVL() - DVL msg epochMillisec: 1730862117.72 2024-11-06T03:01:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:01:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:01:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:01:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:01:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:01:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:01:57Z- handleDvl() - RDI_Pathfinder msg: 1730862117.72 sec; seqNo:570; #beams:0 2024-11-06T03:01:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:01:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.08 2024-11-06T03:01:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862117.74 epoch sec; seqNo:1877; 4061678.54 north; 594397.75 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:01:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862117.33 2024-11-06T03:01:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:57Z- waiting for 1730862121.33, time is 1730862117.74 2024-11-06T03:01:58Z- AHRS_M2 msg: 1730862118.11 epoch sec; seqNo:1761 2024-11-06T03:01:58Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.44 psi 2024-11-06T03:01:58Z- handleDepth()- Depth_Keller msg: 1730862118.12 epoch sec; seqNo:1878; depth 49.59 2024-11-06T03:01:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862118.13 epoch sec; seqNo:1878; 4061678.55 north; 594398.97 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:01:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862117.33 2024-11-06T03:01:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:58Z- waiting for 1730862121.33, time is 1730862118.13 2024-11-06T03:01:58Z- AHRS_M2 msg: 1730862118.53 epoch sec; seqNo:1762 2024-11-06T03:01:58Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.44 psi 2024-11-06T03:01:58Z- handleDepth()- Depth_Keller msg: 1730862118.53 epoch sec; seqNo:1879; depth 49.61 2024-11-06T03:01:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862118.54 epoch sec; seqNo:1879; 4061678.55 north; 594398.97 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:01:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862117.33 2024-11-06T03:01:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:58Z- waiting for 1730862121.33, time is 1730862118.54 2024-11-06T03:01:58Z- AHRS_M2 msg: 1730862118.92 epoch sec; seqNo:1763 2024-11-06T03:01:58Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.44 psi 2024-11-06T03:01:58Z- handleDepth()- Depth_Keller msg: 1730862118.92 epoch sec; seqNo:1880; depth 49.62 2024-11-06T03:01:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862118.94 epoch sec; seqNo:1880; 4061678.55 north; 594398.97 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:01:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862117.33 2024-11-06T03:01:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:59Z- waiting for 1730862121.33, time is 1730862118.94 2024-11-06T03:01:59Z- AHRS_M2 msg: 1730862119.32 epoch sec; seqNo:1764 2024-11-06T03:01:59Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.44 psi 2024-11-06T03:01:59Z- handleDepth()- Depth_Keller msg: 1730862119.33 epoch sec; seqNo:1881; depth 49.61 2024-11-06T03:01:59Z- handleDVL() - DVL msg epochMillisec: 1730862119.36 2024-11-06T03:01:59Z- handleDvl() - RDI_Pathfinder msg: 1730862119.36 sec; seqNo:571; #beams:4 2024-11-06T03:01:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.10 , 31.65 , 31.34 , 34.86 2024-11-06T03:01:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , -0.05 2024-11-06T03:01:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862119.37 epoch sec; seqNo:1881; 4061678.94 north; 594400.18 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:01:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:01:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862117.33 2024-11-06T03:01:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:59Z- waiting for 1730862121.33, time is 1730862119.37 2024-11-06T03:01:59Z- AHRS_M2 msg: 1730862119.73 epoch sec; seqNo:1765 2024-11-06T03:01:59Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.44 psi 2024-11-06T03:01:59Z- handleDepth()- Depth_Keller msg: 1730862119.74 epoch sec; seqNo:1882; depth 49.64 2024-11-06T03:01:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862119.75 epoch sec; seqNo:1882; 4061678.94 north; 594400.18 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:01:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:01:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862117.33 2024-11-06T03:01:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:01:59Z- waiting for 1730862121.33, time is 1730862119.75 2024-11-06T03:02:00Z- AHRS_M2 msg: 1730862120.13 epoch sec; seqNo:1766 2024-11-06T03:02:00Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.44 psi 2024-11-06T03:02:00Z- handleDepth()- Depth_Keller msg: 1730862120.14 epoch sec; seqNo:1883; depth 49.65 2024-11-06T03:02:00Z- handleDVL() - DVL msg epochMillisec: 1730862120.14 2024-11-06T03:02:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:00Z- handleDvl() - RDI_Pathfinder msg: 1730862120.14 sec; seqNo:572; #beams:0 2024-11-06T03:02:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , -0.05 2024-11-06T03:02:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862120.15 epoch sec; seqNo:1883; 4061678.94 north; 594400.18 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:02:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862117.33 2024-11-06T03:02:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:00Z- waiting for 1730862121.33, time is 1730862120.15 2024-11-06T03:02:00Z- AHRS_M2 msg: 1730862120.54 epoch sec; seqNo:1767 2024-11-06T03:02:00Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.44 psi 2024-11-06T03:02:00Z- handleDepth()- Depth_Keller msg: 1730862120.55 epoch sec; seqNo:1884; depth 49.67 2024-11-06T03:02:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862120.56 epoch sec; seqNo:1884; 4061678.94 north; 594400.18 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:02:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862117.33 2024-11-06T03:02:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:00Z- waiting for 1730862121.33, time is 1730862120.56 2024-11-06T03:02:00Z- AHRS_M2 msg: 1730862120.94 epoch sec; seqNo:1768 2024-11-06T03:02:00Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.44 psi 2024-11-06T03:02:00Z- handleDepth()- Depth_Keller msg: 1730862120.94 epoch sec; seqNo:1885; depth 49.68 2024-11-06T03:02:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862120.95 epoch sec; seqNo:1885; 4061678.96 north; 594401.40 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:02:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862117.33 2024-11-06T03:02:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:01Z- waiting for 1730862121.33, time is 1730862120.95 2024-11-06T03:02:01Z- handleDepth()- Depth_Keller msg: 1730862121.36 epoch sec; seqNo:1886; depth 49.70 2024-11-06T03:02:01Z- handleDVL() - DVL msg epochMillisec: 1730862121.39 2024-11-06T03:02:01Z- handleDvl() - RDI_Pathfinder msg: 1730862121.39 sec; seqNo:573; #beams:4 2024-11-06T03:02:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.33 , 31.04 , 31.34 , 34.25 2024-11-06T03:02:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , -0.03 2024-11-06T03:02:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862121.40 epoch sec; seqNo:1886; 4061678.96 north; 594401.40 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:02:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862117.33 2024-11-06T03:02:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:01Z- waiting for 1730862121.33, time is 1730862121.40 2024-11-06T03:02:01Z- LcmTrn::updateTrn() >>>> pose time 1730862121.40, meas time 1730862121.39 <<<< 2024-11-06T03:02:01Z- LcmTrn::updateTrn() equating pose 1730862121.39 and meas times 1730862121.39 2024-11-06T03:02:01Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862121.39 2024-11-06T03:02:01Z- TerrainNav::motionUpdate() - currEstimate = (4061679, 594401.40, 49.70) 2024-11-06T03:02:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:02:01Z- TerrainNav:: SubcloudNIS is 0.98, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:02:01Z- TNavPF::Projecting Measurements 2024-11-06T03:02:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:02:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:02:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:02:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:02:01Z- TNavFilter::4 good beams. 2024-11-06T03:02:01Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:02:01Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:02:01Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:02:01Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:02:01Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:02:01Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:02:01Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:02:01Z- TerrainMapOctree: (vehN, vehE ) = (4061678.96, 594401.40). 2024-11-06T03:02:01Z- TerrainMapOctree: Min Distance = 167.24. 2024-11-06T03:02:01Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:02:01Z- Forcing Subcloud Comparison 2024-11-06T03:02:01Z- Weighting particles with subcloud comparison 2024-11-06T03:02:01Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:01Z- meanExpectedMeasDiff: -3.277112 delta_rms_squared: 0.916606 alpha: 0.978646 2024-11-06T03:02:01Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:01Z- meanExpectedMeasDiff: -0.545089 delta_rms_squared: 0.634960 alpha: 0.969464 2024-11-06T03:02:01Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:01Z- meanExpectedMeasDiff: -3.765584 delta_rms_squared: 1.123127 alpha: 0.982504 2024-11-06T03:02:01Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:01Z- meanExpectedMeasDiff: -0.032703 delta_rms_squared: 0.973763 alpha: 0.979874 2024-11-06T03:02:01Z- Subcloud NIS: 0.973688 2024-11-06T03:02:01Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:02:01Z- TNavPF:: sumWeights = 0.057730 2024-11-06T03:02:01Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:02:01Z- TNavPF::First Term of Map Covariance Matrix is 0.29 2024-11-06T03:02:01Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T03:02:01Z- TerrainNav::Resampling particle filter... 2024-11-06T03:02:01Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862121.39 sec, ping # 0. 2024-11-06T03:02:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:02:01Z- variances([0], [2], and psi_berg) = 0.468574 0.076683 0.000000 2024-11-06T03:02:01Z- publishEstimatesDecoder() - at 1730862121.39 sec 2024-11-06T03:02:01Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:02:01Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061711.17 594385.06 54.83 2024-11-06T03:02:01Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061711.32 594384.91 56.01 lat/lon: 36.70 -121.94 2024-11-06T03:02:01Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.47 0.08 0.28 0.00 2024-11-06T03:02:01Z- AHRS_M2 msg: 1730862121.75 epoch sec; seqNo:1769 2024-11-06T03:02:01Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.45 psi 2024-11-06T03:02:01Z- handleDepth()- Depth_Keller msg: 1730862121.75 epoch sec; seqNo:1887; depth 49.74 2024-11-06T03:02:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862121.79 epoch sec; seqNo:1887; 4061678.96 north; 594401.40 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:02:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862121.39 2024-11-06T03:02:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:01Z- waiting for 1730862125.39, time is 1730862121.79 2024-11-06T03:02:02Z- AHRS_M2 msg: 1730862122.15 epoch sec; seqNo:1770 2024-11-06T03:02:02Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.45 psi 2024-11-06T03:02:02Z- handleDepth()- Depth_Keller msg: 1730862122.16 epoch sec; seqNo:1888; depth 49.75 2024-11-06T03:02:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862122.18 epoch sec; seqNo:1888; 4061678.96 north; 594401.40 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:02:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862121.39 2024-11-06T03:02:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:02Z- waiting for 1730862125.39, time is 1730862122.18 2024-11-06T03:02:02Z- AHRS_M2 msg: 1730862122.56 epoch sec; seqNo:1771 2024-11-06T03:02:02Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.45 psi 2024-11-06T03:02:02Z- handleDepth()- Depth_Keller msg: 1730862122.57 epoch sec; seqNo:1889; depth 49.78 2024-11-06T03:02:02Z- handleDVL() - DVL msg epochMillisec: 1730862122.57 2024-11-06T03:02:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:02Z- handleDvl() - RDI_Pathfinder msg: 1730862122.57 sec; seqNo:574; #beams:0 2024-11-06T03:02:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , -0.03 2024-11-06T03:02:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862122.58 epoch sec; seqNo:1889; 4061679.35 north; 594402.62 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:02:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862121.39 2024-11-06T03:02:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:02Z- waiting for 1730862125.39, time is 1730862122.58 2024-11-06T03:02:02Z- AHRS_M2 msg: 1730862122.97 epoch sec; seqNo:1772 2024-11-06T03:02:02Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.45 psi 2024-11-06T03:02:02Z- handleDepth()- Depth_Keller msg: 1730862122.97 epoch sec; seqNo:1890; depth 49.80 2024-11-06T03:02:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862122.99 epoch sec; seqNo:1890; 4061679.35 north; 594402.62 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:02:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862121.39 2024-11-06T03:02:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:03Z- waiting for 1730862125.39, time is 1730862122.99 2024-11-06T03:02:03Z- AHRS_M2 msg: 1730862123.36 epoch sec; seqNo:1773 2024-11-06T03:02:03Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.45 psi 2024-11-06T03:02:03Z- handleDepth()- Depth_Keller msg: 1730862123.38 epoch sec; seqNo:1891; depth 49.82 2024-11-06T03:02:03Z- handleDVL() - DVL msg epochMillisec: 1730862123.40 2024-11-06T03:02:03Z- handleDvl() - RDI_Pathfinder msg: 1730862123.40 sec; seqNo:575; #beams:4 2024-11-06T03:02:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.50 , 30.89 , 31.19 , 33.03 2024-11-06T03:02:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.01 2024-11-06T03:02:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862123.41 epoch sec; seqNo:1891; 4061679.35 north; 594402.62 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:02:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862121.39 2024-11-06T03:02:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:03Z- waiting for 1730862125.39, time is 1730862123.41 2024-11-06T03:02:03Z- AHRS_M2 msg: 1730862123.77 epoch sec; seqNo:1774 2024-11-06T03:02:03Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.45 psi 2024-11-06T03:02:03Z- handleDepth()- Depth_Keller msg: 1730862123.78 epoch sec; seqNo:1892; depth 49.85 2024-11-06T03:02:03Z- handleDVL() - DVL msg epochMillisec: 1730862123.78 2024-11-06T03:02:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:03Z- handleDvl() - RDI_Pathfinder msg: 1730862123.78 sec; seqNo:576; #beams:0 2024-11-06T03:02:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.01 2024-11-06T03:02:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862123.81 epoch sec; seqNo:1892; 4061679.35 north; 594402.62 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:02:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862121.39 2024-11-06T03:02:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:03Z- waiting for 1730862125.39, time is 1730862123.81 2024-11-06T03:02:04Z- AHRS_M2 msg: 1730862124.17 epoch sec; seqNo:1775 2024-11-06T03:02:04Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.45 psi 2024-11-06T03:02:04Z- handleDepth()- Depth_Keller msg: 1730862124.18 epoch sec; seqNo:1893; depth 49.88 2024-11-06T03:02:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862124.19 epoch sec; seqNo:1893; 4061679.36 north; 594403.84 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:02:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862121.39 2024-11-06T03:02:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:04Z- waiting for 1730862125.39, time is 1730862124.19 2024-11-06T03:02:04Z- AHRS_M2 msg: 1730862124.58 epoch sec; seqNo:1776 2024-11-06T03:02:04Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.45 psi 2024-11-06T03:02:04Z- handleDepth()- Depth_Keller msg: 1730862124.58 epoch sec; seqNo:1894; depth 49.89 2024-11-06T03:02:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862124.59 epoch sec; seqNo:1894; 4061679.36 north; 594403.84 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:02:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862121.39 2024-11-06T03:02:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:04Z- waiting for 1730862125.39, time is 1730862124.59 2024-11-06T03:02:04Z- AHRS_M2 msg: 1730862124.98 epoch sec; seqNo:1777 2024-11-06T03:02:04Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.45 psi 2024-11-06T03:02:04Z- handleDepth()- Depth_Keller msg: 1730862124.99 epoch sec; seqNo:1895; depth 49.92 2024-11-06T03:02:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862125.00 epoch sec; seqNo:1895; 4061679.36 north; 594403.84 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:02:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862121.39 2024-11-06T03:02:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:05Z- waiting for 1730862125.39, time is 1730862125.00 2024-11-06T03:02:05Z- AHRS_M2 msg: 1730862125.38 epoch sec; seqNo:1778 2024-11-06T03:02:05Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 1.45 psi 2024-11-06T03:02:05Z- handleDepth()- Depth_Keller msg: 1730862125.39 epoch sec; seqNo:1896; depth 49.94 2024-11-06T03:02:05Z- handleDVL() - DVL msg epochMillisec: 1730862125.42 2024-11-06T03:02:05Z- handleDvl() - RDI_Pathfinder msg: 1730862125.42 sec; seqNo:577; #beams:4 2024-11-06T03:02:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 30.73 , 30.73 , 31.19 , 31.50 2024-11-06T03:02:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , 0.01 2024-11-06T03:02:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862125.43 epoch sec; seqNo:1896; 4061679.38 north; 594405.06 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:02:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862121.39 2024-11-06T03:02:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:05Z- waiting for 1730862125.39, time is 1730862125.43 2024-11-06T03:02:05Z- LcmTrn::updateTrn() >>>> pose time 1730862125.43, meas time 1730862125.42 <<<< 2024-11-06T03:02:05Z- LcmTrn::updateTrn() equating pose 1730862125.42 and meas times 1730862125.42 2024-11-06T03:02:05Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862125.42 2024-11-06T03:02:05Z- TerrainNav::motionUpdate() - currEstimate = (4061679, 594405.06, 49.94) 2024-11-06T03:02:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:02:05Z- TerrainNav:: SubcloudNIS is 0.97, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:02:05Z- TNavPF::Projecting Measurements 2024-11-06T03:02:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:02:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:02:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:02:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:02:05Z- TNavFilter::4 good beams. 2024-11-06T03:02:05Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:02:05Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:02:05Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:02:05Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:02:05Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:02:05Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:02:05Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:02:05Z- TerrainMapOctree: (vehN, vehE ) = (4061679.38, 594405.06). 2024-11-06T03:02:05Z- TerrainMapOctree: Min Distance = 170.39. 2024-11-06T03:02:05Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:02:05Z- Forcing Subcloud Comparison 2024-11-06T03:02:05Z- Weighting particles with subcloud comparison 2024-11-06T03:02:05Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:05Z- meanExpectedMeasDiff: -6.154950 delta_rms_squared: 0.946531 alpha: 0.979307 2024-11-06T03:02:05Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:05Z- meanExpectedMeasDiff: 0.343531 delta_rms_squared: 0.589918 alpha: 0.967209 2024-11-06T03:02:05Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:05Z- meanExpectedMeasDiff: -3.876338 delta_rms_squared: 0.791258 alpha: 0.975347 2024-11-06T03:02:05Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:05Z- meanExpectedMeasDiff: -1.193813 delta_rms_squared: 0.667912 alpha: 0.970927 2024-11-06T03:02:05Z- Subcloud NIS: 0.983900 2024-11-06T03:02:05Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:02:05Z- TNavPF:: sumWeights = 0.002342 2024-11-06T03:02:05Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:02:05Z- TNavPF::First Term of Map Covariance Matrix is 0.05 2024-11-06T03:02:05Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:02:05Z- TerrainNav::Resampling particle filter... 2024-11-06T03:02:05Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862125.42 sec, ping # 0. 2024-11-06T03:02:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:02:05Z- variances([0], [2], and psi_berg) = 0.522954 0.081473 0.000000 2024-11-06T03:02:05Z- publishEstimatesDecoder() - at 1730862125.42 sec 2024-11-06T03:02:05Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:02:05Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061711.74 594388.46 56.54 2024-11-06T03:02:05Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061711.73 594388.58 57.41 lat/lon: 36.70 -121.94 2024-11-06T03:02:05Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.52 0.08 0.28 0.00 2024-11-06T03:02:05Z- AHRS_M2 msg: 1730862125.79 epoch sec; seqNo:1779 2024-11-06T03:02:05Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 1.45 psi 2024-11-06T03:02:05Z- handleDepth()- Depth_Keller msg: 1730862125.80 epoch sec; seqNo:1897; depth 49.97 2024-11-06T03:02:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862125.81 epoch sec; seqNo:1897; 4061679.38 north; 594405.06 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:02:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862125.42 2024-11-06T03:02:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:05Z- waiting for 1730862129.42, time is 1730862125.81 2024-11-06T03:02:06Z- AHRS_M2 msg: 1730862126.19 epoch sec; seqNo:1780 2024-11-06T03:02:06Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 1.45 psi 2024-11-06T03:02:06Z- handleDepth()- Depth_Keller msg: 1730862126.20 epoch sec; seqNo:1898; depth 49.97 2024-11-06T03:02:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862126.23 epoch sec; seqNo:1898; 4061679.76 north; 594405.06 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:02:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862125.42 2024-11-06T03:02:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:06Z- waiting for 1730862129.42, time is 1730862126.23 2024-11-06T03:02:06Z- AHRS_M2 msg: 1730862126.60 epoch sec; seqNo:1781 2024-11-06T03:02:06Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 1.46 psi 2024-11-06T03:02:06Z- handleDepth()- Depth_Keller msg: 1730862126.61 epoch sec; seqNo:1899; depth 50.01 2024-11-06T03:02:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862126.62 epoch sec; seqNo:1899; 4061679.76 north; 594405.06 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:02:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862125.42 2024-11-06T03:02:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:06Z- waiting for 1730862129.42, time is 1730862126.62 2024-11-06T03:02:07Z- AHRS_M2 msg: 1730862127.00 epoch sec; seqNo:1782 2024-11-06T03:02:07Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.46 psi 2024-11-06T03:02:07Z- handleDepth()- Depth_Keller msg: 1730862127.00 epoch sec; seqNo:1900; depth 50.02 2024-11-06T03:02:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862127.01 epoch sec; seqNo:1900; 4061679.77 north; 594406.28 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:02:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862125.42 2024-11-06T03:02:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:07Z- waiting for 1730862129.42, time is 1730862127.01 2024-11-06T03:02:07Z- AHRS_M2 msg: 1730862127.41 epoch sec; seqNo:1783 2024-11-06T03:02:07Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.46 psi 2024-11-06T03:02:07Z- handleDepth()- Depth_Keller msg: 1730862127.42 epoch sec; seqNo:1901; depth 50.02 2024-11-06T03:02:07Z- handleDVL() - DVL msg epochMillisec: 1730862127.44 2024-11-06T03:02:07Z- handleDvl() - RDI_Pathfinder msg: 1730862127.44 sec; seqNo:578; #beams:4 2024-11-06T03:02:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.50 , 30.43 , 31.50 , 31.34 2024-11-06T03:02:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , 0.00 2024-11-06T03:02:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862127.47 epoch sec; seqNo:1901; 4061679.77 north; 594406.28 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:02:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862125.42 2024-11-06T03:02:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:07Z- waiting for 1730862129.42, time is 1730862127.47 2024-11-06T03:02:07Z- AHRS_M2 msg: 1730862127.81 epoch sec; seqNo:1784 2024-11-06T03:02:07Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.46 psi 2024-11-06T03:02:07Z- handleDepth()- Depth_Keller msg: 1730862127.81 epoch sec; seqNo:1902; depth 50.04 2024-11-06T03:02:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862127.84 epoch sec; seqNo:1902; 4061679.77 north; 594406.28 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:02:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862125.42 2024-11-06T03:02:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:07Z- waiting for 1730862129.42, time is 1730862127.84 2024-11-06T03:02:08Z- AHRS_M2 msg: 1730862128.21 epoch sec; seqNo:1785 2024-11-06T03:02:08Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.46 psi 2024-11-06T03:02:08Z- handleDepth()- Depth_Keller msg: 1730862128.22 epoch sec; seqNo:1903; depth 50.04 2024-11-06T03:02:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862128.23 epoch sec; seqNo:1903; 4061679.77 north; 594406.28 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:02:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862125.42 2024-11-06T03:02:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:08Z- waiting for 1730862129.42, time is 1730862128.23 2024-11-06T03:02:08Z- AHRS_M2 msg: 1730862128.63 epoch sec; seqNo:1786 2024-11-06T03:02:08Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.46 psi 2024-11-06T03:02:08Z- handleDepth()- Depth_Keller msg: 1730862128.64 epoch sec; seqNo:1904; depth 50.06 2024-11-06T03:02:08Z- handleDVL() - DVL msg epochMillisec: 1730862128.64 2024-11-06T03:02:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:08Z- handleDvl() - RDI_Pathfinder msg: 1730862128.64 sec; seqNo:579; #beams:0 2024-11-06T03:02:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , 0.00 2024-11-06T03:02:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862128.68 epoch sec; seqNo:1904; 4061679.78 north; 594407.50 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:02:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862125.42 2024-11-06T03:02:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:08Z- waiting for 1730862129.42, time is 1730862128.68 2024-11-06T03:02:09Z- AHRS_M2 msg: 1730862129.02 epoch sec; seqNo:1787 2024-11-06T03:02:09Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.46 psi 2024-11-06T03:02:09Z- handleDepth()- Depth_Keller msg: 1730862129.03 epoch sec; seqNo:1905; depth 50.08 2024-11-06T03:02:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862129.04 epoch sec; seqNo:1905; 4061679.78 north; 594407.50 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:02:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862125.42 2024-11-06T03:02:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:09Z- waiting for 1730862129.42, time is 1730862129.04 2024-11-06T03:02:09Z- AHRS_M2 msg: 1730862129.42 epoch sec; seqNo:1788 2024-11-06T03:02:09Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.46 psi 2024-11-06T03:02:09Z- handleDepth()- Depth_Keller msg: 1730862129.43 epoch sec; seqNo:1906; depth 50.08 2024-11-06T03:02:09Z- handleDVL() - DVL msg epochMillisec: 1730862129.45 2024-11-06T03:02:09Z- handleDvl() - RDI_Pathfinder msg: 1730862129.45 sec; seqNo:580; #beams:4 2024-11-06T03:02:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.87 , 30.27 , 31.50 , 31.04 2024-11-06T03:02:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.05 2024-11-06T03:02:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862129.50 epoch sec; seqNo:1906; 4061679.78 north; 594407.50 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:02:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862125.42 2024-11-06T03:02:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:09Z- waiting for 1730862129.42, time is 1730862129.50 2024-11-06T03:02:09Z- LcmTrn::updateTrn() >>>> pose time 1730862129.50, meas time 1730862129.45 <<<< 2024-11-06T03:02:09Z- LcmTrn::updateTrn() equating pose 1730862129.45 and meas times 1730862129.45 2024-11-06T03:02:09Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862129.45 2024-11-06T03:02:09Z- TerrainNav::motionUpdate() - currEstimate = (4061680, 594407.50, 50.08) 2024-11-06T03:02:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:02:09Z- TerrainNav:: SubcloudNIS is 0.98, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:02:09Z- TNavPF::Projecting Measurements 2024-11-06T03:02:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:02:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:02:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:02:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:02:09Z- TNavFilter::4 good beams. 2024-11-06T03:02:09Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:02:09Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:02:09Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:02:09Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:02:09Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:02:09Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:02:09Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:02:09Z- TerrainMapOctree: (vehN, vehE ) = (4061679.78, 594407.50). 2024-11-06T03:02:09Z- TerrainMapOctree: Min Distance = 172.44. 2024-11-06T03:02:09Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:02:09Z- Forcing Subcloud Comparison 2024-11-06T03:02:09Z- Weighting particles with subcloud comparison 2024-11-06T03:02:09Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:09Z- meanExpectedMeasDiff: -3.225231 delta_rms_squared: 0.830642 alpha: 0.976488 2024-11-06T03:02:09Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:09Z- meanExpectedMeasDiff: 1.150682 delta_rms_squared: 0.581054 alpha: 0.966725 2024-11-06T03:02:09Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:09Z- meanExpectedMeasDiff: -1.899195 delta_rms_squared: 0.701247 alpha: 0.972270 2024-11-06T03:02:09Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:09Z- meanExpectedMeasDiff: -1.305280 delta_rms_squared: 0.716584 alpha: 0.972848 2024-11-06T03:02:09Z- Subcloud NIS: 0.982163 2024-11-06T03:02:09Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:02:09Z- TNavPF:: sumWeights = 0.105294 2024-11-06T03:02:09Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:02:09Z- TNavPF::First Term of Map Covariance Matrix is 0.60 2024-11-06T03:02:09Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T03:02:09Z- TerrainNav::Resampling particle filter... 2024-11-06T03:02:09Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862129.45 sec, ping # 0. 2024-11-06T03:02:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:02:09Z- variances([0], [2], and psi_berg) = 0.528933 0.086118 0.000000 2024-11-06T03:02:09Z- publishEstimatesDecoder() - at 1730862129.45 sec 2024-11-06T03:02:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:02:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061712.16 594390.93 57.19 2024-11-06T03:02:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061712.15 594391.01 58.07 lat/lon: 36.70 -121.94 2024-11-06T03:02:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.53 0.09 0.28 0.00 2024-11-06T03:02:09Z- AHRS_M2 msg: 1730862129.83 epoch sec; seqNo:1789 2024-11-06T03:02:09Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.46 psi 2024-11-06T03:02:09Z- handleDepth()- Depth_Keller msg: 1730862129.84 epoch sec; seqNo:1907; depth 50.11 2024-11-06T03:02:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862129.86 epoch sec; seqNo:1907; 4061679.80 north; 594408.72 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:02:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862129.45 2024-11-06T03:02:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:09Z- waiting for 1730862133.45, time is 1730862129.86 2024-11-06T03:02:10Z- AHRS_M2 msg: 1730862130.23 epoch sec; seqNo:1790 2024-11-06T03:02:10Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.46 psi 2024-11-06T03:02:10Z- handleDepth()- Depth_Keller msg: 1730862130.24 epoch sec; seqNo:1908; depth 50.13 2024-11-06T03:02:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862130.25 epoch sec; seqNo:1908; 4061680.18 north; 594408.71 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:02:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862129.45 2024-11-06T03:02:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:10Z- waiting for 1730862133.45, time is 1730862130.25 2024-11-06T03:02:10Z- AHRS_M2 msg: 1730862130.65 epoch sec; seqNo:1791 2024-11-06T03:02:10Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.46 psi 2024-11-06T03:02:10Z- handleDepth()- Depth_Keller msg: 1730862130.67 epoch sec; seqNo:1909; depth 50.14 2024-11-06T03:02:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862130.67 epoch sec; seqNo:1909; 4061680.18 north; 594408.71 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:02:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862129.45 2024-11-06T03:02:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:10Z- waiting for 1730862133.45, time is 1730862130.67 2024-11-06T03:02:11Z- AHRS_M2 msg: 1730862131.04 epoch sec; seqNo:1792 2024-11-06T03:02:11Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.47 psi 2024-11-06T03:02:11Z- handleDepth()- Depth_Keller msg: 1730862131.04 epoch sec; seqNo:1910; depth 50.15 2024-11-06T03:02:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862131.05 epoch sec; seqNo:1910; 4061680.19 north; 594409.93 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:02:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862129.45 2024-11-06T03:02:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:11Z- waiting for 1730862133.45, time is 1730862131.05 2024-11-06T03:02:11Z- AHRS_M2 msg: 1730862131.44 epoch sec; seqNo:1793 2024-11-06T03:02:11Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T03:02:11Z- handleDepth()- Depth_Keller msg: 1730862131.45 epoch sec; seqNo:1911; depth 50.17 2024-11-06T03:02:11Z- handleDVL() - DVL msg epochMillisec: 1730862131.47 2024-11-06T03:02:11Z- handleDvl() - RDI_Pathfinder msg: 1730862131.47 sec; seqNo:581; #beams:4 2024-11-06T03:02:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.33 , 30.12 , 31.19 , 30.73 2024-11-06T03:02:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.08 2024-11-06T03:02:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862131.50 epoch sec; seqNo:1911; 4061680.19 north; 594409.93 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:02:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862129.45 2024-11-06T03:02:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:11Z- waiting for 1730862133.45, time is 1730862131.50 2024-11-06T03:02:11Z- AHRS_M2 msg: 1730862131.85 epoch sec; seqNo:1794 2024-11-06T03:02:11Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.47 psi 2024-11-06T03:02:11Z- handleDepth()- Depth_Keller msg: 1730862131.86 epoch sec; seqNo:1912; depth 50.20 2024-11-06T03:02:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862131.88 epoch sec; seqNo:1912; 4061680.19 north; 594409.93 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:02:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862129.45 2024-11-06T03:02:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:12Z- waiting for 1730862133.45, time is 1730862131.88 2024-11-06T03:02:12Z- AHRS_M2 msg: 1730862132.25 epoch sec; seqNo:1795 2024-11-06T03:02:12Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 1.47 psi 2024-11-06T03:02:12Z- handleDepth()- Depth_Keller msg: 1730862132.26 epoch sec; seqNo:1913; depth 50.21 2024-11-06T03:02:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862132.28 epoch sec; seqNo:1913; 4061680.19 north; 594409.93 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:02:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862129.45 2024-11-06T03:02:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:12Z- waiting for 1730862133.45, time is 1730862132.28 2024-11-06T03:02:12Z- AHRS_M2 msg: 1730862132.66 epoch sec; seqNo:1796 2024-11-06T03:02:12Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.47 psi 2024-11-06T03:02:12Z- handleDepth()- Depth_Keller msg: 1730862132.67 epoch sec; seqNo:1914; depth 50.22 2024-11-06T03:02:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862132.68 epoch sec; seqNo:1914; 4061680.20 north; 594411.15 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:02:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862129.45 2024-11-06T03:02:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:12Z- waiting for 1730862133.45, time is 1730862132.68 2024-11-06T03:02:13Z- AHRS_M2 msg: 1730862133.06 epoch sec; seqNo:1797 2024-11-06T03:02:13Z- AHRS_M2 msg: 0.18 phi; -0.02 theta; 1.47 psi 2024-11-06T03:02:13Z- handleDepth()- Depth_Keller msg: 1730862133.07 epoch sec; seqNo:1915; depth 50.24 2024-11-06T03:02:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862133.08 epoch sec; seqNo:1915; 4061680.20 north; 594411.15 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:02:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862129.45 2024-11-06T03:02:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:13Z- waiting for 1730862133.45, time is 1730862133.08 2024-11-06T03:02:13Z- AHRS_M2 msg: 1730862133.46 epoch sec; seqNo:1798 2024-11-06T03:02:13Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.47 psi 2024-11-06T03:02:13Z- handleDepth()- Depth_Keller msg: 1730862133.47 epoch sec; seqNo:1916; depth 50.25 2024-11-06T03:02:13Z- handleDVL() - DVL msg epochMillisec: 1730862133.49 2024-11-06T03:02:13Z- handleDvl() - RDI_Pathfinder msg: 1730862133.49 sec; seqNo:582; #beams:4 2024-11-06T03:02:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.79 , 29.36 , 31.04 , 31.04 2024-11-06T03:02:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , -0.05 2024-11-06T03:02:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862133.51 epoch sec; seqNo:1916; 4061680.20 north; 594411.15 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:02:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862129.45 2024-11-06T03:02:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:13Z- waiting for 1730862133.45, time is 1730862133.51 2024-11-06T03:02:13Z- LcmTrn::updateTrn() >>>> pose time 1730862133.51, meas time 1730862133.49 <<<< 2024-11-06T03:02:13Z- LcmTrn::updateTrn() equating pose 1730862133.49 and meas times 1730862133.49 2024-11-06T03:02:13Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862133.49 2024-11-06T03:02:13Z- TerrainNav::motionUpdate() - currEstimate = (4061680, 594411.15, 50.25) 2024-11-06T03:02:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:02:13Z- TerrainNav:: SubcloudNIS is 0.98, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:02:13Z- TNavPF::Projecting Measurements 2024-11-06T03:02:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:02:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:02:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:02:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:02:13Z- TNavFilter::4 good beams. 2024-11-06T03:02:13Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:02:13Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:02:13Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:02:13Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:02:13Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:02:13Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:02:13Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:02:13Z- TerrainMapOctree: (vehN, vehE ) = (4061680.20, 594411.15). 2024-11-06T03:02:13Z- TerrainMapOctree: Min Distance = 175.62. 2024-11-06T03:02:13Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:02:13Z- Forcing Subcloud Comparison 2024-11-06T03:02:13Z- Weighting particles with subcloud comparison 2024-11-06T03:02:13Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:13Z- meanExpectedMeasDiff: -1.914198 delta_rms_squared: 0.883269 alpha: 0.977858 2024-11-06T03:02:13Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:13Z- meanExpectedMeasDiff: 0.440475 delta_rms_squared: 0.679358 alpha: 0.971402 2024-11-06T03:02:13Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:13Z- meanExpectedMeasDiff: -1.956748 delta_rms_squared: 0.700136 alpha: 0.972227 2024-11-06T03:02:13Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:13Z- meanExpectedMeasDiff: -0.254758 delta_rms_squared: 0.729067 alpha: 0.973300 2024-11-06T03:02:13Z- Subcloud NIS: 0.968280 2024-11-06T03:02:13Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:02:13Z- TNavPF:: sumWeights = 0.306960 2024-11-06T03:02:13Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:02:13Z- TNavPF::First Term of Map Covariance Matrix is 0.44 2024-11-06T03:02:13Z- TNavPF::Calculated NIS Value : 0.19 numBeams normalized NIS: 0.05 2024-11-06T03:02:13Z- TerrainNav::Resampling particle filter... 2024-11-06T03:02:13Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862133.49 sec, ping # 0. 2024-11-06T03:02:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:02:13Z- variances([0], [2], and psi_berg) = 0.486903 0.085819 0.000000 2024-11-06T03:02:13Z- publishEstimatesDecoder() - at 1730862133.49 sec 2024-11-06T03:02:13Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:02:13Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061712.67 594394.98 59.24 2024-11-06T03:02:13Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061712.64 594394.67 58.63 lat/lon: 36.70 -121.94 2024-11-06T03:02:13Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.49 0.09 0.29 0.00 2024-11-06T03:02:13Z- AHRS_M2 msg: 1730862133.87 epoch sec; seqNo:1799 2024-11-06T03:02:13Z- AHRS_M2 msg: 0.17 phi; -0.01 theta; 1.47 psi 2024-11-06T03:02:13Z- handleDepth()- Depth_Keller msg: 1730862133.87 epoch sec; seqNo:1917; depth 50.25 2024-11-06T03:02:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862133.90 epoch sec; seqNo:1917; 4061680.59 north; 594412.37 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:02:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862133.49 2024-11-06T03:02:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:14Z- waiting for 1730862137.49, time is 1730862133.90 2024-11-06T03:02:14Z- AHRS_M2 msg: 1730862134.27 epoch sec; seqNo:1800 2024-11-06T03:02:14Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 1.47 psi 2024-11-06T03:02:14Z- handleDepth()- Depth_Keller msg: 1730862134.28 epoch sec; seqNo:1918; depth 50.29 2024-11-06T03:02:14Z- handleDVL() - DVL msg epochMillisec: 1730862134.32 2024-11-06T03:02:14Z- handleDvl() - RDI_Pathfinder msg: 1730862134.32 sec; seqNo:583; #beams:4 2024-11-06T03:02:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.79 , 29.36 , 31.19 , 31.34 2024-11-06T03:02:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , -0.02 2024-11-06T03:02:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862134.33 epoch sec; seqNo:1918; 4061680.59 north; 594412.37 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:02:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862133.49 2024-11-06T03:02:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:14Z- waiting for 1730862137.49, time is 1730862134.33 2024-11-06T03:02:14Z- AHRS_M2 msg: 1730862134.68 epoch sec; seqNo:1801 2024-11-06T03:02:14Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 1.47 psi 2024-11-06T03:02:14Z- handleDepth()- Depth_Keller msg: 1730862134.70 epoch sec; seqNo:1919; depth 50.29 2024-11-06T03:02:14Z- handleDVL() - DVL msg epochMillisec: 1730862134.70 2024-11-06T03:02:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:14Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:14Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:14Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:14Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:14Z- handleDvl() - RDI_Pathfinder msg: 1730862134.70 sec; seqNo:584; #beams:0 2024-11-06T03:02:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , -0.02 2024-11-06T03:02:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862134.71 epoch sec; seqNo:1919; 4061680.59 north; 594412.37 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:02:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862133.49 2024-11-06T03:02:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:14Z- waiting for 1730862137.49, time is 1730862134.71 2024-11-06T03:02:15Z- AHRS_M2 msg: 1730862135.08 epoch sec; seqNo:1802 2024-11-06T03:02:15Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 1.47 psi 2024-11-06T03:02:15Z- handleDepth()- Depth_Keller msg: 1730862135.09 epoch sec; seqNo:1920; depth 50.29 2024-11-06T03:02:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862135.10 epoch sec; seqNo:1920; 4061680.59 north; 594412.37 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:02:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862133.49 2024-11-06T03:02:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:15Z- waiting for 1730862137.49, time is 1730862135.10 2024-11-06T03:02:15Z- AHRS_M2 msg: 1730862135.48 epoch sec; seqNo:1803 2024-11-06T03:02:15Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.47 psi 2024-11-06T03:02:15Z- handleDepth()- Depth_Keller msg: 1730862135.49 epoch sec; seqNo:1921; depth 50.31 2024-11-06T03:02:15Z- handleDVL() - DVL msg epochMillisec: 1730862135.51 2024-11-06T03:02:15Z- handleDvl() - RDI_Pathfinder msg: 1730862135.51 sec; seqNo:585; #beams:4 2024-11-06T03:02:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.79 , 29.20 , 31.19 , 31.65 2024-11-06T03:02:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , 0.02 2024-11-06T03:02:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862135.52 epoch sec; seqNo:1921; 4061680.61 north; 594413.59 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:02:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862133.49 2024-11-06T03:02:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:15Z- waiting for 1730862137.49, time is 1730862135.52 2024-11-06T03:02:15Z- AHRS_M2 msg: 1730862135.89 epoch sec; seqNo:1804 2024-11-06T03:02:15Z- AHRS_M2 msg: 0.17 phi; 0.01 theta; 1.48 psi 2024-11-06T03:02:15Z- handleDepth()- Depth_Keller msg: 1730862135.90 epoch sec; seqNo:1922; depth 50.33 2024-11-06T03:02:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862135.91 epoch sec; seqNo:1922; 4061680.61 north; 594413.59 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:02:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862133.49 2024-11-06T03:02:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:16Z- waiting for 1730862137.49, time is 1730862135.91 2024-11-06T03:02:16Z- AHRS_M2 msg: 1730862136.29 epoch sec; seqNo:1805 2024-11-06T03:02:16Z- AHRS_M2 msg: 0.17 phi; 0.01 theta; 1.48 psi 2024-11-06T03:02:16Z- handleDepth()- Depth_Keller msg: 1730862136.30 epoch sec; seqNo:1923; depth 50.32 2024-11-06T03:02:16Z- handleDVL() - DVL msg epochMillisec: 1730862136.33 2024-11-06T03:02:16Z- handleDvl() - RDI_Pathfinder msg: 1730862136.33 sec; seqNo:586; #beams:4 2024-11-06T03:02:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.25 , 28.75 , 31.34 , 31.19 2024-11-06T03:02:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.04 2024-11-06T03:02:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862136.34 epoch sec; seqNo:1923; 4061680.61 north; 594413.59 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:02:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862133.49 2024-11-06T03:02:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:16Z- waiting for 1730862137.49, time is 1730862136.34 2024-11-06T03:02:16Z- AHRS_M2 msg: 1730862136.70 epoch sec; seqNo:1806 2024-11-06T03:02:16Z- AHRS_M2 msg: 0.17 phi; 0.01 theta; 1.48 psi 2024-11-06T03:02:16Z- handleDepth()- Depth_Keller msg: 1730862136.70 epoch sec; seqNo:1924; depth 50.32 2024-11-06T03:02:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862136.72 epoch sec; seqNo:1924; 4061680.61 north; 594413.59 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:02:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862133.49 2024-11-06T03:02:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:16Z- waiting for 1730862137.49, time is 1730862136.72 2024-11-06T03:02:17Z- AHRS_M2 msg: 1730862137.10 epoch sec; seqNo:1807 2024-11-06T03:02:17Z- AHRS_M2 msg: 0.17 phi; 0.01 theta; 1.48 psi 2024-11-06T03:02:17Z- handleDepth()- Depth_Keller msg: 1730862137.12 epoch sec; seqNo:1925; depth 50.32 2024-11-06T03:02:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862137.13 epoch sec; seqNo:1925; 4061681.00 north; 594414.81 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:02:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862133.49 2024-11-06T03:02:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:17Z- waiting for 1730862137.49, time is 1730862137.13 2024-11-06T03:02:17Z- AHRS_M2 msg: 1730862137.50 epoch sec; seqNo:1808 2024-11-06T03:02:17Z- AHRS_M2 msg: 0.17 phi; 0.01 theta; 1.49 psi 2024-11-06T03:02:17Z- handleDepth()- Depth_Keller msg: 1730862137.51 epoch sec; seqNo:1926; depth 50.35 2024-11-06T03:02:17Z- handleDVL() - DVL msg epochMillisec: 1730862137.54 2024-11-06T03:02:17Z- handleDvl() - RDI_Pathfinder msg: 1730862137.54 sec; seqNo:587; #beams:4 2024-11-06T03:02:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.56 , 28.75 , 31.34 , 30.73 2024-11-06T03:02:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , 0.04 2024-11-06T03:02:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862137.55 epoch sec; seqNo:1926; 4061681.00 north; 594414.81 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:02:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862133.49 2024-11-06T03:02:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:17Z- waiting for 1730862137.49, time is 1730862137.55 2024-11-06T03:02:17Z- LcmTrn::updateTrn() >>>> pose time 1730862137.55, meas time 1730862137.54 <<<< 2024-11-06T03:02:17Z- LcmTrn::updateTrn() equating pose 1730862137.54 and meas times 1730862137.54 2024-11-06T03:02:17Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862137.54 2024-11-06T03:02:17Z- TerrainNav::motionUpdate() - currEstimate = (4061681, 594414.81, 50.35) 2024-11-06T03:02:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:02:17Z- TerrainNav:: SubcloudNIS is 0.97, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:02:17Z- TNavPF::Projecting Measurements 2024-11-06T03:02:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:02:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:02:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:02:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:02:17Z- TNavFilter::4 good beams. 2024-11-06T03:02:17Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:02:17Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:02:17Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:02:17Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:02:17Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:02:17Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:02:17Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:02:17Z- TerrainMapOctree: (vehN, vehE ) = (4061681.00, 594414.81). 2024-11-06T03:02:17Z- TerrainMapOctree: Min Distance = 178.67. 2024-11-06T03:02:17Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:02:17Z- Forcing Subcloud Comparison 2024-11-06T03:02:17Z- Weighting particles with subcloud comparison 2024-11-06T03:02:17Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:17Z- meanExpectedMeasDiff: -0.520028 delta_rms_squared: 0.899996 alpha: 0.978261 2024-11-06T03:02:17Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:17Z- meanExpectedMeasDiff: 0.005600 delta_rms_squared: 0.600940 alpha: 0.967791 2024-11-06T03:02:17Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:17Z- meanExpectedMeasDiff: -1.666770 delta_rms_squared: 0.733300 alpha: 0.973450 2024-11-06T03:02:17Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:17Z- meanExpectedMeasDiff: 1.374365 delta_rms_squared: 1.186079 alpha: 0.983417 2024-11-06T03:02:17Z- Subcloud NIS: 0.949868 2024-11-06T03:02:17Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:02:17Z- TNavPF:: sumWeights = 0.339047 2024-11-06T03:02:17Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:02:17Z- TNavPF::First Term of Map Covariance Matrix is 0.19 2024-11-06T03:02:17Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T03:02:17Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862137.54 sec, ping # 0. 2024-11-06T03:02:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:02:17Z- variances([0], [2], and psi_berg) = 0.536136 0.099461 0.000000 2024-11-06T03:02:17Z- publishEstimatesDecoder() - at 1730862137.54 sec 2024-11-06T03:02:17Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:02:17Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061711.18 594399.14 58.64 2024-11-06T03:02:17Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061713.34 594398.29 58.78 lat/lon: 36.70 -121.94 2024-11-06T03:02:17Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.54 0.10 0.28 0.00 2024-11-06T03:02:17Z- AHRS_M2 msg: 1730862137.91 epoch sec; seqNo:1809 2024-11-06T03:02:17Z- AHRS_M2 msg: 0.17 phi; 0.01 theta; 1.49 psi 2024-11-06T03:02:17Z- handleDepth()- Depth_Keller msg: 1730862137.92 epoch sec; seqNo:1927; depth 50.33 2024-11-06T03:02:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862137.94 epoch sec; seqNo:1927; 4061681.00 north; 594414.81 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:02:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862137.54 2024-11-06T03:02:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:18Z- waiting for 1730862141.54, time is 1730862137.94 2024-11-06T03:02:18Z- AHRS_M2 msg: 1730862138.31 epoch sec; seqNo:1810 2024-11-06T03:02:18Z- AHRS_M2 msg: 0.17 phi; 0.01 theta; 1.49 psi 2024-11-06T03:02:18Z- handleDepth()- Depth_Keller msg: 1730862138.32 epoch sec; seqNo:1928; depth 50.33 2024-11-06T03:02:18Z- handleDVL() - DVL msg epochMillisec: 1730862138.35 2024-11-06T03:02:18Z- handleDvl() - RDI_Pathfinder msg: 1730862138.35 sec; seqNo:588; #beams:4 2024-11-06T03:02:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.71 , 28.75 , 31.04 , 30.73 2024-11-06T03:02:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.02 2024-11-06T03:02:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862138.37 epoch sec; seqNo:1928; 4061681.01 north; 594416.03 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:02:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862137.54 2024-11-06T03:02:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:18Z- waiting for 1730862141.54, time is 1730862138.37 2024-11-06T03:02:18Z- AHRS_M2 msg: 1730862138.72 epoch sec; seqNo:1811 2024-11-06T03:02:18Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 1.49 psi 2024-11-06T03:02:18Z- handleDepth()- Depth_Keller msg: 1730862138.73 epoch sec; seqNo:1929; depth 50.34 2024-11-06T03:02:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862138.76 epoch sec; seqNo:1929; 4061681.01 north; 594416.03 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:02:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862137.54 2024-11-06T03:02:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:18Z- waiting for 1730862141.54, time is 1730862138.76 2024-11-06T03:02:19Z- handleDepth()- Depth_Keller msg: 1730862139.15 epoch sec; seqNo:1930; depth 50.34 2024-11-06T03:02:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862139.15 epoch sec; seqNo:1930; 4061681.01 north; 594416.03 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:02:19Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:02:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862137.54 2024-11-06T03:02:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:19Z- waiting for 1730862141.54, time is 1730862139.15 2024-11-06T03:02:19Z- handleDepth()- Depth_Keller msg: 1730862139.54 epoch sec; seqNo:1931; depth 50.34 2024-11-06T03:02:19Z- handleDVL() - DVL msg epochMillisec: 1730862139.57 2024-11-06T03:02:19Z- handleDvl() - RDI_Pathfinder msg: 1730862139.57 sec; seqNo:589; #beams:4 2024-11-06T03:02:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.71 , 29.05 , 31.04 , 30.58 2024-11-06T03:02:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , 0.00 2024-11-06T03:02:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862139.58 epoch sec; seqNo:1931; 4061681.01 north; 594416.03 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:02:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862137.54 2024-11-06T03:02:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:19Z- waiting for 1730862141.54, time is 1730862139.58 2024-11-06T03:02:19Z- AHRS_M2 msg: 1730862139.94 epoch sec; seqNo:1812 2024-11-06T03:02:19Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.50 psi 2024-11-06T03:02:19Z- handleDepth()- Depth_Keller msg: 1730862139.95 epoch sec; seqNo:1932; depth 50.34 2024-11-06T03:02:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862139.96 epoch sec; seqNo:1932; 4061681.03 north; 594417.25 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:02:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862137.54 2024-11-06T03:02:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:20Z- waiting for 1730862141.54, time is 1730862139.96 2024-11-06T03:02:20Z- AHRS_M2 msg: 1730862140.33 epoch sec; seqNo:1813 2024-11-06T03:02:20Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.51 psi 2024-11-06T03:02:20Z- handleDepth()- Depth_Keller msg: 1730862140.34 epoch sec; seqNo:1933; depth 50.32 2024-11-06T03:02:20Z- handleDVL() - DVL msg epochMillisec: 1730862140.37 2024-11-06T03:02:20Z- handleDvl() - RDI_Pathfinder msg: 1730862140.37 sec; seqNo:590; #beams:4 2024-11-06T03:02:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.94 , 29.36 , 31.19 , 30.43 2024-11-06T03:02:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , -0.03 2024-11-06T03:02:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862140.38 epoch sec; seqNo:1933; 4061681.03 north; 594417.25 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:02:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862137.54 2024-11-06T03:02:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:20Z- waiting for 1730862141.54, time is 1730862140.38 2024-11-06T03:02:20Z- AHRS_M2 msg: 1730862140.74 epoch sec; seqNo:1814 2024-11-06T03:02:20Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.51 psi 2024-11-06T03:02:20Z- handleDepth()- Depth_Keller msg: 1730862140.75 epoch sec; seqNo:1934; depth 50.32 2024-11-06T03:02:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862140.76 epoch sec; seqNo:1934; 4061681.03 north; 594417.25 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:02:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862137.54 2024-11-06T03:02:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:20Z- waiting for 1730862141.54, time is 1730862140.76 2024-11-06T03:02:21Z- AHRS_M2 msg: 1730862141.14 epoch sec; seqNo:1815 2024-11-06T03:02:21Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.51 psi 2024-11-06T03:02:21Z- handleDepth()- Depth_Keller msg: 1730862141.15 epoch sec; seqNo:1935; depth 50.31 2024-11-06T03:02:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862141.16 epoch sec; seqNo:1935; 4061681.04 north; 594418.47 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:02:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862137.54 2024-11-06T03:02:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:21Z- waiting for 1730862141.54, time is 1730862141.16 2024-11-06T03:02:21Z- AHRS_M2 msg: 1730862141.54 epoch sec; seqNo:1816 2024-11-06T03:02:21Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.51 psi 2024-11-06T03:02:21Z- handleDepth()- Depth_Keller msg: 1730862141.55 epoch sec; seqNo:1936; depth 50.30 2024-11-06T03:02:21Z- handleDVL() - DVL msg epochMillisec: 1730862141.57 2024-11-06T03:02:21Z- handleDvl() - RDI_Pathfinder msg: 1730862141.57 sec; seqNo:591; #beams:4 2024-11-06T03:02:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.56 , 29.05 , 31.34 , 30.43 2024-11-06T03:02:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.05 2024-11-06T03:02:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862141.59 epoch sec; seqNo:1936; 4061681.04 north; 594418.47 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:02:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862137.54 2024-11-06T03:02:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:21Z- waiting for 1730862141.54, time is 1730862141.59 2024-11-06T03:02:21Z- LcmTrn::updateTrn() >>>> pose time 1730862141.59, meas time 1730862141.57 <<<< 2024-11-06T03:02:21Z- LcmTrn::updateTrn() equating pose 1730862141.57 and meas times 1730862141.57 2024-11-06T03:02:21Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862141.57 2024-11-06T03:02:21Z- TerrainNav::motionUpdate() - currEstimate = (4061681, 594418.47, 50.30) 2024-11-06T03:02:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:02:21Z- TerrainNav:: SubcloudNIS is 0.95, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:02:21Z- TNavPF::Projecting Measurements 2024-11-06T03:02:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:02:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:02:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:02:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:02:21Z- TNavFilter::4 good beams. 2024-11-06T03:02:21Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:02:21Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:02:21Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:02:21Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:02:21Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:02:21Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:02:21Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:02:21Z- TerrainMapOctree: (vehN, vehE ) = (4061681.04, 594418.47). 2024-11-06T03:02:21Z- TerrainMapOctree: Min Distance = 182.03. 2024-11-06T03:02:21Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:02:21Z- Forcing Subcloud Comparison 2024-11-06T03:02:21Z- Weighting particles with subcloud comparison 2024-11-06T03:02:21Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:21Z- meanExpectedMeasDiff: -0.009035 delta_rms_squared: 0.871674 alpha: 0.977570 2024-11-06T03:02:21Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:21Z- meanExpectedMeasDiff: 0.129501 delta_rms_squared: 0.605684 alpha: 0.968035 2024-11-06T03:02:21Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:21Z- meanExpectedMeasDiff: -0.612045 delta_rms_squared: 0.741661 alpha: 0.973742 2024-11-06T03:02:21Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:21Z- meanExpectedMeasDiff: 1.689170 delta_rms_squared: 0.699263 alpha: 0.972194 2024-11-06T03:02:21Z- Subcloud NIS: 0.927306 2024-11-06T03:02:21Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:02:21Z- TNavPF:: sumWeights = 0.459944 2024-11-06T03:02:21Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:02:21Z- TNavPF::First Term of Map Covariance Matrix is 0.22 2024-11-06T03:02:21Z- TNavPF::Calculated NIS Value : 0.33 numBeams normalized NIS: 0.08 2024-11-06T03:02:21Z- TerrainNav::Resampling particle filter... 2024-11-06T03:02:21Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862141.57 sec, ping # 0. 2024-11-06T03:02:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:02:21Z- variances([0], [2], and psi_berg) = 0.537801 0.100508 0.000000 2024-11-06T03:02:21Z- publishEstimatesDecoder() - at 1730862141.57 sec 2024-11-06T03:02:21Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:02:21Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061713.54 594402.21 59.88 2024-11-06T03:02:21Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061713.38 594401.95 58.61 lat/lon: 36.70 -121.94 2024-11-06T03:02:21Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.54 0.10 0.29 0.00 2024-11-06T03:02:21Z- AHRS_M2 msg: 1730862141.95 epoch sec; seqNo:1817 2024-11-06T03:02:21Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.51 psi 2024-11-06T03:02:21Z- handleDepth()- Depth_Keller msg: 1730862141.95 epoch sec; seqNo:1937; depth 50.30 2024-11-06T03:02:21Z- handleDVL() - DVL msg epochMillisec: 1730862141.96 2024-11-06T03:02:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:21Z- handleDvl() - RDI_Pathfinder msg: 1730862141.96 sec; seqNo:592; #beams:0 2024-11-06T03:02:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.05 2024-11-06T03:02:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862141.99 epoch sec; seqNo:1937; 4061681.04 north; 594418.47 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:02:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862141.57 2024-11-06T03:02:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:22Z- waiting for 1730862145.57, time is 1730862141.99 2024-11-06T03:02:22Z- AHRS_M2 msg: 1730862142.35 epoch sec; seqNo:1818 2024-11-06T03:02:22Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T03:02:22Z- handleDepth()- Depth_Keller msg: 1730862142.36 epoch sec; seqNo:1938; depth 50.28 2024-11-06T03:02:22Z- handleDVL() - DVL msg epochMillisec: 1730862142.38 2024-11-06T03:02:22Z- handleDvl() - RDI_Pathfinder msg: 1730862142.38 sec; seqNo:593; #beams:4 2024-11-06T03:02:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.86 , 29.20 , 31.04 , 30.27 2024-11-06T03:02:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , -0.06 2024-11-06T03:02:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862142.43 epoch sec; seqNo:1938; 4061681.05 north; 594419.69 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:02:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862141.57 2024-11-06T03:02:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:22Z- waiting for 1730862145.57, time is 1730862142.43 2024-11-06T03:02:22Z- AHRS_M2 msg: 1730862142.76 epoch sec; seqNo:1819 2024-11-06T03:02:22Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.50 psi 2024-11-06T03:02:22Z- handleDepth()- Depth_Keller msg: 1730862142.76 epoch sec; seqNo:1939; depth 50.27 2024-11-06T03:02:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862142.78 epoch sec; seqNo:1939; 4061681.05 north; 594419.69 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:02:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862141.57 2024-11-06T03:02:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:22Z- waiting for 1730862145.57, time is 1730862142.78 2024-11-06T03:02:23Z- AHRS_M2 msg: 1730862143.16 epoch sec; seqNo:1820 2024-11-06T03:02:23Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 1.50 psi 2024-11-06T03:02:23Z- handleDepth()- Depth_Keller msg: 1730862143.17 epoch sec; seqNo:1940; depth 50.24 2024-11-06T03:02:23Z- handleDVL() - DVL msg epochMillisec: 1730862143.18 2024-11-06T03:02:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:23Z- handleDvl() - RDI_Pathfinder msg: 1730862143.18 sec; seqNo:594; #beams:0 2024-11-06T03:02:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , -0.06 2024-11-06T03:02:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862143.18 epoch sec; seqNo:1940; 4061681.05 north; 594419.69 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:02:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862141.57 2024-11-06T03:02:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:23Z- waiting for 1730862145.57, time is 1730862143.18 2024-11-06T03:02:23Z- AHRS_M2 msg: 1730862143.56 epoch sec; seqNo:1821 2024-11-06T03:02:23Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 1.50 psi 2024-11-06T03:02:23Z- handleDepth()- Depth_Keller msg: 1730862143.58 epoch sec; seqNo:1941; depth 50.23 2024-11-06T03:02:23Z- handleDVL() - DVL msg epochMillisec: 1730862143.60 2024-11-06T03:02:23Z- handleDvl() - RDI_Pathfinder msg: 1730862143.60 sec; seqNo:595; #beams:4 2024-11-06T03:02:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.40 , 29.20 , 31.34 , 30.12 2024-11-06T03:02:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , -0.05 2024-11-06T03:02:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862143.62 epoch sec; seqNo:1941; 4061681.05 north; 594419.69 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:02:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862141.57 2024-11-06T03:02:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:23Z- waiting for 1730862145.57, time is 1730862143.62 2024-11-06T03:02:23Z- AHRS_M2 msg: 1730862143.97 epoch sec; seqNo:1822 2024-11-06T03:02:23Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 1.50 psi 2024-11-06T03:02:23Z- handleDepth()- Depth_Keller msg: 1730862143.97 epoch sec; seqNo:1942; depth 50.23 2024-11-06T03:02:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862143.99 epoch sec; seqNo:1942; 4061681.07 north; 594420.91 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:02:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862141.57 2024-11-06T03:02:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:24Z- waiting for 1730862145.57, time is 1730862143.99 2024-11-06T03:02:24Z- AHRS_M2 msg: 1730862144.37 epoch sec; seqNo:1823 2024-11-06T03:02:24Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 1.50 psi 2024-11-06T03:02:24Z- handleDepth()- Depth_Keller msg: 1730862144.38 epoch sec; seqNo:1943; depth 50.21 2024-11-06T03:02:24Z- handleDVL() - DVL msg epochMillisec: 1730862144.40 2024-11-06T03:02:24Z- handleDvl() - RDI_Pathfinder msg: 1730862144.40 sec; seqNo:596; #beams:4 2024-11-06T03:02:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.79 , 29.66 , 31.96 , 30.12 2024-11-06T03:02:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , -0.03 2024-11-06T03:02:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862144.41 epoch sec; seqNo:1943; 4061681.07 north; 594420.91 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:02:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862141.57 2024-11-06T03:02:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:24Z- waiting for 1730862145.57, time is 1730862144.41 2024-11-06T03:02:24Z- AHRS_M2 msg: 1730862144.78 epoch sec; seqNo:1824 2024-11-06T03:02:24Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 1.49 psi 2024-11-06T03:02:24Z- handleDepth()- Depth_Keller msg: 1730862144.80 epoch sec; seqNo:1944; depth 50.18 2024-11-06T03:02:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862144.82 epoch sec; seqNo:1944; 4061681.07 north; 594420.91 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:02:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862141.57 2024-11-06T03:02:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:24Z- waiting for 1730862145.57, time is 1730862144.82 2024-11-06T03:02:25Z- AHRS_M2 msg: 1730862145.18 epoch sec; seqNo:1825 2024-11-06T03:02:25Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 1.49 psi 2024-11-06T03:02:25Z- handleDepth()- Depth_Keller msg: 1730862145.18 epoch sec; seqNo:1945; depth 50.18 2024-11-06T03:02:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862145.19 epoch sec; seqNo:1945; 4061681.45 north; 594420.90 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:02:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862141.57 2024-11-06T03:02:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:25Z- waiting for 1730862145.57, time is 1730862145.19 2024-11-06T03:02:25Z- AHRS_M2 msg: 1730862145.59 epoch sec; seqNo:1826 2024-11-06T03:02:25Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 1.49 psi 2024-11-06T03:02:25Z- handleDepth()- Depth_Keller msg: 1730862145.59 epoch sec; seqNo:1946; depth 50.17 2024-11-06T03:02:25Z- handleDVL() - DVL msg epochMillisec: 1730862145.62 2024-11-06T03:02:25Z- handleDvl() - RDI_Pathfinder msg: 1730862145.62 sec; seqNo:597; #beams:4 2024-11-06T03:02:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.94 , 29.66 , 31.80 , 30.12 2024-11-06T03:02:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.06 , -0.01 2024-11-06T03:02:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862145.65 epoch sec; seqNo:1946; 4061681.46 north; 594422.12 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:02:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862141.57 2024-11-06T03:02:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:25Z- waiting for 1730862145.57, time is 1730862145.65 2024-11-06T03:02:25Z- LcmTrn::updateTrn() >>>> pose time 1730862145.65, meas time 1730862145.62 <<<< 2024-11-06T03:02:25Z- LcmTrn::updateTrn() equating pose 1730862145.62 and meas times 1730862145.62 2024-11-06T03:02:25Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862145.62 2024-11-06T03:02:25Z- TerrainNav::motionUpdate() - currEstimate = (4061681, 594422.12, 50.17) 2024-11-06T03:02:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:02:25Z- TerrainNav:: SubcloudNIS is 0.93, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:02:25Z- TNavPF::Projecting Measurements 2024-11-06T03:02:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:02:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:02:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:02:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:02:25Z- TNavFilter::4 good beams. 2024-11-06T03:02:25Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:02:25Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:02:25Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:02:25Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:02:25Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:02:25Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:02:25Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:02:25Z- TerrainMapOctree: (vehN, vehE ) = (4061681.46, 594422.12). 2024-11-06T03:02:25Z- TerrainMapOctree: Min Distance = 185.27. 2024-11-06T03:02:25Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:02:25Z- Forcing Subcloud Comparison 2024-11-06T03:02:25Z- Weighting particles with subcloud comparison 2024-11-06T03:02:25Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:25Z- meanExpectedMeasDiff: 0.522208 delta_rms_squared: 0.943334 alpha: 0.979239 2024-11-06T03:02:25Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:25Z- meanExpectedMeasDiff: -0.845824 delta_rms_squared: 0.650443 alpha: 0.970169 2024-11-06T03:02:25Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:25Z- meanExpectedMeasDiff: -0.662541 delta_rms_squared: 1.002277 alpha: 0.980436 2024-11-06T03:02:25Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:25Z- meanExpectedMeasDiff: 1.452537 delta_rms_squared: 0.690472 alpha: 0.971850 2024-11-06T03:02:25Z- Subcloud NIS: 0.898928 2024-11-06T03:02:25Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:02:25Z- TNavPF:: sumWeights = 0.419275 2024-11-06T03:02:25Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:02:25Z- TNavPF::First Term of Map Covariance Matrix is 0.26 2024-11-06T03:02:25Z- TNavPF::Calculated NIS Value : 0.29 numBeams normalized NIS: 0.07 2024-11-06T03:02:25Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862145.62 sec, ping # 0. 2024-11-06T03:02:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:02:25Z- variances([0], [2], and psi_berg) = 0.505252 0.105824 0.000000 2024-11-06T03:02:25Z- publishEstimatesDecoder() - at 1730862145.62 sec 2024-11-06T03:02:25Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:02:25Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061714.03 594404.82 58.28 2024-11-06T03:02:25Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061713.76 594405.59 58.44 lat/lon: 36.70 -121.94 2024-11-06T03:02:25Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.51 0.11 0.30 0.00 2024-11-06T03:02:26Z- AHRS_M2 msg: 1730862146.05 epoch sec; seqNo:1827 2024-11-06T03:02:26Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 1.49 psi 2024-11-06T03:02:26Z- handleDepth()- Depth_Keller msg: 1730862146.06 epoch sec; seqNo:1947; depth 50.14 2024-11-06T03:02:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862146.07 epoch sec; seqNo:1947; 4061681.46 north; 594422.12 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:02:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862145.62 2024-11-06T03:02:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:26Z- waiting for 1730862149.62, time is 1730862146.07 2024-11-06T03:02:26Z- AHRS_M2 msg: 1730862146.39 epoch sec; seqNo:1828 2024-11-06T03:02:26Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.49 psi 2024-11-06T03:02:26Z- handleDepth()- Depth_Keller msg: 1730862146.40 epoch sec; seqNo:1948; depth 50.14 2024-11-06T03:02:26Z- handleDVL() - DVL msg epochMillisec: 1730862146.42 2024-11-06T03:02:26Z- handleDvl() - RDI_Pathfinder msg: 1730862146.42 sec; seqNo:598; #beams:4 2024-11-06T03:02:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.10 , 29.51 , 31.96 , 30.12 2024-11-06T03:02:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , 0.02 2024-11-06T03:02:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862146.43 epoch sec; seqNo:1948; 4061681.46 north; 594422.12 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:02:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862145.62 2024-11-06T03:02:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:26Z- waiting for 1730862149.62, time is 1730862146.43 2024-11-06T03:02:26Z- AHRS_M2 msg: 1730862146.80 epoch sec; seqNo:1829 2024-11-06T03:02:26Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.49 psi 2024-11-06T03:02:26Z- handleDepth()- Depth_Keller msg: 1730862146.80 epoch sec; seqNo:1949; depth 50.14 2024-11-06T03:02:26Z- handleDVL() - DVL msg epochMillisec: 1730862146.81 2024-11-06T03:02:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:26Z- handleDvl() - RDI_Pathfinder msg: 1730862146.81 sec; seqNo:599; #beams:0 2024-11-06T03:02:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , 0.02 2024-11-06T03:02:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862146.82 epoch sec; seqNo:1949; 4061681.47 north; 594423.34 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:02:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862145.62 2024-11-06T03:02:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:26Z- waiting for 1730862149.62, time is 1730862146.82 2024-11-06T03:02:27Z- AHRS_M2 msg: 1730862147.20 epoch sec; seqNo:1830 2024-11-06T03:02:27Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.49 psi 2024-11-06T03:02:27Z- handleDepth()- Depth_Keller msg: 1730862147.21 epoch sec; seqNo:1950; depth 50.13 2024-11-06T03:02:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862147.21 epoch sec; seqNo:1950; 4061681.47 north; 594423.34 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:02:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862145.62 2024-11-06T03:02:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:27Z- waiting for 1730862149.62, time is 1730862147.21 2024-11-06T03:02:27Z- handleDepth()- Depth_Keller msg: 1730862147.62 epoch sec; seqNo:1951; depth 50.12 2024-11-06T03:02:27Z- handleDVL() - DVL msg epochMillisec: 1730862147.64 2024-11-06T03:02:27Z- handleDvl() - RDI_Pathfinder msg: 1730862147.64 sec; seqNo:600; #beams:4 2024-11-06T03:02:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.56 , 29.66 , 32.57 , 30.12 2024-11-06T03:02:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.03 2024-11-06T03:02:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862147.67 epoch sec; seqNo:1951; 4061681.47 north; 594423.34 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:02:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862145.62 2024-11-06T03:02:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:27Z- waiting for 1730862149.62, time is 1730862147.67 2024-11-06T03:02:28Z- AHRS_M2 msg: 1730862148.01 epoch sec; seqNo:1831 2024-11-06T03:02:28Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.48 psi 2024-11-06T03:02:28Z- handleDepth()- Depth_Keller msg: 1730862148.02 epoch sec; seqNo:1952; depth 50.11 2024-11-06T03:02:28Z- handleDVL() - DVL msg epochMillisec: 1730862148.03 2024-11-06T03:02:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:28Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:28Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:28Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:28Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:28Z- handleDvl() - RDI_Pathfinder msg: 1730862148.03 sec; seqNo:601; #beams:0 2024-11-06T03:02:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.03 2024-11-06T03:02:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862148.05 epoch sec; seqNo:1952; 4061681.85 north; 594423.34 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:02:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862145.62 2024-11-06T03:02:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:28Z- waiting for 1730862149.62, time is 1730862148.05 2024-11-06T03:02:28Z- AHRS_M2 msg: 1730862148.41 epoch sec; seqNo:1832 2024-11-06T03:02:28Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.48 psi 2024-11-06T03:02:28Z- handleDepth()- Depth_Keller msg: 1730862148.42 epoch sec; seqNo:1953; depth 50.10 2024-11-06T03:02:28Z- handleDVL() - DVL msg epochMillisec: 1730862148.44 2024-11-06T03:02:28Z- handleDvl() - RDI_Pathfinder msg: 1730862148.44 sec; seqNo:602; #beams:4 2024-11-06T03:02:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.86 , 29.51 , 33.03 , 30.43 2024-11-06T03:02:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , 0.04 2024-11-06T03:02:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862148.46 epoch sec; seqNo:1953; 4061681.87 north; 594424.56 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:02:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862145.62 2024-11-06T03:02:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:28Z- waiting for 1730862149.62, time is 1730862148.46 2024-11-06T03:02:28Z- AHRS_M2 msg: 1730862148.82 epoch sec; seqNo:1833 2024-11-06T03:02:28Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.48 psi 2024-11-06T03:02:28Z- handleDepth()- Depth_Keller msg: 1730862148.82 epoch sec; seqNo:1954; depth 50.09 2024-11-06T03:02:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862148.84 epoch sec; seqNo:1954; 4061681.87 north; 594424.56 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:02:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862145.62 2024-11-06T03:02:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:28Z- waiting for 1730862149.62, time is 1730862148.84 2024-11-06T03:02:29Z- AHRS_M2 msg: 1730862149.22 epoch sec; seqNo:1834 2024-11-06T03:02:29Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.48 psi 2024-11-06T03:02:29Z- handleDepth()- Depth_Keller msg: 1730862149.23 epoch sec; seqNo:1955; depth 50.08 2024-11-06T03:02:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862149.24 epoch sec; seqNo:1955; 4061681.87 north; 594424.56 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:02:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862145.62 2024-11-06T03:02:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:29Z- waiting for 1730862149.62, time is 1730862149.24 2024-11-06T03:02:29Z- AHRS_M2 msg: 1730862149.62 epoch sec; seqNo:1835 2024-11-06T03:02:29Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 1.48 psi 2024-11-06T03:02:29Z- handleDepth()- Depth_Keller msg: 1730862149.63 epoch sec; seqNo:1956; depth 50.07 2024-11-06T03:02:29Z- handleDVL() - DVL msg epochMillisec: 1730862149.66 2024-11-06T03:02:29Z- handleDvl() - RDI_Pathfinder msg: 1730862149.66 sec; seqNo:603; #beams:4 2024-11-06T03:02:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.63 , 29.20 , 33.03 , 30.43 2024-11-06T03:02:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , 0.05 2024-11-06T03:02:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862149.68 epoch sec; seqNo:1956; 4061681.87 north; 594424.56 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:02:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862145.62 2024-11-06T03:02:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:29Z- waiting for 1730862149.62, time is 1730862149.68 2024-11-06T03:02:29Z- LcmTrn::updateTrn() >>>> pose time 1730862149.68, meas time 1730862149.66 <<<< 2024-11-06T03:02:29Z- LcmTrn::updateTrn() equating pose 1730862149.66 and meas times 1730862149.66 2024-11-06T03:02:29Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862149.66 2024-11-06T03:02:29Z- TerrainNav::motionUpdate() - currEstimate = (4061682, 594424.56, 50.07) 2024-11-06T03:02:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:02:29Z- TerrainNav:: SubcloudNIS is 0.90, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:02:29Z- TNavPF::Projecting Measurements 2024-11-06T03:02:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:02:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:02:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:02:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:02:29Z- TNavFilter::4 good beams. 2024-11-06T03:02:29Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:02:29Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:02:29Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:02:29Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:02:29Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:02:29Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:02:29Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:02:29Z- TerrainMapOctree: (vehN, vehE ) = (4061681.87, 594424.56). 2024-11-06T03:02:29Z- TerrainMapOctree: Min Distance = 187.39. 2024-11-06T03:02:29Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:02:29Z- Forcing Subcloud Comparison 2024-11-06T03:02:29Z- Weighting particles with subcloud comparison 2024-11-06T03:02:29Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:29Z- meanExpectedMeasDiff: 0.933054 delta_rms_squared: 0.827262 alpha: 0.976395 2024-11-06T03:02:29Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:29Z- meanExpectedMeasDiff: -1.386743 delta_rms_squared: 0.638309 alpha: 0.969619 2024-11-06T03:02:29Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:29Z- meanExpectedMeasDiff: 0.143457 delta_rms_squared: 0.656875 alpha: 0.970452 2024-11-06T03:02:29Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:29Z- meanExpectedMeasDiff: 1.416013 delta_rms_squared: 0.597344 alpha: 0.967603 2024-11-06T03:02:29Z- Subcloud NIS: 0.868736 2024-11-06T03:02:29Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:02:29Z- TNavPF:: sumWeights = 0.363645 2024-11-06T03:02:29Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:02:29Z- TNavPF::First Term of Map Covariance Matrix is 0.28 2024-11-06T03:02:29Z- TNavPF::Calculated NIS Value : 0.28 numBeams normalized NIS: 0.07 2024-11-06T03:02:29Z- TerrainNav::Resampling particle filter... 2024-11-06T03:02:29Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862149.66 sec, ping # 0. 2024-11-06T03:02:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:02:29Z- variances([0], [2], and psi_berg) = 0.566260 0.105844 0.000000 2024-11-06T03:02:29Z- publishEstimatesDecoder() - at 1730862149.66 sec 2024-11-06T03:02:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:02:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061714.29 594408.34 58.97 2024-11-06T03:02:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061714.13 594408.05 58.27 lat/lon: 36.70 -121.94 2024-11-06T03:02:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.11 0.33 0.00 2024-11-06T03:02:30Z- AHRS_M2 msg: 1730862150.03 epoch sec; seqNo:1836 2024-11-06T03:02:30Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 1.47 psi 2024-11-06T03:02:30Z- handleDepth()- Depth_Keller msg: 1730862150.03 epoch sec; seqNo:1957; depth 50.05 2024-11-06T03:02:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862150.05 epoch sec; seqNo:1957; 4061681.88 north; 594425.78 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:02:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862149.66 2024-11-06T03:02:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:30Z- waiting for 1730862153.66, time is 1730862150.05 2024-11-06T03:02:30Z- AHRS_M2 msg: 1730862150.44 epoch sec; seqNo:1837 2024-11-06T03:02:30Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 1.47 psi 2024-11-06T03:02:30Z- handleDepth()- Depth_Keller msg: 1730862150.44 epoch sec; seqNo:1958; depth 50.04 2024-11-06T03:02:30Z- handleDVL() - DVL msg epochMillisec: 1730862150.46 2024-11-06T03:02:30Z- handleDvl() - RDI_Pathfinder msg: 1730862150.46 sec; seqNo:604; #beams:4 2024-11-06T03:02:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 29.66 , 33.03 , 30.43 2024-11-06T03:02:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , 0.03 2024-11-06T03:02:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862150.48 epoch sec; seqNo:1958; 4061681.88 north; 594425.78 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:02:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862149.66 2024-11-06T03:02:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:30Z- waiting for 1730862153.66, time is 1730862150.48 2024-11-06T03:02:30Z- AHRS_M2 msg: 1730862150.84 epoch sec; seqNo:1838 2024-11-06T03:02:30Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 1.47 psi 2024-11-06T03:02:30Z- handleDepth()- Depth_Keller msg: 1730862150.84 epoch sec; seqNo:1959; depth 50.03 2024-11-06T03:02:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862150.86 epoch sec; seqNo:1959; 4061681.88 north; 594425.78 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:02:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862149.66 2024-11-06T03:02:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:30Z- waiting for 1730862153.66, time is 1730862150.86 2024-11-06T03:02:31Z- AHRS_M2 msg: 1730862151.24 epoch sec; seqNo:1839 2024-11-06T03:02:31Z- AHRS_M2 msg: 0.11 phi; 0.05 theta; 1.46 psi 2024-11-06T03:02:31Z- handleDepth()- Depth_Keller msg: 1730862151.24 epoch sec; seqNo:1960; depth 50.00 2024-11-06T03:02:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862151.25 epoch sec; seqNo:1960; 4061681.89 north; 594427.00 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:02:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862149.66 2024-11-06T03:02:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:31Z- waiting for 1730862153.66, time is 1730862151.25 2024-11-06T03:02:31Z- AHRS_M2 msg: 1730862151.65 epoch sec; seqNo:1840 2024-11-06T03:02:31Z- AHRS_M2 msg: 0.11 phi; 0.05 theta; 1.46 psi 2024-11-06T03:02:31Z- handleDepth()- Depth_Keller msg: 1730862151.66 epoch sec; seqNo:1961; depth 49.99 2024-11-06T03:02:31Z- handleDVL() - DVL msg epochMillisec: 1730862151.74 2024-11-06T03:02:31Z- handleDvl() - RDI_Pathfinder msg: 1730862151.74 sec; seqNo:605; #beams:4 2024-11-06T03:02:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.86 , 30.12 , 33.18 , 30.58 2024-11-06T03:02:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , 0.01 2024-11-06T03:02:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862151.75 epoch sec; seqNo:1961; 4061681.89 north; 594427.00 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:02:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862149.66 2024-11-06T03:02:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:31Z- waiting for 1730862153.66, time is 1730862151.75 2024-11-06T03:02:32Z- handleDepth()- Depth_Keller msg: 1730862152.07 epoch sec; seqNo:1962; depth 49.97 2024-11-06T03:02:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862152.09 epoch sec; seqNo:1962; 4061681.89 north; 594427.00 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:02:32Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:02:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862149.66 2024-11-06T03:02:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:32Z- waiting for 1730862153.66, time is 1730862152.09 2024-11-06T03:02:32Z- AHRS_M2 msg: 1730862152.45 epoch sec; seqNo:1841 2024-11-06T03:02:32Z- AHRS_M2 msg: 0.10 phi; 0.05 theta; 1.46 psi 2024-11-06T03:02:32Z- handleDepth()- Depth_Keller msg: 1730862152.46 epoch sec; seqNo:1963; depth 49.95 2024-11-06T03:02:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862152.47 epoch sec; seqNo:1963; 4061681.91 north; 594428.22 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:02:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862149.66 2024-11-06T03:02:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:32Z- waiting for 1730862153.66, time is 1730862152.47 2024-11-06T03:02:32Z- AHRS_M2 msg: 1730862152.85 epoch sec; seqNo:1842 2024-11-06T03:02:32Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 1.46 psi 2024-11-06T03:02:32Z- handleDepth()- Depth_Keller msg: 1730862152.86 epoch sec; seqNo:1964; depth 49.92 2024-11-06T03:02:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862152.87 epoch sec; seqNo:1964; 4061682.28 north; 594428.22 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:02:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862149.66 2024-11-06T03:02:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:33Z- waiting for 1730862153.66, time is 1730862152.87 2024-11-06T03:02:33Z- AHRS_M2 msg: 1730862153.26 epoch sec; seqNo:1843 2024-11-06T03:02:33Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 1.47 psi 2024-11-06T03:02:33Z- handleDepth()- Depth_Keller msg: 1730862153.27 epoch sec; seqNo:1965; depth 49.89 2024-11-06T03:02:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862153.28 epoch sec; seqNo:1965; 4061682.28 north; 594428.22 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:02:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862149.66 2024-11-06T03:02:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:33Z- waiting for 1730862153.66, time is 1730862153.28 2024-11-06T03:02:33Z- AHRS_M2 msg: 1730862153.66 epoch sec; seqNo:1844 2024-11-06T03:02:33Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 1.47 psi 2024-11-06T03:02:33Z- handleDepth()- Depth_Keller msg: 1730862153.67 epoch sec; seqNo:1966; depth 49.87 2024-11-06T03:02:33Z- handleDVL() - DVL msg epochMillisec: 1730862153.69 2024-11-06T03:02:33Z- handleDvl() - RDI_Pathfinder msg: 1730862153.69 sec; seqNo:606; #beams:4 2024-11-06T03:02:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.71 , 30.43 , 33.18 , 30.73 2024-11-06T03:02:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , -0.04 2024-11-06T03:02:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862153.71 epoch sec; seqNo:1966; 4061682.28 north; 594428.22 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:02:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862149.66 2024-11-06T03:02:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:33Z- waiting for 1730862153.66, time is 1730862153.71 2024-11-06T03:02:33Z- LcmTrn::updateTrn() >>>> pose time 1730862153.71, meas time 1730862153.69 <<<< 2024-11-06T03:02:33Z- LcmTrn::updateTrn() equating pose 1730862153.69 and meas times 1730862153.69 2024-11-06T03:02:33Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862153.69 2024-11-06T03:02:33Z- TerrainNav::motionUpdate() - currEstimate = (4061682, 594428.22, 49.87) 2024-11-06T03:02:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:02:33Z- TerrainNav:: SubcloudNIS is 0.87, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:02:33Z- TNavPF::Projecting Measurements 2024-11-06T03:02:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:02:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:02:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:02:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:02:33Z- TNavFilter::4 good beams. 2024-11-06T03:02:33Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:02:33Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:02:33Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:02:33Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:02:33Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:02:33Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:02:33Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:02:33Z- TerrainMapOctree: (vehN, vehE ) = (4061682.28, 594428.22). 2024-11-06T03:02:33Z- TerrainMapOctree: Min Distance = 190.65. 2024-11-06T03:02:33Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:02:33Z- Forcing Subcloud Comparison 2024-11-06T03:02:33Z- Weighting particles with subcloud comparison 2024-11-06T03:02:33Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:33Z- meanExpectedMeasDiff: -0.910403 delta_rms_squared: 0.882962 alpha: 0.977851 2024-11-06T03:02:33Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:33Z- meanExpectedMeasDiff: 0.290839 delta_rms_squared: 0.654023 alpha: 0.970327 2024-11-06T03:02:33Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:33Z- meanExpectedMeasDiff: 0.275574 delta_rms_squared: 0.824096 alpha: 0.976306 2024-11-06T03:02:33Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:33Z- meanExpectedMeasDiff: 0.527367 delta_rms_squared: 0.729630 alpha: 0.973320 2024-11-06T03:02:33Z- Subcloud NIS: 0.846998 2024-11-06T03:02:33Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:02:33Z- TNavPF:: sumWeights = 0.608450 2024-11-06T03:02:33Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:02:33Z- TNavPF::First Term of Map Covariance Matrix is 0.37 2024-11-06T03:02:33Z- TNavPF::Calculated NIS Value : 0.25 numBeams normalized NIS: 0.06 2024-11-06T03:02:33Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862153.69 sec, ping # 0. 2024-11-06T03:02:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:02:33Z- variances([0], [2], and psi_berg) = 0.559765 0.108530 0.000000 2024-11-06T03:02:33Z- publishEstimatesDecoder() - at 1730862153.69 sec 2024-11-06T03:02:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:02:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061715.92 594411.47 58.50 2024-11-06T03:02:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061714.55 594411.71 58.06 lat/lon: 36.70 -121.94 2024-11-06T03:02:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.56 0.11 0.31 0.00 2024-11-06T03:02:34Z- AHRS_M2 msg: 1730862154.07 epoch sec; seqNo:1845 2024-11-06T03:02:34Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 1.47 psi 2024-11-06T03:02:34Z- handleDepth()- Depth_Keller msg: 1730862154.09 epoch sec; seqNo:1967; depth 49.84 2024-11-06T03:02:34Z- handleDVL() - DVL msg epochMillisec: 1730862154.09 2024-11-06T03:02:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:34Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:34Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:34Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:34Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:34Z- handleDvl() - RDI_Pathfinder msg: 1730862154.09 sec; seqNo:607; #beams:0 2024-11-06T03:02:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , -0.04 2024-11-06T03:02:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862154.10 epoch sec; seqNo:1967; 4061682.30 north; 594429.44 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:02:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862153.69 2024-11-06T03:02:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:34Z- waiting for 1730862157.69, time is 1730862154.10 2024-11-06T03:02:34Z- AHRS_M2 msg: 1730862154.47 epoch sec; seqNo:1846 2024-11-06T03:02:34Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 1.47 psi 2024-11-06T03:02:34Z- handleDepth()- Depth_Keller msg: 1730862154.48 epoch sec; seqNo:1968; depth 49.82 2024-11-06T03:02:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862154.49 epoch sec; seqNo:1968; 4061682.30 north; 594429.44 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:02:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862153.69 2024-11-06T03:02:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:34Z- waiting for 1730862157.69, time is 1730862154.49 2024-11-06T03:02:34Z- AHRS_M2 msg: 1730862154.87 epoch sec; seqNo:1847 2024-11-06T03:02:34Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.47 psi 2024-11-06T03:02:34Z- handleDepth()- Depth_Keller msg: 1730862154.88 epoch sec; seqNo:1969; depth 49.80 2024-11-06T03:02:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862154.90 epoch sec; seqNo:1969; 4061682.30 north; 594429.44 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:02:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862153.69 2024-11-06T03:02:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:35Z- waiting for 1730862157.69, time is 1730862154.90 2024-11-06T03:02:35Z- handleDepth()- Depth_Keller msg: 1730862155.30 epoch sec; seqNo:1970; depth 49.77 2024-11-06T03:02:35Z- handleDVL() - DVL msg epochMillisec: 1730862155.30 2024-11-06T03:02:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:35Z- handleDvl() - RDI_Pathfinder msg: 1730862155.30 sec; seqNo:608; #beams:0 2024-11-06T03:02:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , -0.04 2024-11-06T03:02:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862155.31 epoch sec; seqNo:1970; 4061682.31 north; 594430.66 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:02:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862153.69 2024-11-06T03:02:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:35Z- waiting for 1730862157.69, time is 1730862155.31 2024-11-06T03:02:35Z- AHRS_M2 msg: 1730862155.69 epoch sec; seqNo:1848 2024-11-06T03:02:35Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.46 psi 2024-11-06T03:02:35Z- handleDepth()- Depth_Keller msg: 1730862155.69 epoch sec; seqNo:1971; depth 49.74 2024-11-06T03:02:35Z- handleDVL() - DVL msg epochMillisec: 1730862155.74 2024-11-06T03:02:35Z- handleDvl() - RDI_Pathfinder msg: 1730862155.74 sec; seqNo:609; #beams:4 2024-11-06T03:02:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.56 , 30.12 , 32.87 , 30.73 2024-11-06T03:02:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , -0.05 2024-11-06T03:02:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862155.76 epoch sec; seqNo:1971; 4061682.31 north; 594430.66 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:02:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862153.69 2024-11-06T03:02:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:35Z- waiting for 1730862157.69, time is 1730862155.76 2024-11-06T03:02:36Z- AHRS_M2 msg: 1730862156.09 epoch sec; seqNo:1849 2024-11-06T03:02:36Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.46 psi 2024-11-06T03:02:36Z- handleDepth()- Depth_Keller msg: 1730862156.11 epoch sec; seqNo:1972; depth 49.73 2024-11-06T03:02:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862156.13 epoch sec; seqNo:1972; 4061682.31 north; 594430.66 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:02:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862153.69 2024-11-06T03:02:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:36Z- waiting for 1730862157.69, time is 1730862156.13 2024-11-06T03:02:36Z- AHRS_M2 msg: 1730862156.49 epoch sec; seqNo:1850 2024-11-06T03:02:36Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 1.46 psi 2024-11-06T03:02:36Z- handleDepth()- Depth_Keller msg: 1730862156.50 epoch sec; seqNo:1973; depth 49.71 2024-11-06T03:02:36Z- handleDVL() - DVL msg epochMillisec: 1730862156.51 2024-11-06T03:02:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:36Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:36Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:36Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:36Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:36Z- handleDvl() - RDI_Pathfinder msg: 1730862156.51 sec; seqNo:610; #beams:0 2024-11-06T03:02:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , -0.05 2024-11-06T03:02:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862156.53 epoch sec; seqNo:1973; 4061682.31 north; 594430.66 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:02:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862153.69 2024-11-06T03:02:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:36Z- waiting for 1730862157.69, time is 1730862156.53 2024-11-06T03:02:36Z- AHRS_M2 msg: 1730862156.90 epoch sec; seqNo:1851 2024-11-06T03:02:36Z- AHRS_M2 msg: 0.11 phi; 0.03 theta; 1.45 psi 2024-11-06T03:02:36Z- handleDepth()- Depth_Keller msg: 1730862156.91 epoch sec; seqNo:1974; depth 49.68 2024-11-06T03:02:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862156.92 epoch sec; seqNo:1974; 4061682.33 north; 594431.88 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:02:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862153.69 2024-11-06T03:02:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:37Z- waiting for 1730862157.69, time is 1730862156.92 2024-11-06T03:02:37Z- AHRS_M2 msg: 1730862157.31 epoch sec; seqNo:1852 2024-11-06T03:02:37Z- AHRS_M2 msg: 0.11 phi; 0.03 theta; 1.45 psi 2024-11-06T03:02:37Z- handleDepth()- Depth_Keller msg: 1730862157.31 epoch sec; seqNo:1975; depth 49.67 2024-11-06T03:02:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862157.32 epoch sec; seqNo:1975; 4061682.33 north; 594431.88 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:02:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862153.69 2024-11-06T03:02:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:37Z- waiting for 1730862157.69, time is 1730862157.32 2024-11-06T03:02:37Z- AHRS_M2 msg: 1730862157.70 epoch sec; seqNo:1853 2024-11-06T03:02:37Z- AHRS_M2 msg: 0.11 phi; 0.03 theta; 1.45 psi 2024-11-06T03:02:37Z- handleDepth()- Depth_Keller msg: 1730862157.71 epoch sec; seqNo:1976; depth 49.66 2024-11-06T03:02:37Z- handleDVL() - DVL msg epochMillisec: 1730862157.74 2024-11-06T03:02:37Z- handleDvl() - RDI_Pathfinder msg: 1730862157.74 sec; seqNo:611; #beams:4 2024-11-06T03:02:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.71 , 30.12 , 32.72 , 30.89 2024-11-06T03:02:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.07 , -0.01 2024-11-06T03:02:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862157.75 epoch sec; seqNo:1976; 4061682.70 north; 594431.87 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:02:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862153.69 2024-11-06T03:02:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:37Z- waiting for 1730862157.69, time is 1730862157.75 2024-11-06T03:02:37Z- LcmTrn::updateTrn() >>>> pose time 1730862157.75, meas time 1730862157.74 <<<< 2024-11-06T03:02:37Z- LcmTrn::updateTrn() equating pose 1730862157.74 and meas times 1730862157.74 2024-11-06T03:02:37Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862157.74 2024-11-06T03:02:37Z- TerrainNav::motionUpdate() - currEstimate = (4061683, 594431.87, 49.66) 2024-11-06T03:02:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:02:37Z- TerrainNav:: SubcloudNIS is 0.85, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:02:37Z- TNavPF::Projecting Measurements 2024-11-06T03:02:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:02:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:02:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:02:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:02:37Z- TNavFilter::4 good beams. 2024-11-06T03:02:37Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:02:37Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:02:37Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:02:37Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:02:37Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:02:37Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:02:37Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:02:37Z- TerrainMapOctree: (vehN, vehE ) = (4061682.70, 594431.87). 2024-11-06T03:02:37Z- TerrainMapOctree: Min Distance = 193.93. 2024-11-06T03:02:37Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:02:37Z- Forcing Subcloud Comparison 2024-11-06T03:02:37Z- Weighting particles with subcloud comparison 2024-11-06T03:02:37Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:37Z- meanExpectedMeasDiff: -1.108945 delta_rms_squared: 0.852577 alpha: 0.977079 2024-11-06T03:02:37Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:37Z- meanExpectedMeasDiff: -0.278472 delta_rms_squared: 0.656819 alpha: 0.970450 2024-11-06T03:02:38Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:38Z- meanExpectedMeasDiff: -0.499202 delta_rms_squared: 1.456109 alpha: 0.986451 2024-11-06T03:02:38Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:38Z- meanExpectedMeasDiff: -0.934077 delta_rms_squared: 0.655881 alpha: 0.970409 2024-11-06T03:02:38Z- Subcloud NIS: 0.834725 2024-11-06T03:02:38Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:02:38Z- TNavPF:: sumWeights = 0.557911 2024-11-06T03:02:38Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:02:38Z- TNavPF::First Term of Map Covariance Matrix is 0.31 2024-11-06T03:02:38Z- TNavPF::Calculated NIS Value : 0.14 numBeams normalized NIS: 0.03 2024-11-06T03:02:38Z- TerrainNav::Resampling particle filter... 2024-11-06T03:02:38Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862157.74 sec, ping # 0. 2024-11-06T03:02:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:02:38Z- variances([0], [2], and psi_berg) = 0.573517 0.113593 0.000000 2024-11-06T03:02:38Z- publishEstimatesDecoder() - at 1730862157.74 sec 2024-11-06T03:02:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:02:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061714.98 594416.02 57.95 2024-11-06T03:02:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061714.93 594415.36 58.13 lat/lon: 36.70 -121.94 2024-11-06T03:02:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.11 0.29 0.00 2024-11-06T03:02:38Z- handleDepth()- Depth_Keller msg: 1730862158.13 epoch sec; seqNo:1977; depth 49.64 2024-11-06T03:02:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862158.15 epoch sec; seqNo:1977; 4061682.70 north; 594431.87 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:02:38Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:02:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862157.74 2024-11-06T03:02:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:38Z- waiting for 1730862161.74, time is 1730862158.15 2024-11-06T03:02:38Z- AHRS_M2 msg: 1730862158.51 epoch sec; seqNo:1854 2024-11-06T03:02:38Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.44 psi 2024-11-06T03:02:38Z- handleDepth()- Depth_Keller msg: 1730862158.52 epoch sec; seqNo:1978; depth 49.64 2024-11-06T03:02:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862158.53 epoch sec; seqNo:1978; 4061682.72 north; 594433.09 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:02:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862157.74 2024-11-06T03:02:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:38Z- waiting for 1730862161.74, time is 1730862158.53 2024-11-06T03:02:38Z- AHRS_M2 msg: 1730862158.92 epoch sec; seqNo:1855 2024-11-06T03:02:38Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.44 psi 2024-11-06T03:02:38Z- handleDepth()- Depth_Keller msg: 1730862158.92 epoch sec; seqNo:1979; depth 49.63 2024-11-06T03:02:38Z- handleDVL() - DVL msg epochMillisec: 1730862158.92 2024-11-06T03:02:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:38Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:38Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:38Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:38Z- handleDvl() - RDI_Pathfinder msg: 1730862158.92 sec; seqNo:612; #beams:0 2024-11-06T03:02:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.07 , -0.01 2024-11-06T03:02:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862158.93 epoch sec; seqNo:1979; 4061682.72 north; 594433.09 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:02:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862157.74 2024-11-06T03:02:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:39Z- waiting for 1730862161.74, time is 1730862158.93 2024-11-06T03:02:39Z- AHRS_M2 msg: 1730862159.32 epoch sec; seqNo:1856 2024-11-06T03:02:39Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.44 psi 2024-11-06T03:02:39Z- handleDepth()- Depth_Keller msg: 1730862159.33 epoch sec; seqNo:1980; depth 49.62 2024-11-06T03:02:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862159.34 epoch sec; seqNo:1980; 4061682.72 north; 594433.09 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:02:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862157.74 2024-11-06T03:02:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:39Z- waiting for 1730862161.74, time is 1730862159.34 2024-11-06T03:02:39Z- AHRS_M2 msg: 1730862159.73 epoch sec; seqNo:1857 2024-11-06T03:02:39Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.44 psi 2024-11-06T03:02:39Z- handleDepth()- Depth_Keller msg: 1730862159.73 epoch sec; seqNo:1981; depth 49.63 2024-11-06T03:02:39Z- handleDVL() - DVL msg epochMillisec: 1730862159.77 2024-11-06T03:02:39Z- handleDvl() - RDI_Pathfinder msg: 1730862159.77 sec; seqNo:613; #beams:4 2024-11-06T03:02:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.71 , 29.97 , 32.11 , 31.04 2024-11-06T03:02:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , 0.04 2024-11-06T03:02:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862159.79 epoch sec; seqNo:1981; 4061683.11 north; 594434.31 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:02:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862157.74 2024-11-06T03:02:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:39Z- waiting for 1730862161.74, time is 1730862159.79 2024-11-06T03:02:40Z- AHRS_M2 msg: 1730862160.13 epoch sec; seqNo:1858 2024-11-06T03:02:40Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.44 psi 2024-11-06T03:02:40Z- handleDepth()- Depth_Keller msg: 1730862160.14 epoch sec; seqNo:1982; depth 49.61 2024-11-06T03:02:40Z- handleDVL() - DVL msg epochMillisec: 1730862160.14 2024-11-06T03:02:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:40Z- handleDvl() - RDI_Pathfinder msg: 1730862160.14 sec; seqNo:614; #beams:0 2024-11-06T03:02:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.08 , 0.04 2024-11-06T03:02:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862160.16 epoch sec; seqNo:1982; 4061683.11 north; 594434.31 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:02:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862157.74 2024-11-06T03:02:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:40Z- waiting for 1730862161.74, time is 1730862160.16 2024-11-06T03:02:40Z- AHRS_M2 msg: 1730862160.54 epoch sec; seqNo:1859 2024-11-06T03:02:40Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.44 psi 2024-11-06T03:02:40Z- handleDepth()- Depth_Keller msg: 1730862160.54 epoch sec; seqNo:1983; depth 49.62 2024-11-06T03:02:40Z- handleDVL() - DVL msg epochMillisec: 1730862160.56 2024-11-06T03:02:40Z- handleDvl() - RDI_Pathfinder msg: 1730862160.56 sec; seqNo:615; #beams:4 2024-11-06T03:02:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.63 , 29.66 , 32.72 , 31.19 2024-11-06T03:02:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.07 , 0.05 2024-11-06T03:02:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862160.58 epoch sec; seqNo:1983; 4061683.11 north; 594434.31 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:02:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862157.74 2024-11-06T03:02:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:40Z- waiting for 1730862161.74, time is 1730862160.58 2024-11-06T03:02:40Z- AHRS_M2 msg: 1730862160.94 epoch sec; seqNo:1860 2024-11-06T03:02:40Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.44 psi 2024-11-06T03:02:40Z- handleDepth()- Depth_Keller msg: 1730862160.94 epoch sec; seqNo:1984; depth 49.61 2024-11-06T03:02:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862160.97 epoch sec; seqNo:1984; 4061683.11 north; 594434.31 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:02:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862157.74 2024-11-06T03:02:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:41Z- waiting for 1730862161.74, time is 1730862160.97 2024-11-06T03:02:41Z- AHRS_M2 msg: 1730862161.35 epoch sec; seqNo:1861 2024-11-06T03:02:41Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.44 psi 2024-11-06T03:02:41Z- handleDepth()- Depth_Keller msg: 1730862161.36 epoch sec; seqNo:1985; depth 49.60 2024-11-06T03:02:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862161.37 epoch sec; seqNo:1985; 4061683.12 north; 594435.53 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:02:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862157.74 2024-11-06T03:02:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:41Z- waiting for 1730862161.74, time is 1730862161.37 2024-11-06T03:02:41Z- AHRS_M2 msg: 1730862161.76 epoch sec; seqNo:1862 2024-11-06T03:02:41Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.44 psi 2024-11-06T03:02:41Z- handleDepth()- Depth_Keller msg: 1730862161.76 epoch sec; seqNo:1986; depth 49.60 2024-11-06T03:02:41Z- handleDVL() - DVL msg epochMillisec: 1730862161.78 2024-11-06T03:02:41Z- handleDvl() - RDI_Pathfinder msg: 1730862161.78 sec; seqNo:616; #beams:4 2024-11-06T03:02:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.78 , 29.97 , 32.72 , 31.50 2024-11-06T03:02:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.04 2024-11-06T03:02:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862161.80 epoch sec; seqNo:1986; 4061683.50 north; 594435.53 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:02:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862157.74 2024-11-06T03:02:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:41Z- waiting for 1730862161.74, time is 1730862161.80 2024-11-06T03:02:41Z- LcmTrn::updateTrn() >>>> pose time 1730862161.80, meas time 1730862161.78 <<<< 2024-11-06T03:02:41Z- LcmTrn::updateTrn() equating pose 1730862161.78 and meas times 1730862161.78 2024-11-06T03:02:41Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862161.78 2024-11-06T03:02:41Z- TerrainNav::motionUpdate() - currEstimate = (4061684, 594435.53, 49.60) 2024-11-06T03:02:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:02:41Z- TerrainNav:: SubcloudNIS is 0.83, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:02:41Z- TNavPF::Projecting Measurements 2024-11-06T03:02:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:02:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:02:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:02:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:02:41Z- TNavFilter::4 good beams. 2024-11-06T03:02:41Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:02:41Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:02:41Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:02:41Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:02:41Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:02:41Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:02:41Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:02:41Z- TerrainMapOctree: (vehN, vehE ) = (4061683.50, 594435.53). 2024-11-06T03:02:41Z- TerrainMapOctree: Min Distance = 197.08. 2024-11-06T03:02:41Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:02:42Z- Forcing Subcloud Comparison 2024-11-06T03:02:42Z- Weighting particles with subcloud comparison 2024-11-06T03:02:42Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:42Z- meanExpectedMeasDiff: -0.300585 delta_rms_squared: 0.855914 alpha: 0.977167 2024-11-06T03:02:42Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:42Z- meanExpectedMeasDiff: 0.071895 delta_rms_squared: 0.628411 alpha: 0.969155 2024-11-06T03:02:42Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:42Z- meanExpectedMeasDiff: -0.958400 delta_rms_squared: 0.748610 alpha: 0.973979 2024-11-06T03:02:42Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:42Z- meanExpectedMeasDiff: -0.735272 delta_rms_squared: 0.658879 alpha: 0.970540 2024-11-06T03:02:42Z- Subcloud NIS: 0.782266 2024-11-06T03:02:42Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:02:42Z- TNavPF:: sumWeights = 0.619141 2024-11-06T03:02:42Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:02:42Z- TNavPF::First Term of Map Covariance Matrix is 0.31 2024-11-06T03:02:42Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.05 2024-11-06T03:02:42Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862161.78 sec, ping # 0. 2024-11-06T03:02:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:02:42Z- variances([0], [2], and psi_berg) = 0.569705 0.116447 0.000000 2024-11-06T03:02:42Z- publishEstimatesDecoder() - at 1730862161.78 sec 2024-11-06T03:02:42Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:02:42Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061714.97 594419.30 58.39 2024-11-06T03:02:42Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061715.70 594419.00 58.24 lat/lon: 36.70 -121.94 2024-11-06T03:02:42Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.12 0.31 0.00 2024-11-06T03:02:42Z- AHRS_M2 msg: 1730862162.17 epoch sec; seqNo:1863 2024-11-06T03:02:42Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.44 psi 2024-11-06T03:02:42Z- handleDepth()- Depth_Keller msg: 1730862162.17 epoch sec; seqNo:1987; depth 49.58 2024-11-06T03:02:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862162.19 epoch sec; seqNo:1987; 4061683.50 north; 594435.53 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:02:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862161.78 2024-11-06T03:02:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:42Z- waiting for 1730862165.78, time is 1730862162.19 2024-11-06T03:02:42Z- handleDepth()- Depth_Keller msg: 1730862162.57 epoch sec; seqNo:1988; depth 49.58 2024-11-06T03:02:42Z- handleDVL() - DVL msg epochMillisec: 1730862162.58 2024-11-06T03:02:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:42Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:42Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:42Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:42Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:42Z- handleDvl() - RDI_Pathfinder msg: 1730862162.58 sec; seqNo:617; #beams:0 2024-11-06T03:02:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.04 2024-11-06T03:02:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862162.59 epoch sec; seqNo:1988; 4061683.50 north; 594435.53 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:02:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862161.78 2024-11-06T03:02:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:42Z- waiting for 1730862165.78, time is 1730862162.59 2024-11-06T03:02:42Z- AHRS_M2 msg: 1730862162.96 epoch sec; seqNo:1864 2024-11-06T03:02:42Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.45 psi 2024-11-06T03:02:42Z- handleDepth()- Depth_Keller msg: 1730862162.98 epoch sec; seqNo:1989; depth 49.58 2024-11-06T03:02:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862163.00 epoch sec; seqNo:1989; 4061683.52 north; 594436.75 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:02:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862161.78 2024-11-06T03:02:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:43Z- waiting for 1730862165.78, time is 1730862163.00 2024-11-06T03:02:43Z- AHRS_M2 msg: 1730862163.36 epoch sec; seqNo:1865 2024-11-06T03:02:43Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.45 psi 2024-11-06T03:02:43Z- handleDepth()- Depth_Keller msg: 1730862163.37 epoch sec; seqNo:1990; depth 49.58 2024-11-06T03:02:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862163.38 epoch sec; seqNo:1990; 4061683.52 north; 594436.75 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:02:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862161.78 2024-11-06T03:02:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:43Z- waiting for 1730862165.78, time is 1730862163.38 2024-11-06T03:02:43Z- AHRS_M2 msg: 1730862163.77 epoch sec; seqNo:1866 2024-11-06T03:02:43Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.46 psi 2024-11-06T03:02:43Z- handleDepth()- Depth_Keller msg: 1730862163.77 epoch sec; seqNo:1991; depth 49.58 2024-11-06T03:02:43Z- handleDVL() - DVL msg epochMillisec: 1730862163.79 2024-11-06T03:02:43Z- handleDvl() - RDI_Pathfinder msg: 1730862163.79 sec; seqNo:618; #beams:4 2024-11-06T03:02:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.63 , 29.36 , 33.03 , 31.80 2024-11-06T03:02:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , -0.01 2024-11-06T03:02:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862163.83 epoch sec; seqNo:1991; 4061683.52 north; 594436.75 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:02:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862161.78 2024-11-06T03:02:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:43Z- waiting for 1730862165.78, time is 1730862163.83 2024-11-06T03:02:44Z- AHRS_M2 msg: 1730862164.17 epoch sec; seqNo:1867 2024-11-06T03:02:44Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 1.46 psi 2024-11-06T03:02:44Z- handleDepth()- Depth_Keller msg: 1730862164.18 epoch sec; seqNo:1992; depth 49.56 2024-11-06T03:02:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862164.19 epoch sec; seqNo:1992; 4061683.53 north; 594437.97 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:02:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862161.78 2024-11-06T03:02:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:44Z- waiting for 1730862165.78, time is 1730862164.19 2024-11-06T03:02:44Z- AHRS_M2 msg: 1730862164.58 epoch sec; seqNo:1868 2024-11-06T03:02:44Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 1.46 psi 2024-11-06T03:02:44Z- handleDepth()- Depth_Keller msg: 1730862164.59 epoch sec; seqNo:1993; depth 49.58 2024-11-06T03:02:44Z- handleDVL() - DVL msg epochMillisec: 1730862164.61 2024-11-06T03:02:44Z- handleDvl() - RDI_Pathfinder msg: 1730862164.61 sec; seqNo:619; #beams:4 2024-11-06T03:02:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.08 , 29.20 , 32.57 , 31.80 2024-11-06T03:02:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , -0.03 2024-11-06T03:02:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862164.63 epoch sec; seqNo:1993; 4061683.53 north; 594437.97 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:02:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862161.78 2024-11-06T03:02:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:44Z- waiting for 1730862165.78, time is 1730862164.63 2024-11-06T03:02:44Z- AHRS_M2 msg: 1730862164.98 epoch sec; seqNo:1869 2024-11-06T03:02:44Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 1.46 psi 2024-11-06T03:02:44Z- handleDepth()- Depth_Keller msg: 1730862164.99 epoch sec; seqNo:1994; depth 49.57 2024-11-06T03:02:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862165.01 epoch sec; seqNo:1994; 4061683.91 north; 594437.96 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:02:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862161.78 2024-11-06T03:02:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:45Z- waiting for 1730862165.78, time is 1730862165.01 2024-11-06T03:02:45Z- AHRS_M2 msg: 1730862165.38 epoch sec; seqNo:1870 2024-11-06T03:02:45Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.47 psi 2024-11-06T03:02:45Z- handleDepth()- Depth_Keller msg: 1730862165.39 epoch sec; seqNo:1995; depth 49.56 2024-11-06T03:02:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862165.40 epoch sec; seqNo:1995; 4061683.92 north; 594439.18 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:02:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862161.78 2024-11-06T03:02:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:45Z- waiting for 1730862165.78, time is 1730862165.40 2024-11-06T03:02:45Z- AHRS_M2 msg: 1730862165.79 epoch sec; seqNo:1871 2024-11-06T03:02:45Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.47 psi 2024-11-06T03:02:45Z- handleDepth()- Depth_Keller msg: 1730862165.79 epoch sec; seqNo:1996; depth 49.56 2024-11-06T03:02:45Z- handleDVL() - DVL msg epochMillisec: 1730862165.82 2024-11-06T03:02:45Z- handleDvl() - RDI_Pathfinder msg: 1730862165.82 sec; seqNo:620; #beams:4 2024-11-06T03:02:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.70 , 28.90 , 32.42 , 31.96 2024-11-06T03:02:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , -0.04 2024-11-06T03:02:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862165.84 epoch sec; seqNo:1996; 4061683.92 north; 594439.18 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:02:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862161.78 2024-11-06T03:02:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:45Z- waiting for 1730862165.78, time is 1730862165.84 2024-11-06T03:02:45Z- LcmTrn::updateTrn() >>>> pose time 1730862165.84, meas time 1730862165.82 <<<< 2024-11-06T03:02:45Z- LcmTrn::updateTrn() equating pose 1730862165.82 and meas times 1730862165.82 2024-11-06T03:02:45Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862165.82 2024-11-06T03:02:45Z- TerrainNav::motionUpdate() - currEstimate = (4061684, 594439.18, 49.56) 2024-11-06T03:02:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:02:45Z- TerrainNav:: SubcloudNIS is 0.78, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:02:45Z- TNavPF::Projecting Measurements 2024-11-06T03:02:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:02:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:02:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:02:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:02:45Z- TNavFilter::4 good beams. 2024-11-06T03:02:45Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:02:45Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:02:45Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:02:45Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:02:45Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:02:45Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:02:45Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:02:45Z- TerrainMapOctree: (vehN, vehE ) = (4061683.92, 594439.18). 2024-11-06T03:02:45Z- TerrainMapOctree: Min Distance = 200.39. 2024-11-06T03:02:45Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:02:46Z- Forcing Subcloud Comparison 2024-11-06T03:02:46Z- Weighting particles with subcloud comparison 2024-11-06T03:02:46Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:46Z- meanExpectedMeasDiff: 0.286695 delta_rms_squared: 0.901183 alpha: 0.978289 2024-11-06T03:02:46Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:46Z- meanExpectedMeasDiff: -0.626405 delta_rms_squared: 0.633568 alpha: 0.969399 2024-11-06T03:02:46Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:46Z- meanExpectedMeasDiff: -0.614479 delta_rms_squared: 0.745611 alpha: 0.973877 2024-11-06T03:02:46Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:46Z- meanExpectedMeasDiff: -1.522463 delta_rms_squared: 0.760049 alpha: 0.974361 2024-11-06T03:02:46Z- Subcloud NIS: 0.735020 2024-11-06T03:02:46Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:02:46Z- TNavPF:: sumWeights = 0.513687 2024-11-06T03:02:46Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:02:46Z- TNavPF::First Term of Map Covariance Matrix is 0.29 2024-11-06T03:02:46Z- TNavPF::Calculated NIS Value : 0.26 numBeams normalized NIS: 0.07 2024-11-06T03:02:46Z- TerrainNav::Resampling particle filter... 2024-11-06T03:02:46Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862165.82 sec, ping # 0. 2024-11-06T03:02:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:02:46Z- variances([0], [2], and psi_berg) = 0.569600 0.117760 0.000000 2024-11-06T03:02:46Z- publishEstimatesDecoder() - at 1730862165.82 sec 2024-11-06T03:02:46Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:02:46Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061716.28 594423.18 58.54 2024-11-06T03:02:46Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061716.13 594422.66 58.46 lat/lon: 36.70 -121.94 2024-11-06T03:02:46Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.12 0.30 0.00 2024-11-06T03:02:46Z- AHRS_M2 msg: 1730862166.19 epoch sec; seqNo:1872 2024-11-06T03:02:46Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.47 psi 2024-11-06T03:02:46Z- handleDepth()- Depth_Keller msg: 1730862166.20 epoch sec; seqNo:1997; depth 49.55 2024-11-06T03:02:46Z- handleDVL() - DVL msg epochMillisec: 1730862166.21 2024-11-06T03:02:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:46Z- handleDvl() - RDI_Pathfinder msg: 1730862166.21 sec; seqNo:621; #beams:0 2024-11-06T03:02:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , -0.04 2024-11-06T03:02:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862166.23 epoch sec; seqNo:1997; 4061683.92 north; 594439.18 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:02:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862165.82 2024-11-06T03:02:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:46Z- waiting for 1730862169.82, time is 1730862166.23 2024-11-06T03:02:46Z- AHRS_M2 msg: 1730862166.60 epoch sec; seqNo:1873 2024-11-06T03:02:46Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.47 psi 2024-11-06T03:02:46Z- handleDepth()- Depth_Keller msg: 1730862166.60 epoch sec; seqNo:1998; depth 49.55 2024-11-06T03:02:46Z- handleDVL() - DVL msg epochMillisec: 1730862166.63 2024-11-06T03:02:46Z- handleDvl() - RDI_Pathfinder msg: 1730862166.63 sec; seqNo:622; #beams:4 2024-11-06T03:02:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.24 , 29.20 , 32.11 , 32.11 2024-11-06T03:02:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , -0.03 2024-11-06T03:02:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862166.66 epoch sec; seqNo:1998; 4061683.92 north; 594439.18 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:02:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862165.82 2024-11-06T03:02:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:46Z- waiting for 1730862169.82, time is 1730862166.66 2024-11-06T03:02:47Z- AHRS_M2 msg: 1730862167.00 epoch sec; seqNo:1874 2024-11-06T03:02:47Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.47 psi 2024-11-06T03:02:47Z- handleDepth()- Depth_Keller msg: 1730862167.01 epoch sec; seqNo:1999; depth 49.55 2024-11-06T03:02:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862167.02 epoch sec; seqNo:1999; 4061683.93 north; 594440.40 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:02:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862165.82 2024-11-06T03:02:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:47Z- waiting for 1730862169.82, time is 1730862167.02 2024-11-06T03:02:47Z- handleDepth()- Depth_Keller msg: 1730862167.43 epoch sec; seqNo:2000; depth 49.54 2024-11-06T03:02:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862167.46 epoch sec; seqNo:2000; 4061683.93 north; 594440.40 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T03:02:47Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:02:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862165.82 2024-11-06T03:02:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:47Z- waiting for 1730862169.82, time is 1730862167.46 2024-11-06T03:02:47Z- AHRS_M2 msg: 1730862167.81 epoch sec; seqNo:1875 2024-11-06T03:02:47Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.47 psi 2024-11-06T03:02:47Z- handleDepth()- Depth_Keller msg: 1730862167.82 epoch sec; seqNo:2001; depth 49.55 2024-11-06T03:02:47Z- handleDVL() - DVL msg epochMillisec: 1730862167.85 2024-11-06T03:02:47Z- handleDvl() - RDI_Pathfinder msg: 1730862167.85 sec; seqNo:623; #beams:4 2024-11-06T03:02:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.24 , 29.36 , 32.26 , 32.26 2024-11-06T03:02:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , -0.02 2024-11-06T03:02:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862167.86 epoch sec; seqNo:2001; 4061683.93 north; 594440.40 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:02:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862165.82 2024-11-06T03:02:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:47Z- waiting for 1730862169.82, time is 1730862167.86 2024-11-06T03:02:48Z- AHRS_M2 msg: 1730862168.21 epoch sec; seqNo:1876 2024-11-06T03:02:48Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.47 psi 2024-11-06T03:02:48Z- handleDepth()- Depth_Keller msg: 1730862168.22 epoch sec; seqNo:2002; depth 49.55 2024-11-06T03:02:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862168.23 epoch sec; seqNo:2002; 4061683.95 north; 594441.62 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:02:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862165.82 2024-11-06T03:02:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:48Z- waiting for 1730862169.82, time is 1730862168.23 2024-11-06T03:02:48Z- handleDepth()- Depth_Keller msg: 1730862168.64 epoch sec; seqNo:2003; depth 49.56 2024-11-06T03:02:48Z- handleDVL() - DVL msg epochMillisec: 1730862168.67 2024-11-06T03:02:48Z- handleDvl() - RDI_Pathfinder msg: 1730862168.67 sec; seqNo:624; #beams:4 2024-11-06T03:02:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.93 , 29.20 , 32.11 , 32.26 2024-11-06T03:02:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , -0.01 2024-11-06T03:02:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862168.69 epoch sec; seqNo:2003; 4061683.95 north; 594441.62 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:02:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862165.82 2024-11-06T03:02:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:48Z- waiting for 1730862169.82, time is 1730862168.69 2024-11-06T03:02:49Z- AHRS_M2 msg: 1730862169.02 epoch sec; seqNo:1877 2024-11-06T03:02:49Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T03:02:49Z- handleDepth()- Depth_Keller msg: 1730862169.02 epoch sec; seqNo:2004; depth 49.56 2024-11-06T03:02:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862169.05 epoch sec; seqNo:2004; 4061683.95 north; 594441.62 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:02:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862165.82 2024-11-06T03:02:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:49Z- waiting for 1730862169.82, time is 1730862169.05 2024-11-06T03:02:49Z- AHRS_M2 msg: 1730862169.42 epoch sec; seqNo:1878 2024-11-06T03:02:49Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T03:02:49Z- handleDepth()- Depth_Keller msg: 1730862169.43 epoch sec; seqNo:2005; depth 49.56 2024-11-06T03:02:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862169.44 epoch sec; seqNo:2005; 4061683.95 north; 594441.62 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:02:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862165.82 2024-11-06T03:02:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:49Z- waiting for 1730862169.82, time is 1730862169.44 2024-11-06T03:02:49Z- handleDepth()- Depth_Keller msg: 1730862169.85 epoch sec; seqNo:2006; depth 49.58 2024-11-06T03:02:49Z- handleDVL() - DVL msg epochMillisec: 1730862169.87 2024-11-06T03:02:49Z- handleDvl() - RDI_Pathfinder msg: 1730862169.87 sec; seqNo:625; #beams:4 2024-11-06T03:02:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.63 , 29.20 , 31.96 , 32.57 2024-11-06T03:02:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.00 , -0.00 2024-11-06T03:02:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862169.88 epoch sec; seqNo:2006; 4061683.96 north; 594442.84 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:02:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862165.82 2024-11-06T03:02:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:50Z- waiting for 1730862169.82, time is 1730862169.88 2024-11-06T03:02:50Z- LcmTrn::updateTrn() >>>> pose time 1730862169.88, meas time 1730862169.87 <<<< 2024-11-06T03:02:50Z- LcmTrn::updateTrn() equating pose 1730862169.87 and meas times 1730862169.87 2024-11-06T03:02:50Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862169.87 2024-11-06T03:02:50Z- TerrainNav::motionUpdate() - currEstimate = (4061684, 594442.84, 49.58) 2024-11-06T03:02:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:02:50Z- TerrainNav:: SubcloudNIS is 0.74, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:02:50Z- TNavPF::Projecting Measurements 2024-11-06T03:02:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:02:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:02:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:02:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:02:50Z- TNavFilter::4 good beams. 2024-11-06T03:02:50Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:02:50Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:02:50Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:02:50Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:02:50Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:02:50Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:02:50Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:02:50Z- TerrainMapOctree: (vehN, vehE ) = (4061683.96, 594442.84). 2024-11-06T03:02:50Z- TerrainMapOctree: Min Distance = 203.84. 2024-11-06T03:02:50Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:02:50Z- Forcing Subcloud Comparison 2024-11-06T03:02:50Z- Weighting particles with subcloud comparison 2024-11-06T03:02:50Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:50Z- meanExpectedMeasDiff: -0.521582 delta_rms_squared: 0.897793 alpha: 0.978209 2024-11-06T03:02:50Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:50Z- meanExpectedMeasDiff: 0.109842 delta_rms_squared: 0.797417 alpha: 0.975533 2024-11-06T03:02:50Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:50Z- meanExpectedMeasDiff: -1.576588 delta_rms_squared: 0.772732 alpha: 0.974771 2024-11-06T03:02:50Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:50Z- meanExpectedMeasDiff: -0.966941 delta_rms_squared: 0.771311 alpha: 0.974725 2024-11-06T03:02:50Z- Subcloud NIS: 0.664811 2024-11-06T03:02:50Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:02:50Z- TNavPF:: sumWeights = 0.484847 2024-11-06T03:02:50Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:02:50Z- TNavPF::First Term of Map Covariance Matrix is 0.23 2024-11-06T03:02:50Z- TNavPF::Calculated NIS Value : 0.24 numBeams normalized NIS: 0.06 2024-11-06T03:02:50Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862169.87 sec, ping # 0. 2024-11-06T03:02:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:02:50Z- variances([0], [2], and psi_berg) = 0.568316 0.117668 0.000000 2024-11-06T03:02:50Z- publishEstimatesDecoder() - at 1730862169.87 sec 2024-11-06T03:02:50Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:02:50Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061714.46 594425.97 58.93 2024-11-06T03:02:50Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061716.17 594426.32 58.76 lat/lon: 36.70 -121.94 2024-11-06T03:02:50Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.12 0.29 0.00 2024-11-06T03:02:50Z- AHRS_M2 msg: 1730862170.24 epoch sec; seqNo:1879 2024-11-06T03:02:50Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T03:02:50Z- handleDepth()- Depth_Keller msg: 1730862170.25 epoch sec; seqNo:2007; depth 49.58 2024-11-06T03:02:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862170.27 epoch sec; seqNo:2007; 4061684.34 north; 594442.84 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:02:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862169.87 2024-11-06T03:02:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:50Z- waiting for 1730862173.87, time is 1730862170.27 2024-11-06T03:02:50Z- AHRS_M2 msg: 1730862170.64 epoch sec; seqNo:1880 2024-11-06T03:02:50Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.47 psi 2024-11-06T03:02:50Z- handleDepth()- Depth_Keller msg: 1730862170.65 epoch sec; seqNo:2008; depth 49.58 2024-11-06T03:02:50Z- handleDVL() - DVL msg epochMillisec: 1730862170.67 2024-11-06T03:02:50Z- handleDvl() - RDI_Pathfinder msg: 1730862170.67 sec; seqNo:626; #beams:4 2024-11-06T03:02:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.47 , 29.20 , 31.96 , 32.26 2024-11-06T03:02:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , 0.00 2024-11-06T03:02:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862170.70 epoch sec; seqNo:2008; 4061684.34 north; 594442.84 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:02:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862169.87 2024-11-06T03:02:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:50Z- waiting for 1730862173.87, time is 1730862170.70 2024-11-06T03:02:51Z- AHRS_M2 msg: 1730862171.04 epoch sec; seqNo:1881 2024-11-06T03:02:51Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.48 psi 2024-11-06T03:02:51Z- handleDepth()- Depth_Keller msg: 1730862171.04 epoch sec; seqNo:2009; depth 49.60 2024-11-06T03:02:51Z- handleDVL() - DVL msg epochMillisec: 1730862171.05 2024-11-06T03:02:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:51Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:51Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:51Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:51Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:51Z- handleDvl() - RDI_Pathfinder msg: 1730862171.05 sec; seqNo:627; #beams:0 2024-11-06T03:02:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , 0.00 2024-11-06T03:02:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862171.07 epoch sec; seqNo:2009; 4061684.35 north; 594444.06 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:02:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862169.87 2024-11-06T03:02:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:51Z- waiting for 1730862173.87, time is 1730862171.07 2024-11-06T03:02:51Z- AHRS_M2 msg: 1730862171.44 epoch sec; seqNo:1882 2024-11-06T03:02:51Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 1.48 psi 2024-11-06T03:02:51Z- handleDepth()- Depth_Keller msg: 1730862171.45 epoch sec; seqNo:2010; depth 49.59 2024-11-06T03:02:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862171.46 epoch sec; seqNo:2010; 4061684.35 north; 594444.06 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:02:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862169.87 2024-11-06T03:02:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:51Z- waiting for 1730862173.87, time is 1730862171.46 2024-11-06T03:02:51Z- AHRS_M2 msg: 1730862171.85 epoch sec; seqNo:1883 2024-11-06T03:02:51Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.48 psi 2024-11-06T03:02:51Z- handleDepth()- Depth_Keller msg: 1730862171.86 epoch sec; seqNo:2011; depth 49.63 2024-11-06T03:02:51Z- handleDVL() - DVL msg epochMillisec: 1730862171.87 2024-11-06T03:02:51Z- handleDvl() - RDI_Pathfinder msg: 1730862171.87 sec; seqNo:628; #beams:4 2024-11-06T03:02:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.17 , 29.05 , 31.65 , 32.42 2024-11-06T03:02:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.00 2024-11-06T03:02:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862171.91 epoch sec; seqNo:2011; 4061684.35 north; 594444.06 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:02:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862169.87 2024-11-06T03:02:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:52Z- waiting for 1730862173.87, time is 1730862171.91 2024-11-06T03:02:52Z- AHRS_M2 msg: 1730862172.25 epoch sec; seqNo:1884 2024-11-06T03:02:52Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.48 psi 2024-11-06T03:02:52Z- handleDepth()- Depth_Keller msg: 1730862172.26 epoch sec; seqNo:2012; depth 49.64 2024-11-06T03:02:52Z- handleDVL() - DVL msg epochMillisec: 1730862172.26 2024-11-06T03:02:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:52Z- handleDvl() - RDI_Pathfinder msg: 1730862172.26 sec; seqNo:629; #beams:0 2024-11-06T03:02:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.00 2024-11-06T03:02:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862172.27 epoch sec; seqNo:2012; 4061684.35 north; 594444.06 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:02:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862169.87 2024-11-06T03:02:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:52Z- waiting for 1730862173.87, time is 1730862172.27 2024-11-06T03:02:52Z- AHRS_M2 msg: 1730862172.66 epoch sec; seqNo:1885 2024-11-06T03:02:52Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.48 psi 2024-11-06T03:02:52Z- handleDepth()- Depth_Keller msg: 1730862172.66 epoch sec; seqNo:2013; depth 49.64 2024-11-06T03:02:52Z- handleDVL() - DVL msg epochMillisec: 1730862172.69 2024-11-06T03:02:52Z- handleDvl() - RDI_Pathfinder msg: 1730862172.69 sec; seqNo:630; #beams:4 2024-11-06T03:02:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 29.05 , 31.50 , 32.26 2024-11-06T03:02:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.02 2024-11-06T03:02:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862172.71 epoch sec; seqNo:2013; 4061684.37 north; 594445.28 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:02:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862169.87 2024-11-06T03:02:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:52Z- waiting for 1730862173.87, time is 1730862172.71 2024-11-06T03:02:53Z- handleDepth()- Depth_Keller msg: 1730862173.08 epoch sec; seqNo:2014; depth 49.65 2024-11-06T03:02:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862173.10 epoch sec; seqNo:2014; 4061684.37 north; 594445.28 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:02:53Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:02:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862169.87 2024-11-06T03:02:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:53Z- waiting for 1730862173.87, time is 1730862173.10 2024-11-06T03:02:53Z- AHRS_M2 msg: 1730862173.46 epoch sec; seqNo:1886 2024-11-06T03:02:53Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.48 psi 2024-11-06T03:02:53Z- handleDepth()- Depth_Keller msg: 1730862173.48 epoch sec; seqNo:2015; depth 49.68 2024-11-06T03:02:53Z- handleDVL() - DVL msg epochMillisec: 1730862173.48 2024-11-06T03:02:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:53Z- handleDvl() - RDI_Pathfinder msg: 1730862173.48 sec; seqNo:631; #beams:0 2024-11-06T03:02:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.02 2024-11-06T03:02:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862173.49 epoch sec; seqNo:2015; 4061684.37 north; 594445.28 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:02:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862169.87 2024-11-06T03:02:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:53Z- waiting for 1730862173.87, time is 1730862173.49 2024-11-06T03:02:53Z- AHRS_M2 msg: 1730862173.87 epoch sec; seqNo:1887 2024-11-06T03:02:53Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.48 psi 2024-11-06T03:02:53Z- handleDepth()- Depth_Keller msg: 1730862173.88 epoch sec; seqNo:2016; depth 49.70 2024-11-06T03:02:53Z- handleDVL() - DVL msg epochMillisec: 1730862173.90 2024-11-06T03:02:53Z- handleDvl() - RDI_Pathfinder msg: 1730862173.90 sec; seqNo:632; #beams:4 2024-11-06T03:02:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.78 , 29.05 , 31.34 , 32.57 2024-11-06T03:02:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , -0.03 2024-11-06T03:02:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862173.93 epoch sec; seqNo:2016; 4061684.38 north; 594446.50 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:02:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862169.87 2024-11-06T03:02:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:54Z- waiting for 1730862173.87, time is 1730862173.93 2024-11-06T03:02:54Z- LcmTrn::updateTrn() >>>> pose time 1730862173.93, meas time 1730862173.90 <<<< 2024-11-06T03:02:54Z- LcmTrn::updateTrn() equating pose 1730862173.90 and meas times 1730862173.90 2024-11-06T03:02:54Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862173.90 2024-11-06T03:02:54Z- TerrainNav::motionUpdate() - currEstimate = (4061684, 594446.50, 49.70) 2024-11-06T03:02:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:02:54Z- TerrainNav:: SubcloudNIS is 0.66, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:02:54Z- TNavPF::Projecting Measurements 2024-11-06T03:02:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:02:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:02:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:02:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:02:54Z- TNavFilter::4 good beams. 2024-11-06T03:02:54Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:02:54Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:02:54Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:02:54Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:02:54Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:02:54Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:02:54Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:02:54Z- TerrainMapOctree: (vehN, vehE ) = (4061684.38, 594446.50). 2024-11-06T03:02:54Z- TerrainMapOctree: Min Distance = 207.17. 2024-11-06T03:02:54Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:02:54Z- Forcing Subcloud Comparison 2024-11-06T03:02:54Z- Weighting particles with subcloud comparison 2024-11-06T03:02:54Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:54Z- meanExpectedMeasDiff: 0.518528 delta_rms_squared: 0.841157 alpha: 0.976775 2024-11-06T03:02:54Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:54Z- meanExpectedMeasDiff: 1.160927 delta_rms_squared: 0.621374 alpha: 0.968817 2024-11-06T03:02:54Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:54Z- meanExpectedMeasDiff: -0.260230 delta_rms_squared: 0.669252 alpha: 0.970983 2024-11-06T03:02:54Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:54Z- meanExpectedMeasDiff: -0.722683 delta_rms_squared: 0.764752 alpha: 0.974514 2024-11-06T03:02:54Z- Subcloud NIS: 0.575732 2024-11-06T03:02:54Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:02:54Z- TNavPF:: sumWeights = 0.532366 2024-11-06T03:02:54Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:02:54Z- TNavPF::First Term of Map Covariance Matrix is 0.28 2024-11-06T03:02:54Z- TNavPF::Calculated NIS Value : 0.34 numBeams normalized NIS: 0.09 2024-11-06T03:02:54Z- TerrainNav::Resampling particle filter... 2024-11-06T03:02:54Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862173.90 sec, ping # 0. 2024-11-06T03:02:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:02:54Z- variances([0], [2], and psi_berg) = 0.573030 0.120618 0.000000 2024-11-06T03:02:54Z- publishEstimatesDecoder() - at 1730862173.90 sec 2024-11-06T03:02:54Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:02:54Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061716.78 594430.53 59.11 2024-11-06T03:02:54Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061716.58 594429.98 58.82 lat/lon: 36.70 -121.94 2024-11-06T03:02:54Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.12 0.29 0.00 2024-11-06T03:02:54Z- AHRS_M2 msg: 1730862174.28 epoch sec; seqNo:1888 2024-11-06T03:02:54Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 1.48 psi 2024-11-06T03:02:54Z- handleDepth()- Depth_Keller msg: 1730862174.29 epoch sec; seqNo:2017; depth 49.72 2024-11-06T03:02:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862174.31 epoch sec; seqNo:2017; 4061684.76 north; 594446.50 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:02:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862173.90 2024-11-06T03:02:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:54Z- waiting for 1730862177.90, time is 1730862174.31 2024-11-06T03:02:54Z- AHRS_M2 msg: 1730862174.68 epoch sec; seqNo:1889 2024-11-06T03:02:54Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.48 psi 2024-11-06T03:02:54Z- handleDepth()- Depth_Keller msg: 1730862174.69 epoch sec; seqNo:2018; depth 49.74 2024-11-06T03:02:54Z- handleDVL() - DVL msg epochMillisec: 1730862174.71 2024-11-06T03:02:54Z- handleDvl() - RDI_Pathfinder msg: 1730862174.71 sec; seqNo:633; #beams:4 2024-11-06T03:02:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.78 , 29.05 , 31.65 , 32.42 2024-11-06T03:02:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , -0.04 2024-11-06T03:02:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862174.72 epoch sec; seqNo:2018; 4061684.76 north; 594446.50 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:02:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862173.90 2024-11-06T03:02:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:54Z- waiting for 1730862177.90, time is 1730862174.72 2024-11-06T03:02:55Z- AHRS_M2 msg: 1730862175.08 epoch sec; seqNo:1890 2024-11-06T03:02:55Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T03:02:55Z- handleDepth()- Depth_Keller msg: 1730862175.09 epoch sec; seqNo:2019; depth 49.76 2024-11-06T03:02:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862175.11 epoch sec; seqNo:2019; 4061684.76 north; 594446.50 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:02:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862173.90 2024-11-06T03:02:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:55Z- waiting for 1730862177.90, time is 1730862175.11 2024-11-06T03:02:55Z- AHRS_M2 msg: 1730862175.49 epoch sec; seqNo:1891 2024-11-06T03:02:55Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T03:02:55Z- handleDepth()- Depth_Keller msg: 1730862175.50 epoch sec; seqNo:2020; depth 49.79 2024-11-06T03:02:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862175.51 epoch sec; seqNo:2020; 4061684.77 north; 594447.72 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:02:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862173.90 2024-11-06T03:02:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:55Z- waiting for 1730862177.90, time is 1730862175.51 2024-11-06T03:02:55Z- AHRS_M2 msg: 1730862175.89 epoch sec; seqNo:1892 2024-11-06T03:02:55Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T03:02:55Z- handleDepth()- Depth_Keller msg: 1730862175.89 epoch sec; seqNo:2021; depth 49.82 2024-11-06T03:02:55Z- handleDVL() - DVL msg epochMillisec: 1730862175.93 2024-11-06T03:02:55Z- handleDvl() - RDI_Pathfinder msg: 1730862175.93 sec; seqNo:634; #beams:4 2024-11-06T03:02:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.63 , 28.75 , 31.34 , 32.42 2024-11-06T03:02:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.04 2024-11-06T03:02:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862175.94 epoch sec; seqNo:2021; 4061684.77 north; 594447.72 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:02:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862173.90 2024-11-06T03:02:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:56Z- waiting for 1730862177.90, time is 1730862175.94 2024-11-06T03:02:56Z- AHRS_M2 msg: 1730862176.29 epoch sec; seqNo:1893 2024-11-06T03:02:56Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T03:02:56Z- handleDepth()- Depth_Keller msg: 1730862176.30 epoch sec; seqNo:2022; depth 49.83 2024-11-06T03:02:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862176.33 epoch sec; seqNo:2022; 4061684.77 north; 594447.72 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:02:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862173.90 2024-11-06T03:02:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:56Z- waiting for 1730862177.90, time is 1730862176.33 2024-11-06T03:02:56Z- AHRS_M2 msg: 1730862176.70 epoch sec; seqNo:1894 2024-11-06T03:02:56Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T03:02:56Z- handleDepth()- Depth_Keller msg: 1730862176.71 epoch sec; seqNo:2023; depth 49.85 2024-11-06T03:02:56Z- handleDVL() - DVL msg epochMillisec: 1730862176.73 2024-11-06T03:02:56Z- handleDvl() - RDI_Pathfinder msg: 1730862176.73 sec; seqNo:635; #beams:4 2024-11-06T03:02:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.24 , 28.13 , 31.34 , 32.26 2024-11-06T03:02:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , -0.03 2024-11-06T03:02:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862176.76 epoch sec; seqNo:2023; 4061684.77 north; 594447.72 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:02:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862173.90 2024-11-06T03:02:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:56Z- waiting for 1730862177.90, time is 1730862176.76 2024-11-06T03:02:57Z- AHRS_M2 msg: 1730862177.10 epoch sec; seqNo:1895 2024-11-06T03:02:57Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T03:02:57Z- handleDepth()- Depth_Keller msg: 1730862177.13 epoch sec; seqNo:2024; depth 49.88 2024-11-06T03:02:57Z- handleDVL() - DVL msg epochMillisec: 1730862177.13 2024-11-06T03:02:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:57Z- handleDvl() - RDI_Pathfinder msg: 1730862177.13 sec; seqNo:636; #beams:0 2024-11-06T03:02:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , -0.03 2024-11-06T03:02:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862177.15 epoch sec; seqNo:2024; 4061684.79 north; 594448.94 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:02:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862173.90 2024-11-06T03:02:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:57Z- waiting for 1730862177.90, time is 1730862177.15 2024-11-06T03:02:57Z- AHRS_M2 msg: 1730862177.51 epoch sec; seqNo:1896 2024-11-06T03:02:57Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T03:02:57Z- handleDepth()- Depth_Keller msg: 1730862177.52 epoch sec; seqNo:2025; depth 49.89 2024-11-06T03:02:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862177.53 epoch sec; seqNo:2025; 4061684.79 north; 594448.94 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:02:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862173.90 2024-11-06T03:02:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:57Z- waiting for 1730862177.90, time is 1730862177.53 2024-11-06T03:02:57Z- AHRS_M2 msg: 1730862177.91 epoch sec; seqNo:1897 2024-11-06T03:02:57Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T03:02:57Z- handleDepth()- Depth_Keller msg: 1730862177.92 epoch sec; seqNo:2026; depth 49.93 2024-11-06T03:02:57Z- handleDVL() - DVL msg epochMillisec: 1730862177.94 2024-11-06T03:02:57Z- handleDvl() - RDI_Pathfinder msg: 1730862177.94 sec; seqNo:637; #beams:4 2024-11-06T03:02:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.39 , 27.83 , 31.19 , 32.26 2024-11-06T03:02:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.01 2024-11-06T03:02:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862177.96 epoch sec; seqNo:2026; 4061684.79 north; 594448.94 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:02:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862173.90 2024-11-06T03:02:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:58Z- waiting for 1730862177.90, time is 1730862177.96 2024-11-06T03:02:58Z- LcmTrn::updateTrn() >>>> pose time 1730862177.96, meas time 1730862177.94 <<<< 2024-11-06T03:02:58Z- LcmTrn::updateTrn() equating pose 1730862177.94 and meas times 1730862177.94 2024-11-06T03:02:58Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862177.94 2024-11-06T03:02:58Z- TerrainNav::motionUpdate() - currEstimate = (4061685, 594448.94, 49.93) 2024-11-06T03:02:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:02:58Z- TerrainNav:: SubcloudNIS is 0.58, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:02:58Z- TNavPF::Projecting Measurements 2024-11-06T03:02:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:02:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:02:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:02:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:02:58Z- TNavFilter::4 good beams. 2024-11-06T03:02:58Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:02:58Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:02:58Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:02:58Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:02:58Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:02:58Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:02:58Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:02:58Z- TerrainMapOctree: (vehN, vehE ) = (4061684.79, 594448.94). 2024-11-06T03:02:58Z- TerrainMapOctree: Min Distance = 209.35. 2024-11-06T03:02:58Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:02:58Z- Forcing Subcloud Comparison 2024-11-06T03:02:58Z- Weighting particles with subcloud comparison 2024-11-06T03:02:58Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:58Z- meanExpectedMeasDiff: 0.864981 delta_rms_squared: 0.883812 alpha: 0.977871 2024-11-06T03:02:58Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:58Z- meanExpectedMeasDiff: 0.316908 delta_rms_squared: 0.614755 alpha: 0.968492 2024-11-06T03:02:58Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:58Z- meanExpectedMeasDiff: -0.457649 delta_rms_squared: 0.811516 alpha: 0.975948 2024-11-06T03:02:58Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:02:58Z- meanExpectedMeasDiff: -0.770057 delta_rms_squared: 1.045325 alpha: 0.981226 2024-11-06T03:02:58Z- Subcloud NIS: 0.502754 2024-11-06T03:02:58Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:02:58Z- TNavPF:: sumWeights = 0.563745 2024-11-06T03:02:58Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:02:58Z- TNavPF::First Term of Map Covariance Matrix is 0.34 2024-11-06T03:02:58Z- TNavPF::Calculated NIS Value : 0.27 numBeams normalized NIS: 0.07 2024-11-06T03:02:58Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862177.94 sec, ping # 0. 2024-11-06T03:02:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:02:58Z- variances([0], [2], and psi_berg) = 0.542691 0.120575 0.000000 2024-11-06T03:02:58Z- publishEstimatesDecoder() - at 1730862177.94 sec 2024-11-06T03:02:58Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:02:58Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061717.14 594433.26 58.97 2024-11-06T03:02:58Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061717.02 594432.40 59.05 lat/lon: 36.70 -121.94 2024-11-06T03:02:58Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.54 0.12 0.28 0.00 2024-11-06T03:02:58Z- AHRS_M2 msg: 1730862178.34 epoch sec; seqNo:1898 2024-11-06T03:02:58Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T03:02:58Z- handleDepth()- Depth_Keller msg: 1730862178.35 epoch sec; seqNo:2027; depth 49.95 2024-11-06T03:02:58Z- handleDVL() - DVL msg epochMillisec: 1730862178.35 2024-11-06T03:02:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:02:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:02:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:02:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:02:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:02:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:02:58Z- handleDvl() - RDI_Pathfinder msg: 1730862178.35 sec; seqNo:638; #beams:0 2024-11-06T03:02:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:02:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.01 2024-11-06T03:02:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862178.37 epoch sec; seqNo:2027; 4061685.18 north; 594450.15 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:02:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862177.94 2024-11-06T03:02:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:58Z- waiting for 1730862181.94, time is 1730862178.37 2024-11-06T03:02:58Z- AHRS_M2 msg: 1730862178.73 epoch sec; seqNo:1899 2024-11-06T03:02:58Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T03:02:58Z- handleDepth()- Depth_Keller msg: 1730862178.74 epoch sec; seqNo:2028; depth 49.97 2024-11-06T03:02:58Z- handleDVL() - DVL msg epochMillisec: 1730862178.76 2024-11-06T03:02:58Z- handleDvl() - RDI_Pathfinder msg: 1730862178.76 sec; seqNo:639; #beams:4 2024-11-06T03:02:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.54 , 28.44 , 31.34 , 31.65 2024-11-06T03:02:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , 0.01 2024-11-06T03:02:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862178.77 epoch sec; seqNo:2028; 4061685.18 north; 594450.15 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:02:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:02:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862177.94 2024-11-06T03:02:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:58Z- waiting for 1730862181.94, time is 1730862178.77 2024-11-06T03:02:59Z- AHRS_M2 msg: 1730862179.13 epoch sec; seqNo:1900 2024-11-06T03:02:59Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.49 psi 2024-11-06T03:02:59Z- handleDepth()- Depth_Keller msg: 1730862179.13 epoch sec; seqNo:2029; depth 50.01 2024-11-06T03:02:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862179.17 epoch sec; seqNo:2029; 4061685.18 north; 594450.15 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:02:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:02:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862177.94 2024-11-06T03:02:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:59Z- waiting for 1730862181.94, time is 1730862179.17 2024-11-06T03:02:59Z- handleDepth()- Depth_Keller msg: 1730862179.56 epoch sec; seqNo:2030; depth 50.03 2024-11-06T03:02:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862179.58 epoch sec; seqNo:2030; 4061685.18 north; 594450.15 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:02:59Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:02:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862177.94 2024-11-06T03:02:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:02:59Z- waiting for 1730862181.94, time is 1730862179.58 2024-11-06T03:02:59Z- AHRS_M2 msg: 1730862179.94 epoch sec; seqNo:1901 2024-11-06T03:02:59Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.49 psi 2024-11-06T03:02:59Z- handleDepth()- Depth_Keller msg: 1730862179.95 epoch sec; seqNo:2031; depth 50.04 2024-11-06T03:02:59Z- handleDVL() - DVL msg epochMillisec: 1730862179.97 2024-11-06T03:02:59Z- handleDvl() - RDI_Pathfinder msg: 1730862179.97 sec; seqNo:640; #beams:4 2024-11-06T03:02:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.54 , 28.29 , 31.50 , 31.50 2024-11-06T03:02:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , 0.02 2024-11-06T03:03:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862180.00 epoch sec; seqNo:2031; 4061685.19 north; 594451.37 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:03:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862177.94 2024-11-06T03:03:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:00Z- waiting for 1730862181.94, time is 1730862180.00 2024-11-06T03:03:00Z- AHRS_M2 msg: 1730862180.34 epoch sec; seqNo:1902 2024-11-06T03:03:00Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.49 psi 2024-11-06T03:03:00Z- handleDepth()- Depth_Keller msg: 1730862180.35 epoch sec; seqNo:2032; depth 50.07 2024-11-06T03:03:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862180.37 epoch sec; seqNo:2032; 4061685.19 north; 594451.37 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:03:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862177.94 2024-11-06T03:03:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:00Z- waiting for 1730862181.94, time is 1730862180.37 2024-11-06T03:03:00Z- AHRS_M2 msg: 1730862180.76 epoch sec; seqNo:1903 2024-11-06T03:03:00Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.49 psi 2024-11-06T03:03:00Z- handleDepth()- Depth_Keller msg: 1730862180.77 epoch sec; seqNo:2033; depth 50.09 2024-11-06T03:03:00Z- handleDVL() - DVL msg epochMillisec: 1730862180.80 2024-11-06T03:03:00Z- handleDvl() - RDI_Pathfinder msg: 1730862180.80 sec; seqNo:641; #beams:4 2024-11-06T03:03:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.70 , 27.52 , 31.34 , 31.19 2024-11-06T03:03:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , 0.04 2024-11-06T03:03:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862180.81 epoch sec; seqNo:2033; 4061685.19 north; 594451.37 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:03:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862177.94 2024-11-06T03:03:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:00Z- waiting for 1730862181.94, time is 1730862180.81 2024-11-06T03:03:01Z- AHRS_M2 msg: 1730862181.15 epoch sec; seqNo:1904 2024-11-06T03:03:01Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.49 psi 2024-11-06T03:03:01Z- handleDepth()- Depth_Keller msg: 1730862181.15 epoch sec; seqNo:2034; depth 50.11 2024-11-06T03:03:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862181.17 epoch sec; seqNo:2034; 4061685.21 north; 594452.59 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:03:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862177.94 2024-11-06T03:03:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:01Z- waiting for 1730862181.94, time is 1730862181.17 2024-11-06T03:03:01Z- AHRS_M2 msg: 1730862181.55 epoch sec; seqNo:1905 2024-11-06T03:03:01Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.49 psi 2024-11-06T03:03:01Z- handleDepth()- Depth_Keller msg: 1730862181.58 epoch sec; seqNo:2035; depth 50.11 2024-11-06T03:03:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862181.59 epoch sec; seqNo:2035; 4061685.21 north; 594452.59 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:03:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862177.94 2024-11-06T03:03:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:01Z- waiting for 1730862181.94, time is 1730862181.59 2024-11-06T03:03:01Z- AHRS_M2 msg: 1730862181.95 epoch sec; seqNo:1906 2024-11-06T03:03:01Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T03:03:01Z- handleDepth()- Depth_Keller msg: 1730862181.97 epoch sec; seqNo:2036; depth 50.11 2024-11-06T03:03:02Z- handleDVL() - DVL msg epochMillisec: 1730862181.99 2024-11-06T03:03:02Z- handleDvl() - RDI_Pathfinder msg: 1730862181.99 sec; seqNo:642; #beams:4 2024-11-06T03:03:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.39 , 27.52 , 31.65 , 31.04 2024-11-06T03:03:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.05 , 0.03 2024-11-06T03:03:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862182.01 epoch sec; seqNo:2036; 4061685.21 north; 594452.59 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:03:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862177.94 2024-11-06T03:03:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:02Z- waiting for 1730862181.94, time is 1730862182.01 2024-11-06T03:03:02Z- LcmTrn::updateTrn() >>>> pose time 1730862182.01, meas time 1730862181.99 <<<< 2024-11-06T03:03:02Z- LcmTrn::updateTrn() equating pose 1730862181.99 and meas times 1730862181.99 2024-11-06T03:03:02Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862181.99 2024-11-06T03:03:02Z- TerrainNav::motionUpdate() - currEstimate = (4061685, 594452.59, 50.11) 2024-11-06T03:03:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:03:02Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:03:02Z- TNavPF::Projecting Measurements 2024-11-06T03:03:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:03:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:03:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:03:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:03:02Z- TNavFilter::4 good beams. 2024-11-06T03:03:02Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:03:02Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:03:02Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:03:02Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:03:02Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:03:02Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:03:02Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:03:02Z- TerrainMapOctree: (vehN, vehE ) = (4061685.21, 594452.59). 2024-11-06T03:03:02Z- TerrainMapOctree: Min Distance = 212.70. 2024-11-06T03:03:02Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:03:02Z- Forcing Subcloud Comparison 2024-11-06T03:03:02Z- Weighting particles with subcloud comparison 2024-11-06T03:03:02Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:02Z- meanExpectedMeasDiff: 1.628136 delta_rms_squared: 0.830364 alpha: 0.976481 2024-11-06T03:03:02Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:02Z- meanExpectedMeasDiff: 0.599289 delta_rms_squared: 0.657973 alpha: 0.970500 2024-11-06T03:03:02Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:02Z- meanExpectedMeasDiff: -1.176835 delta_rms_squared: 0.778079 alpha: 0.974940 2024-11-06T03:03:02Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:02Z- meanExpectedMeasDiff: -0.018691 delta_rms_squared: 0.703854 alpha: 0.972370 2024-11-06T03:03:02Z- Subcloud NIS: 0.421997 2024-11-06T03:03:02Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:03:02Z- TNavPF:: sumWeights = 0.402196 2024-11-06T03:03:02Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:03:02Z- TNavPF::First Term of Map Covariance Matrix is 0.53 2024-11-06T03:03:02Z- TNavPF::Calculated NIS Value : 0.26 numBeams normalized NIS: 0.07 2024-11-06T03:03:02Z- TerrainNav::Resampling particle filter... 2024-11-06T03:03:02Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862181.99 sec, ping # 0. 2024-11-06T03:03:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:03:02Z- variances([0], [2], and psi_berg) = 0.537736 0.118754 0.000000 2024-11-06T03:03:02Z- publishEstimatesDecoder() - at 1730862181.99 sec 2024-11-06T03:03:02Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:03:02Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061717.56 594436.61 58.46 2024-11-06T03:03:02Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061717.54 594436.07 59.15 lat/lon: 36.70 -121.94 2024-11-06T03:03:02Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.54 0.12 0.30 0.00 2024-11-06T03:03:02Z- AHRS_M2 msg: 1730862182.36 epoch sec; seqNo:1907 2024-11-06T03:03:02Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T03:03:02Z- handleDepth()- Depth_Keller msg: 1730862182.37 epoch sec; seqNo:2037; depth 50.13 2024-11-06T03:03:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862182.39 epoch sec; seqNo:2037; 4061685.22 north; 594453.81 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:03:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862181.99 2024-11-06T03:03:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:02Z- waiting for 1730862185.99, time is 1730862182.39 2024-11-06T03:03:02Z- AHRS_M2 msg: 1730862182.79 epoch sec; seqNo:1908 2024-11-06T03:03:02Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T03:03:02Z- handleDepth()- Depth_Keller msg: 1730862182.80 epoch sec; seqNo:2038; depth 50.14 2024-11-06T03:03:02Z- handleDVL() - DVL msg epochMillisec: 1730862182.82 2024-11-06T03:03:02Z- handleDvl() - RDI_Pathfinder msg: 1730862182.82 sec; seqNo:643; #beams:4 2024-11-06T03:03:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.08 , 27.37 , 31.50 , 30.58 2024-11-06T03:03:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , 0.01 2024-11-06T03:03:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862182.85 epoch sec; seqNo:2038; 4061685.22 north; 594453.81 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:03:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862181.99 2024-11-06T03:03:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:02Z- waiting for 1730862185.99, time is 1730862182.85 2024-11-06T03:03:03Z- AHRS_M2 msg: 1730862183.17 epoch sec; seqNo:1909 2024-11-06T03:03:03Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.49 psi 2024-11-06T03:03:03Z- handleDepth()- Depth_Keller msg: 1730862183.18 epoch sec; seqNo:2039; depth 50.15 2024-11-06T03:03:03Z- handleDVL() - DVL msg epochMillisec: 1730862183.18 2024-11-06T03:03:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:03:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:03:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:03:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:03:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:03:03Z- handleDvl() - RDI_Pathfinder msg: 1730862183.18 sec; seqNo:644; #beams:0 2024-11-06T03:03:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:03:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , 0.01 2024-11-06T03:03:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862183.21 epoch sec; seqNo:2039; 4061685.22 north; 594453.81 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:03:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862181.99 2024-11-06T03:03:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:03Z- waiting for 1730862185.99, time is 1730862183.21 2024-11-06T03:03:03Z- AHRS_M2 msg: 1730862183.57 epoch sec; seqNo:1910 2024-11-06T03:03:03Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.50 psi 2024-11-06T03:03:03Z- handleDepth()- Depth_Keller msg: 1730862183.59 epoch sec; seqNo:2040; depth 50.14 2024-11-06T03:03:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862183.60 epoch sec; seqNo:2040; 4061685.22 north; 594453.81 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:03:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862181.99 2024-11-06T03:03:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:03Z- waiting for 1730862185.99, time is 1730862183.60 2024-11-06T03:03:03Z- AHRS_M2 msg: 1730862183.97 epoch sec; seqNo:1911 2024-11-06T03:03:03Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.50 psi 2024-11-06T03:03:03Z- handleDepth()- Depth_Keller msg: 1730862183.99 epoch sec; seqNo:2041; depth 50.15 2024-11-06T03:03:04Z- handleDVL() - DVL msg epochMillisec: 1730862184.01 2024-11-06T03:03:04Z- handleDvl() - RDI_Pathfinder msg: 1730862184.01 sec; seqNo:645; #beams:4 2024-11-06T03:03:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.78 , 28.13 , 31.50 , 30.27 2024-11-06T03:03:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , -0.02 2024-11-06T03:03:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862184.02 epoch sec; seqNo:2041; 4061685.23 north; 594455.04 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:03:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862181.99 2024-11-06T03:03:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:04Z- waiting for 1730862185.99, time is 1730862184.02 2024-11-06T03:03:04Z- AHRS_M2 msg: 1730862184.38 epoch sec; seqNo:1912 2024-11-06T03:03:04Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.50 psi 2024-11-06T03:03:04Z- handleDepth()- Depth_Keller msg: 1730862184.39 epoch sec; seqNo:2042; depth 50.13 2024-11-06T03:03:04Z- handleDVL() - DVL msg epochMillisec: 1730862184.39 2024-11-06T03:03:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:03:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:03:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:03:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:03:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:03:04Z- handleDvl() - RDI_Pathfinder msg: 1730862184.39 sec; seqNo:646; #beams:0 2024-11-06T03:03:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:03:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , -0.02 2024-11-06T03:03:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862184.41 epoch sec; seqNo:2042; 4061685.23 north; 594455.04 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:03:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862181.99 2024-11-06T03:03:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:04Z- waiting for 1730862185.99, time is 1730862184.41 2024-11-06T03:03:04Z- AHRS_M2 msg: 1730862184.82 epoch sec; seqNo:1913 2024-11-06T03:03:04Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.50 psi 2024-11-06T03:03:04Z- handleDepth()- Depth_Keller msg: 1730862184.82 epoch sec; seqNo:2043; depth 50.14 2024-11-06T03:03:04Z- handleDVL() - DVL msg epochMillisec: 1730862184.85 2024-11-06T03:03:04Z- handleDvl() - RDI_Pathfinder msg: 1730862184.85 sec; seqNo:647; #beams:4 2024-11-06T03:03:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.78 , 27.98 , 31.80 , 30.73 2024-11-06T03:03:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.04 , -0.04 2024-11-06T03:03:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862184.88 epoch sec; seqNo:2043; 4061685.23 north; 594455.04 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:03:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862181.99 2024-11-06T03:03:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:04Z- waiting for 1730862185.99, time is 1730862184.88 2024-11-06T03:03:05Z- handleDepth()- Depth_Keller msg: 1730862185.21 epoch sec; seqNo:2044; depth 50.12 2024-11-06T03:03:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862185.22 epoch sec; seqNo:2044; 4061685.25 north; 594456.26 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:03:05Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:03:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862181.99 2024-11-06T03:03:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:05Z- waiting for 1730862185.99, time is 1730862185.22 2024-11-06T03:03:05Z- AHRS_M2 msg: 1730862185.59 epoch sec; seqNo:1914 2024-11-06T03:03:05Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.51 psi 2024-11-06T03:03:05Z- handleDepth()- Depth_Keller msg: 1730862185.61 epoch sec; seqNo:2045; depth 50.12 2024-11-06T03:03:05Z- handleDVL() - DVL msg epochMillisec: 1730862185.61 2024-11-06T03:03:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:03:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:03:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:03:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:03:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:03:05Z- handleDvl() - RDI_Pathfinder msg: 1730862185.61 sec; seqNo:648; #beams:0 2024-11-06T03:03:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:03:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.04 , -0.04 2024-11-06T03:03:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862185.62 epoch sec; seqNo:2045; 4061685.25 north; 594456.26 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:03:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862181.99 2024-11-06T03:03:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:05Z- waiting for 1730862185.99, time is 1730862185.62 2024-11-06T03:03:06Z- AHRS_M2 msg: 1730862186.01 epoch sec; seqNo:1915 2024-11-06T03:03:06Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.51 psi 2024-11-06T03:03:06Z- handleDepth()- Depth_Keller msg: 1730862186.01 epoch sec; seqNo:2046; depth 50.10 2024-11-06T03:03:06Z- handleDVL() - DVL msg epochMillisec: 1730862186.04 2024-11-06T03:03:06Z- handleDvl() - RDI_Pathfinder msg: 1730862186.04 sec; seqNo:649; #beams:4 2024-11-06T03:03:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.78 , 27.67 , 32.41 , 30.58 2024-11-06T03:03:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.01 , -0.06 2024-11-06T03:03:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862186.05 epoch sec; seqNo:2046; 4061685.25 north; 594456.26 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:03:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862181.99 2024-11-06T03:03:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:06Z- waiting for 1730862185.99, time is 1730862186.05 2024-11-06T03:03:06Z- LcmTrn::updateTrn() >>>> pose time 1730862186.05, meas time 1730862186.04 <<<< 2024-11-06T03:03:06Z- LcmTrn::updateTrn() equating pose 1730862186.04 and meas times 1730862186.04 2024-11-06T03:03:06Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862186.04 2024-11-06T03:03:06Z- TerrainNav::motionUpdate() - currEstimate = (4061685, 594456.26, 50.10) 2024-11-06T03:03:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:03:06Z- TerrainNav:: SubcloudNIS is 0.42, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:03:06Z- TNavPF::Projecting Measurements 2024-11-06T03:03:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:03:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:03:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:03:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:03:06Z- TNavFilter::4 good beams. 2024-11-06T03:03:06Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:03:06Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:03:06Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:03:06Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:03:06Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:03:06Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:03:06Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:03:06Z- TerrainMapOctree: (vehN, vehE ) = (4061685.25, 594456.26). 2024-11-06T03:03:06Z- TerrainMapOctree: Min Distance = 216.18. 2024-11-06T03:03:06Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:03:06Z- Forcing Subcloud Comparison 2024-11-06T03:03:06Z- Weighting particles with subcloud comparison 2024-11-06T03:03:06Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:06Z- meanExpectedMeasDiff: 1.021416 delta_rms_squared: 0.858582 alpha: 0.977236 2024-11-06T03:03:06Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:06Z- meanExpectedMeasDiff: -0.138761 delta_rms_squared: 0.632228 alpha: 0.969336 2024-11-06T03:03:06Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:06Z- meanExpectedMeasDiff: -1.247748 delta_rms_squared: 0.748660 alpha: 0.973981 2024-11-06T03:03:06Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:06Z- meanExpectedMeasDiff: -0.585113 delta_rms_squared: 1.553550 alpha: 0.987290 2024-11-06T03:03:06Z- Subcloud NIS: 0.378428 2024-11-06T03:03:06Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:03:06Z- TNavPF:: sumWeights = 0.442940 2024-11-06T03:03:06Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:03:06Z- TNavPF::First Term of Map Covariance Matrix is 0.39 2024-11-06T03:03:06Z- TNavPF::Calculated NIS Value : 0.24 numBeams normalized NIS: 0.06 2024-11-06T03:03:06Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862186.04 sec, ping # 0. 2024-11-06T03:03:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:03:06Z- variances([0], [2], and psi_berg) = 0.553240 0.125050 0.000000 2024-11-06T03:03:06Z- publishEstimatesDecoder() - at 1730862186.04 sec 2024-11-06T03:03:06Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:03:06Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061717.89 594439.86 59.21 2024-11-06T03:03:06Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061717.55 594439.74 59.23 lat/lon: 36.70 -121.94 2024-11-06T03:03:06Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.55 0.13 0.29 0.00 2024-11-06T03:03:06Z- AHRS_M2 msg: 1730862186.43 epoch sec; seqNo:1916 2024-11-06T03:03:06Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.51 psi 2024-11-06T03:03:06Z- handleDepth()- Depth_Keller msg: 1730862186.44 epoch sec; seqNo:2047; depth 50.08 2024-11-06T03:03:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862186.49 epoch sec; seqNo:2047; 4061685.25 north; 594456.26 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:03:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862186.04 2024-11-06T03:03:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:06Z- waiting for 1730862190.04, time is 1730862186.49 2024-11-06T03:03:06Z- AHRS_M2 msg: 1730862186.84 epoch sec; seqNo:1917 2024-11-06T03:03:06Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.51 psi 2024-11-06T03:03:06Z- handleDepth()- Depth_Keller msg: 1730862186.85 epoch sec; seqNo:2048; depth 50.07 2024-11-06T03:03:06Z- handleDVL() - DVL msg epochMillisec: 1730862186.88 2024-11-06T03:03:06Z- handleDvl() - RDI_Pathfinder msg: 1730862186.88 sec; seqNo:650; #beams:4 2024-11-06T03:03:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.63 , 27.52 , 32.72 , 30.58 2024-11-06T03:03:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , -0.06 2024-11-06T03:03:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862186.90 epoch sec; seqNo:2048; 4061685.26 north; 594457.48 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:03:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862186.04 2024-11-06T03:03:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:07Z- waiting for 1730862190.04, time is 1730862186.90 2024-11-06T03:03:07Z- AHRS_M2 msg: 1730862187.26 epoch sec; seqNo:1918 2024-11-06T03:03:07Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.51 psi 2024-11-06T03:03:07Z- handleDepth()- Depth_Keller msg: 1730862187.27 epoch sec; seqNo:2049; depth 50.05 2024-11-06T03:03:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862187.31 epoch sec; seqNo:2049; 4061685.26 north; 594457.48 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:03:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862186.04 2024-11-06T03:03:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:07Z- waiting for 1730862190.04, time is 1730862187.31 2024-11-06T03:03:07Z- AHRS_M2 msg: 1730862187.69 epoch sec; seqNo:1919 2024-11-06T03:03:07Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.50 psi 2024-11-06T03:03:07Z- handleDepth()- Depth_Keller msg: 1730862187.69 epoch sec; seqNo:2050; depth 50.04 2024-11-06T03:03:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862187.70 epoch sec; seqNo:2050; 4061685.64 north; 594457.47 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:03:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862186.04 2024-11-06T03:03:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:07Z- waiting for 1730862190.04, time is 1730862187.70 2024-11-06T03:03:08Z- handleDepth()- Depth_Keller msg: 1730862188.09 epoch sec; seqNo:2051; depth 50.03 2024-11-06T03:03:08Z- handleDVL() - DVL msg epochMillisec: 1730862188.11 2024-11-06T03:03:08Z- handleDvl() - RDI_Pathfinder msg: 1730862188.11 sec; seqNo:651; #beams:4 2024-11-06T03:03:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.93 , 27.83 , 33.94 , 30.58 2024-11-06T03:03:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , -0.04 2024-11-06T03:03:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862188.13 epoch sec; seqNo:2051; 4061685.65 north; 594458.69 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:03:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862186.04 2024-11-06T03:03:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:08Z- waiting for 1730862190.04, time is 1730862188.13 2024-11-06T03:03:08Z- AHRS_M2 msg: 1730862188.48 epoch sec; seqNo:1920 2024-11-06T03:03:08Z- AHRS_M2 msg: 0.11 phi; 0.03 theta; 1.49 psi 2024-11-06T03:03:08Z- handleDepth()- Depth_Keller msg: 1730862188.48 epoch sec; seqNo:2052; depth 49.99 2024-11-06T03:03:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862188.50 epoch sec; seqNo:2052; 4061685.65 north; 594458.69 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:03:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862186.04 2024-11-06T03:03:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:08Z- waiting for 1730862190.04, time is 1730862188.50 2024-11-06T03:03:08Z- AHRS_M2 msg: 1730862188.89 epoch sec; seqNo:1921 2024-11-06T03:03:08Z- AHRS_M2 msg: 0.11 phi; 0.03 theta; 1.49 psi 2024-11-06T03:03:08Z- handleDepth()- Depth_Keller msg: 1730862188.89 epoch sec; seqNo:2053; depth 49.99 2024-11-06T03:03:08Z- handleDVL() - DVL msg epochMillisec: 1730862188.92 2024-11-06T03:03:08Z- handleDvl() - RDI_Pathfinder msg: 1730862188.92 sec; seqNo:652; #beams:4 2024-11-06T03:03:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.17 , 27.83 , 34.40 , 30.58 2024-11-06T03:03:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.09 , -0.01 2024-11-06T03:03:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862188.93 epoch sec; seqNo:2053; 4061685.65 north; 594458.69 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:03:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862186.04 2024-11-06T03:03:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:09Z- waiting for 1730862190.04, time is 1730862188.93 2024-11-06T03:03:09Z- AHRS_M2 msg: 1730862189.28 epoch sec; seqNo:1922 2024-11-06T03:03:09Z- AHRS_M2 msg: 0.11 phi; 0.03 theta; 1.48 psi 2024-11-06T03:03:09Z- handleDepth()- Depth_Keller msg: 1730862189.28 epoch sec; seqNo:2054; depth 49.96 2024-11-06T03:03:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862189.30 epoch sec; seqNo:2054; 4061685.65 north; 594458.69 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:03:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862186.04 2024-11-06T03:03:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:09Z- waiting for 1730862190.04, time is 1730862189.30 2024-11-06T03:03:09Z- AHRS_M2 msg: 1730862189.68 epoch sec; seqNo:1923 2024-11-06T03:03:09Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 1.48 psi 2024-11-06T03:03:09Z- handleDepth()- Depth_Keller msg: 1730862189.70 epoch sec; seqNo:2055; depth 49.95 2024-11-06T03:03:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862189.72 epoch sec; seqNo:2055; 4061685.67 north; 594459.91 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:03:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862186.04 2024-11-06T03:03:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:09Z- waiting for 1730862190.04, time is 1730862189.72 2024-11-06T03:03:10Z- AHRS_M2 msg: 1730862190.09 epoch sec; seqNo:1924 2024-11-06T03:03:10Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 1.48 psi 2024-11-06T03:03:10Z- handleDepth()- Depth_Keller msg: 1730862190.09 epoch sec; seqNo:2056; depth 49.93 2024-11-06T03:03:10Z- handleDVL() - DVL msg epochMillisec: 1730862190.12 2024-11-06T03:03:10Z- handleDvl() - RDI_Pathfinder msg: 1730862190.12 sec; seqNo:653; #beams:4 2024-11-06T03:03:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 27.98 , 34.71 , 30.73 2024-11-06T03:03:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.09 , 0.02 2024-11-06T03:03:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862190.13 epoch sec; seqNo:2056; 4061685.67 north; 594459.91 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:03:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862186.04 2024-11-06T03:03:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:10Z- waiting for 1730862190.04, time is 1730862190.13 2024-11-06T03:03:10Z- LcmTrn::updateTrn() >>>> pose time 1730862190.13, meas time 1730862190.12 <<<< 2024-11-06T03:03:10Z- LcmTrn::updateTrn() equating pose 1730862190.12 and meas times 1730862190.12 2024-11-06T03:03:10Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862190.12 2024-11-06T03:03:10Z- TerrainNav::motionUpdate() - currEstimate = (4061686, 594459.91, 49.93) 2024-11-06T03:03:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:03:10Z- TerrainNav:: SubcloudNIS is 0.38, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:03:10Z- TNavPF::Projecting Measurements 2024-11-06T03:03:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:03:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:03:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:03:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:03:10Z- TNavFilter::4 good beams. 2024-11-06T03:03:10Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:03:10Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:03:10Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:03:10Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:03:10Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:03:10Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:03:10Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:03:10Z- TerrainMapOctree: (vehN, vehE ) = (4061685.67, 594459.91). 2024-11-06T03:03:10Z- TerrainMapOctree: Min Distance = 219.55. 2024-11-06T03:03:10Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:03:10Z- Forcing Subcloud Comparison 2024-11-06T03:03:10Z- Weighting particles with subcloud comparison 2024-11-06T03:03:10Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:10Z- meanExpectedMeasDiff: 0.963310 delta_rms_squared: 0.830755 alpha: 0.976491 2024-11-06T03:03:10Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:10Z- meanExpectedMeasDiff: -1.152157 delta_rms_squared: 0.539031 alpha: 0.964224 2024-11-06T03:03:10Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:10Z- meanExpectedMeasDiff: 0.739599 delta_rms_squared: 0.755407 alpha: 0.974207 2024-11-06T03:03:10Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:10Z- meanExpectedMeasDiff: 0.419662 delta_rms_squared: 0.697536 alpha: 0.972127 2024-11-06T03:03:10Z- Subcloud NIS: 0.344526 2024-11-06T03:03:10Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:03:10Z- TNavPF:: sumWeights = 0.491424 2024-11-06T03:03:10Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:03:10Z- TNavPF::First Term of Map Covariance Matrix is 0.31 2024-11-06T03:03:10Z- TNavPF::Calculated NIS Value : 0.35 numBeams normalized NIS: 0.09 2024-11-06T03:03:10Z- TerrainNav::Resampling particle filter... 2024-11-06T03:03:10Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862190.12 sec, ping # 0. 2024-11-06T03:03:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:03:10Z- variances([0], [2], and psi_berg) = 0.547769 0.127850 0.000000 2024-11-06T03:03:10Z- publishEstimatesDecoder() - at 1730862190.12 sec 2024-11-06T03:03:10Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:03:10Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061718.03 594443.70 58.84 2024-11-06T03:03:10Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061717.94 594443.40 58.98 lat/lon: 36.70 -121.94 2024-11-06T03:03:10Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.55 0.13 0.30 0.00 2024-11-06T03:03:10Z- AHRS_M2 msg: 1730862190.49 epoch sec; seqNo:1925 2024-11-06T03:03:10Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 1.47 psi 2024-11-06T03:03:10Z- handleDepth()- Depth_Keller msg: 1730862190.50 epoch sec; seqNo:2057; depth 49.92 2024-11-06T03:03:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862190.51 epoch sec; seqNo:2057; 4061686.04 north; 594459.91 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:03:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862190.12 2024-11-06T03:03:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:10Z- waiting for 1730862194.12, time is 1730862190.51 2024-11-06T03:03:10Z- handleDepth()- Depth_Keller msg: 1730862190.94 epoch sec; seqNo:2058; depth 49.91 2024-11-06T03:03:10Z- handleDVL() - DVL msg epochMillisec: 1730862190.96 2024-11-06T03:03:10Z- handleDvl() - RDI_Pathfinder msg: 1730862190.96 sec; seqNo:654; #beams:4 2024-11-06T03:03:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.47 , 28.13 , 34.40 , 30.89 2024-11-06T03:03:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.09 , 0.05 2024-11-06T03:03:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862190.97 epoch sec; seqNo:2058; 4061686.04 north; 594459.91 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:03:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862190.12 2024-11-06T03:03:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:11Z- waiting for 1730862194.12, time is 1730862190.97 2024-11-06T03:03:11Z- AHRS_M2 msg: 1730862191.30 epoch sec; seqNo:1926 2024-11-06T03:03:11Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 1.47 psi 2024-11-06T03:03:11Z- handleDepth()- Depth_Keller msg: 1730862191.30 epoch sec; seqNo:2059; depth 49.90 2024-11-06T03:03:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862191.32 epoch sec; seqNo:2059; 4061686.06 north; 594461.13 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:03:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862190.12 2024-11-06T03:03:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:11Z- waiting for 1730862194.12, time is 1730862191.32 2024-11-06T03:03:11Z- AHRS_M2 msg: 1730862191.70 epoch sec; seqNo:1927 2024-11-06T03:03:11Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 1.47 psi 2024-11-06T03:03:11Z- handleDepth()- Depth_Keller msg: 1730862191.71 epoch sec; seqNo:2060; depth 49.89 2024-11-06T03:03:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862191.72 epoch sec; seqNo:2060; 4061686.06 north; 594461.13 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:03:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862190.12 2024-11-06T03:03:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:11Z- waiting for 1730862194.12, time is 1730862191.72 2024-11-06T03:03:12Z- AHRS_M2 msg: 1730862192.11 epoch sec; seqNo:1928 2024-11-06T03:03:12Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 1.47 psi 2024-11-06T03:03:12Z- handleDepth()- Depth_Keller msg: 1730862192.12 epoch sec; seqNo:2061; depth 49.88 2024-11-06T03:03:12Z- handleDVL() - DVL msg epochMillisec: 1730862192.14 2024-11-06T03:03:12Z- handleDvl() - RDI_Pathfinder msg: 1730862192.14 sec; seqNo:655; #beams:4 2024-11-06T03:03:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.17 , 28.13 , 35.47 , 31.04 2024-11-06T03:03:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , 0.06 2024-11-06T03:03:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862192.19 epoch sec; seqNo:2061; 4061686.06 north; 594461.13 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:03:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862190.12 2024-11-06T03:03:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:12Z- waiting for 1730862194.12, time is 1730862192.19 2024-11-06T03:03:12Z- AHRS_M2 msg: 1730862192.52 epoch sec; seqNo:1929 2024-11-06T03:03:12Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 1.46 psi 2024-11-06T03:03:12Z- handleDepth()- Depth_Keller msg: 1730862192.52 epoch sec; seqNo:2062; depth 49.86 2024-11-06T03:03:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862192.54 epoch sec; seqNo:2062; 4061686.06 north; 594461.13 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:03:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862190.12 2024-11-06T03:03:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:12Z- waiting for 1730862194.12, time is 1730862192.54 2024-11-06T03:03:12Z- AHRS_M2 msg: 1730862192.95 epoch sec; seqNo:1930 2024-11-06T03:03:12Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.46 psi 2024-11-06T03:03:12Z- handleDepth()- Depth_Keller msg: 1730862192.95 epoch sec; seqNo:2063; depth 49.85 2024-11-06T03:03:12Z- handleDVL() - DVL msg epochMillisec: 1730862192.97 2024-11-06T03:03:12Z- handleDvl() - RDI_Pathfinder msg: 1730862192.97 sec; seqNo:656; #beams:4 2024-11-06T03:03:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.93 , 28.13 , 35.93 , 31.19 2024-11-06T03:03:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.06 2024-11-06T03:03:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862193.00 epoch sec; seqNo:2063; 4061686.45 north; 594462.34 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:03:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862190.12 2024-11-06T03:03:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:13Z- waiting for 1730862194.12, time is 1730862193.00 2024-11-06T03:03:13Z- AHRS_M2 msg: 1730862193.32 epoch sec; seqNo:1931 2024-11-06T03:03:13Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.46 psi 2024-11-06T03:03:13Z- handleDepth()- Depth_Keller msg: 1730862193.33 epoch sec; seqNo:2064; depth 49.82 2024-11-06T03:03:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862193.35 epoch sec; seqNo:2064; 4061686.45 north; 594462.34 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:03:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862190.12 2024-11-06T03:03:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:13Z- waiting for 1730862194.12, time is 1730862193.35 2024-11-06T03:03:13Z- AHRS_M2 msg: 1730862193.73 epoch sec; seqNo:1932 2024-11-06T03:03:13Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 1.46 psi 2024-11-06T03:03:13Z- handleDepth()- Depth_Keller msg: 1730862193.73 epoch sec; seqNo:2065; depth 49.81 2024-11-06T03:03:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862193.74 epoch sec; seqNo:2065; 4061686.45 north; 594462.34 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:03:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862190.12 2024-11-06T03:03:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:13Z- waiting for 1730862194.12, time is 1730862193.74 2024-11-06T03:03:14Z- AHRS_M2 msg: 1730862194.13 epoch sec; seqNo:1933 2024-11-06T03:03:14Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.46 psi 2024-11-06T03:03:14Z- handleDepth()- Depth_Keller msg: 1730862194.14 epoch sec; seqNo:2066; depth 49.78 2024-11-06T03:03:14Z- handleDVL() - DVL msg epochMillisec: 1730862194.16 2024-11-06T03:03:14Z- handleDvl() - RDI_Pathfinder msg: 1730862194.16 sec; seqNo:657; #beams:4 2024-11-06T03:03:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.93 , 27.83 , 36.24 , 31.34 2024-11-06T03:03:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , 0.05 2024-11-06T03:03:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862194.17 epoch sec; seqNo:2066; 4061686.46 north; 594463.57 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:03:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862190.12 2024-11-06T03:03:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:14Z- waiting for 1730862194.12, time is 1730862194.17 2024-11-06T03:03:14Z- LcmTrn::updateTrn() >>>> pose time 1730862194.17, meas time 1730862194.16 <<<< 2024-11-06T03:03:14Z- LcmTrn::updateTrn() equating pose 1730862194.16 and meas times 1730862194.16 2024-11-06T03:03:14Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862194.16 2024-11-06T03:03:14Z- TerrainNav::motionUpdate() - currEstimate = (4061686, 594463.57, 49.78) 2024-11-06T03:03:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:03:14Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:03:14Z- TNavPF::Projecting Measurements 2024-11-06T03:03:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:03:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:03:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:03:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:03:14Z- TNavFilter::4 good beams. 2024-11-06T03:03:14Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:03:14Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:03:14Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:03:14Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:03:14Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:03:14Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:03:14Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:03:14Z- TerrainMapOctree: (vehN, vehE ) = (4061686.46, 594463.57). 2024-11-06T03:03:14Z- TerrainMapOctree: Min Distance = 222.82. 2024-11-06T03:03:14Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:03:14Z- Forcing Subcloud Comparison 2024-11-06T03:03:14Z- Weighting particles with subcloud comparison 2024-11-06T03:03:14Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:14Z- meanExpectedMeasDiff: -0.031590 delta_rms_squared: 1.486807 alpha: 0.986727 2024-11-06T03:03:14Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:14Z- meanExpectedMeasDiff: -2.400187 delta_rms_squared: 0.627961 alpha: 0.969134 2024-11-06T03:03:14Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:14Z- meanExpectedMeasDiff: 1.990474 delta_rms_squared: 0.754345 alpha: 0.974172 2024-11-06T03:03:14Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:14Z- meanExpectedMeasDiff: 0.147004 delta_rms_squared: 0.729238 alpha: 0.973306 2024-11-06T03:03:14Z- Subcloud NIS: 0.330748 2024-11-06T03:03:14Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:03:14Z- TNavPF:: sumWeights = 0.163775 2024-11-06T03:03:14Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:03:14Z- TNavPF::First Term of Map Covariance Matrix is 0.14 2024-11-06T03:03:14Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.02 2024-11-06T03:03:14Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862194.16 sec, ping # 0. 2024-11-06T03:03:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:03:14Z- variances([0], [2], and psi_berg) = 0.535302 0.125964 0.000000 2024-11-06T03:03:14Z- publishEstimatesDecoder() - at 1730862194.16 sec 2024-11-06T03:03:14Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:03:14Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061716.79 594446.54 58.26 2024-11-06T03:03:14Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061718.80 594447.07 58.88 lat/lon: 36.70 -121.94 2024-11-06T03:03:14Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.54 0.13 0.30 0.00 2024-11-06T03:03:14Z- AHRS_M2 msg: 1730862194.53 epoch sec; seqNo:1934 2024-11-06T03:03:14Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.46 psi 2024-11-06T03:03:14Z- handleDepth()- Depth_Keller msg: 1730862194.55 epoch sec; seqNo:2067; depth 49.76 2024-11-06T03:03:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862194.56 epoch sec; seqNo:2067; 4061686.46 north; 594463.57 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:03:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862194.16 2024-11-06T03:03:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:14Z- waiting for 1730862198.16, time is 1730862194.56 2024-11-06T03:03:14Z- AHRS_M2 msg: 1730862194.97 epoch sec; seqNo:1935 2024-11-06T03:03:14Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.47 psi 2024-11-06T03:03:14Z- handleDepth()- Depth_Keller msg: 1730862194.97 epoch sec; seqNo:2068; depth 49.75 2024-11-06T03:03:15Z- handleDVL() - DVL msg epochMillisec: 1730862194.99 2024-11-06T03:03:15Z- handleDvl() - RDI_Pathfinder msg: 1730862194.99 sec; seqNo:658; #beams:4 2024-11-06T03:03:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.47 , 27.98 , 36.08 , 31.34 2024-11-06T03:03:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.03 2024-11-06T03:03:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862195.01 epoch sec; seqNo:2068; 4061686.46 north; 594463.57 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:03:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862194.16 2024-11-06T03:03:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:15Z- waiting for 1730862198.16, time is 1730862195.01 2024-11-06T03:03:15Z- AHRS_M2 msg: 1730862195.34 epoch sec; seqNo:1936 2024-11-06T03:03:15Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.47 psi 2024-11-06T03:03:15Z- handleDepth()- Depth_Keller msg: 1730862195.35 epoch sec; seqNo:2069; depth 49.73 2024-11-06T03:03:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862195.36 epoch sec; seqNo:2069; 4061686.46 north; 594463.57 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:03:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862194.16 2024-11-06T03:03:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:15Z- waiting for 1730862198.16, time is 1730862195.36 2024-11-06T03:03:15Z- AHRS_M2 msg: 1730862195.75 epoch sec; seqNo:1937 2024-11-06T03:03:15Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.47 psi 2024-11-06T03:03:15Z- handleDepth()- Depth_Keller msg: 1730862195.76 epoch sec; seqNo:2070; depth 49.70 2024-11-06T03:03:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862195.77 epoch sec; seqNo:2070; 4061686.48 north; 594464.79 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:03:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862194.16 2024-11-06T03:03:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:15Z- waiting for 1730862198.16, time is 1730862195.77 2024-11-06T03:03:16Z- AHRS_M2 msg: 1730862196.15 epoch sec; seqNo:1938 2024-11-06T03:03:16Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.47 psi 2024-11-06T03:03:16Z- handleDepth()- Depth_Keller msg: 1730862196.17 epoch sec; seqNo:2071; depth 49.68 2024-11-06T03:03:16Z- handleDVL() - DVL msg epochMillisec: 1730862196.19 2024-11-06T03:03:16Z- handleDvl() - RDI_Pathfinder msg: 1730862196.19 sec; seqNo:659; #beams:4 2024-11-06T03:03:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 28.13 , 35.63 , 31.50 2024-11-06T03:03:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , 0.01 2024-11-06T03:03:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862196.20 epoch sec; seqNo:2071; 4061686.48 north; 594464.79 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:03:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862194.16 2024-11-06T03:03:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:16Z- waiting for 1730862198.16, time is 1730862196.20 2024-11-06T03:03:16Z- AHRS_M2 msg: 1730862196.55 epoch sec; seqNo:1939 2024-11-06T03:03:16Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.47 psi 2024-11-06T03:03:16Z- handleDepth()- Depth_Keller msg: 1730862196.56 epoch sec; seqNo:2072; depth 49.67 2024-11-06T03:03:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862196.57 epoch sec; seqNo:2072; 4061686.48 north; 594464.79 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:03:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862194.16 2024-11-06T03:03:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:16Z- waiting for 1730862198.16, time is 1730862196.57 2024-11-06T03:03:17Z- AHRS_M2 msg: 1730862197.00 epoch sec; seqNo:1940 2024-11-06T03:03:17Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.47 psi 2024-11-06T03:03:17Z- handleDepth()- Depth_Keller msg: 1730862197.00 epoch sec; seqNo:2073; depth 49.64 2024-11-06T03:03:17Z- handleDVL() - DVL msg epochMillisec: 1730862197.03 2024-11-06T03:03:17Z- handleDvl() - RDI_Pathfinder msg: 1730862197.03 sec; seqNo:660; #beams:4 2024-11-06T03:03:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 28.13 , 35.63 , 31.80 2024-11-06T03:03:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , 0.00 2024-11-06T03:03:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862197.04 epoch sec; seqNo:2073; 4061686.87 north; 594466.00 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:03:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862194.16 2024-11-06T03:03:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:17Z- waiting for 1730862198.16, time is 1730862197.04 2024-11-06T03:03:17Z- AHRS_M2 msg: 1730862197.37 epoch sec; seqNo:1941 2024-11-06T03:03:17Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.47 psi 2024-11-06T03:03:17Z- handleDepth()- Depth_Keller msg: 1730862197.37 epoch sec; seqNo:2074; depth 49.61 2024-11-06T03:03:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862197.38 epoch sec; seqNo:2074; 4061686.87 north; 594466.00 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:03:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862194.16 2024-11-06T03:03:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:17Z- waiting for 1730862198.16, time is 1730862197.38 2024-11-06T03:03:17Z- AHRS_M2 msg: 1730862197.77 epoch sec; seqNo:1942 2024-11-06T03:03:17Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.47 psi 2024-11-06T03:03:17Z- handleDepth()- Depth_Keller msg: 1730862197.78 epoch sec; seqNo:2075; depth 49.60 2024-11-06T03:03:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862197.79 epoch sec; seqNo:2075; 4061686.87 north; 594466.00 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:03:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862194.16 2024-11-06T03:03:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:17Z- waiting for 1730862198.16, time is 1730862197.79 2024-11-06T03:03:18Z- AHRS_M2 msg: 1730862198.18 epoch sec; seqNo:1943 2024-11-06T03:03:18Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.47 psi 2024-11-06T03:03:18Z- handleDepth()- Depth_Keller msg: 1730862198.18 epoch sec; seqNo:2076; depth 49.58 2024-11-06T03:03:18Z- handleDVL() - DVL msg epochMillisec: 1730862198.20 2024-11-06T03:03:18Z- handleDvl() - RDI_Pathfinder msg: 1730862198.20 sec; seqNo:661; #beams:4 2024-11-06T03:03:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.71 , 28.13 , 35.47 , 31.80 2024-11-06T03:03:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , -0.01 2024-11-06T03:03:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862198.23 epoch sec; seqNo:2076; 4061686.88 north; 594467.22 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:03:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862194.16 2024-11-06T03:03:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:18Z- waiting for 1730862198.16, time is 1730862198.23 2024-11-06T03:03:18Z- LcmTrn::updateTrn() >>>> pose time 1730862198.23, meas time 1730862198.20 <<<< 2024-11-06T03:03:18Z- LcmTrn::updateTrn() equating pose 1730862198.20 and meas times 1730862198.20 2024-11-06T03:03:18Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862198.20 2024-11-06T03:03:18Z- TerrainNav::motionUpdate() - currEstimate = (4061687, 594467.22, 49.58) 2024-11-06T03:03:18Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:03:18Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:03:18Z- TNavPF::Projecting Measurements 2024-11-06T03:03:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:03:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:03:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:03:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:03:18Z- TNavFilter::4 good beams. 2024-11-06T03:03:18Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:03:18Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:03:18Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:03:18Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:03:18Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:03:18Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:03:18Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:03:18Z- TerrainMapOctree: (vehN, vehE ) = (4061686.88, 594467.22). 2024-11-06T03:03:18Z- TerrainMapOctree: Min Distance = 226.21. 2024-11-06T03:03:18Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:03:18Z- Forcing Subcloud Comparison 2024-11-06T03:03:18Z- Weighting particles with subcloud comparison 2024-11-06T03:03:18Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:18Z- meanExpectedMeasDiff: -2.039628 delta_rms_squared: 0.872822 alpha: 0.977599 2024-11-06T03:03:18Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:18Z- meanExpectedMeasDiff: -2.813292 delta_rms_squared: 0.598890 alpha: 0.967684 2024-11-06T03:03:18Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:18Z- meanExpectedMeasDiff: 1.479796 delta_rms_squared: 0.745277 alpha: 0.973866 2024-11-06T03:03:18Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:18Z- meanExpectedMeasDiff: -0.639129 delta_rms_squared: 0.726128 alpha: 0.973195 2024-11-06T03:03:18Z- Subcloud NIS: 0.326858 2024-11-06T03:03:18Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:03:18Z- TNavPF:: sumWeights = 0.110353 2024-11-06T03:03:18Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:03:18Z- TNavPF::First Term of Map Covariance Matrix is 0.27 2024-11-06T03:03:18Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T03:03:18Z- TerrainNav::Resampling particle filter... 2024-11-06T03:03:18Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862198.20 sec, ping # 0. 2024-11-06T03:03:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:03:18Z- variances([0], [2], and psi_berg) = 0.605488 0.134782 0.000000 2024-11-06T03:03:18Z- publishEstimatesDecoder() - at 1730862198.20 sec 2024-11-06T03:03:18Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:03:18Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061719.19 594451.00 58.16 2024-11-06T03:03:18Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061719.05 594450.72 59.05 lat/lon: 36.70 -121.94 2024-11-06T03:03:18Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.61 0.13 0.33 0.00 2024-11-06T03:03:18Z- AHRS_M2 msg: 1730862198.58 epoch sec; seqNo:1944 2024-11-06T03:03:18Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.47 psi 2024-11-06T03:03:18Z- handleDepth()- Depth_Keller msg: 1730862198.58 epoch sec; seqNo:2077; depth 49.55 2024-11-06T03:03:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862198.61 epoch sec; seqNo:2077; 4061686.88 north; 594467.22 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:03:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862198.20 2024-11-06T03:03:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:18Z- waiting for 1730862202.20, time is 1730862198.61 2024-11-06T03:03:19Z- AHRS_M2 msg: 1730862199.01 epoch sec; seqNo:1945 2024-11-06T03:03:19Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.47 psi 2024-11-06T03:03:19Z- handleDepth()- Depth_Keller msg: 1730862199.03 epoch sec; seqNo:2078; depth 49.53 2024-11-06T03:03:19Z- handleDVL() - DVL msg epochMillisec: 1730862199.05 2024-11-06T03:03:19Z- handleDvl() - RDI_Pathfinder msg: 1730862199.05 sec; seqNo:662; #beams:4 2024-11-06T03:03:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 27.52 , 35.63 , 32.11 2024-11-06T03:03:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , -0.01 2024-11-06T03:03:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862199.08 epoch sec; seqNo:2078; 4061686.88 north; 594467.22 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T03:03:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862198.20 2024-11-06T03:03:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:19Z- waiting for 1730862202.20, time is 1730862199.08 2024-11-06T03:03:19Z- AHRS_M2 msg: 1730862199.38 epoch sec; seqNo:1946 2024-11-06T03:03:19Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.47 psi 2024-11-06T03:03:19Z- handleDepth()- Depth_Keller msg: 1730862199.39 epoch sec; seqNo:2079; depth 49.50 2024-11-06T03:03:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862199.41 epoch sec; seqNo:2079; 4061686.88 north; 594467.22 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:03:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862198.20 2024-11-06T03:03:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:19Z- waiting for 1730862202.20, time is 1730862199.41 2024-11-06T03:03:19Z- AHRS_M2 msg: 1730862199.79 epoch sec; seqNo:1947 2024-11-06T03:03:19Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.47 psi 2024-11-06T03:03:19Z- handleDepth()- Depth_Keller msg: 1730862199.80 epoch sec; seqNo:2080; depth 49.50 2024-11-06T03:03:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862199.81 epoch sec; seqNo:2080; 4061686.90 north; 594468.44 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:03:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862198.20 2024-11-06T03:03:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:19Z- waiting for 1730862202.20, time is 1730862199.81 2024-11-06T03:03:20Z- AHRS_M2 msg: 1730862200.19 epoch sec; seqNo:1948 2024-11-06T03:03:20Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.47 psi 2024-11-06T03:03:20Z- handleDepth()- Depth_Keller msg: 1730862200.21 epoch sec; seqNo:2081; depth 49.48 2024-11-06T03:03:20Z- handleDVL() - DVL msg epochMillisec: 1730862200.23 2024-11-06T03:03:20Z- handleDvl() - RDI_Pathfinder msg: 1730862200.23 sec; seqNo:663; #beams:4 2024-11-06T03:03:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.47 , 28.13 , 35.78 , 32.11 2024-11-06T03:03:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , -0.01 2024-11-06T03:03:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862200.24 epoch sec; seqNo:2081; 4061686.90 north; 594468.44 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:03:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862198.20 2024-11-06T03:03:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:20Z- waiting for 1730862202.20, time is 1730862200.24 2024-11-06T03:03:20Z- handleDepth()- Depth_Keller msg: 1730862200.61 epoch sec; seqNo:2082; depth 49.45 2024-11-06T03:03:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862200.63 epoch sec; seqNo:2082; 4061686.90 north; 594468.44 east; 36.70 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:03:20Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:03:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862198.20 2024-11-06T03:03:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:20Z- waiting for 1730862202.20, time is 1730862200.63 2024-11-06T03:03:21Z- AHRS_M2 msg: 1730862201.05 epoch sec; seqNo:1949 2024-11-06T03:03:21Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.47 psi 2024-11-06T03:03:21Z- handleDepth()- Depth_Keller msg: 1730862201.06 epoch sec; seqNo:2083; depth 49.43 2024-11-06T03:03:21Z- handleDVL() - DVL msg epochMillisec: 1730862201.08 2024-11-06T03:03:21Z- handleDvl() - RDI_Pathfinder msg: 1730862201.08 sec; seqNo:664; #beams:4 2024-11-06T03:03:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.78 , 27.98 , 35.17 , 32.26 2024-11-06T03:03:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.00 2024-11-06T03:03:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862201.11 epoch sec; seqNo:2083; 4061686.91 north; 594469.66 east; 36.70 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T03:03:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862198.20 2024-11-06T03:03:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:21Z- waiting for 1730862202.20, time is 1730862201.11 2024-11-06T03:03:21Z- AHRS_M2 msg: 1730862201.40 epoch sec; seqNo:1950 2024-11-06T03:03:21Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.47 psi 2024-11-06T03:03:21Z- handleDepth()- Depth_Keller msg: 1730862201.41 epoch sec; seqNo:2084; depth 49.43 2024-11-06T03:03:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862201.43 epoch sec; seqNo:2084; 4061687.29 north; 594469.66 east; 36.70 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T03:03:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862198.20 2024-11-06T03:03:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:21Z- waiting for 1730862202.20, time is 1730862201.43 2024-11-06T03:03:21Z- AHRS_M2 msg: 1730862201.81 epoch sec; seqNo:1951 2024-11-06T03:03:21Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.46 psi 2024-11-06T03:03:21Z- handleDepth()- Depth_Keller msg: 1730862201.81 epoch sec; seqNo:2085; depth 49.40 2024-11-06T03:03:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862201.83 epoch sec; seqNo:2085; 4061687.29 north; 594469.66 east; 36.70 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T03:03:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862198.20 2024-11-06T03:03:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:21Z- waiting for 1730862202.20, time is 1730862201.83 2024-11-06T03:03:22Z- handleDepth()- Depth_Keller msg: 1730862202.23 epoch sec; seqNo:2086; depth 49.38 2024-11-06T03:03:22Z- handleDVL() - DVL msg epochMillisec: 1730862202.26 2024-11-06T03:03:22Z- handleDvl() - RDI_Pathfinder msg: 1730862202.26 sec; seqNo:665; #beams:4 2024-11-06T03:03:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.63 , 28.13 , 35.32 , 32.42 2024-11-06T03:03:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.02 2024-11-06T03:03:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862202.27 epoch sec; seqNo:2086; 4061687.29 north; 594469.66 east; 36.70 lat; -121.94 lon; 49.38 depth; utmZone: 10 2024-11-06T03:03:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862198.20 2024-11-06T03:03:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:22Z- waiting for 1730862202.20, time is 1730862202.27 2024-11-06T03:03:22Z- LcmTrn::updateTrn() >>>> pose time 1730862202.27, meas time 1730862202.26 <<<< 2024-11-06T03:03:22Z- LcmTrn::updateTrn() equating pose 1730862202.26 and meas times 1730862202.26 2024-11-06T03:03:22Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862202.26 2024-11-06T03:03:22Z- TerrainNav::motionUpdate() - currEstimate = (4061687, 594469.66, 49.38) 2024-11-06T03:03:22Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:03:22Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:03:22Z- TNavPF::Projecting Measurements 2024-11-06T03:03:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:03:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:03:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:03:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:03:22Z- TNavFilter::4 good beams. 2024-11-06T03:03:22Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:03:22Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:03:22Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:03:22Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:03:22Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:03:22Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:03:22Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:03:22Z- TerrainMapOctree: (vehN, vehE ) = (4061687.29, 594469.66). 2024-11-06T03:03:22Z- TerrainMapOctree: Min Distance = 228.43. 2024-11-06T03:03:22Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:03:22Z- Forcing Subcloud Comparison 2024-11-06T03:03:22Z- Weighting particles with subcloud comparison 2024-11-06T03:03:22Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:22Z- meanExpectedMeasDiff: -1.171138 delta_rms_squared: 0.927493 alpha: 0.978892 2024-11-06T03:03:22Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:22Z- meanExpectedMeasDiff: -2.867432 delta_rms_squared: 0.653442 alpha: 0.970302 2024-11-06T03:03:22Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:22Z- meanExpectedMeasDiff: 1.820092 delta_rms_squared: 0.767012 alpha: 0.974587 2024-11-06T03:03:22Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:22Z- meanExpectedMeasDiff: -0.218245 delta_rms_squared: 0.781271 alpha: 0.975040 2024-11-06T03:03:22Z- Subcloud NIS: 0.329745 2024-11-06T03:03:22Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:03:22Z- TNavPF:: sumWeights = 0.117356 2024-11-06T03:03:22Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:03:22Z- TNavPF::First Term of Map Covariance Matrix is 0.12 2024-11-06T03:03:22Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.02 2024-11-06T03:03:22Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862202.26 sec, ping # 0. 2024-11-06T03:03:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:03:22Z- variances([0], [2], and psi_berg) = 0.606833 0.133262 0.000000 2024-11-06T03:03:22Z- publishEstimatesDecoder() - at 1730862202.26 sec 2024-11-06T03:03:22Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:03:22Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061719.75 594453.20 59.44 2024-11-06T03:03:22Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061719.49 594453.17 59.12 lat/lon: 36.70 -121.94 2024-11-06T03:03:22Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.61 0.13 0.30 0.00 2024-11-06T03:03:22Z- AHRS_M2 msg: 1730862202.62 epoch sec; seqNo:1952 2024-11-06T03:03:22Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.46 psi 2024-11-06T03:03:22Z- handleDepth()- Depth_Keller msg: 1730862202.65 epoch sec; seqNo:2087; depth 49.37 2024-11-06T03:03:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862202.68 epoch sec; seqNo:2087; 4061687.30 north; 594470.88 east; 36.70 lat; -121.94 lon; 49.37 depth; utmZone: 10 2024-11-06T03:03:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862202.26 2024-11-06T03:03:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:22Z- waiting for 1730862206.26, time is 1730862202.68 2024-11-06T03:03:23Z- AHRS_M2 msg: 1730862203.03 epoch sec; seqNo:1953 2024-11-06T03:03:23Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.46 psi 2024-11-06T03:03:23Z- handleDepth()- Depth_Keller msg: 1730862203.06 epoch sec; seqNo:2088; depth 49.35 2024-11-06T03:03:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862203.10 epoch sec; seqNo:2088; 4061687.30 north; 594470.88 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:03:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862202.26 2024-11-06T03:03:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:23Z- waiting for 1730862206.26, time is 1730862203.10 2024-11-06T03:03:23Z- AHRS_M2 msg: 1730862203.42 epoch sec; seqNo:1954 2024-11-06T03:03:23Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.46 psi 2024-11-06T03:03:23Z- handleDepth()- Depth_Keller msg: 1730862203.43 epoch sec; seqNo:2089; depth 49.33 2024-11-06T03:03:23Z- handleDVL() - DVL msg epochMillisec: 1730862203.45 2024-11-06T03:03:23Z- handleDvl() - RDI_Pathfinder msg: 1730862203.45 sec; seqNo:666; #beams:4 2024-11-06T03:03:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.63 , 27.52 , 35.17 , 32.26 2024-11-06T03:03:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , 0.04 2024-11-06T03:03:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862203.47 epoch sec; seqNo:2089; 4061687.30 north; 594470.88 east; 36.70 lat; -121.94 lon; 49.33 depth; utmZone: 10 2024-11-06T03:03:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862202.26 2024-11-06T03:03:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:23Z- waiting for 1730862206.26, time is 1730862203.47 2024-11-06T03:03:23Z- AHRS_M2 msg: 1730862203.83 epoch sec; seqNo:1955 2024-11-06T03:03:23Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.46 psi 2024-11-06T03:03:23Z- handleDepth()- Depth_Keller msg: 1730862203.83 epoch sec; seqNo:2090; depth 49.34 2024-11-06T03:03:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862203.85 epoch sec; seqNo:2090; 4061687.68 north; 594470.87 east; 36.70 lat; -121.94 lon; 49.34 depth; utmZone: 10 2024-11-06T03:03:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862202.26 2024-11-06T03:03:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:23Z- waiting for 1730862206.26, time is 1730862203.85 2024-11-06T03:03:24Z- AHRS_M2 msg: 1730862204.23 epoch sec; seqNo:1956 2024-11-06T03:03:24Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.45 psi 2024-11-06T03:03:24Z- handleDepth()- Depth_Keller msg: 1730862204.24 epoch sec; seqNo:2091; depth 49.32 2024-11-06T03:03:24Z- handleDVL() - DVL msg epochMillisec: 1730862204.26 2024-11-06T03:03:24Z- handleDvl() - RDI_Pathfinder msg: 1730862204.26 sec; seqNo:667; #beams:4 2024-11-06T03:03:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.08 , 28.29 , 35.17 , 32.26 2024-11-06T03:03:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , 0.06 2024-11-06T03:03:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862204.30 epoch sec; seqNo:2091; 4061687.69 north; 594472.09 east; 36.70 lat; -121.94 lon; 49.32 depth; utmZone: 10 2024-11-06T03:03:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862202.26 2024-11-06T03:03:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:24Z- waiting for 1730862206.26, time is 1730862204.30 2024-11-06T03:03:24Z- AHRS_M2 msg: 1730862204.64 epoch sec; seqNo:1957 2024-11-06T03:03:24Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.45 psi 2024-11-06T03:03:24Z- handleDepth()- Depth_Keller msg: 1730862204.65 epoch sec; seqNo:2092; depth 49.31 2024-11-06T03:03:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862204.67 epoch sec; seqNo:2092; 4061687.69 north; 594472.09 east; 36.70 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T03:03:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862202.26 2024-11-06T03:03:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:24Z- waiting for 1730862206.26, time is 1730862204.67 2024-11-06T03:03:25Z- AHRS_M2 msg: 1730862205.04 epoch sec; seqNo:1958 2024-11-06T03:03:25Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.45 psi 2024-11-06T03:03:25Z- handleDepth()- Depth_Keller msg: 1730862205.08 epoch sec; seqNo:2093; depth 49.30 2024-11-06T03:03:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862205.12 epoch sec; seqNo:2093; 4061687.69 north; 594472.09 east; 36.70 lat; -121.94 lon; 49.30 depth; utmZone: 10 2024-11-06T03:03:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862202.26 2024-11-06T03:03:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:25Z- waiting for 1730862206.26, time is 1730862205.12 2024-11-06T03:03:25Z- AHRS_M2 msg: 1730862205.45 epoch sec; seqNo:1959 2024-11-06T03:03:25Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.45 psi 2024-11-06T03:03:25Z- handleDepth()- Depth_Keller msg: 1730862205.45 epoch sec; seqNo:2094; depth 49.29 2024-11-06T03:03:25Z- handleDVL() - DVL msg epochMillisec: 1730862205.47 2024-11-06T03:03:25Z- handleDvl() - RDI_Pathfinder msg: 1730862205.47 sec; seqNo:668; #beams:4 2024-11-06T03:03:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.54 , 28.59 , 35.32 , 32.72 2024-11-06T03:03:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , 0.05 2024-11-06T03:03:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862205.50 epoch sec; seqNo:2094; 4061687.71 north; 594473.32 east; 36.70 lat; -121.94 lon; 49.29 depth; utmZone: 10 2024-11-06T03:03:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862202.26 2024-11-06T03:03:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:25Z- waiting for 1730862206.26, time is 1730862205.50 2024-11-06T03:03:25Z- AHRS_M2 msg: 1730862205.85 epoch sec; seqNo:1960 2024-11-06T03:03:25Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.46 psi 2024-11-06T03:03:25Z- handleDepth()- Depth_Keller msg: 1730862205.86 epoch sec; seqNo:2095; depth 49.27 2024-11-06T03:03:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862205.87 epoch sec; seqNo:2095; 4061687.71 north; 594473.32 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T03:03:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862202.26 2024-11-06T03:03:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:26Z- waiting for 1730862206.26, time is 1730862205.87 2024-11-06T03:03:26Z- AHRS_M2 msg: 1730862206.25 epoch sec; seqNo:1961 2024-11-06T03:03:26Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.46 psi 2024-11-06T03:03:26Z- handleDepth()- Depth_Keller msg: 1730862206.26 epoch sec; seqNo:2096; depth 49.26 2024-11-06T03:03:26Z- handleDVL() - DVL msg epochMillisec: 1730862206.28 2024-11-06T03:03:26Z- handleDvl() - RDI_Pathfinder msg: 1730862206.28 sec; seqNo:669; #beams:4 2024-11-06T03:03:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.78 , 28.75 , 35.17 , 33.18 2024-11-06T03:03:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.03 2024-11-06T03:03:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862206.29 epoch sec; seqNo:2096; 4061687.71 north; 594473.32 east; 36.70 lat; -121.94 lon; 49.26 depth; utmZone: 10 2024-11-06T03:03:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862202.26 2024-11-06T03:03:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:26Z- waiting for 1730862206.26, time is 1730862206.29 2024-11-06T03:03:26Z- LcmTrn::updateTrn() >>>> pose time 1730862206.29, meas time 1730862206.28 <<<< 2024-11-06T03:03:26Z- LcmTrn::updateTrn() equating pose 1730862206.28 and meas times 1730862206.28 2024-11-06T03:03:26Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862206.28 2024-11-06T03:03:26Z- TerrainNav::motionUpdate() - currEstimate = (4061688, 594473.32, 49.26) 2024-11-06T03:03:26Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:03:26Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:03:26Z- TNavPF::Projecting Measurements 2024-11-06T03:03:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:03:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:03:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:03:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:03:26Z- TNavFilter::4 good beams. 2024-11-06T03:03:26Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:03:26Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:03:26Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:03:26Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:03:26Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:03:26Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:03:26Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:03:26Z- TerrainMapOctree: (vehN, vehE ) = (4061687.71, 594473.32). 2024-11-06T03:03:26Z- TerrainMapOctree: Min Distance = 231.84. 2024-11-06T03:03:26Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:03:26Z- Forcing Subcloud Comparison 2024-11-06T03:03:26Z- Weighting particles with subcloud comparison 2024-11-06T03:03:26Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:26Z- meanExpectedMeasDiff: -0.582593 delta_rms_squared: 0.882317 alpha: 0.977835 2024-11-06T03:03:26Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:26Z- meanExpectedMeasDiff: -2.187090 delta_rms_squared: 0.635767 alpha: 0.969501 2024-11-06T03:03:26Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:26Z- meanExpectedMeasDiff: 1.833145 delta_rms_squared: 1.302000 alpha: 0.984871 2024-11-06T03:03:26Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:26Z- meanExpectedMeasDiff: 0.344109 delta_rms_squared: 0.514216 alpha: 0.962562 2024-11-06T03:03:26Z- Subcloud NIS: 0.315431 2024-11-06T03:03:26Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:03:26Z- TNavPF:: sumWeights = 0.204360 2024-11-06T03:03:26Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:03:26Z- TNavPF::First Term of Map Covariance Matrix is 0.13 2024-11-06T03:03:26Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T03:03:26Z- TerrainNav::Resampling particle filter... 2024-11-06T03:03:26Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862206.28 sec, ping # 0. 2024-11-06T03:03:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:03:26Z- variances([0], [2], and psi_berg) = 0.686786 0.130950 0.000000 2024-11-06T03:03:26Z- publishEstimatesDecoder() - at 1730862206.28 sec 2024-11-06T03:03:26Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:03:26Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061720.15 594457.26 59.02 2024-11-06T03:03:26Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061719.96 594456.86 59.09 lat/lon: 36.70 -121.94 2024-11-06T03:03:26Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.69 0.13 0.30 0.00 2024-11-06T03:03:26Z- AHRS_M2 msg: 1730862206.67 epoch sec; seqNo:1962 2024-11-06T03:03:26Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.46 psi 2024-11-06T03:03:26Z- handleDepth()- Depth_Keller msg: 1730862206.68 epoch sec; seqNo:2097; depth 49.25 2024-11-06T03:03:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862206.71 epoch sec; seqNo:2097; 4061688.09 north; 594473.31 east; 36.70 lat; -121.94 lon; 49.25 depth; utmZone: 10 2024-11-06T03:03:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862206.28 2024-11-06T03:03:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:26Z- waiting for 1730862210.28, time is 1730862206.71 2024-11-06T03:03:27Z- AHRS_M2 msg: 1730862207.06 epoch sec; seqNo:1963 2024-11-06T03:03:27Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.46 psi 2024-11-06T03:03:27Z- handleDepth()- Depth_Keller msg: 1730862207.07 epoch sec; seqNo:2098; depth 49.25 2024-11-06T03:03:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862207.10 epoch sec; seqNo:2098; 4061688.10 north; 594474.53 east; 36.70 lat; -121.94 lon; 49.25 depth; utmZone: 10 2024-11-06T03:03:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862206.28 2024-11-06T03:03:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:27Z- waiting for 1730862210.28, time is 1730862207.10 2024-11-06T03:03:27Z- AHRS_M2 msg: 1730862207.48 epoch sec; seqNo:1964 2024-11-06T03:03:27Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.47 psi 2024-11-06T03:03:27Z- handleDepth()- Depth_Keller msg: 1730862207.49 epoch sec; seqNo:2099; depth 49.23 2024-11-06T03:03:27Z- handleDVL() - DVL msg epochMillisec: 1730862207.52 2024-11-06T03:03:27Z- handleDvl() - RDI_Pathfinder msg: 1730862207.52 sec; seqNo:670; #beams:4 2024-11-06T03:03:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.78 , 28.75 , 35.01 , 32.57 2024-11-06T03:03:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.00 2024-11-06T03:03:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862207.53 epoch sec; seqNo:2099; 4061688.10 north; 594474.53 east; 36.70 lat; -121.94 lon; 49.23 depth; utmZone: 10 2024-11-06T03:03:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862206.28 2024-11-06T03:03:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:27Z- waiting for 1730862210.28, time is 1730862207.53 2024-11-06T03:03:27Z- AHRS_M2 msg: 1730862207.87 epoch sec; seqNo:1965 2024-11-06T03:03:27Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T03:03:27Z- handleDepth()- Depth_Keller msg: 1730862207.88 epoch sec; seqNo:2100; depth 49.24 2024-11-06T03:03:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862207.89 epoch sec; seqNo:2100; 4061688.10 north; 594474.53 east; 36.70 lat; -121.94 lon; 49.24 depth; utmZone: 10 2024-11-06T03:03:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862206.28 2024-11-06T03:03:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:28Z- waiting for 1730862210.28, time is 1730862207.89 2024-11-06T03:03:28Z- AHRS_M2 msg: 1730862208.27 epoch sec; seqNo:1966 2024-11-06T03:03:28Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T03:03:28Z- handleDepth()- Depth_Keller msg: 1730862208.27 epoch sec; seqNo:2101; depth 49.22 2024-11-06T03:03:28Z- handleDVL() - DVL msg epochMillisec: 1730862208.30 2024-11-06T03:03:28Z- handleDvl() - RDI_Pathfinder msg: 1730862208.30 sec; seqNo:671; #beams:4 2024-11-06T03:03:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.63 , 29.05 , 35.17 , 32.72 2024-11-06T03:03:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , -0.03 2024-11-06T03:03:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862208.31 epoch sec; seqNo:2101; 4061688.11 north; 594475.75 east; 36.70 lat; -121.94 lon; 49.22 depth; utmZone: 10 2024-11-06T03:03:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862206.28 2024-11-06T03:03:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:28Z- waiting for 1730862210.28, time is 1730862208.31 2024-11-06T03:03:28Z- AHRS_M2 msg: 1730862208.67 epoch sec; seqNo:1967 2024-11-06T03:03:28Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T03:03:28Z- handleDepth()- Depth_Keller msg: 1730862208.68 epoch sec; seqNo:2102; depth 49.22 2024-11-06T03:03:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862208.71 epoch sec; seqNo:2102; 4061688.11 north; 594475.75 east; 36.70 lat; -121.94 lon; 49.22 depth; utmZone: 10 2024-11-06T03:03:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862206.28 2024-11-06T03:03:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:28Z- waiting for 1730862210.28, time is 1730862208.71 2024-11-06T03:03:29Z- AHRS_M2 msg: 1730862209.08 epoch sec; seqNo:1968 2024-11-06T03:03:29Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.47 psi 2024-11-06T03:03:29Z- handleDepth()- Depth_Keller msg: 1730862209.09 epoch sec; seqNo:2103; depth 49.22 2024-11-06T03:03:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862209.10 epoch sec; seqNo:2103; 4061688.11 north; 594475.75 east; 36.70 lat; -121.94 lon; 49.22 depth; utmZone: 10 2024-11-06T03:03:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862206.28 2024-11-06T03:03:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:29Z- waiting for 1730862210.28, time is 1730862209.10 2024-11-06T03:03:29Z- AHRS_M2 msg: 1730862209.48 epoch sec; seqNo:1969 2024-11-06T03:03:29Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.48 psi 2024-11-06T03:03:29Z- handleDepth()- Depth_Keller msg: 1730862209.49 epoch sec; seqNo:2104; depth 49.20 2024-11-06T03:03:29Z- handleDVL() - DVL msg epochMillisec: 1730862209.51 2024-11-06T03:03:29Z- handleDvl() - RDI_Pathfinder msg: 1730862209.51 sec; seqNo:672; #beams:4 2024-11-06T03:03:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.78 , 29.05 , 35.17 , 32.72 2024-11-06T03:03:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.03 2024-11-06T03:03:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862209.53 epoch sec; seqNo:2104; 4061688.11 north; 594475.75 east; 36.70 lat; -121.94 lon; 49.20 depth; utmZone: 10 2024-11-06T03:03:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862206.28 2024-11-06T03:03:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:29Z- waiting for 1730862210.28, time is 1730862209.53 2024-11-06T03:03:29Z- AHRS_M2 msg: 1730862209.89 epoch sec; seqNo:1970 2024-11-06T03:03:29Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.48 psi 2024-11-06T03:03:29Z- handleDepth()- Depth_Keller msg: 1730862209.89 epoch sec; seqNo:2105; depth 49.19 2024-11-06T03:03:29Z- handleDVL() - DVL msg epochMillisec: 1730862209.89 2024-11-06T03:03:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:03:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:03:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:03:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:03:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:03:29Z- handleDvl() - RDI_Pathfinder msg: 1730862209.89 sec; seqNo:673; #beams:0 2024-11-06T03:03:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:03:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.03 2024-11-06T03:03:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862209.92 epoch sec; seqNo:2105; 4061688.13 north; 594476.97 east; 36.70 lat; -121.94 lon; 49.19 depth; utmZone: 10 2024-11-06T03:03:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862206.28 2024-11-06T03:03:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:30Z- waiting for 1730862210.28, time is 1730862209.92 2024-11-06T03:03:30Z- AHRS_M2 msg: 1730862210.29 epoch sec; seqNo:1971 2024-11-06T03:03:30Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T03:03:30Z- handleDepth()- Depth_Keller msg: 1730862210.30 epoch sec; seqNo:2106; depth 49.20 2024-11-06T03:03:30Z- handleDVL() - DVL msg epochMillisec: 1730862210.32 2024-11-06T03:03:30Z- handleDvl() - RDI_Pathfinder msg: 1730862210.32 sec; seqNo:674; #beams:4 2024-11-06T03:03:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.63 , 28.90 , 34.86 , 32.87 2024-11-06T03:03:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , -0.00 2024-11-06T03:03:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862210.35 epoch sec; seqNo:2106; 4061688.51 north; 594476.97 east; 36.70 lat; -121.94 lon; 49.20 depth; utmZone: 10 2024-11-06T03:03:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862206.28 2024-11-06T03:03:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:30Z- waiting for 1730862210.28, time is 1730862210.35 2024-11-06T03:03:30Z- LcmTrn::updateTrn() >>>> pose time 1730862210.35, meas time 1730862210.32 <<<< 2024-11-06T03:03:30Z- LcmTrn::updateTrn() equating pose 1730862210.32 and meas times 1730862210.32 2024-11-06T03:03:30Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862210.32 2024-11-06T03:03:30Z- TerrainNav::motionUpdate() - currEstimate = (4061689, 594476.97, 49.20) 2024-11-06T03:03:30Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:03:30Z- TerrainNav:: SubcloudNIS is 0.32, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:03:30Z- TNavPF::Projecting Measurements 2024-11-06T03:03:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:03:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:03:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:03:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:03:30Z- TNavFilter::4 good beams. 2024-11-06T03:03:30Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:03:30Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:03:30Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:03:30Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:03:30Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:03:30Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:03:30Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:03:30Z- TerrainMapOctree: (vehN, vehE ) = (4061688.51, 594476.97). 2024-11-06T03:03:30Z- TerrainMapOctree: Min Distance = 235.15. 2024-11-06T03:03:30Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:03:30Z- Forcing Subcloud Comparison 2024-11-06T03:03:30Z- Weighting particles with subcloud comparison 2024-11-06T03:03:30Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:30Z- meanExpectedMeasDiff: -1.208751 delta_rms_squared: 0.916358 alpha: 0.978641 2024-11-06T03:03:30Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:30Z- meanExpectedMeasDiff: -1.844743 delta_rms_squared: 0.635281 alpha: 0.969479 2024-11-06T03:03:30Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:30Z- meanExpectedMeasDiff: 0.913753 delta_rms_squared: 0.724648 alpha: 0.973142 2024-11-06T03:03:30Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:30Z- meanExpectedMeasDiff: -0.987608 delta_rms_squared: 1.515791 alpha: 0.986977 2024-11-06T03:03:30Z- Subcloud NIS: 0.312056 2024-11-06T03:03:30Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:03:30Z- TNavPF:: sumWeights = 0.302741 2024-11-06T03:03:30Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:03:30Z- TNavPF::First Term of Map Covariance Matrix is 0.23 2024-11-06T03:03:30Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.05 2024-11-06T03:03:30Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862210.32 sec, ping # 0. 2024-11-06T03:03:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:03:30Z- variances([0], [2], and psi_berg) = 0.702990 0.135749 0.000000 2024-11-06T03:03:30Z- publishEstimatesDecoder() - at 1730862210.32 sec 2024-11-06T03:03:30Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:03:30Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061721.81 594459.67 59.72 2024-11-06T03:03:30Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061720.78 594460.53 59.35 lat/lon: 36.70 -121.94 2024-11-06T03:03:30Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.14 0.29 0.00 2024-11-06T03:03:30Z- AHRS_M2 msg: 1730862210.69 epoch sec; seqNo:1972 2024-11-06T03:03:30Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T03:03:30Z- handleDepth()- Depth_Keller msg: 1730862210.70 epoch sec; seqNo:2107; depth 49.20 2024-11-06T03:03:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862210.73 epoch sec; seqNo:2107; 4061688.51 north; 594476.97 east; 36.70 lat; -121.94 lon; 49.20 depth; utmZone: 10 2024-11-06T03:03:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862210.32 2024-11-06T03:03:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:30Z- waiting for 1730862214.32, time is 1730862210.73 2024-11-06T03:03:31Z- AHRS_M2 msg: 1730862211.10 epoch sec; seqNo:1973 2024-11-06T03:03:31Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T03:03:31Z- handleDepth()- Depth_Keller msg: 1730862211.11 epoch sec; seqNo:2108; depth 49.20 2024-11-06T03:03:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862211.11 epoch sec; seqNo:2108; 4061688.52 north; 594478.19 east; 36.70 lat; -121.94 lon; 49.20 depth; utmZone: 10 2024-11-06T03:03:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862210.32 2024-11-06T03:03:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:31Z- waiting for 1730862214.32, time is 1730862211.11 2024-11-06T03:03:31Z- AHRS_M2 msg: 1730862211.50 epoch sec; seqNo:1974 2024-11-06T03:03:31Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.47 psi 2024-11-06T03:03:31Z- handleDepth()- Depth_Keller msg: 1730862211.51 epoch sec; seqNo:2109; depth 49.20 2024-11-06T03:03:31Z- handleDVL() - DVL msg epochMillisec: 1730862211.53 2024-11-06T03:03:31Z- handleDvl() - RDI_Pathfinder msg: 1730862211.53 sec; seqNo:675; #beams:4 2024-11-06T03:03:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.47 , 29.05 , 34.71 , 33.18 2024-11-06T03:03:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.04 , 0.04 2024-11-06T03:03:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862211.54 epoch sec; seqNo:2109; 4061688.52 north; 594478.19 east; 36.70 lat; -121.94 lon; 49.20 depth; utmZone: 10 2024-11-06T03:03:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862210.32 2024-11-06T03:03:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:31Z- waiting for 1730862214.32, time is 1730862211.54 2024-11-06T03:03:31Z- AHRS_M2 msg: 1730862211.91 epoch sec; seqNo:1975 2024-11-06T03:03:31Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.47 psi 2024-11-06T03:03:31Z- handleDepth()- Depth_Keller msg: 1730862211.91 epoch sec; seqNo:2110; depth 49.20 2024-11-06T03:03:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862211.93 epoch sec; seqNo:2110; 4061688.52 north; 594478.19 east; 36.70 lat; -121.94 lon; 49.20 depth; utmZone: 10 2024-11-06T03:03:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862210.32 2024-11-06T03:03:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:32Z- waiting for 1730862214.32, time is 1730862211.93 2024-11-06T03:03:32Z- AHRS_M2 msg: 1730862212.31 epoch sec; seqNo:1976 2024-11-06T03:03:32Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T03:03:32Z- handleDepth()- Depth_Keller msg: 1730862212.31 epoch sec; seqNo:2111; depth 49.22 2024-11-06T03:03:32Z- handleDVL() - DVL msg epochMillisec: 1730862212.35 2024-11-06T03:03:32Z- handleDvl() - RDI_Pathfinder msg: 1730862212.35 sec; seqNo:676; #beams:4 2024-11-06T03:03:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.63 , 29.36 , 34.71 , 33.18 2024-11-06T03:03:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , 0.07 2024-11-06T03:03:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862212.36 epoch sec; seqNo:2111; 4061688.52 north; 594478.19 east; 36.70 lat; -121.94 lon; 49.22 depth; utmZone: 10 2024-11-06T03:03:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862210.32 2024-11-06T03:03:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:32Z- waiting for 1730862214.32, time is 1730862212.36 2024-11-06T03:03:32Z- AHRS_M2 msg: 1730862212.71 epoch sec; seqNo:1977 2024-11-06T03:03:32Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.47 psi 2024-11-06T03:03:32Z- handleDepth()- Depth_Keller msg: 1730862212.72 epoch sec; seqNo:2112; depth 49.22 2024-11-06T03:03:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862212.73 epoch sec; seqNo:2112; 4061688.53 north; 594479.41 east; 36.70 lat; -121.94 lon; 49.22 depth; utmZone: 10 2024-11-06T03:03:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862210.32 2024-11-06T03:03:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:32Z- waiting for 1730862214.32, time is 1730862212.73 2024-11-06T03:03:33Z- AHRS_M2 msg: 1730862213.12 epoch sec; seqNo:1978 2024-11-06T03:03:33Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.47 psi 2024-11-06T03:03:33Z- handleDepth()- Depth_Keller msg: 1730862213.13 epoch sec; seqNo:2113; depth 49.22 2024-11-06T03:03:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862213.14 epoch sec; seqNo:2113; 4061688.53 north; 594479.41 east; 36.70 lat; -121.94 lon; 49.22 depth; utmZone: 10 2024-11-06T03:03:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862210.32 2024-11-06T03:03:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:33Z- waiting for 1730862214.32, time is 1730862213.14 2024-11-06T03:03:33Z- AHRS_M2 msg: 1730862213.52 epoch sec; seqNo:1979 2024-11-06T03:03:33Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.47 psi 2024-11-06T03:03:33Z- handleDepth()- Depth_Keller msg: 1730862213.53 epoch sec; seqNo:2114; depth 49.22 2024-11-06T03:03:33Z- handleDVL() - DVL msg epochMillisec: 1730862213.55 2024-11-06T03:03:33Z- handleDvl() - RDI_Pathfinder msg: 1730862213.55 sec; seqNo:677; #beams:4 2024-11-06T03:03:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.63 , 29.36 , 34.71 , 33.49 2024-11-06T03:03:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.10 2024-11-06T03:03:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862213.56 epoch sec; seqNo:2114; 4061688.91 north; 594479.40 east; 36.70 lat; -121.94 lon; 49.22 depth; utmZone: 10 2024-11-06T03:03:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862210.32 2024-11-06T03:03:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:33Z- waiting for 1730862214.32, time is 1730862213.56 2024-11-06T03:03:33Z- AHRS_M2 msg: 1730862213.93 epoch sec; seqNo:1980 2024-11-06T03:03:33Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.47 psi 2024-11-06T03:03:33Z- handleDepth()- Depth_Keller msg: 1730862213.94 epoch sec; seqNo:2115; depth 49.24 2024-11-06T03:03:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862213.96 epoch sec; seqNo:2115; 4061688.91 north; 594479.40 east; 36.70 lat; -121.94 lon; 49.24 depth; utmZone: 10 2024-11-06T03:03:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862210.32 2024-11-06T03:03:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:34Z- waiting for 1730862214.32, time is 1730862213.96 2024-11-06T03:03:34Z- AHRS_M2 msg: 1730862214.34 epoch sec; seqNo:1981 2024-11-06T03:03:34Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.47 psi 2024-11-06T03:03:34Z- handleDepth()- Depth_Keller msg: 1730862214.35 epoch sec; seqNo:2116; depth 49.24 2024-11-06T03:03:34Z- handleDVL() - DVL msg epochMillisec: 1730862214.38 2024-11-06T03:03:34Z- handleDvl() - RDI_Pathfinder msg: 1730862214.38 sec; seqNo:678; #beams:4 2024-11-06T03:03:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.78 , 29.20 , 34.56 , 33.64 2024-11-06T03:03:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.11 2024-11-06T03:03:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862214.39 epoch sec; seqNo:2116; 4061688.93 north; 594480.62 east; 36.70 lat; -121.94 lon; 49.24 depth; utmZone: 10 2024-11-06T03:03:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862210.32 2024-11-06T03:03:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:34Z- waiting for 1730862214.32, time is 1730862214.39 2024-11-06T03:03:34Z- LcmTrn::updateTrn() >>>> pose time 1730862214.39, meas time 1730862214.38 <<<< 2024-11-06T03:03:34Z- LcmTrn::updateTrn() equating pose 1730862214.38 and meas times 1730862214.38 2024-11-06T03:03:34Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862214.38 2024-11-06T03:03:34Z- TerrainNav::motionUpdate() - currEstimate = (4061689, 594480.62, 49.24) 2024-11-06T03:03:34Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:03:34Z- TerrainNav:: SubcloudNIS is 0.31, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:03:34Z- TNavPF::Projecting Measurements 2024-11-06T03:03:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:03:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:03:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:03:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:03:34Z- TNavFilter::4 good beams. 2024-11-06T03:03:34Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:03:34Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:03:34Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:03:34Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:03:34Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:03:34Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:03:34Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:03:34Z- TerrainMapOctree: (vehN, vehE ) = (4061688.93, 594480.62). 2024-11-06T03:03:34Z- TerrainMapOctree: Min Distance = 238.57. 2024-11-06T03:03:34Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:03:34Z- Forcing Subcloud Comparison 2024-11-06T03:03:34Z- Weighting particles with subcloud comparison 2024-11-06T03:03:34Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:34Z- meanExpectedMeasDiff: -0.508007 delta_rms_squared: 0.863714 alpha: 0.977368 2024-11-06T03:03:34Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:34Z- meanExpectedMeasDiff: -1.198880 delta_rms_squared: 0.603367 alpha: 0.967916 2024-11-06T03:03:34Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:34Z- meanExpectedMeasDiff: 1.235749 delta_rms_squared: 0.677242 alpha: 0.971316 2024-11-06T03:03:34Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:34Z- meanExpectedMeasDiff: 0.800379 delta_rms_squared: 0.759043 alpha: 0.974327 2024-11-06T03:03:34Z- Subcloud NIS: 0.304879 2024-11-06T03:03:34Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:03:34Z- TNavPF:: sumWeights = 0.430342 2024-11-06T03:03:34Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:03:34Z- TNavPF::First Term of Map Covariance Matrix is 0.24 2024-11-06T03:03:34Z- TNavPF::Calculated NIS Value : 0.33 numBeams normalized NIS: 0.08 2024-11-06T03:03:34Z- TerrainNav::Resampling particle filter... 2024-11-06T03:03:34Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862214.38 sec, ping # 0. 2024-11-06T03:03:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:03:34Z- variances([0], [2], and psi_berg) = 0.700184 0.139655 0.000000 2024-11-06T03:03:34Z- publishEstimatesDecoder() - at 1730862214.38 sec 2024-11-06T03:03:34Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:03:34Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061721.39 594464.68 58.33 2024-11-06T03:03:34Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061721.20 594464.18 59.37 lat/lon: 36.70 -121.94 2024-11-06T03:03:34Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.14 0.29 0.00 2024-11-06T03:03:34Z- AHRS_M2 msg: 1730862214.73 epoch sec; seqNo:1982 2024-11-06T03:03:34Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.47 psi 2024-11-06T03:03:34Z- handleDepth()- Depth_Keller msg: 1730862214.74 epoch sec; seqNo:2117; depth 49.25 2024-11-06T03:03:34Z- handleDVL() - DVL msg epochMillisec: 1730862214.74 2024-11-06T03:03:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:03:34Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:03:34Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:03:34Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:03:34Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:03:34Z- handleDvl() - RDI_Pathfinder msg: 1730862214.74 sec; seqNo:679; #beams:0 2024-11-06T03:03:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:03:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.11 2024-11-06T03:03:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862214.76 epoch sec; seqNo:2117; 4061688.93 north; 594480.62 east; 36.70 lat; -121.94 lon; 49.25 depth; utmZone: 10 2024-11-06T03:03:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862214.38 2024-11-06T03:03:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:34Z- waiting for 1730862218.38, time is 1730862214.76 2024-11-06T03:03:35Z- AHRS_M2 msg: 1730862215.14 epoch sec; seqNo:1983 2024-11-06T03:03:35Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.47 psi 2024-11-06T03:03:35Z- handleDepth()- Depth_Keller msg: 1730862215.15 epoch sec; seqNo:2118; depth 49.24 2024-11-06T03:03:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862215.17 epoch sec; seqNo:2118; 4061688.93 north; 594480.62 east; 36.70 lat; -121.94 lon; 49.24 depth; utmZone: 10 2024-11-06T03:03:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862214.38 2024-11-06T03:03:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:35Z- waiting for 1730862218.38, time is 1730862215.17 2024-11-06T03:03:35Z- AHRS_M2 msg: 1730862215.54 epoch sec; seqNo:1984 2024-11-06T03:03:35Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.48 psi 2024-11-06T03:03:35Z- handleDepth()- Depth_Keller msg: 1730862215.55 epoch sec; seqNo:2119; depth 49.26 2024-11-06T03:03:35Z- handleDVL() - DVL msg epochMillisec: 1730862215.58 2024-11-06T03:03:35Z- handleDvl() - RDI_Pathfinder msg: 1730862215.58 sec; seqNo:680; #beams:4 2024-11-06T03:03:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.93 , 29.20 , 34.71 , 33.94 2024-11-06T03:03:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , 0.08 2024-11-06T03:03:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862215.59 epoch sec; seqNo:2119; 4061688.94 north; 594481.84 east; 36.70 lat; -121.94 lon; 49.26 depth; utmZone: 10 2024-11-06T03:03:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862214.38 2024-11-06T03:03:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:35Z- waiting for 1730862218.38, time is 1730862215.59 2024-11-06T03:03:35Z- AHRS_M2 msg: 1730862215.95 epoch sec; seqNo:1985 2024-11-06T03:03:35Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.48 psi 2024-11-06T03:03:35Z- handleDepth()- Depth_Keller msg: 1730862215.96 epoch sec; seqNo:2120; depth 49.25 2024-11-06T03:03:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862215.98 epoch sec; seqNo:2120; 4061688.94 north; 594481.84 east; 36.70 lat; -121.94 lon; 49.25 depth; utmZone: 10 2024-11-06T03:03:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862214.38 2024-11-06T03:03:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:36Z- waiting for 1730862218.38, time is 1730862215.98 2024-11-06T03:03:36Z- AHRS_M2 msg: 1730862216.35 epoch sec; seqNo:1986 2024-11-06T03:03:36Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.48 psi 2024-11-06T03:03:36Z- handleDepth()- Depth_Keller msg: 1730862216.36 epoch sec; seqNo:2121; depth 49.28 2024-11-06T03:03:36Z- handleDVL() - DVL msg epochMillisec: 1730862216.38 2024-11-06T03:03:36Z- handleDvl() - RDI_Pathfinder msg: 1730862216.38 sec; seqNo:681; #beams:4 2024-11-06T03:03:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.08 , 29.66 , 34.86 , 33.94 2024-11-06T03:03:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.00 , 0.05 2024-11-06T03:03:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862216.39 epoch sec; seqNo:2121; 4061688.94 north; 594481.84 east; 36.70 lat; -121.94 lon; 49.28 depth; utmZone: 10 2024-11-06T03:03:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862214.38 2024-11-06T03:03:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:36Z- waiting for 1730862218.38, time is 1730862216.39 2024-11-06T03:03:36Z- AHRS_M2 msg: 1730862216.75 epoch sec; seqNo:1987 2024-11-06T03:03:36Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.48 psi 2024-11-06T03:03:36Z- handleDepth()- Depth_Keller msg: 1730862216.76 epoch sec; seqNo:2122; depth 49.27 2024-11-06T03:03:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862216.78 epoch sec; seqNo:2122; 4061688.95 north; 594483.07 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T03:03:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862214.38 2024-11-06T03:03:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:36Z- waiting for 1730862218.38, time is 1730862216.78 2024-11-06T03:03:37Z- AHRS_M2 msg: 1730862217.16 epoch sec; seqNo:1988 2024-11-06T03:03:37Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.49 psi 2024-11-06T03:03:37Z- handleDepth()- Depth_Keller msg: 1730862217.17 epoch sec; seqNo:2123; depth 49.27 2024-11-06T03:03:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862217.18 epoch sec; seqNo:2123; 4061688.95 north; 594483.07 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T03:03:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862214.38 2024-11-06T03:03:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:37Z- waiting for 1730862218.38, time is 1730862217.18 2024-11-06T03:03:37Z- AHRS_M2 msg: 1730862217.57 epoch sec; seqNo:1989 2024-11-06T03:03:37Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.49 psi 2024-11-06T03:03:37Z- handleDepth()- Depth_Keller msg: 1730862217.57 epoch sec; seqNo:2124; depth 49.27 2024-11-06T03:03:37Z- handleDVL() - DVL msg epochMillisec: 1730862217.60 2024-11-06T03:03:37Z- handleDvl() - RDI_Pathfinder msg: 1730862217.60 sec; seqNo:682; #beams:4 2024-11-06T03:03:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.39 , 29.66 , 34.86 , 33.94 2024-11-06T03:03:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , 0.01 2024-11-06T03:03:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862217.61 epoch sec; seqNo:2124; 4061689.33 north; 594483.06 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T03:03:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862214.38 2024-11-06T03:03:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:37Z- waiting for 1730862218.38, time is 1730862217.61 2024-11-06T03:03:37Z- AHRS_M2 msg: 1730862217.97 epoch sec; seqNo:1990 2024-11-06T03:03:37Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.49 psi 2024-11-06T03:03:37Z- handleDepth()- Depth_Keller msg: 1730862217.97 epoch sec; seqNo:2125; depth 49.27 2024-11-06T03:03:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862218.00 epoch sec; seqNo:2125; 4061689.33 north; 594483.06 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T03:03:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862214.38 2024-11-06T03:03:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:38Z- waiting for 1730862218.38, time is 1730862218.00 2024-11-06T03:03:38Z- handleDepth()- Depth_Keller msg: 1730862218.39 epoch sec; seqNo:2126; depth 49.27 2024-11-06T03:03:38Z- handleDVL() - DVL msg epochMillisec: 1730862218.42 2024-11-06T03:03:38Z- handleDvl() - RDI_Pathfinder msg: 1730862218.42 sec; seqNo:683; #beams:4 2024-11-06T03:03:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.54 , 29.82 , 34.86 , 33.94 2024-11-06T03:03:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , -0.02 2024-11-06T03:03:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862218.43 epoch sec; seqNo:2126; 4061689.34 north; 594484.28 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T03:03:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862214.38 2024-11-06T03:03:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:38Z- waiting for 1730862218.38, time is 1730862218.43 2024-11-06T03:03:38Z- LcmTrn::updateTrn() >>>> pose time 1730862218.43, meas time 1730862218.42 <<<< 2024-11-06T03:03:38Z- LcmTrn::updateTrn() equating pose 1730862218.42 and meas times 1730862218.42 2024-11-06T03:03:38Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862218.42 2024-11-06T03:03:38Z- TerrainNav::motionUpdate() - currEstimate = (4061689, 594484.28, 49.27) 2024-11-06T03:03:38Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:03:38Z- TerrainNav:: SubcloudNIS is 0.30, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:03:38Z- TNavPF::Projecting Measurements 2024-11-06T03:03:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:03:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:03:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:03:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:03:38Z- TNavFilter::4 good beams. 2024-11-06T03:03:38Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:03:38Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:03:38Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:03:38Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:03:38Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:03:38Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:03:38Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:03:38Z- TerrainMapOctree: (vehN, vehE ) = (4061689.34, 594484.28). 2024-11-06T03:03:38Z- TerrainMapOctree: Min Distance = 242.01. 2024-11-06T03:03:38Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:03:38Z- Forcing Subcloud Comparison 2024-11-06T03:03:38Z- Weighting particles with subcloud comparison 2024-11-06T03:03:38Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:38Z- meanExpectedMeasDiff: 0.118198 delta_rms_squared: 0.880714 alpha: 0.977795 2024-11-06T03:03:38Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:38Z- meanExpectedMeasDiff: -0.515341 delta_rms_squared: 0.607588 alpha: 0.968132 2024-11-06T03:03:38Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:38Z- meanExpectedMeasDiff: 1.624305 delta_rms_squared: 0.681813 alpha: 0.971502 2024-11-06T03:03:38Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:38Z- meanExpectedMeasDiff: 0.962788 delta_rms_squared: 0.741931 alpha: 0.973751 2024-11-06T03:03:38Z- Subcloud NIS: 0.301368 2024-11-06T03:03:38Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:03:38Z- TNavPF:: sumWeights = 0.472732 2024-11-06T03:03:38Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:03:38Z- TNavPF::First Term of Map Covariance Matrix is 0.24 2024-11-06T03:03:38Z- TNavPF::Calculated NIS Value : 0.30 numBeams normalized NIS: 0.08 2024-11-06T03:03:38Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862218.42 sec, ping # 0. 2024-11-06T03:03:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:03:38Z- variances([0], [2], and psi_berg) = 0.709694 0.142448 0.000000 2024-11-06T03:03:38Z- publishEstimatesDecoder() - at 1730862218.42 sec 2024-11-06T03:03:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:03:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061720.17 594468.67 59.02 2024-11-06T03:03:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061721.61 594467.84 59.21 lat/lon: 36.70 -121.94 2024-11-06T03:03:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.71 0.14 0.30 0.00 2024-11-06T03:03:38Z- AHRS_M2 msg: 1730862218.80 epoch sec; seqNo:1991 2024-11-06T03:03:38Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.49 psi 2024-11-06T03:03:38Z- handleDepth()- Depth_Keller msg: 1730862218.81 epoch sec; seqNo:2127; depth 49.25 2024-11-06T03:03:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862218.85 epoch sec; seqNo:2127; 4061689.34 north; 594484.28 east; 36.70 lat; -121.94 lon; 49.25 depth; utmZone: 10 2024-11-06T03:03:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862218.42 2024-11-06T03:03:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:38Z- waiting for 1730862222.42, time is 1730862218.85 2024-11-06T03:03:39Z- AHRS_M2 msg: 1730862219.18 epoch sec; seqNo:1992 2024-11-06T03:03:39Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.49 psi 2024-11-06T03:03:39Z- handleDepth()- Depth_Keller msg: 1730862219.19 epoch sec; seqNo:2128; depth 49.26 2024-11-06T03:03:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862219.20 epoch sec; seqNo:2128; 4061689.34 north; 594484.28 east; 36.70 lat; -121.94 lon; 49.26 depth; utmZone: 10 2024-11-06T03:03:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862218.42 2024-11-06T03:03:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:39Z- waiting for 1730862222.42, time is 1730862219.20 2024-11-06T03:03:39Z- AHRS_M2 msg: 1730862219.58 epoch sec; seqNo:1993 2024-11-06T03:03:39Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:39Z- handleDepth()- Depth_Keller msg: 1730862219.59 epoch sec; seqNo:2129; depth 49.25 2024-11-06T03:03:39Z- handleDVL() - DVL msg epochMillisec: 1730862219.62 2024-11-06T03:03:39Z- handleDvl() - RDI_Pathfinder msg: 1730862219.62 sec; seqNo:684; #beams:4 2024-11-06T03:03:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.46 , 29.51 , 34.86 , 33.79 2024-11-06T03:03:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , -0.04 2024-11-06T03:03:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862219.64 epoch sec; seqNo:2129; 4061689.36 north; 594485.50 east; 36.70 lat; -121.94 lon; 49.25 depth; utmZone: 10 2024-11-06T03:03:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862218.42 2024-11-06T03:03:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:39Z- waiting for 1730862222.42, time is 1730862219.64 2024-11-06T03:03:40Z- AHRS_M2 msg: 1730862220.01 epoch sec; seqNo:1994 2024-11-06T03:03:40Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:40Z- handleDepth()- Depth_Keller msg: 1730862220.01 epoch sec; seqNo:2130; depth 49.25 2024-11-06T03:03:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862220.03 epoch sec; seqNo:2130; 4061689.36 north; 594485.50 east; 36.70 lat; -121.94 lon; 49.25 depth; utmZone: 10 2024-11-06T03:03:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862218.42 2024-11-06T03:03:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:40Z- waiting for 1730862222.42, time is 1730862220.03 2024-11-06T03:03:40Z- AHRS_M2 msg: 1730862220.39 epoch sec; seqNo:1995 2024-11-06T03:03:40Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:40Z- handleDepth()- Depth_Keller msg: 1730862220.42 epoch sec; seqNo:2131; depth 49.24 2024-11-06T03:03:40Z- handleDVL() - DVL msg epochMillisec: 1730862220.44 2024-11-06T03:03:40Z- handleDvl() - RDI_Pathfinder msg: 1730862220.44 sec; seqNo:685; #beams:4 2024-11-06T03:03:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.46 , 29.36 , 34.56 , 33.64 2024-11-06T03:03:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , -0.04 2024-11-06T03:03:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862220.45 epoch sec; seqNo:2131; 4061689.36 north; 594485.50 east; 36.70 lat; -121.94 lon; 49.24 depth; utmZone: 10 2024-11-06T03:03:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862218.42 2024-11-06T03:03:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:40Z- waiting for 1730862222.42, time is 1730862220.45 2024-11-06T03:03:40Z- AHRS_M2 msg: 1730862220.80 epoch sec; seqNo:1996 2024-11-06T03:03:40Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:40Z- handleDepth()- Depth_Keller msg: 1730862220.80 epoch sec; seqNo:2132; depth 49.23 2024-11-06T03:03:40Z- handleDVL() - DVL msg epochMillisec: 1730862220.80 2024-11-06T03:03:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:03:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:03:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:03:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:03:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:03:40Z- handleDvl() - RDI_Pathfinder msg: 1730862220.80 sec; seqNo:686; #beams:0 2024-11-06T03:03:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:03:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , -0.04 2024-11-06T03:03:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862220.82 epoch sec; seqNo:2132; 4061689.37 north; 594486.72 east; 36.70 lat; -121.94 lon; 49.23 depth; utmZone: 10 2024-11-06T03:03:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862218.42 2024-11-06T03:03:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:40Z- waiting for 1730862222.42, time is 1730862220.82 2024-11-06T03:03:41Z- AHRS_M2 msg: 1730862221.20 epoch sec; seqNo:1997 2024-11-06T03:03:41Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:41Z- handleDepth()- Depth_Keller msg: 1730862221.20 epoch sec; seqNo:2133; depth 49.23 2024-11-06T03:03:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862221.22 epoch sec; seqNo:2133; 4061689.75 north; 594486.72 east; 36.70 lat; -121.94 lon; 49.23 depth; utmZone: 10 2024-11-06T03:03:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862218.42 2024-11-06T03:03:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:41Z- waiting for 1730862222.42, time is 1730862221.22 2024-11-06T03:03:41Z- AHRS_M2 msg: 1730862221.60 epoch sec; seqNo:1998 2024-11-06T03:03:41Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:41Z- handleDepth()- Depth_Keller msg: 1730862221.61 epoch sec; seqNo:2134; depth 49.23 2024-11-06T03:03:41Z- handleDVL() - DVL msg epochMillisec: 1730862221.64 2024-11-06T03:03:41Z- handleDvl() - RDI_Pathfinder msg: 1730862221.64 sec; seqNo:687; #beams:4 2024-11-06T03:03:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.22 , 29.36 , 34.40 , 33.79 2024-11-06T03:03:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , -0.02 2024-11-06T03:03:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862221.67 epoch sec; seqNo:2134; 4061689.75 north; 594486.72 east; 36.70 lat; -121.94 lon; 49.23 depth; utmZone: 10 2024-11-06T03:03:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862218.42 2024-11-06T03:03:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:41Z- waiting for 1730862222.42, time is 1730862221.67 2024-11-06T03:03:42Z- AHRS_M2 msg: 1730862222.01 epoch sec; seqNo:1999 2024-11-06T03:03:42Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:42Z- handleDepth()- Depth_Keller msg: 1730862222.01 epoch sec; seqNo:2135; depth 49.22 2024-11-06T03:03:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862222.05 epoch sec; seqNo:2135; 4061689.75 north; 594486.72 east; 36.70 lat; -121.94 lon; 49.22 depth; utmZone: 10 2024-11-06T03:03:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862218.42 2024-11-06T03:03:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:42Z- waiting for 1730862222.42, time is 1730862222.05 2024-11-06T03:03:42Z- AHRS_M2 msg: 1730862222.42 epoch sec; seqNo:2000 2024-11-06T03:03:42Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:42Z- handleDepth()- Depth_Keller msg: 1730862222.43 epoch sec; seqNo:2136; depth 49.23 2024-11-06T03:03:42Z- handleDVL() - DVL msg epochMillisec: 1730862222.46 2024-11-06T03:03:42Z- handleDvl() - RDI_Pathfinder msg: 1730862222.46 sec; seqNo:688; #beams:4 2024-11-06T03:03:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.22 , 29.20 , 34.56 , 33.64 2024-11-06T03:03:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.06 , 0.00 2024-11-06T03:03:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862222.51 epoch sec; seqNo:2136; 4061689.76 north; 594487.94 east; 36.70 lat; -121.94 lon; 49.23 depth; utmZone: 10 2024-11-06T03:03:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862218.42 2024-11-06T03:03:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:42Z- waiting for 1730862222.42, time is 1730862222.51 2024-11-06T03:03:42Z- LcmTrn::updateTrn() >>>> pose time 1730862222.51, meas time 1730862222.46 <<<< 2024-11-06T03:03:42Z- LcmTrn::updateTrn() equating pose 1730862222.46 and meas times 1730862222.46 2024-11-06T03:03:42Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862222.46 2024-11-06T03:03:42Z- TerrainNav::motionUpdate() - currEstimate = (4061690, 594487.94, 49.23) 2024-11-06T03:03:42Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:03:42Z- TerrainNav:: SubcloudNIS is 0.30, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:03:42Z- TNavPF::Projecting Measurements 2024-11-06T03:03:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:03:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:03:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:03:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:03:42Z- TNavFilter::4 good beams. 2024-11-06T03:03:42Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:03:42Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:03:42Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:03:42Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:03:42Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:03:42Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:03:42Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:03:42Z- TerrainMapOctree: (vehN, vehE ) = (4061689.76, 594487.94). 2024-11-06T03:03:42Z- TerrainMapOctree: Min Distance = 245.44. 2024-11-06T03:03:42Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:03:42Z- Forcing Subcloud Comparison 2024-11-06T03:03:42Z- Weighting particles with subcloud comparison 2024-11-06T03:03:42Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:42Z- meanExpectedMeasDiff: 1.537083 delta_rms_squared: 0.894898 alpha: 0.978140 2024-11-06T03:03:42Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:42Z- meanExpectedMeasDiff: -1.432473 delta_rms_squared: 0.621003 alpha: 0.968799 2024-11-06T03:03:42Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:42Z- meanExpectedMeasDiff: 1.208708 delta_rms_squared: 0.687770 alpha: 0.971742 2024-11-06T03:03:42Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:42Z- meanExpectedMeasDiff: -0.402748 delta_rms_squared: 0.545974 alpha: 0.964663 2024-11-06T03:03:42Z- Subcloud NIS: 0.306607 2024-11-06T03:03:42Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:03:42Z- TNavPF:: sumWeights = 0.333731 2024-11-06T03:03:42Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:03:42Z- TNavPF::First Term of Map Covariance Matrix is 0.48 2024-11-06T03:03:42Z- TNavPF::Calculated NIS Value : 0.25 numBeams normalized NIS: 0.06 2024-11-06T03:03:42Z- TerrainNav::Resampling particle filter... 2024-11-06T03:03:42Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862222.46 sec, ping # 0. 2024-11-06T03:03:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:03:42Z- variances([0], [2], and psi_berg) = 0.692086 0.144666 0.000000 2024-11-06T03:03:42Z- publishEstimatesDecoder() - at 1730862222.46 sec 2024-11-06T03:03:42Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:03:42Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061722.30 594471.96 59.23 2024-11-06T03:03:42Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061722.06 594471.48 59.07 lat/lon: 36.70 -121.94 2024-11-06T03:03:42Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.69 0.14 0.32 0.00 2024-11-06T03:03:42Z- AHRS_M2 msg: 1730862222.82 epoch sec; seqNo:2001 2024-11-06T03:03:42Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:42Z- handleDepth()- Depth_Keller msg: 1730862222.82 epoch sec; seqNo:2137; depth 49.23 2024-11-06T03:03:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862222.84 epoch sec; seqNo:2137; 4061689.76 north; 594487.94 east; 36.70 lat; -121.94 lon; 49.23 depth; utmZone: 10 2024-11-06T03:03:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862222.46 2024-11-06T03:03:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:42Z- waiting for 1730862226.46, time is 1730862222.84 2024-11-06T03:03:43Z- AHRS_M2 msg: 1730862223.22 epoch sec; seqNo:2002 2024-11-06T03:03:43Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:43Z- handleDepth()- Depth_Keller msg: 1730862223.22 epoch sec; seqNo:2138; depth 49.24 2024-11-06T03:03:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862223.24 epoch sec; seqNo:2138; 4061689.76 north; 594487.94 east; 36.70 lat; -121.94 lon; 49.24 depth; utmZone: 10 2024-11-06T03:03:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862222.46 2024-11-06T03:03:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:43Z- waiting for 1730862226.46, time is 1730862223.24 2024-11-06T03:03:43Z- AHRS_M2 msg: 1730862223.62 epoch sec; seqNo:2003 2024-11-06T03:03:43Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:43Z- handleDepth()- Depth_Keller msg: 1730862223.63 epoch sec; seqNo:2139; depth 49.24 2024-11-06T03:03:43Z- handleDVL() - DVL msg epochMillisec: 1730862223.66 2024-11-06T03:03:43Z- handleDvl() - RDI_Pathfinder msg: 1730862223.66 sec; seqNo:689; #beams:4 2024-11-06T03:03:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.38 , 29.66 , 34.71 , 33.79 2024-11-06T03:03:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.04 , 0.03 2024-11-06T03:03:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862223.68 epoch sec; seqNo:2139; 4061689.76 north; 594487.94 east; 36.70 lat; -121.94 lon; 49.24 depth; utmZone: 10 2024-11-06T03:03:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862222.46 2024-11-06T03:03:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:43Z- waiting for 1730862226.46, time is 1730862223.68 2024-11-06T03:03:44Z- AHRS_M2 msg: 1730862224.03 epoch sec; seqNo:2004 2024-11-06T03:03:44Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:44Z- handleDepth()- Depth_Keller msg: 1730862224.03 epoch sec; seqNo:2140; depth 49.26 2024-11-06T03:03:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862224.05 epoch sec; seqNo:2140; 4061689.78 north; 594489.16 east; 36.70 lat; -121.94 lon; 49.26 depth; utmZone: 10 2024-11-06T03:03:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862222.46 2024-11-06T03:03:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:44Z- waiting for 1730862226.46, time is 1730862224.05 2024-11-06T03:03:44Z- AHRS_M2 msg: 1730862224.43 epoch sec; seqNo:2005 2024-11-06T03:03:44Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:44Z- handleDepth()- Depth_Keller msg: 1730862224.44 epoch sec; seqNo:2141; depth 49.26 2024-11-06T03:03:44Z- handleDVL() - DVL msg epochMillisec: 1730862224.46 2024-11-06T03:03:44Z- handleDvl() - RDI_Pathfinder msg: 1730862224.46 sec; seqNo:690; #beams:4 2024-11-06T03:03:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.84 , 29.82 , 34.56 , 33.79 2024-11-06T03:03:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , 0.04 2024-11-06T03:03:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862224.47 epoch sec; seqNo:2141; 4061689.78 north; 594489.16 east; 36.70 lat; -121.94 lon; 49.26 depth; utmZone: 10 2024-11-06T03:03:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862222.46 2024-11-06T03:03:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:44Z- waiting for 1730862226.46, time is 1730862224.47 2024-11-06T03:03:44Z- AHRS_M2 msg: 1730862224.84 epoch sec; seqNo:2006 2024-11-06T03:03:44Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:44Z- handleDepth()- Depth_Keller msg: 1730862224.84 epoch sec; seqNo:2142; depth 49.28 2024-11-06T03:03:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862224.86 epoch sec; seqNo:2142; 4061690.16 north; 594489.15 east; 36.70 lat; -121.94 lon; 49.28 depth; utmZone: 10 2024-11-06T03:03:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862222.46 2024-11-06T03:03:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:44Z- waiting for 1730862226.46, time is 1730862224.86 2024-11-06T03:03:45Z- AHRS_M2 msg: 1730862225.24 epoch sec; seqNo:2007 2024-11-06T03:03:45Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:45Z- handleDepth()- Depth_Keller msg: 1730862225.25 epoch sec; seqNo:2143; depth 49.28 2024-11-06T03:03:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862225.26 epoch sec; seqNo:2143; 4061690.16 north; 594489.15 east; 36.70 lat; -121.94 lon; 49.28 depth; utmZone: 10 2024-11-06T03:03:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862222.46 2024-11-06T03:03:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:45Z- waiting for 1730862226.46, time is 1730862225.26 2024-11-06T03:03:45Z- AHRS_M2 msg: 1730862225.65 epoch sec; seqNo:2008 2024-11-06T03:03:45Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:45Z- handleDepth()- Depth_Keller msg: 1730862225.65 epoch sec; seqNo:2144; depth 49.30 2024-11-06T03:03:45Z- handleDVL() - DVL msg epochMillisec: 1730862225.67 2024-11-06T03:03:45Z- handleDvl() - RDI_Pathfinder msg: 1730862225.67 sec; seqNo:691; #beams:4 2024-11-06T03:03:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.84 , 30.12 , 34.86 , 33.33 2024-11-06T03:03:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , 0.05 2024-11-06T03:03:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862225.70 epoch sec; seqNo:2144; 4061690.17 north; 594490.37 east; 36.70 lat; -121.94 lon; 49.30 depth; utmZone: 10 2024-11-06T03:03:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862222.46 2024-11-06T03:03:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:45Z- waiting for 1730862226.46, time is 1730862225.70 2024-11-06T03:03:46Z- AHRS_M2 msg: 1730862226.05 epoch sec; seqNo:2009 2024-11-06T03:03:46Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:46Z- handleDepth()- Depth_Keller msg: 1730862226.06 epoch sec; seqNo:2145; depth 49.31 2024-11-06T03:03:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862226.07 epoch sec; seqNo:2145; 4061690.17 north; 594490.37 east; 36.70 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T03:03:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862222.46 2024-11-06T03:03:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:46Z- waiting for 1730862226.46, time is 1730862226.07 2024-11-06T03:03:46Z- AHRS_M2 msg: 1730862226.45 epoch sec; seqNo:2010 2024-11-06T03:03:46Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:46Z- handleDepth()- Depth_Keller msg: 1730862226.47 epoch sec; seqNo:2146; depth 49.33 2024-11-06T03:03:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862226.47 epoch sec; seqNo:2146; 4061690.17 north; 594490.37 east; 36.70 lat; -121.94 lon; 49.33 depth; utmZone: 10 2024-11-06T03:03:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862222.46 2024-11-06T03:03:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:46Z- waiting for 1730862226.46, time is 1730862226.47 2024-11-06T03:03:46Z- LcmTrn::updateTrn() >>>> pose time 1730862226.47, meas time 1730862225.67 <<<< 2024-11-06T03:03:46Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862226.47 2024-11-06T03:03:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862225.67 sec, ping # 0; waiting for INS data more recent than 1730862222.46... 2024-11-06T03:03:46Z- TerrainNav::motionUpdate() - currEstimate = (4061690, 594490.37, 49.33) 2024-11-06T03:03:46Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:03:46Z- TerrainNav:: SubcloudNIS is 0.31, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:03:46Z- TNavPF::Projecting Measurements 2024-11-06T03:03:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:03:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:03:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:03:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:03:46Z- TNavFilter::4 good beams. 2024-11-06T03:03:46Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:03:46Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:03:46Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:03:46Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:03:46Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:03:46Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:03:46Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:03:46Z- TerrainMapOctree: (vehN, vehE ) = (4061690.09, 594489.89). 2024-11-06T03:03:46Z- TerrainMapOctree: Min Distance = 247.26. 2024-11-06T03:03:46Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:03:46Z- Forcing Subcloud Comparison 2024-11-06T03:03:46Z- Weighting particles with subcloud comparison 2024-11-06T03:03:46Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:46Z- meanExpectedMeasDiff: 2.063478 delta_rms_squared: 0.953018 alpha: 0.979445 2024-11-06T03:03:46Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:46Z- meanExpectedMeasDiff: -0.509367 delta_rms_squared: 0.658178 alpha: 0.970509 2024-11-06T03:03:46Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:46Z- meanExpectedMeasDiff: 1.495313 delta_rms_squared: 0.730062 alpha: 0.973336 2024-11-06T03:03:46Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:46Z- meanExpectedMeasDiff: -1.274759 delta_rms_squared: 0.709491 alpha: 0.972584 2024-11-06T03:03:46Z- Subcloud NIS: 0.313532 2024-11-06T03:03:46Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:03:46Z- TNavPF:: sumWeights = 0.267694 2024-11-06T03:03:46Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:03:46Z- TNavPF::First Term of Map Covariance Matrix is 0.62 2024-11-06T03:03:46Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T03:03:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862225.67 sec. 2024-11-06T03:03:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:03:46Z- variances([0], [2], and psi_berg) = 0.690305 0.145207 0.000000 2024-11-06T03:03:46Z- publishEstimatesDecoder() - at 1730862226.47 sec 2024-11-06T03:03:46Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:03:46Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061723.41 594473.38 59.41 2024-11-06T03:03:46Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061722.51 594473.91 58.97 lat/lon: 36.70 -121.94 2024-11-06T03:03:46Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.69 0.15 0.58 0.00 2024-11-06T03:03:46Z- AHRS_M2 msg: 1730862226.87 epoch sec; seqNo:2011 2024-11-06T03:03:46Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:46Z- handleDepth()- Depth_Keller msg: 1730862226.88 epoch sec; seqNo:2147; depth 49.35 2024-11-06T03:03:46Z- handleDVL() - DVL msg epochMillisec: 1730862226.89 2024-11-06T03:03:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:03:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:03:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:03:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:03:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:03:46Z- handleDvl() - RDI_Pathfinder msg: 1730862226.89 sec; seqNo:692; #beams:0 2024-11-06T03:03:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:03:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , 0.05 2024-11-06T03:03:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862226.89 epoch sec; seqNo:2147; 4061690.18 north; 594491.59 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:03:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862226.47 2024-11-06T03:03:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:47Z- waiting for 1730862230.47, time is 1730862226.89 2024-11-06T03:03:47Z- AHRS_M2 msg: 1730862227.26 epoch sec; seqNo:2012 2024-11-06T03:03:47Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:47Z- handleDepth()- Depth_Keller msg: 1730862227.26 epoch sec; seqNo:2148; depth 49.38 2024-11-06T03:03:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862227.27 epoch sec; seqNo:2148; 4061690.18 north; 594491.59 east; 36.70 lat; -121.94 lon; 49.38 depth; utmZone: 10 2024-11-06T03:03:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862226.47 2024-11-06T03:03:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:47Z- waiting for 1730862230.47, time is 1730862227.27 2024-11-06T03:03:47Z- AHRS_M2 msg: 1730862227.67 epoch sec; seqNo:2013 2024-11-06T03:03:47Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.50 psi 2024-11-06T03:03:47Z- handleDepth()- Depth_Keller msg: 1730862227.68 epoch sec; seqNo:2149; depth 49.38 2024-11-06T03:03:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862227.70 epoch sec; seqNo:2149; 4061690.18 north; 594491.59 east; 36.70 lat; -121.94 lon; 49.38 depth; utmZone: 10 2024-11-06T03:03:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862226.47 2024-11-06T03:03:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:47Z- waiting for 1730862230.47, time is 1730862227.70 2024-11-06T03:03:48Z- AHRS_M2 msg: 1730862228.07 epoch sec; seqNo:2014 2024-11-06T03:03:48Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.50 psi 2024-11-06T03:03:48Z- handleDepth()- Depth_Keller msg: 1730862228.08 epoch sec; seqNo:2150; depth 49.39 2024-11-06T03:03:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862228.10 epoch sec; seqNo:2150; 4061690.18 north; 594491.59 east; 36.70 lat; -121.94 lon; 49.39 depth; utmZone: 10 2024-11-06T03:03:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862226.47 2024-11-06T03:03:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:48Z- waiting for 1730862230.47, time is 1730862228.10 2024-11-06T03:03:48Z- AHRS_M2 msg: 1730862228.47 epoch sec; seqNo:2015 2024-11-06T03:03:48Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.50 psi 2024-11-06T03:03:48Z- handleDepth()- Depth_Keller msg: 1730862228.48 epoch sec; seqNo:2151; depth 49.43 2024-11-06T03:03:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862228.49 epoch sec; seqNo:2151; 4061690.20 north; 594492.82 east; 36.70 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T03:03:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862226.47 2024-11-06T03:03:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:48Z- waiting for 1730862230.47, time is 1730862228.49 2024-11-06T03:03:48Z- AHRS_M2 msg: 1730862228.88 epoch sec; seqNo:2016 2024-11-06T03:03:48Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.51 psi 2024-11-06T03:03:48Z- handleDepth()- Depth_Keller msg: 1730862228.88 epoch sec; seqNo:2152; depth 49.45 2024-11-06T03:03:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862228.89 epoch sec; seqNo:2152; 4061690.20 north; 594492.82 east; 36.70 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:03:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862226.47 2024-11-06T03:03:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:49Z- waiting for 1730862230.47, time is 1730862228.89 2024-11-06T03:03:49Z- AHRS_M2 msg: 1730862229.28 epoch sec; seqNo:2017 2024-11-06T03:03:49Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.51 psi 2024-11-06T03:03:49Z- handleDepth()- Depth_Keller msg: 1730862229.30 epoch sec; seqNo:2153; depth 49.45 2024-11-06T03:03:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862229.31 epoch sec; seqNo:2153; 4061690.20 north; 594492.82 east; 36.70 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:03:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862226.47 2024-11-06T03:03:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:49Z- waiting for 1730862230.47, time is 1730862229.31 2024-11-06T03:03:49Z- AHRS_M2 msg: 1730862229.69 epoch sec; seqNo:2018 2024-11-06T03:03:49Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.51 psi 2024-11-06T03:03:49Z- handleDepth()- Depth_Keller msg: 1730862229.70 epoch sec; seqNo:2154; depth 49.48 2024-11-06T03:03:49Z- handleDVL() - DVL msg epochMillisec: 1730862229.72 2024-11-06T03:03:49Z- handleDvl() - RDI_Pathfinder msg: 1730862229.72 sec; seqNo:693; #beams:4 2024-11-06T03:03:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.24 , 30.58 , 34.83 , 33.47 2024-11-06T03:03:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , 0.06 2024-11-06T03:03:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862229.74 epoch sec; seqNo:2154; 4061690.20 north; 594492.82 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:03:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862226.47 2024-11-06T03:03:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:49Z- waiting for 1730862230.47, time is 1730862229.74 2024-11-06T03:03:50Z- AHRS_M2 msg: 1730862230.09 epoch sec; seqNo:2019 2024-11-06T03:03:50Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.51 psi 2024-11-06T03:03:50Z- handleDepth()- Depth_Keller msg: 1730862230.09 epoch sec; seqNo:2155; depth 49.49 2024-11-06T03:03:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862230.12 epoch sec; seqNo:2155; 4061690.59 north; 594494.03 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:03:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862226.47 2024-11-06T03:03:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:50Z- waiting for 1730862230.47, time is 1730862230.12 2024-11-06T03:03:50Z- AHRS_M2 msg: 1730862230.50 epoch sec; seqNo:2020 2024-11-06T03:03:50Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.51 psi 2024-11-06T03:03:50Z- handleDepth()- Depth_Keller msg: 1730862230.51 epoch sec; seqNo:2156; depth 49.50 2024-11-06T03:03:50Z- handleDVL() - DVL msg epochMillisec: 1730862230.52 2024-11-06T03:03:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:03:50Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:03:50Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:03:50Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:03:50Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:03:50Z- handleDvl() - RDI_Pathfinder msg: 1730862230.52 sec; seqNo:694; #beams:0 2024-11-06T03:03:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:03:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , 0.06 2024-11-06T03:03:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862230.52 epoch sec; seqNo:2156; 4061690.59 north; 594494.03 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:03:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862226.47 2024-11-06T03:03:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:50Z- waiting for 1730862230.47, time is 1730862230.52 2024-11-06T03:03:50Z- LcmTrn::updateTrn() >>>> pose time 1730862230.52, meas time 1730862230.52 <<<< 2024-11-06T03:03:50Z- LcmTrn::updateTrn() equating pose 1730862230.52 and meas times 1730862230.52 2024-11-06T03:03:50Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862230.52 2024-11-06T03:03:50Z- TerrainNav::motionUpdate() - currEstimate = (4061691, 594494.03, 49.50) 2024-11-06T03:03:50Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:03:50Z- TerrainNav:: SubcloudNIS is 0.31, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:03:50Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:03:50Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:03:50Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:03:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:03:50Z- variances([0], [2], and psi_berg) = 0.692263 0.147827 0.000000 2024-11-06T03:03:50Z- publishEstimatesDecoder() - at 1730862230.52 sec 2024-11-06T03:03:50Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:03:50Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061723.81 594477.08 59.65 2024-11-06T03:03:50Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061722.93 594477.57 59.14 lat/lon: 36.70 -121.94 2024-11-06T03:03:50Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.69 0.15 0.82 0.00 2024-11-06T03:03:50Z- AHRS_M2 msg: 1730862230.89 epoch sec; seqNo:2021 2024-11-06T03:03:50Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.51 psi 2024-11-06T03:03:50Z- handleDepth()- Depth_Keller msg: 1730862230.91 epoch sec; seqNo:2157; depth 49.54 2024-11-06T03:03:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862230.92 epoch sec; seqNo:2157; 4061690.59 north; 594494.03 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T03:03:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862230.52 2024-11-06T03:03:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:51Z- waiting for 1730862234.52, time is 1730862230.92 2024-11-06T03:03:51Z- AHRS_M2 msg: 1730862231.30 epoch sec; seqNo:2022 2024-11-06T03:03:51Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 1.51 psi 2024-11-06T03:03:51Z- handleDepth()- Depth_Keller msg: 1730862231.30 epoch sec; seqNo:2158; depth 49.54 2024-11-06T03:03:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862231.32 epoch sec; seqNo:2158; 4061690.60 north; 594495.25 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T03:03:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862230.52 2024-11-06T03:03:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:51Z- waiting for 1730862234.52, time is 1730862231.32 2024-11-06T03:03:51Z- handleDepth()- Depth_Keller msg: 1730862231.73 epoch sec; seqNo:2159; depth 49.55 2024-11-06T03:03:51Z- handleDVL() - DVL msg epochMillisec: 1730862231.75 2024-11-06T03:03:51Z- handleDvl() - RDI_Pathfinder msg: 1730862231.75 sec; seqNo:695; #beams:4 2024-11-06T03:03:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.41 , 31.26 , 34.66 , 33.98 2024-11-06T03:03:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , 0.05 2024-11-06T03:03:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862231.77 epoch sec; seqNo:2159; 4061690.60 north; 594495.25 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:03:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862230.52 2024-11-06T03:03:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:51Z- waiting for 1730862234.52, time is 1730862231.77 2024-11-06T03:03:52Z- AHRS_M2 msg: 1730862232.11 epoch sec; seqNo:2023 2024-11-06T03:03:52Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.52 psi 2024-11-06T03:03:52Z- handleDepth()- Depth_Keller msg: 1730862232.12 epoch sec; seqNo:2160; depth 49.56 2024-11-06T03:03:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862232.14 epoch sec; seqNo:2160; 4061690.60 north; 594495.25 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:03:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862230.52 2024-11-06T03:03:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:52Z- waiting for 1730862234.52, time is 1730862232.14 2024-11-06T03:03:52Z- AHRS_M2 msg: 1730862232.52 epoch sec; seqNo:2024 2024-11-06T03:03:52Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.52 psi 2024-11-06T03:03:52Z- handleDepth()- Depth_Keller msg: 1730862232.53 epoch sec; seqNo:2161; depth 49.57 2024-11-06T03:03:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862232.56 epoch sec; seqNo:2161; 4061690.60 north; 594495.25 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:03:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862230.52 2024-11-06T03:03:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:52Z- waiting for 1730862234.52, time is 1730862232.56 2024-11-06T03:03:52Z- AHRS_M2 msg: 1730862232.91 epoch sec; seqNo:2025 2024-11-06T03:03:52Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.52 psi 2024-11-06T03:03:52Z- handleDepth()- Depth_Keller msg: 1730862232.92 epoch sec; seqNo:2162; depth 49.57 2024-11-06T03:03:52Z- handleDVL() - DVL msg epochMillisec: 1730862232.92 2024-11-06T03:03:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:03:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:03:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:03:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:03:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:03:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:03:52Z- handleDvl() - RDI_Pathfinder msg: 1730862232.92 sec; seqNo:696; #beams:0 2024-11-06T03:03:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:03:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , 0.05 2024-11-06T03:03:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862232.93 epoch sec; seqNo:2162; 4061690.62 north; 594496.47 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:03:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862230.52 2024-11-06T03:03:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:53Z- waiting for 1730862234.52, time is 1730862232.93 2024-11-06T03:03:53Z- AHRS_M2 msg: 1730862233.32 epoch sec; seqNo:2026 2024-11-06T03:03:53Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.52 psi 2024-11-06T03:03:53Z- handleDepth()- Depth_Keller msg: 1730862233.33 epoch sec; seqNo:2163; depth 49.57 2024-11-06T03:03:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862233.34 epoch sec; seqNo:2163; 4061690.62 north; 594496.47 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:03:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862230.52 2024-11-06T03:03:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:53Z- waiting for 1730862234.52, time is 1730862233.34 2024-11-06T03:03:53Z- handleDepth()- Depth_Keller msg: 1730862233.75 epoch sec; seqNo:2164; depth 49.58 2024-11-06T03:03:53Z- handleDVL() - DVL msg epochMillisec: 1730862233.77 2024-11-06T03:03:53Z- handleDvl() - RDI_Pathfinder msg: 1730862233.77 sec; seqNo:697; #beams:4 2024-11-06T03:03:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.58 , 31.94 , 34.83 , 33.98 2024-11-06T03:03:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , 0.01 2024-11-06T03:03:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862233.78 epoch sec; seqNo:2164; 4061690.62 north; 594496.47 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:03:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862230.52 2024-11-06T03:03:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:53Z- waiting for 1730862234.52, time is 1730862233.78 2024-11-06T03:03:54Z- AHRS_M2 msg: 1730862234.13 epoch sec; seqNo:2027 2024-11-06T03:03:54Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.52 psi 2024-11-06T03:03:54Z- handleDepth()- Depth_Keller msg: 1730862234.13 epoch sec; seqNo:2165; depth 49.58 2024-11-06T03:03:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862234.15 epoch sec; seqNo:2165; 4061690.63 north; 594497.69 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:03:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862230.52 2024-11-06T03:03:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:54Z- waiting for 1730862234.52, time is 1730862234.15 2024-11-06T03:03:54Z- AHRS_M2 msg: 1730862234.53 epoch sec; seqNo:2028 2024-11-06T03:03:54Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.52 psi 2024-11-06T03:03:54Z- handleDepth()- Depth_Keller msg: 1730862234.55 epoch sec; seqNo:2166; depth 49.59 2024-11-06T03:03:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862234.56 epoch sec; seqNo:2166; 4061690.63 north; 594497.69 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:03:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862230.52 2024-11-06T03:03:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:54Z- waiting for 1730862234.52, time is 1730862234.56 2024-11-06T03:03:54Z- LcmTrn::updateTrn() >>>> pose time 1730862234.56, meas time 1730862233.77 <<<< 2024-11-06T03:03:54Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862234.56 2024-11-06T03:03:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862233.77 sec, ping # 0; waiting for INS data more recent than 1730862230.52... 2024-11-06T03:03:54Z- TerrainNav::motionUpdate() - currEstimate = (4061691, 594497.69, 49.59) 2024-11-06T03:03:54Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:03:54Z- TerrainNav:: SubcloudNIS is 0.31, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:03:54Z- TNavPF::Projecting Measurements 2024-11-06T03:03:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:03:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:03:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:03:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:03:54Z- TNavFilter::4 good beams. 2024-11-06T03:03:54Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:03:54Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:03:54Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:03:54Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:03:54Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:03:54Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:03:54Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:03:54Z- TerrainMapOctree: (vehN, vehE ) = (4061690.62, 594496.98). 2024-11-06T03:03:54Z- TerrainMapOctree: Min Distance = 254.02. 2024-11-06T03:03:54Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:03:54Z- Forcing Subcloud Comparison 2024-11-06T03:03:54Z- Weighting particles with subcloud comparison 2024-11-06T03:03:54Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:54Z- meanExpectedMeasDiff: 3.152798 delta_rms_squared: 1.698038 alpha: 0.988359 2024-11-06T03:03:54Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:54Z- meanExpectedMeasDiff: 0.624545 delta_rms_squared: 1.288443 alpha: 0.984715 2024-11-06T03:03:54Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:54Z- meanExpectedMeasDiff: 1.718543 delta_rms_squared: 1.327940 alpha: 0.985163 2024-11-06T03:03:54Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:03:54Z- meanExpectedMeasDiff: -0.780042 delta_rms_squared: 1.569998 alpha: 0.987421 2024-11-06T03:03:54Z- Subcloud NIS: 0.325774 2024-11-06T03:03:54Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:03:54Z- TNavPF:: sumWeights = 0.165448 2024-11-06T03:03:54Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:03:54Z- TNavPF::First Term of Map Covariance Matrix is 0.80 2024-11-06T03:03:54Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T03:03:54Z- TerrainNav::Resampling particle filter... 2024-11-06T03:03:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862233.77 sec. 2024-11-06T03:03:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:03:54Z- variances([0], [2], and psi_berg) = 0.684223 0.148215 0.000000 2024-11-06T03:03:54Z- publishEstimatesDecoder() - at 1730862234.56 sec 2024-11-06T03:03:54Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:03:54Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061723.15 594481.81 58.23 2024-11-06T03:03:54Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061722.99 594481.23 58.65 lat/lon: 36.70 -121.94 2024-11-06T03:03:54Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.68 0.15 0.70 0.00 2024-11-06T03:03:54Z- AHRS_M2 msg: 1730862234.93 epoch sec; seqNo:2029 2024-11-06T03:03:54Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.52 psi 2024-11-06T03:03:54Z- handleDepth()- Depth_Keller msg: 1730862234.95 epoch sec; seqNo:2167; depth 49.60 2024-11-06T03:03:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862234.96 epoch sec; seqNo:2167; 4061690.63 north; 594497.69 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:03:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862234.56 2024-11-06T03:03:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:55Z- waiting for 1730862238.56, time is 1730862234.96 2024-11-06T03:03:55Z- AHRS_M2 msg: 1730862235.34 epoch sec; seqNo:2030 2024-11-06T03:03:55Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.52 psi 2024-11-06T03:03:55Z- handleDepth()- Depth_Keller msg: 1730862235.35 epoch sec; seqNo:2168; depth 49.60 2024-11-06T03:03:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862235.36 epoch sec; seqNo:2168; 4061690.64 north; 594498.91 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:03:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862234.56 2024-11-06T03:03:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:55Z- waiting for 1730862238.56, time is 1730862235.36 2024-11-06T03:03:55Z- AHRS_M2 msg: 1730862235.75 epoch sec; seqNo:2031 2024-11-06T03:03:55Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.53 psi 2024-11-06T03:03:55Z- handleDepth()- Depth_Keller msg: 1730862235.76 epoch sec; seqNo:2169; depth 49.59 2024-11-06T03:03:55Z- handleDVL() - DVL msg epochMillisec: 1730862235.79 2024-11-06T03:03:55Z- handleDvl() - RDI_Pathfinder msg: 1730862235.79 sec; seqNo:698; #beams:4 2024-11-06T03:03:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.07 , 32.11 , 34.66 , 34.49 2024-11-06T03:03:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , -0.02 2024-11-06T03:03:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862235.80 epoch sec; seqNo:2169; 4061690.64 north; 594498.91 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:03:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862234.56 2024-11-06T03:03:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:55Z- waiting for 1730862238.56, time is 1730862235.80 2024-11-06T03:03:56Z- AHRS_M2 msg: 1730862236.15 epoch sec; seqNo:2032 2024-11-06T03:03:56Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.53 psi 2024-11-06T03:03:56Z- handleDepth()- Depth_Keller msg: 1730862236.16 epoch sec; seqNo:2170; depth 49.59 2024-11-06T03:03:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862236.17 epoch sec; seqNo:2170; 4061691.02 north; 594498.91 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:03:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862234.56 2024-11-06T03:03:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:56Z- waiting for 1730862238.56, time is 1730862236.17 2024-11-06T03:03:56Z- AHRS_M2 msg: 1730862236.55 epoch sec; seqNo:2033 2024-11-06T03:03:56Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.53 psi 2024-11-06T03:03:56Z- handleDepth()- Depth_Keller msg: 1730862236.56 epoch sec; seqNo:2171; depth 49.59 2024-11-06T03:03:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862236.57 epoch sec; seqNo:2171; 4061691.02 north; 594498.91 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:03:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862234.56 2024-11-06T03:03:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:56Z- waiting for 1730862238.56, time is 1730862236.57 2024-11-06T03:03:56Z- AHRS_M2 msg: 1730862236.96 epoch sec; seqNo:2034 2024-11-06T03:03:56Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.53 psi 2024-11-06T03:03:56Z- handleDepth()- Depth_Keller msg: 1730862236.96 epoch sec; seqNo:2172; depth 49.60 2024-11-06T03:03:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862236.97 epoch sec; seqNo:2172; 4061691.03 north; 594500.13 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:03:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862234.56 2024-11-06T03:03:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:57Z- waiting for 1730862238.56, time is 1730862236.97 2024-11-06T03:03:57Z- AHRS_M2 msg: 1730862237.36 epoch sec; seqNo:2035 2024-11-06T03:03:57Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.53 psi 2024-11-06T03:03:57Z- handleDepth()- Depth_Keller msg: 1730862237.37 epoch sec; seqNo:2173; depth 49.59 2024-11-06T03:03:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862237.38 epoch sec; seqNo:2173; 4061691.03 north; 594500.13 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:03:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862234.56 2024-11-06T03:03:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:57Z- waiting for 1730862238.56, time is 1730862237.38 2024-11-06T03:03:57Z- AHRS_M2 msg: 1730862237.76 epoch sec; seqNo:2036 2024-11-06T03:03:57Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.53 psi 2024-11-06T03:03:57Z- handleDepth()- Depth_Keller msg: 1730862237.77 epoch sec; seqNo:2174; depth 49.62 2024-11-06T03:03:57Z- handleDVL() - DVL msg epochMillisec: 1730862237.80 2024-11-06T03:03:57Z- handleDvl() - RDI_Pathfinder msg: 1730862237.80 sec; seqNo:699; #beams:4 2024-11-06T03:03:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.90 , 31.94 , 34.15 , 34.83 2024-11-06T03:03:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , -0.03 2024-11-06T03:03:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862237.81 epoch sec; seqNo:2174; 4061691.03 north; 594500.13 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:03:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:03:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862234.56 2024-11-06T03:03:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:57Z- waiting for 1730862238.56, time is 1730862237.81 2024-11-06T03:03:58Z- AHRS_M2 msg: 1730862238.17 epoch sec; seqNo:2037 2024-11-06T03:03:58Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.53 psi 2024-11-06T03:03:58Z- handleDepth()- Depth_Keller msg: 1730862238.17 epoch sec; seqNo:2175; depth 49.61 2024-11-06T03:03:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862238.19 epoch sec; seqNo:2175; 4061691.05 north; 594501.35 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:03:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862234.56 2024-11-06T03:03:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:58Z- waiting for 1730862238.56, time is 1730862238.19 2024-11-06T03:03:58Z- AHRS_M2 msg: 1730862238.58 epoch sec; seqNo:2038 2024-11-06T03:03:58Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.52 psi 2024-11-06T03:03:58Z- handleDepth()- Depth_Keller msg: 1730862238.58 epoch sec; seqNo:2176; depth 49.63 2024-11-06T03:03:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862238.59 epoch sec; seqNo:2176; 4061691.05 north; 594501.35 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:03:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:03:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862234.56 2024-11-06T03:03:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:03:58Z- waiting for 1730862238.56, time is 1730862238.59 2024-11-06T03:03:58Z- LcmTrn::updateTrn() >>>> pose time 1730862238.59, meas time 1730862237.80 <<<< 2024-11-06T03:03:58Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862238.59 2024-11-06T03:03:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862237.80 sec, ping # 0; waiting for INS data more recent than 1730862234.56... 2024-11-06T03:03:58Z- TerrainNav::motionUpdate() - currEstimate = (4061691, 594501.35, 49.63) 2024-11-06T03:03:58Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:03:58Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:03:58Z- TNavPF::Projecting Measurements 2024-11-06T03:03:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:03:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:03:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:03:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:03:58Z- TNavFilter::4 good beams. 2024-11-06T03:03:58Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:03:58Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:03:58Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:03:58Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:03:58Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:03:58Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:03:58Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:03:58Z- TerrainMapOctree: (vehN, vehE ) = (4061690.97, 594500.63). 2024-11-06T03:03:58Z- TerrainMapOctree: Min Distance = 256.26. 2024-11-06T03:03:58Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:03:59Z- TerrainMapOctree: Switching to tile 5. 2024-11-06T03:04:00Z- TerrainMapOctree::Octree tile load /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile5.bo took 1.234973 seconds. 2024-11-06T03:04:00Z- Forcing Subcloud Comparison 2024-11-06T03:04:00Z- Weighting particles with subcloud comparison 2024-11-06T03:04:00Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:00Z- meanExpectedMeasDiff: 1.811620 delta_rms_squared: 1.525807 alpha: 0.987062 2024-11-06T03:04:00Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:00Z- meanExpectedMeasDiff: -0.416514 delta_rms_squared: 1.096572 alpha: 0.982088 2024-11-06T03:04:00Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:00Z- meanExpectedMeasDiff: 0.445886 delta_rms_squared: 1.190597 alpha: 0.983479 2024-11-06T03:04:00Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:00Z- meanExpectedMeasDiff: -0.582155 delta_rms_squared: 1.408270 alpha: 0.985997 2024-11-06T03:04:00Z- Subcloud NIS: 0.324569 2024-11-06T03:04:00Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:04:00Z- TNavPF:: sumWeights = 0.405546 2024-11-06T03:04:00Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:04:00Z- TNavPF::First Term of Map Covariance Matrix is 0.65 2024-11-06T03:04:00Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.05 2024-11-06T03:04:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862237.80 sec. 2024-11-06T03:04:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:04:00Z- variances([0], [2], and psi_berg) = 0.689522 0.152357 0.000000 2024-11-06T03:04:00Z- publishEstimatesDecoder() - at 1730862238.59 sec 2024-11-06T03:04:00Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:04:00Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061721.41 594484.71 59.05 2024-11-06T03:04:00Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061723.41 594484.88 58.56 lat/lon: 36.70 -121.94 2024-11-06T03:04:00Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.69 0.15 0.65 0.00 2024-11-06T03:04:00Z- handleDepth()- Depth_Keller msg: 1730862239.00 epoch sec; seqNo:2177; depth 49.64 2024-11-06T03:04:00Z- handleDVL() - DVL msg epochMillisec: 1730862239.00 2024-11-06T03:04:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:04:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:04:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:04:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:04:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:04:00Z- handleDvl() - RDI_Pathfinder msg: 1730862239.00 sec; seqNo:700; #beams:0 2024-11-06T03:04:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:04:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , -0.03 2024-11-06T03:04:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862239.01 epoch sec; seqNo:2177; 4061691.05 north; 594501.35 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:04:00Z- handleDepth()- Depth_Keller msg: 1730862239.40 epoch sec; seqNo:2178; depth 49.64 2024-11-06T03:04:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862239.41 epoch sec; seqNo:2178; 4061691.05 north; 594501.35 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:04:00Z- AHRS_M2 msg: 1730862239.79 epoch sec; seqNo:2039 2024-11-06T03:04:00Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:00Z- handleDepth()- Depth_Keller msg: 1730862239.80 epoch sec; seqNo:2179; depth 49.65 2024-11-06T03:04:00Z- handleDVL() - DVL msg epochMillisec: 1730862239.81 2024-11-06T03:04:00Z- handleDvl() - RDI_Pathfinder msg: 1730862239.81 sec; seqNo:701; #beams:4 2024-11-06T03:04:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.74 , 32.45 , 34.15 , 34.49 2024-11-06T03:04:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.03 , -0.00 2024-11-06T03:04:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862239.83 epoch sec; seqNo:2179; 4061691.06 north; 594502.57 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:04:00Z- AHRS_M2 msg: 1730862240.19 epoch sec; seqNo:2040 2024-11-06T03:04:00Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:00Z- handleDepth()- Depth_Keller msg: 1730862240.20 epoch sec; seqNo:2180; depth 49.68 2024-11-06T03:04:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862240.22 epoch sec; seqNo:2180; 4061691.06 north; 594502.57 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:04:00Z- AHRS_M2 msg: 1730862240.60 epoch sec; seqNo:2041 2024-11-06T03:04:00Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:00Z- handleDepth()- Depth_Keller msg: 1730862240.61 epoch sec; seqNo:2181; depth 49.67 2024-11-06T03:04:00Z- 13 msgs in 200 ms and 14 handle calls 2024-11-06T03:04:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862238.59 2024-11-06T03:04:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:00Z- waiting for 1730862242.59, time is 1730862240.60 2024-11-06T03:04:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862240.62 epoch sec; seqNo:2181; 4061691.06 north; 594502.57 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:04:00Z- 1 msgs in 200 ms and 2 handle calls 2024-11-06T03:04:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862238.59 2024-11-06T03:04:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:00Z- waiting for 1730862242.59, time is 1730862240.62 2024-11-06T03:04:01Z- AHRS_M2 msg: 1730862241.00 epoch sec; seqNo:2042 2024-11-06T03:04:01Z- AHRS_M2 msg: 0.12 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:01Z- handleDepth()- Depth_Keller msg: 1730862241.00 epoch sec; seqNo:2182; depth 49.71 2024-11-06T03:04:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862241.01 epoch sec; seqNo:2182; 4061691.06 north; 594502.57 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:04:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862238.59 2024-11-06T03:04:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:01Z- waiting for 1730862242.59, time is 1730862241.01 2024-11-06T03:04:01Z- AHRS_M2 msg: 1730862241.41 epoch sec; seqNo:2043 2024-11-06T03:04:01Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:01Z- handleDepth()- Depth_Keller msg: 1730862241.42 epoch sec; seqNo:2183; depth 49.73 2024-11-06T03:04:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862241.43 epoch sec; seqNo:2183; 4061691.08 north; 594503.79 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:04:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862238.59 2024-11-06T03:04:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:01Z- waiting for 1730862242.59, time is 1730862241.43 2024-11-06T03:04:01Z- AHRS_M2 msg: 1730862241.80 epoch sec; seqNo:2044 2024-11-06T03:04:01Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:01Z- handleDepth()- Depth_Keller msg: 1730862241.81 epoch sec; seqNo:2184; depth 49.74 2024-11-06T03:04:01Z- handleDVL() - DVL msg epochMillisec: 1730862241.84 2024-11-06T03:04:01Z- handleDvl() - RDI_Pathfinder msg: 1730862241.84 sec; seqNo:702; #beams:4 2024-11-06T03:04:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.56 , 32.96 , 34.49 , 35.17 2024-11-06T03:04:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.03 , 0.05 2024-11-06T03:04:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862241.85 epoch sec; seqNo:2184; 4061691.08 north; 594503.79 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:04:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862238.59 2024-11-06T03:04:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:01Z- waiting for 1730862242.59, time is 1730862241.85 2024-11-06T03:04:02Z- AHRS_M2 msg: 1730862242.21 epoch sec; seqNo:2045 2024-11-06T03:04:02Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:02Z- handleDepth()- Depth_Keller msg: 1730862242.22 epoch sec; seqNo:2185; depth 49.78 2024-11-06T03:04:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862242.23 epoch sec; seqNo:2185; 4061691.08 north; 594503.79 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:04:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862238.59 2024-11-06T03:04:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:02Z- waiting for 1730862242.59, time is 1730862242.23 2024-11-06T03:04:02Z- AHRS_M2 msg: 1730862242.62 epoch sec; seqNo:2046 2024-11-06T03:04:02Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:02Z- handleDepth()- Depth_Keller msg: 1730862242.63 epoch sec; seqNo:2186; depth 49.81 2024-11-06T03:04:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862242.65 epoch sec; seqNo:2186; 4061691.45 north; 594503.79 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:04:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862238.59 2024-11-06T03:04:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:02Z- waiting for 1730862242.59, time is 1730862242.65 2024-11-06T03:04:02Z- LcmTrn::updateTrn() >>>> pose time 1730862242.65, meas time 1730862241.84 <<<< 2024-11-06T03:04:02Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862242.65 2024-11-06T03:04:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862241.84 sec, ping # 0; waiting for INS data more recent than 1730862238.59... 2024-11-06T03:04:02Z- TerrainNav::motionUpdate() - currEstimate = (4061691, 594503.79, 49.81) 2024-11-06T03:04:02Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:04:02Z- TerrainNav:: SubcloudNIS is 0.32, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:04:02Z- TNavPF::Projecting Measurements 2024-11-06T03:04:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:04:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:04:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:04:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:04:02Z- TNavFilter::4 good beams. 2024-11-06T03:04:02Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:04:02Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:04:02Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:04:02Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:04:02Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:04:02Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:04:02Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:04:02Z- TerrainMapOctree: (vehN, vehE ) = (4061691.37, 594503.30). 2024-11-06T03:04:02Z- TerrainMapOctree: Min Distance = 253.57. 2024-11-06T03:04:02Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:04:02Z- Forcing Subcloud Comparison 2024-11-06T03:04:02Z- Weighting particles with subcloud comparison 2024-11-06T03:04:02Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:02Z- meanExpectedMeasDiff: 1.855445 delta_rms_squared: 1.446637 alpha: 0.986363 2024-11-06T03:04:02Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:02Z- meanExpectedMeasDiff: 0.477751 delta_rms_squared: 1.055525 alpha: 0.981404 2024-11-06T03:04:02Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:02Z- meanExpectedMeasDiff: 0.830512 delta_rms_squared: 1.134096 alpha: 0.982670 2024-11-06T03:04:02Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:02Z- meanExpectedMeasDiff: -0.101413 delta_rms_squared: 1.334513 alpha: 0.985235 2024-11-06T03:04:02Z- Subcloud NIS: 0.332125 2024-11-06T03:04:02Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:04:02Z- TNavPF:: sumWeights = 0.450931 2024-11-06T03:04:02Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:04:02Z- TNavPF::First Term of Map Covariance Matrix is 0.55 2024-11-06T03:04:02Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.05 2024-11-06T03:04:02Z- TerrainNav::Resampling particle filter... 2024-11-06T03:04:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862241.84 sec. 2024-11-06T03:04:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:04:02Z- variances([0], [2], and psi_berg) = 0.691570 0.152089 0.000000 2024-11-06T03:04:02Z- publishEstimatesDecoder() - at 1730862242.65 sec 2024-11-06T03:04:02Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:04:02Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061724.04 594487.93 58.17 2024-11-06T03:04:02Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061723.81 594487.31 58.38 lat/lon: 36.70 -121.94 2024-11-06T03:04:02Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.69 0.15 0.65 0.00 2024-11-06T03:04:03Z- handleDepth()- Depth_Keller msg: 1730862243.03 epoch sec; seqNo:2187; depth 49.82 2024-11-06T03:04:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862243.05 epoch sec; seqNo:2187; 4061691.47 north; 594505.01 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:04:03Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:04:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862242.65 2024-11-06T03:04:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:03Z- waiting for 1730862246.65, time is 1730862243.05 2024-11-06T03:04:03Z- AHRS_M2 msg: 1730862243.42 epoch sec; seqNo:2047 2024-11-06T03:04:03Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.50 psi 2024-11-06T03:04:03Z- handleDepth()- Depth_Keller msg: 1730862243.43 epoch sec; seqNo:2188; depth 49.85 2024-11-06T03:04:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862243.44 epoch sec; seqNo:2188; 4061691.47 north; 594505.01 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:04:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862242.65 2024-11-06T03:04:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:03Z- waiting for 1730862246.65, time is 1730862243.44 2024-11-06T03:04:03Z- AHRS_M2 msg: 1730862243.83 epoch sec; seqNo:2048 2024-11-06T03:04:03Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.50 psi 2024-11-06T03:04:03Z- handleDepth()- Depth_Keller msg: 1730862243.84 epoch sec; seqNo:2189; depth 49.86 2024-11-06T03:04:03Z- handleDVL() - DVL msg epochMillisec: 1730862243.87 2024-11-06T03:04:03Z- handleDvl() - RDI_Pathfinder msg: 1730862243.87 sec; seqNo:703; #beams:4 2024-11-06T03:04:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.07 , 33.30 , 35.00 , 35.51 2024-11-06T03:04:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , 0.10 2024-11-06T03:04:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862243.88 epoch sec; seqNo:2189; 4061691.47 north; 594505.01 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:04:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862242.65 2024-11-06T03:04:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:03Z- waiting for 1730862246.65, time is 1730862243.88 2024-11-06T03:04:04Z- AHRS_M2 msg: 1730862244.23 epoch sec; seqNo:2049 2024-11-06T03:04:04Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.50 psi 2024-11-06T03:04:04Z- handleDepth()- Depth_Keller msg: 1730862244.25 epoch sec; seqNo:2190; depth 49.90 2024-11-06T03:04:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862244.26 epoch sec; seqNo:2190; 4061691.47 north; 594505.01 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:04:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862242.65 2024-11-06T03:04:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:04Z- waiting for 1730862246.65, time is 1730862244.26 2024-11-06T03:04:04Z- AHRS_M2 msg: 1730862244.64 epoch sec; seqNo:2050 2024-11-06T03:04:04Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.50 psi 2024-11-06T03:04:04Z- handleDepth()- Depth_Keller msg: 1730862244.65 epoch sec; seqNo:2191; depth 49.92 2024-11-06T03:04:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862244.67 epoch sec; seqNo:2191; 4061691.48 north; 594506.23 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:04:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862242.65 2024-11-06T03:04:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:04Z- waiting for 1730862246.65, time is 1730862244.67 2024-11-06T03:04:05Z- AHRS_M2 msg: 1730862245.04 epoch sec; seqNo:2051 2024-11-06T03:04:05Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.50 psi 2024-11-06T03:04:05Z- handleDepth()- Depth_Keller msg: 1730862245.05 epoch sec; seqNo:2192; depth 49.94 2024-11-06T03:04:05Z- handleDVL() - DVL msg epochMillisec: 1730862245.05 2024-11-06T03:04:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:04:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:04:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:04:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:04:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:04:05Z- handleDvl() - RDI_Pathfinder msg: 1730862245.05 sec; seqNo:704; #beams:0 2024-11-06T03:04:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:04:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , 0.10 2024-11-06T03:04:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862245.06 epoch sec; seqNo:2192; 4061691.48 north; 594506.23 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:04:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862242.65 2024-11-06T03:04:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:05Z- waiting for 1730862246.65, time is 1730862245.06 2024-11-06T03:04:05Z- AHRS_M2 msg: 1730862245.45 epoch sec; seqNo:2052 2024-11-06T03:04:05Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.50 psi 2024-11-06T03:04:05Z- handleDepth()- Depth_Keller msg: 1730862245.45 epoch sec; seqNo:2193; depth 49.96 2024-11-06T03:04:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862245.46 epoch sec; seqNo:2193; 4061691.48 north; 594506.23 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:04:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862242.65 2024-11-06T03:04:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:05Z- waiting for 1730862246.65, time is 1730862245.46 2024-11-06T03:04:05Z- AHRS_M2 msg: 1730862245.85 epoch sec; seqNo:2053 2024-11-06T03:04:05Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.50 psi 2024-11-06T03:04:05Z- handleDepth()- Depth_Keller msg: 1730862245.85 epoch sec; seqNo:2194; depth 49.98 2024-11-06T03:04:05Z- handleDVL() - DVL msg epochMillisec: 1730862245.88 2024-11-06T03:04:05Z- handleDvl() - RDI_Pathfinder msg: 1730862245.88 sec; seqNo:705; #beams:4 2024-11-06T03:04:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.41 , 33.30 , 34.83 , 35.85 2024-11-06T03:04:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , 0.09 2024-11-06T03:04:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862245.90 epoch sec; seqNo:2194; 4061691.49 north; 594507.45 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:04:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862242.65 2024-11-06T03:04:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:06Z- waiting for 1730862246.65, time is 1730862245.90 2024-11-06T03:04:06Z- AHRS_M2 msg: 1730862246.25 epoch sec; seqNo:2054 2024-11-06T03:04:06Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.50 psi 2024-11-06T03:04:06Z- handleDepth()- Depth_Keller msg: 1730862246.26 epoch sec; seqNo:2195; depth 50.00 2024-11-06T03:04:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862246.29 epoch sec; seqNo:2195; 4061691.49 north; 594507.45 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:04:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862242.65 2024-11-06T03:04:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:06Z- waiting for 1730862246.65, time is 1730862246.29 2024-11-06T03:04:06Z- AHRS_M2 msg: 1730862246.66 epoch sec; seqNo:2055 2024-11-06T03:04:06Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:06Z- handleDepth()- Depth_Keller msg: 1730862246.67 epoch sec; seqNo:2196; depth 50.02 2024-11-06T03:04:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862246.67 epoch sec; seqNo:2196; 4061691.49 north; 594507.45 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:04:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862242.65 2024-11-06T03:04:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:06Z- waiting for 1730862246.65, time is 1730862246.67 2024-11-06T03:04:06Z- LcmTrn::updateTrn() >>>> pose time 1730862246.67, meas time 1730862245.88 <<<< 2024-11-06T03:04:06Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862246.67 2024-11-06T03:04:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862245.88 sec, ping # 0; waiting for INS data more recent than 1730862242.65... 2024-11-06T03:04:06Z- TerrainNav::motionUpdate() - currEstimate = (4061691, 594507.45, 50.02) 2024-11-06T03:04:06Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:04:06Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:04:06Z- TNavPF::Projecting Measurements 2024-11-06T03:04:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:04:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:04:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:04:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:04:06Z- TNavFilter::4 good beams. 2024-11-06T03:04:06Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:04:06Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:04:06Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:04:06Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:04:06Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:04:06Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:04:06Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:04:06Z- TerrainMapOctree: (vehN, vehE ) = (4061691.49, 594506.72). 2024-11-06T03:04:06Z- TerrainMapOctree: Min Distance = 250.21. 2024-11-06T03:04:06Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:04:06Z- Forcing Subcloud Comparison 2024-11-06T03:04:06Z- Weighting particles with subcloud comparison 2024-11-06T03:04:06Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:06Z- meanExpectedMeasDiff: 2.643359 delta_rms_squared: 1.406491 alpha: 0.985980 2024-11-06T03:04:06Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:06Z- meanExpectedMeasDiff: 0.562285 delta_rms_squared: 1.039283 alpha: 0.981119 2024-11-06T03:04:06Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:06Z- meanExpectedMeasDiff: 1.041399 delta_rms_squared: 1.107815 alpha: 0.982267 2024-11-06T03:04:06Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:06Z- meanExpectedMeasDiff: 0.354222 delta_rms_squared: 1.297260 alpha: 0.984817 2024-11-06T03:04:06Z- Subcloud NIS: 0.340761 2024-11-06T03:04:06Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:04:06Z- TNavPF:: sumWeights = 0.345874 2024-11-06T03:04:06Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:04:06Z- TNavPF::First Term of Map Covariance Matrix is 0.78 2024-11-06T03:04:06Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T03:04:06Z- TerrainNav::Resampling particle filter... 2024-11-06T03:04:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862245.88 sec. 2024-11-06T03:04:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:04:06Z- variances([0], [2], and psi_berg) = 0.695459 0.151329 0.000000 2024-11-06T03:04:06Z- publishEstimatesDecoder() - at 1730862246.67 sec 2024-11-06T03:04:06Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:04:06Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061724.04 594491.62 57.88 2024-11-06T03:04:06Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061723.85 594490.98 58.03 lat/lon: 36.70 -121.94 2024-11-06T03:04:06Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.15 0.65 0.00 2024-11-06T03:04:07Z- AHRS_M2 msg: 1730862247.06 epoch sec; seqNo:2056 2024-11-06T03:04:07Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:07Z- handleDepth()- Depth_Keller msg: 1730862247.06 epoch sec; seqNo:2197; depth 50.03 2024-11-06T03:04:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862247.10 epoch sec; seqNo:2197; 4061691.87 north; 594507.44 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:04:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862246.67 2024-11-06T03:04:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:07Z- waiting for 1730862250.67, time is 1730862247.10 2024-11-06T03:04:07Z- AHRS_M2 msg: 1730862247.46 epoch sec; seqNo:2057 2024-11-06T03:04:07Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:07Z- handleDepth()- Depth_Keller msg: 1730862247.47 epoch sec; seqNo:2198; depth 50.04 2024-11-06T03:04:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862247.56 epoch sec; seqNo:2198; 4061691.89 north; 594508.66 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:04:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862246.67 2024-11-06T03:04:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:07Z- waiting for 1730862250.67, time is 1730862247.56 2024-11-06T03:04:07Z- AHRS_M2 msg: 1730862247.87 epoch sec; seqNo:2058 2024-11-06T03:04:07Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:07Z- handleDepth()- Depth_Keller msg: 1730862247.87 epoch sec; seqNo:2199; depth 50.04 2024-11-06T03:04:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862247.89 epoch sec; seqNo:2199; 4061691.89 north; 594508.66 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:04:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862246.67 2024-11-06T03:04:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:08Z- waiting for 1730862250.67, time is 1730862247.89 2024-11-06T03:04:08Z- AHRS_M2 msg: 1730862248.27 epoch sec; seqNo:2059 2024-11-06T03:04:08Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:08Z- handleDepth()- Depth_Keller msg: 1730862248.28 epoch sec; seqNo:2200; depth 50.03 2024-11-06T03:04:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862248.29 epoch sec; seqNo:2200; 4061691.89 north; 594508.66 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:04:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862246.67 2024-11-06T03:04:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:08Z- waiting for 1730862250.67, time is 1730862248.29 2024-11-06T03:04:08Z- AHRS_M2 msg: 1730862248.68 epoch sec; seqNo:2060 2024-11-06T03:04:08Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:08Z- handleDepth()- Depth_Keller msg: 1730862248.68 epoch sec; seqNo:2201; depth 50.05 2024-11-06T03:04:08Z- handleDVL() - DVL msg epochMillisec: 1730862248.69 2024-11-06T03:04:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:04:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:04:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:04:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:04:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:04:08Z- handleDvl() - RDI_Pathfinder msg: 1730862248.69 sec; seqNo:706; #beams:0 2024-11-06T03:04:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:04:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , 0.09 2024-11-06T03:04:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862248.70 epoch sec; seqNo:2201; 4061691.90 north; 594509.88 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:04:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862246.67 2024-11-06T03:04:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:08Z- waiting for 1730862250.67, time is 1730862248.70 2024-11-06T03:04:09Z- AHRS_M2 msg: 1730862249.08 epoch sec; seqNo:2061 2024-11-06T03:04:09Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:09Z- handleDepth()- Depth_Keller msg: 1730862249.08 epoch sec; seqNo:2202; depth 50.06 2024-11-06T03:04:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862249.09 epoch sec; seqNo:2202; 4061691.90 north; 594509.88 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:04:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862246.67 2024-11-06T03:04:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:09Z- waiting for 1730862250.67, time is 1730862249.09 2024-11-06T03:04:09Z- AHRS_M2 msg: 1730862249.49 epoch sec; seqNo:2062 2024-11-06T03:04:09Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:09Z- handleDepth()- Depth_Keller msg: 1730862249.51 epoch sec; seqNo:2203; depth 50.05 2024-11-06T03:04:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862249.56 epoch sec; seqNo:2203; 4061691.90 north; 594509.88 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:04:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862246.67 2024-11-06T03:04:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:09Z- waiting for 1730862250.67, time is 1730862249.56 2024-11-06T03:04:09Z- AHRS_M2 msg: 1730862249.89 epoch sec; seqNo:2063 2024-11-06T03:04:09Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:09Z- handleDepth()- Depth_Keller msg: 1730862249.90 epoch sec; seqNo:2204; depth 50.05 2024-11-06T03:04:09Z- handleDVL() - DVL msg epochMillisec: 1730862249.92 2024-11-06T03:04:09Z- handleDvl() - RDI_Pathfinder msg: 1730862249.92 sec; seqNo:707; #beams:4 2024-11-06T03:04:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.41 , 33.30 , 35.17 , 35.68 2024-11-06T03:04:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.04 2024-11-06T03:04:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862249.94 epoch sec; seqNo:2204; 4061691.91 north; 594511.10 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:04:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862246.67 2024-11-06T03:04:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:10Z- waiting for 1730862250.67, time is 1730862249.94 2024-11-06T03:04:10Z- AHRS_M2 msg: 1730862250.29 epoch sec; seqNo:2064 2024-11-06T03:04:10Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:10Z- handleDepth()- Depth_Keller msg: 1730862250.30 epoch sec; seqNo:2205; depth 50.04 2024-11-06T03:04:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862250.33 epoch sec; seqNo:2205; 4061691.91 north; 594511.10 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:04:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862246.67 2024-11-06T03:04:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:10Z- waiting for 1730862250.67, time is 1730862250.33 2024-11-06T03:04:10Z- AHRS_M2 msg: 1730862250.70 epoch sec; seqNo:2065 2024-11-06T03:04:10Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:10Z- handleDepth()- Depth_Keller msg: 1730862250.71 epoch sec; seqNo:2206; depth 50.03 2024-11-06T03:04:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862250.72 epoch sec; seqNo:2206; 4061691.91 north; 594511.10 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:04:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862246.67 2024-11-06T03:04:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:10Z- waiting for 1730862250.67, time is 1730862250.72 2024-11-06T03:04:10Z- LcmTrn::updateTrn() >>>> pose time 1730862250.72, meas time 1730862249.92 <<<< 2024-11-06T03:04:10Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862250.72 2024-11-06T03:04:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862249.92 sec, ping # 0; waiting for INS data more recent than 1730862246.67... 2024-11-06T03:04:10Z- TerrainNav::motionUpdate() - currEstimate = (4061692, 594511.10, 50.03) 2024-11-06T03:04:10Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:04:10Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:04:10Z- TNavPF::Projecting Measurements 2024-11-06T03:04:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:04:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:04:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:04:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:04:10Z- TNavFilter::4 good beams. 2024-11-06T03:04:10Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:04:10Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:04:10Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:04:10Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:04:10Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:04:10Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:04:10Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:04:10Z- TerrainMapOctree: (vehN, vehE ) = (4061691.83, 594510.38). 2024-11-06T03:04:10Z- TerrainMapOctree: Min Distance = 246.58. 2024-11-06T03:04:10Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:04:10Z- Forcing Subcloud Comparison 2024-11-06T03:04:10Z- Weighting particles with subcloud comparison 2024-11-06T03:04:10Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:10Z- meanExpectedMeasDiff: 1.994143 delta_rms_squared: 1.410379 alpha: 0.986018 2024-11-06T03:04:10Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:10Z- meanExpectedMeasDiff: 0.049633 delta_rms_squared: 1.039345 alpha: 0.981120 2024-11-06T03:04:10Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:10Z- meanExpectedMeasDiff: 0.750574 delta_rms_squared: 1.114369 alpha: 0.982369 2024-11-06T03:04:10Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:10Z- meanExpectedMeasDiff: -1.428730 delta_rms_squared: 1.096590 alpha: 0.982088 2024-11-06T03:04:10Z- Subcloud NIS: 0.341288 2024-11-06T03:04:10Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:04:10Z- TNavPF:: sumWeights = 0.307235 2024-11-06T03:04:10Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:04:11Z- TNavPF::First Term of Map Covariance Matrix is 0.68 2024-11-06T03:04:11Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.05 2024-11-06T03:04:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862249.92 sec. 2024-11-06T03:04:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:04:11Z- variances([0], [2], and psi_berg) = 0.722330 0.160240 0.000000 2024-11-06T03:04:11Z- publishEstimatesDecoder() - at 1730862250.72 sec 2024-11-06T03:04:11Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:04:11Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061723.43 594493.86 57.74 2024-11-06T03:04:11Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061724.22 594494.62 57.87 lat/lon: 36.70 -121.94 2024-11-06T03:04:11Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.72 0.16 0.69 0.00 2024-11-06T03:04:11Z- AHRS_M2 msg: 1730862251.10 epoch sec; seqNo:2066 2024-11-06T03:04:11Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:11Z- handleDepth()- Depth_Keller msg: 1730862251.10 epoch sec; seqNo:2207; depth 50.03 2024-11-06T03:04:11Z- handleDVL() - DVL msg epochMillisec: 1730862251.11 2024-11-06T03:04:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:04:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:04:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:04:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:04:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:04:11Z- handleDvl() - RDI_Pathfinder msg: 1730862251.11 sec; seqNo:708; #beams:0 2024-11-06T03:04:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:04:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.04 2024-11-06T03:04:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862251.13 epoch sec; seqNo:2207; 4061691.93 north; 594512.32 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:04:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862250.72 2024-11-06T03:04:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:11Z- waiting for 1730862254.72, time is 1730862251.13 2024-11-06T03:04:11Z- AHRS_M2 msg: 1730862251.56 epoch sec; seqNo:2067 2024-11-06T03:04:11Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 1.51 psi 2024-11-06T03:04:11Z- handleDepth()- Depth_Keller msg: 1730862251.56 epoch sec; seqNo:2208; depth 50.01 2024-11-06T03:04:11Z- handleDVL() - DVL msg epochMillisec: 1730862251.59 2024-11-06T03:04:11Z- handleDvl() - RDI_Pathfinder msg: 1730862251.59 sec; seqNo:709; #beams:4 2024-11-06T03:04:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.90 , 33.30 , 35.00 , 35.51 2024-11-06T03:04:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , -0.04 2024-11-06T03:04:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862251.60 epoch sec; seqNo:2208; 4061691.93 north; 594512.32 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:04:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862250.72 2024-11-06T03:04:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:11Z- waiting for 1730862254.72, time is 1730862251.60 2024-11-06T03:04:11Z- AHRS_M2 msg: 1730862251.91 epoch sec; seqNo:2068 2024-11-06T03:04:11Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 1.51 psi 2024-11-06T03:04:11Z- handleDepth()- Depth_Keller msg: 1730862251.91 epoch sec; seqNo:2209; depth 50.02 2024-11-06T03:04:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862251.93 epoch sec; seqNo:2209; 4061691.93 north; 594512.32 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:04:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862250.72 2024-11-06T03:04:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:12Z- waiting for 1730862254.72, time is 1730862251.93 2024-11-06T03:04:12Z- AHRS_M2 msg: 1730862252.31 epoch sec; seqNo:2069 2024-11-06T03:04:12Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 1.51 psi 2024-11-06T03:04:12Z- handleDepth()- Depth_Keller msg: 1730862252.32 epoch sec; seqNo:2210; depth 50.01 2024-11-06T03:04:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862252.33 epoch sec; seqNo:2210; 4061691.93 north; 594512.32 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:04:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862250.72 2024-11-06T03:04:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:12Z- waiting for 1730862254.72, time is 1730862252.33 2024-11-06T03:04:12Z- AHRS_M2 msg: 1730862252.71 epoch sec; seqNo:2070 2024-11-06T03:04:12Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 1.51 psi 2024-11-06T03:04:12Z- handleDepth()- Depth_Keller msg: 1730862252.73 epoch sec; seqNo:2211; depth 50.02 2024-11-06T03:04:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862252.73 epoch sec; seqNo:2211; 4061691.94 north; 594513.54 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:04:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862250.72 2024-11-06T03:04:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:12Z- waiting for 1730862254.72, time is 1730862252.73 2024-11-06T03:04:13Z- AHRS_M2 msg: 1730862253.12 epoch sec; seqNo:2071 2024-11-06T03:04:13Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 1.50 psi 2024-11-06T03:04:13Z- handleDepth()- Depth_Keller msg: 1730862253.13 epoch sec; seqNo:2212; depth 50.01 2024-11-06T03:04:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862253.14 epoch sec; seqNo:2212; 4061691.94 north; 594513.54 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:04:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862250.72 2024-11-06T03:04:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:13Z- waiting for 1730862254.72, time is 1730862253.14 2024-11-06T03:04:13Z- AHRS_M2 msg: 1730862253.56 epoch sec; seqNo:2072 2024-11-06T03:04:13Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 1.50 psi 2024-11-06T03:04:13Z- handleDepth()- Depth_Keller msg: 1730862253.56 epoch sec; seqNo:2213; depth 50.02 2024-11-06T03:04:13Z- handleDVL() - DVL msg epochMillisec: 1730862253.59 2024-11-06T03:04:13Z- handleDvl() - RDI_Pathfinder msg: 1730862253.59 sec; seqNo:710; #beams:4 2024-11-06T03:04:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.56 , 33.13 , 34.83 , 35.17 2024-11-06T03:04:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.04 , 0.01 2024-11-06T03:04:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862253.61 epoch sec; seqNo:2213; 4061692.32 north; 594513.54 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:04:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862250.72 2024-11-06T03:04:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:13Z- waiting for 1730862254.72, time is 1730862253.61 2024-11-06T03:04:13Z- AHRS_M2 msg: 1730862253.93 epoch sec; seqNo:2073 2024-11-06T03:04:13Z- AHRS_M2 msg: 0.12 phi; -0.06 theta; 1.50 psi 2024-11-06T03:04:13Z- handleDepth()- Depth_Keller msg: 1730862253.93 epoch sec; seqNo:2214; depth 50.00 2024-11-06T03:04:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862253.94 epoch sec; seqNo:2214; 4061692.32 north; 594513.54 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:04:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862250.72 2024-11-06T03:04:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:14Z- waiting for 1730862254.72, time is 1730862253.94 2024-11-06T03:04:14Z- AHRS_M2 msg: 1730862254.33 epoch sec; seqNo:2074 2024-11-06T03:04:14Z- AHRS_M2 msg: 0.12 phi; -0.07 theta; 1.50 psi 2024-11-06T03:04:14Z- handleDepth()- Depth_Keller msg: 1730862254.34 epoch sec; seqNo:2215; depth 50.03 2024-11-06T03:04:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862254.35 epoch sec; seqNo:2215; 4061692.33 north; 594514.76 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:04:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862250.72 2024-11-06T03:04:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:14Z- waiting for 1730862254.72, time is 1730862254.35 2024-11-06T03:04:14Z- AHRS_M2 msg: 1730862254.73 epoch sec; seqNo:2075 2024-11-06T03:04:14Z- AHRS_M2 msg: 0.12 phi; -0.07 theta; 1.50 psi 2024-11-06T03:04:14Z- handleDepth()- Depth_Keller msg: 1730862254.74 epoch sec; seqNo:2216; depth 50.03 2024-11-06T03:04:14Z- handleDVL() - DVL msg epochMillisec: 1730862254.74 2024-11-06T03:04:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:04:14Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:04:14Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:04:14Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:04:14Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:04:14Z- handleDvl() - RDI_Pathfinder msg: 1730862254.74 sec; seqNo:711; #beams:0 2024-11-06T03:04:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:04:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.04 , 0.01 2024-11-06T03:04:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862254.76 epoch sec; seqNo:2216; 4061692.33 north; 594514.76 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:04:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862250.72 2024-11-06T03:04:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:14Z- waiting for 1730862254.72, time is 1730862254.76 2024-11-06T03:04:14Z- LcmTrn::updateTrn() >>>> pose time 1730862254.76, meas time 1730862254.74 <<<< 2024-11-06T03:04:14Z- LcmTrn::updateTrn() equating pose 1730862254.74 and meas times 1730862254.74 2024-11-06T03:04:14Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862254.74 2024-11-06T03:04:14Z- TerrainNav::motionUpdate() - currEstimate = (4061692, 594514.76, 50.03) 2024-11-06T03:04:14Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:04:14Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:04:14Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:04:14Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:04:14Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:04:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:04:14Z- variances([0], [2], and psi_berg) = 0.723630 0.162145 0.000000 2024-11-06T03:04:14Z- publishEstimatesDecoder() - at 1730862254.74 sec 2024-11-06T03:04:14Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:04:14Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061723.85 594497.54 57.87 2024-11-06T03:04:14Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061724.64 594498.27 57.88 lat/lon: 36.70 -121.94 2024-11-06T03:04:14Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.72 0.16 0.92 0.00 2024-11-06T03:04:15Z- AHRS_M2 msg: 1730862255.14 epoch sec; seqNo:2076 2024-11-06T03:04:15Z- AHRS_M2 msg: 0.12 phi; -0.07 theta; 1.50 psi 2024-11-06T03:04:15Z- handleDepth()- Depth_Keller msg: 1730862255.15 epoch sec; seqNo:2217; depth 50.04 2024-11-06T03:04:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862255.15 epoch sec; seqNo:2217; 4061692.33 north; 594514.76 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:04:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862254.74 2024-11-06T03:04:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:15Z- waiting for 1730862258.74, time is 1730862255.15 2024-11-06T03:04:15Z- AHRS_M2 msg: 1730862255.56 epoch sec; seqNo:2077 2024-11-06T03:04:15Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.50 psi 2024-11-06T03:04:15Z- handleDepth()- Depth_Keller msg: 1730862255.57 epoch sec; seqNo:2218; depth 50.06 2024-11-06T03:04:15Z- handleDVL() - DVL msg epochMillisec: 1730862255.60 2024-11-06T03:04:15Z- handleDvl() - RDI_Pathfinder msg: 1730862255.60 sec; seqNo:712; #beams:4 2024-11-06T03:04:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.56 , 32.96 , 34.66 , 35.17 2024-11-06T03:04:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.05 , 0.04 2024-11-06T03:04:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862255.61 epoch sec; seqNo:2218; 4061692.33 north; 594514.76 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:04:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862254.74 2024-11-06T03:04:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:15Z- waiting for 1730862258.74, time is 1730862255.61 2024-11-06T03:04:15Z- AHRS_M2 msg: 1730862255.95 epoch sec; seqNo:2078 2024-11-06T03:04:15Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.50 psi 2024-11-06T03:04:15Z- handleDepth()- Depth_Keller msg: 1730862255.95 epoch sec; seqNo:2219; depth 50.07 2024-11-06T03:04:15Z- handleDVL() - DVL msg epochMillisec: 1730862255.96 2024-11-06T03:04:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:04:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:04:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:04:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:04:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:04:15Z- handleDvl() - RDI_Pathfinder msg: 1730862255.96 sec; seqNo:713; #beams:0 2024-11-06T03:04:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:04:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.05 , 0.04 2024-11-06T03:04:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862255.98 epoch sec; seqNo:2219; 4061692.35 north; 594515.98 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:04:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862254.74 2024-11-06T03:04:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:16Z- waiting for 1730862258.74, time is 1730862255.98 2024-11-06T03:04:16Z- AHRS_M2 msg: 1730862256.35 epoch sec; seqNo:2079 2024-11-06T03:04:16Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.50 psi 2024-11-06T03:04:16Z- handleDepth()- Depth_Keller msg: 1730862256.36 epoch sec; seqNo:2220; depth 50.08 2024-11-06T03:04:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862256.38 epoch sec; seqNo:2220; 4061692.35 north; 594515.98 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:04:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862254.74 2024-11-06T03:04:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:16Z- waiting for 1730862258.74, time is 1730862256.38 2024-11-06T03:04:16Z- AHRS_M2 msg: 1730862256.76 epoch sec; seqNo:2080 2024-11-06T03:04:16Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.50 psi 2024-11-06T03:04:16Z- handleDepth()- Depth_Keller msg: 1730862256.77 epoch sec; seqNo:2221; depth 50.10 2024-11-06T03:04:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862256.78 epoch sec; seqNo:2221; 4061692.35 north; 594515.98 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:04:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862254.74 2024-11-06T03:04:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:16Z- waiting for 1730862258.74, time is 1730862256.78 2024-11-06T03:04:17Z- AHRS_M2 msg: 1730862257.18 epoch sec; seqNo:2081 2024-11-06T03:04:17Z- AHRS_M2 msg: 0.12 phi; -0.07 theta; 1.50 psi 2024-11-06T03:04:17Z- handleDepth()- Depth_Keller msg: 1730862257.19 epoch sec; seqNo:2222; depth 50.13 2024-11-06T03:04:17Z- handleDVL() - DVL msg epochMillisec: 1730862257.19 2024-11-06T03:04:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:04:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:04:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:04:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:04:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:04:17Z- handleDvl() - RDI_Pathfinder msg: 1730862257.19 sec; seqNo:714; #beams:0 2024-11-06T03:04:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:04:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.05 , 0.04 2024-11-06T03:04:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862257.20 epoch sec; seqNo:2222; 4061692.73 north; 594515.98 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:04:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862254.74 2024-11-06T03:04:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:17Z- waiting for 1730862258.74, time is 1730862257.20 2024-11-06T03:04:17Z- AHRS_M2 msg: 1730862257.57 epoch sec; seqNo:2082 2024-11-06T03:04:17Z- AHRS_M2 msg: 0.12 phi; -0.07 theta; 1.49 psi 2024-11-06T03:04:17Z- handleDepth()- Depth_Keller msg: 1730862257.57 epoch sec; seqNo:2223; depth 50.14 2024-11-06T03:04:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862257.60 epoch sec; seqNo:2223; 4061692.74 north; 594517.20 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:04:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862254.74 2024-11-06T03:04:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:17Z- waiting for 1730862258.74, time is 1730862257.60 2024-11-06T03:04:17Z- AHRS_M2 msg: 1730862257.97 epoch sec; seqNo:2083 2024-11-06T03:04:17Z- AHRS_M2 msg: 0.12 phi; -0.07 theta; 1.49 psi 2024-11-06T03:04:17Z- handleDepth()- Depth_Keller msg: 1730862257.97 epoch sec; seqNo:2224; depth 50.18 2024-11-06T03:04:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862257.99 epoch sec; seqNo:2224; 4061692.74 north; 594517.20 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:04:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862254.74 2024-11-06T03:04:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:18Z- waiting for 1730862258.74, time is 1730862257.99 2024-11-06T03:04:18Z- AHRS_M2 msg: 1730862258.37 epoch sec; seqNo:2084 2024-11-06T03:04:18Z- AHRS_M2 msg: 0.12 phi; -0.07 theta; 1.49 psi 2024-11-06T03:04:18Z- handleDepth()- Depth_Keller msg: 1730862258.38 epoch sec; seqNo:2225; depth 50.20 2024-11-06T03:04:18Z- handleDVL() - DVL msg epochMillisec: 1730862258.38 2024-11-06T03:04:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:04:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:04:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:04:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:04:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:04:18Z- handleDvl() - RDI_Pathfinder msg: 1730862258.38 sec; seqNo:715; #beams:0 2024-11-06T03:04:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:04:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.05 , 0.04 2024-11-06T03:04:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862258.39 epoch sec; seqNo:2225; 4061692.74 north; 594517.20 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:04:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862254.74 2024-11-06T03:04:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:18Z- waiting for 1730862258.74, time is 1730862258.39 2024-11-06T03:04:18Z- AHRS_M2 msg: 1730862258.77 epoch sec; seqNo:2085 2024-11-06T03:04:18Z- AHRS_M2 msg: 0.13 phi; -0.07 theta; 1.49 psi 2024-11-06T03:04:18Z- handleDepth()- Depth_Keller msg: 1730862258.79 epoch sec; seqNo:2226; depth 50.23 2024-11-06T03:04:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862258.81 epoch sec; seqNo:2226; 4061692.74 north; 594517.20 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:04:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862254.74 2024-11-06T03:04:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:18Z- waiting for 1730862258.74, time is 1730862258.81 2024-11-06T03:04:18Z- LcmTrn::updateTrn() >>>> pose time 1730862258.81, meas time 1730862258.38 <<<< 2024-11-06T03:04:18Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862258.81 2024-11-06T03:04:18Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:04:18Z- TerrainNav::motionUpdate() - currEstimate = (4061693, 594517.20, 50.23) 2024-11-06T03:04:18Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:04:18Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:04:18Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:04:18Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:04:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:04:18Z- variances([0], [2], and psi_berg) = 0.725197 0.163064 0.000000 2024-11-06T03:04:18Z- publishEstimatesDecoder() - at 1730862258.81 sec 2024-11-06T03:04:18Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:04:18Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061724.27 594499.97 57.67 2024-11-06T03:04:18Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061725.04 594500.71 58.08 lat/lon: 36.70 -121.94 2024-11-06T03:04:18Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.73 0.16 1.18 0.00 2024-11-06T03:04:19Z- AHRS_M2 msg: 1730862259.18 epoch sec; seqNo:2086 2024-11-06T03:04:19Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 1.49 psi 2024-11-06T03:04:19Z- handleDepth()- Depth_Keller msg: 1730862259.20 epoch sec; seqNo:2227; depth 50.25 2024-11-06T03:04:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862259.21 epoch sec; seqNo:2227; 4061692.75 north; 594518.42 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:04:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862258.81 2024-11-06T03:04:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:19Z- waiting for 1730862262.81, time is 1730862259.21 2024-11-06T03:04:19Z- AHRS_M2 msg: 1730862259.58 epoch sec; seqNo:2087 2024-11-06T03:04:19Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 1.49 psi 2024-11-06T03:04:19Z- handleDepth()- Depth_Keller msg: 1730862259.59 epoch sec; seqNo:2228; depth 50.28 2024-11-06T03:04:19Z- handleDVL() - DVL msg epochMillisec: 1730862259.62 2024-11-06T03:04:19Z- handleDvl() - RDI_Pathfinder msg: 1730862259.62 sec; seqNo:716; #beams:4 2024-11-06T03:04:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.07 , 33.30 , 34.49 , 35.85 2024-11-06T03:04:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.08 2024-11-06T03:04:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862259.64 epoch sec; seqNo:2228; 4061692.75 north; 594518.42 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:04:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862258.81 2024-11-06T03:04:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:19Z- waiting for 1730862262.81, time is 1730862259.64 2024-11-06T03:04:20Z- AHRS_M2 msg: 1730862259.99 epoch sec; seqNo:2088 2024-11-06T03:04:20Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 1.49 psi 2024-11-06T03:04:20Z- handleDepth()- Depth_Keller msg: 1730862260.00 epoch sec; seqNo:2229; depth 50.30 2024-11-06T03:04:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862260.01 epoch sec; seqNo:2229; 4061692.75 north; 594518.42 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:04:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862258.81 2024-11-06T03:04:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:20Z- waiting for 1730862262.81, time is 1730862260.01 2024-11-06T03:04:20Z- AHRS_M2 msg: 1730862260.39 epoch sec; seqNo:2089 2024-11-06T03:04:20Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 1.49 psi 2024-11-06T03:04:20Z- handleDepth()- Depth_Keller msg: 1730862260.40 epoch sec; seqNo:2230; depth 50.32 2024-11-06T03:04:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862260.41 epoch sec; seqNo:2230; 4061692.75 north; 594518.42 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:04:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862258.81 2024-11-06T03:04:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:20Z- waiting for 1730862262.81, time is 1730862260.41 2024-11-06T03:04:20Z- handleDepth()- Depth_Keller msg: 1730862260.82 epoch sec; seqNo:2231; depth 50.34 2024-11-06T03:04:20Z- handleDVL() - DVL msg epochMillisec: 1730862260.83 2024-11-06T03:04:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:04:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:04:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:04:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:04:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:04:20Z- handleDvl() - RDI_Pathfinder msg: 1730862260.83 sec; seqNo:717; #beams:0 2024-11-06T03:04:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:04:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.08 2024-11-06T03:04:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862260.83 epoch sec; seqNo:2231; 4061693.14 north; 594519.63 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:04:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862258.81 2024-11-06T03:04:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:20Z- waiting for 1730862262.81, time is 1730862260.83 2024-11-06T03:04:21Z- AHRS_M2 msg: 1730862261.20 epoch sec; seqNo:2090 2024-11-06T03:04:21Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 1.50 psi 2024-11-06T03:04:21Z- handleDepth()- Depth_Keller msg: 1730862261.21 epoch sec; seqNo:2232; depth 50.37 2024-11-06T03:04:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862261.22 epoch sec; seqNo:2232; 4061693.14 north; 594519.63 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:04:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862258.81 2024-11-06T03:04:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:21Z- waiting for 1730862262.81, time is 1730862261.22 2024-11-06T03:04:21Z- AHRS_M2 msg: 1730862261.61 epoch sec; seqNo:2091 2024-11-06T03:04:21Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 1.50 psi 2024-11-06T03:04:21Z- handleDepth()- Depth_Keller msg: 1730862261.61 epoch sec; seqNo:2233; depth 50.39 2024-11-06T03:04:21Z- handleDVL() - DVL msg epochMillisec: 1730862261.64 2024-11-06T03:04:21Z- handleDvl() - RDI_Pathfinder msg: 1730862261.64 sec; seqNo:718; #beams:4 2024-11-06T03:04:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.90 , 33.30 , 34.49 , 35.85 2024-11-06T03:04:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , 0.06 2024-11-06T03:04:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862261.68 epoch sec; seqNo:2233; 4061693.14 north; 594519.63 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:04:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862258.81 2024-11-06T03:04:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:21Z- waiting for 1730862262.81, time is 1730862261.68 2024-11-06T03:04:22Z- AHRS_M2 msg: 1730862262.01 epoch sec; seqNo:2092 2024-11-06T03:04:22Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 1.50 psi 2024-11-06T03:04:22Z- handleDepth()- Depth_Keller msg: 1730862262.02 epoch sec; seqNo:2234; depth 50.40 2024-11-06T03:04:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862262.04 epoch sec; seqNo:2234; 4061693.14 north; 594519.63 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:04:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862258.81 2024-11-06T03:04:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:22Z- waiting for 1730862262.81, time is 1730862262.04 2024-11-06T03:04:22Z- AHRS_M2 msg: 1730862262.41 epoch sec; seqNo:2093 2024-11-06T03:04:22Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 1.50 psi 2024-11-06T03:04:22Z- handleDepth()- Depth_Keller msg: 1730862262.42 epoch sec; seqNo:2235; depth 50.43 2024-11-06T03:04:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862262.43 epoch sec; seqNo:2235; 4061693.16 north; 594520.85 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:04:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862258.81 2024-11-06T03:04:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:22Z- waiting for 1730862262.81, time is 1730862262.43 2024-11-06T03:04:22Z- AHRS_M2 msg: 1730862262.83 epoch sec; seqNo:2094 2024-11-06T03:04:22Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 1.50 psi 2024-11-06T03:04:22Z- handleDepth()- Depth_Keller msg: 1730862262.83 epoch sec; seqNo:2236; depth 50.43 2024-11-06T03:04:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862262.84 epoch sec; seqNo:2236; 4061693.16 north; 594520.85 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:04:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862258.81 2024-11-06T03:04:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:22Z- waiting for 1730862262.81, time is 1730862262.84 2024-11-06T03:04:22Z- LcmTrn::updateTrn() >>>> pose time 1730862262.84, meas time 1730862261.64 <<<< 2024-11-06T03:04:22Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862262.84 2024-11-06T03:04:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862261.64 sec, ping # 0; waiting for INS data more recent than 1730862258.81... 2024-11-06T03:04:22Z- TerrainNav::motionUpdate() - currEstimate = (4061693, 594520.85, 50.43) 2024-11-06T03:04:22Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:04:22Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:04:23Z- TNavPF::Projecting Measurements 2024-11-06T03:04:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:04:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:04:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:04:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:04:23Z- TNavFilter::4 good beams. 2024-11-06T03:04:23Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:04:23Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:04:23Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:04:23Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:04:23Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:04:23Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:04:23Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:04:23Z- TerrainMapOctree: (vehN, vehE ) = (4061693.03, 594519.76). 2024-11-06T03:04:23Z- TerrainMapOctree: Min Distance = 237.18. 2024-11-06T03:04:23Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:04:23Z- Forcing Subcloud Comparison 2024-11-06T03:04:23Z- Weighting particles with subcloud comparison 2024-11-06T03:04:23Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:23Z- meanExpectedMeasDiff: 0.343364 delta_rms_squared: 2.294181 alpha: 0.991358 2024-11-06T03:04:23Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:23Z- meanExpectedMeasDiff: 0.235378 delta_rms_squared: 1.683959 alpha: 0.988263 2024-11-06T03:04:23Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:23Z- meanExpectedMeasDiff: 0.444143 delta_rms_squared: 1.785892 alpha: 0.988925 2024-11-06T03:04:23Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:23Z- meanExpectedMeasDiff: -1.461753 delta_rms_squared: 2.146882 alpha: 0.990770 2024-11-06T03:04:23Z- Subcloud NIS: 0.349204 2024-11-06T03:04:23Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:04:23Z- TNavPF:: sumWeights = 0.411232 2024-11-06T03:04:23Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:04:23Z- TNavPF::First Term of Map Covariance Matrix is 0.34 2024-11-06T03:04:23Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T03:04:23Z- TerrainNav::Resampling particle filter... 2024-11-06T03:04:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862261.64 sec. 2024-11-06T03:04:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:04:23Z- variances([0], [2], and psi_berg) = 0.714294 0.165920 0.000000 2024-11-06T03:04:23Z- publishEstimatesDecoder() - at 1730862262.84 sec 2024-11-06T03:04:23Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:04:23Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061725.68 594505.06 58.21 2024-11-06T03:04:23Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061725.47 594504.37 58.34 lat/lon: 36.70 -121.94 2024-11-06T03:04:23Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.71 0.17 0.73 0.00 2024-11-06T03:04:23Z- AHRS_M2 msg: 1730862263.22 epoch sec; seqNo:2095 2024-11-06T03:04:23Z- AHRS_M2 msg: 0.14 phi; -0.08 theta; 1.50 psi 2024-11-06T03:04:23Z- handleDepth()- Depth_Keller msg: 1730862263.23 epoch sec; seqNo:2237; depth 50.46 2024-11-06T03:04:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862263.24 epoch sec; seqNo:2237; 4061693.16 north; 594520.85 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:04:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862262.84 2024-11-06T03:04:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:23Z- waiting for 1730862266.84, time is 1730862263.24 2024-11-06T03:04:23Z- AHRS_M2 msg: 1730862263.62 epoch sec; seqNo:2096 2024-11-06T03:04:23Z- AHRS_M2 msg: 0.14 phi; -0.08 theta; 1.50 psi 2024-11-06T03:04:23Z- handleDepth()- Depth_Keller msg: 1730862263.63 epoch sec; seqNo:2238; depth 50.47 2024-11-06T03:04:23Z- handleDVL() - DVL msg epochMillisec: 1730862263.67 2024-11-06T03:04:23Z- handleDvl() - RDI_Pathfinder msg: 1730862263.67 sec; seqNo:719; #beams:4 2024-11-06T03:04:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.07 , 33.30 , 34.49 , 35.85 2024-11-06T03:04:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.00 2024-11-06T03:04:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862263.68 epoch sec; seqNo:2238; 4061693.17 north; 594522.07 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:04:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862262.84 2024-11-06T03:04:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:23Z- waiting for 1730862266.84, time is 1730862263.68 2024-11-06T03:04:24Z- AHRS_M2 msg: 1730862264.03 epoch sec; seqNo:2097 2024-11-06T03:04:24Z- AHRS_M2 msg: 0.14 phi; -0.08 theta; 1.50 psi 2024-11-06T03:04:24Z- handleDepth()- Depth_Keller msg: 1730862264.03 epoch sec; seqNo:2239; depth 50.48 2024-11-06T03:04:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862264.06 epoch sec; seqNo:2239; 4061693.17 north; 594522.07 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:04:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862262.84 2024-11-06T03:04:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:24Z- waiting for 1730862266.84, time is 1730862264.06 2024-11-06T03:04:24Z- AHRS_M2 msg: 1730862264.43 epoch sec; seqNo:2098 2024-11-06T03:04:24Z- AHRS_M2 msg: 0.14 phi; -0.08 theta; 1.51 psi 2024-11-06T03:04:24Z- handleDepth()- Depth_Keller msg: 1730862264.43 epoch sec; seqNo:2240; depth 50.49 2024-11-06T03:04:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862264.45 epoch sec; seqNo:2240; 4061693.17 north; 594522.07 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:04:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862262.84 2024-11-06T03:04:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:24Z- waiting for 1730862266.84, time is 1730862264.45 2024-11-06T03:04:24Z- AHRS_M2 msg: 1730862264.84 epoch sec; seqNo:2099 2024-11-06T03:04:24Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 1.51 psi 2024-11-06T03:04:24Z- handleDepth()- Depth_Keller msg: 1730862264.84 epoch sec; seqNo:2241; depth 50.49 2024-11-06T03:04:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862264.85 epoch sec; seqNo:2241; 4061693.18 north; 594523.29 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:04:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862262.84 2024-11-06T03:04:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:24Z- waiting for 1730862266.84, time is 1730862264.85 2024-11-06T03:04:25Z- AHRS_M2 msg: 1730862265.24 epoch sec; seqNo:2100 2024-11-06T03:04:25Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 1.51 psi 2024-11-06T03:04:25Z- handleDepth()- Depth_Keller msg: 1730862265.25 epoch sec; seqNo:2242; depth 50.52 2024-11-06T03:04:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862265.26 epoch sec; seqNo:2242; 4061693.18 north; 594523.29 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:04:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862262.84 2024-11-06T03:04:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:25Z- waiting for 1730862266.84, time is 1730862265.26 2024-11-06T03:04:25Z- AHRS_M2 msg: 1730862265.65 epoch sec; seqNo:2101 2024-11-06T03:04:25Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 1.51 psi 2024-11-06T03:04:25Z- handleDepth()- Depth_Keller msg: 1730862265.66 epoch sec; seqNo:2243; depth 50.52 2024-11-06T03:04:25Z- handleDVL() - DVL msg epochMillisec: 1730862265.68 2024-11-06T03:04:25Z- handleDvl() - RDI_Pathfinder msg: 1730862265.68 sec; seqNo:720; #beams:4 2024-11-06T03:04:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.07 , 32.96 , 34.49 , 35.51 2024-11-06T03:04:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , -0.05 2024-11-06T03:04:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862265.71 epoch sec; seqNo:2243; 4061693.18 north; 594523.29 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:04:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862262.84 2024-11-06T03:04:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:25Z- waiting for 1730862266.84, time is 1730862265.71 2024-11-06T03:04:26Z- AHRS_M2 msg: 1730862266.05 epoch sec; seqNo:2102 2024-11-06T03:04:26Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 1.51 psi 2024-11-06T03:04:26Z- handleDepth()- Depth_Keller msg: 1730862266.06 epoch sec; seqNo:2244; depth 50.53 2024-11-06T03:04:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862266.08 epoch sec; seqNo:2244; 4061693.18 north; 594523.29 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:04:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862262.84 2024-11-06T03:04:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:26Z- waiting for 1730862266.84, time is 1730862266.08 2024-11-06T03:04:26Z- AHRS_M2 msg: 1730862266.45 epoch sec; seqNo:2103 2024-11-06T03:04:26Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:26Z- handleDepth()- Depth_Keller msg: 1730862266.45 epoch sec; seqNo:2245; depth 50.54 2024-11-06T03:04:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862266.46 epoch sec; seqNo:2245; 4061693.20 north; 594524.51 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:04:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862262.84 2024-11-06T03:04:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:26Z- waiting for 1730862266.84, time is 1730862266.46 2024-11-06T03:04:26Z- AHRS_M2 msg: 1730862266.86 epoch sec; seqNo:2104 2024-11-06T03:04:26Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:26Z- handleDepth()- Depth_Keller msg: 1730862266.86 epoch sec; seqNo:2246; depth 50.54 2024-11-06T03:04:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862266.87 epoch sec; seqNo:2246; 4061693.20 north; 594524.51 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:04:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862262.84 2024-11-06T03:04:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:27Z- waiting for 1730862266.84, time is 1730862266.87 2024-11-06T03:04:27Z- LcmTrn::updateTrn() >>>> pose time 1730862266.87, meas time 1730862265.68 <<<< 2024-11-06T03:04:27Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862266.87 2024-11-06T03:04:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862265.68 sec, ping # 0; waiting for INS data more recent than 1730862262.84... 2024-11-06T03:04:27Z- TerrainNav::motionUpdate() - currEstimate = (4061693, 594524.51, 50.54) 2024-11-06T03:04:27Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:04:27Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:04:27Z- TNavPF::Projecting Measurements 2024-11-06T03:04:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:04:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:04:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:04:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:04:27Z- TNavFilter::4 good beams. 2024-11-06T03:04:27Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:04:27Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:04:27Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:04:27Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:04:27Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:04:27Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:04:27Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:04:27Z- TerrainMapOctree: (vehN, vehE ) = (4061693.19, 594523.43). 2024-11-06T03:04:27Z- TerrainMapOctree: Min Distance = 233.58. 2024-11-06T03:04:27Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:04:27Z- Forcing Subcloud Comparison 2024-11-06T03:04:27Z- Weighting particles with subcloud comparison 2024-11-06T03:04:27Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:27Z- meanExpectedMeasDiff: 0.391914 delta_rms_squared: 1.598309 alpha: 0.987641 2024-11-06T03:04:27Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:27Z- meanExpectedMeasDiff: 0.316132 delta_rms_squared: 1.135249 alpha: 0.982688 2024-11-06T03:04:27Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:27Z- meanExpectedMeasDiff: 0.579112 delta_rms_squared: 1.225809 alpha: 0.983946 2024-11-06T03:04:27Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:27Z- meanExpectedMeasDiff: -1.536077 delta_rms_squared: 1.464904 alpha: 0.986531 2024-11-06T03:04:27Z- Subcloud NIS: 0.345306 2024-11-06T03:04:27Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:04:27Z- TNavPF:: sumWeights = 0.436819 2024-11-06T03:04:27Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:04:27Z- TNavPF::First Term of Map Covariance Matrix is 0.32 2024-11-06T03:04:27Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T03:04:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862265.68 sec. 2024-11-06T03:04:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:04:27Z- variances([0], [2], and psi_berg) = 0.717523 0.168824 0.000000 2024-11-06T03:04:27Z- publishEstimatesDecoder() - at 1730862266.87 sec 2024-11-06T03:04:27Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:04:27Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061726.36 594508.06 58.73 2024-11-06T03:04:27Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061725.51 594508.02 58.51 lat/lon: 36.70 -121.94 2024-11-06T03:04:27Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.72 0.17 0.65 0.00 2024-11-06T03:04:27Z- AHRS_M2 msg: 1730862267.26 epoch sec; seqNo:2105 2024-11-06T03:04:27Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:27Z- handleDepth()- Depth_Keller msg: 1730862267.26 epoch sec; seqNo:2247; depth 50.55 2024-11-06T03:04:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862267.27 epoch sec; seqNo:2247; 4061693.20 north; 594524.51 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:04:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862266.87 2024-11-06T03:04:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:27Z- waiting for 1730862270.87, time is 1730862267.27 2024-11-06T03:04:27Z- AHRS_M2 msg: 1730862267.66 epoch sec; seqNo:2106 2024-11-06T03:04:27Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:27Z- handleDepth()- Depth_Keller msg: 1730862267.67 epoch sec; seqNo:2248; depth 50.56 2024-11-06T03:04:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862267.69 epoch sec; seqNo:2248; 4061693.59 north; 594525.73 east; 36.70 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T03:04:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862266.87 2024-11-06T03:04:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:27Z- waiting for 1730862270.87, time is 1730862267.69 2024-11-06T03:04:28Z- AHRS_M2 msg: 1730862268.07 epoch sec; seqNo:2107 2024-11-06T03:04:28Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:28Z- handleDepth()- Depth_Keller msg: 1730862268.08 epoch sec; seqNo:2249; depth 50.58 2024-11-06T03:04:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862268.08 epoch sec; seqNo:2249; 4061693.59 north; 594525.73 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T03:04:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862266.87 2024-11-06T03:04:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:28Z- waiting for 1730862270.87, time is 1730862268.08 2024-11-06T03:04:28Z- AHRS_M2 msg: 1730862268.47 epoch sec; seqNo:2108 2024-11-06T03:04:28Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.51 psi 2024-11-06T03:04:28Z- handleDepth()- Depth_Keller msg: 1730862268.48 epoch sec; seqNo:2250; depth 50.59 2024-11-06T03:04:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862268.49 epoch sec; seqNo:2250; 4061693.59 north; 594525.73 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:04:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862266.87 2024-11-06T03:04:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:28Z- waiting for 1730862270.87, time is 1730862268.49 2024-11-06T03:04:28Z- AHRS_M2 msg: 1730862268.88 epoch sec; seqNo:2109 2024-11-06T03:04:28Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.50 psi 2024-11-06T03:04:28Z- handleDepth()- Depth_Keller msg: 1730862268.88 epoch sec; seqNo:2251; depth 50.60 2024-11-06T03:04:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862268.90 epoch sec; seqNo:2251; 4061693.59 north; 594525.73 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:04:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862266.87 2024-11-06T03:04:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:29Z- waiting for 1730862270.87, time is 1730862268.90 2024-11-06T03:04:29Z- AHRS_M2 msg: 1730862269.28 epoch sec; seqNo:2110 2024-11-06T03:04:29Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.50 psi 2024-11-06T03:04:29Z- handleDepth()- Depth_Keller msg: 1730862269.29 epoch sec; seqNo:2252; depth 50.61 2024-11-06T03:04:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862269.31 epoch sec; seqNo:2252; 4061693.60 north; 594526.95 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:04:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862266.87 2024-11-06T03:04:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:29Z- waiting for 1730862270.87, time is 1730862269.31 2024-11-06T03:04:29Z- AHRS_M2 msg: 1730862269.68 epoch sec; seqNo:2111 2024-11-06T03:04:29Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.50 psi 2024-11-06T03:04:29Z- handleDepth()- Depth_Keller msg: 1730862269.69 epoch sec; seqNo:2253; depth 50.61 2024-11-06T03:04:29Z- handleDVL() - DVL msg epochMillisec: 1730862269.71 2024-11-06T03:04:29Z- handleDvl() - RDI_Pathfinder msg: 1730862269.71 sec; seqNo:721; #beams:4 2024-11-06T03:04:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.90 , 32.62 , 34.49 , 35.17 2024-11-06T03:04:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.05 , 0.01 2024-11-06T03:04:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862269.74 epoch sec; seqNo:2253; 4061693.60 north; 594526.95 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:04:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862266.87 2024-11-06T03:04:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:29Z- waiting for 1730862270.87, time is 1730862269.74 2024-11-06T03:04:30Z- AHRS_M2 msg: 1730862270.09 epoch sec; seqNo:2112 2024-11-06T03:04:30Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.50 psi 2024-11-06T03:04:30Z- handleDepth()- Depth_Keller msg: 1730862270.10 epoch sec; seqNo:2254; depth 50.66 2024-11-06T03:04:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862270.11 epoch sec; seqNo:2254; 4061693.60 north; 594526.95 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T03:04:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862266.87 2024-11-06T03:04:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:30Z- waiting for 1730862270.87, time is 1730862270.11 2024-11-06T03:04:30Z- AHRS_M2 msg: 1730862270.49 epoch sec; seqNo:2113 2024-11-06T03:04:30Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.50 psi 2024-11-06T03:04:30Z- handleDepth()- Depth_Keller msg: 1730862270.51 epoch sec; seqNo:2255; depth 50.66 2024-11-06T03:04:30Z- handleDVL() - DVL msg epochMillisec: 1730862270.51 2024-11-06T03:04:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:04:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:04:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:04:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:04:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:04:30Z- handleDvl() - RDI_Pathfinder msg: 1730862270.51 sec; seqNo:722; #beams:0 2024-11-06T03:04:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:04:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.05 , 0.01 2024-11-06T03:04:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862270.52 epoch sec; seqNo:2255; 4061693.60 north; 594526.95 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T03:04:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862266.87 2024-11-06T03:04:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:30Z- waiting for 1730862270.87, time is 1730862270.52 2024-11-06T03:04:30Z- handleDepth()- Depth_Keller msg: 1730862270.92 epoch sec; seqNo:2256; depth 50.68 2024-11-06T03:04:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862270.93 epoch sec; seqNo:2256; 4061693.62 north; 594528.17 east; 36.70 lat; -121.94 lon; 50.68 depth; utmZone: 10 2024-11-06T03:04:31Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:04:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862266.87 2024-11-06T03:04:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:31Z- waiting for 1730862270.87, time is 1730862270.93 2024-11-06T03:04:31Z- LcmTrn::updateTrn() >>>> pose time 1730862270.93, meas time 1730862270.51 <<<< 2024-11-06T03:04:31Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862270.93 2024-11-06T03:04:31Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:04:31Z- TerrainNav::motionUpdate() - currEstimate = (4061694, 594528.17, 50.68) 2024-11-06T03:04:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:04:31Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:04:31Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:04:31Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:04:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:04:31Z- variances([0], [2], and psi_berg) = 0.719613 0.170264 0.000000 2024-11-06T03:04:31Z- publishEstimatesDecoder() - at 1730862270.93 sec 2024-11-06T03:04:31Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:04:31Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061726.82 594511.65 59.01 2024-11-06T03:04:31Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061725.93 594511.68 58.64 lat/lon: 36.70 -121.94 2024-11-06T03:04:31Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.72 0.17 0.90 0.00 2024-11-06T03:04:31Z- AHRS_M2 msg: 1730862271.30 epoch sec; seqNo:2114 2024-11-06T03:04:31Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.50 psi 2024-11-06T03:04:31Z- handleDepth()- Depth_Keller msg: 1730862271.31 epoch sec; seqNo:2257; depth 50.69 2024-11-06T03:04:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862271.32 epoch sec; seqNo:2257; 4061693.62 north; 594528.17 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:04:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862270.93 2024-11-06T03:04:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:31Z- waiting for 1730862274.93, time is 1730862271.32 2024-11-06T03:04:31Z- AHRS_M2 msg: 1730862271.70 epoch sec; seqNo:2115 2024-11-06T03:04:31Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.50 psi 2024-11-06T03:04:31Z- handleDepth()- Depth_Keller msg: 1730862271.71 epoch sec; seqNo:2258; depth 50.71 2024-11-06T03:04:31Z- handleDVL() - DVL msg epochMillisec: 1730862271.73 2024-11-06T03:04:31Z- handleDvl() - RDI_Pathfinder msg: 1730862271.73 sec; seqNo:723; #beams:4 2024-11-06T03:04:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.74 , 32.79 , 34.32 , 35.17 2024-11-06T03:04:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.03 , 0.03 2024-11-06T03:04:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862271.74 epoch sec; seqNo:2258; 4061693.62 north; 594528.17 east; 36.70 lat; -121.94 lon; 50.71 depth; utmZone: 10 2024-11-06T03:04:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862270.93 2024-11-06T03:04:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:31Z- waiting for 1730862274.93, time is 1730862271.74 2024-11-06T03:04:32Z- AHRS_M2 msg: 1730862272.11 epoch sec; seqNo:2116 2024-11-06T03:04:32Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.50 psi 2024-11-06T03:04:32Z- handleDepth()- Depth_Keller msg: 1730862272.11 epoch sec; seqNo:2259; depth 50.73 2024-11-06T03:04:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862272.13 epoch sec; seqNo:2259; 4061693.63 north; 594529.39 east; 36.70 lat; -121.94 lon; 50.73 depth; utmZone: 10 2024-11-06T03:04:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862270.93 2024-11-06T03:04:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:32Z- waiting for 1730862274.93, time is 1730862272.13 2024-11-06T03:04:32Z- AHRS_M2 msg: 1730862272.53 epoch sec; seqNo:2117 2024-11-06T03:04:32Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.50 psi 2024-11-06T03:04:32Z- handleDepth()- Depth_Keller msg: 1730862272.55 epoch sec; seqNo:2260; depth 50.75 2024-11-06T03:04:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862272.57 epoch sec; seqNo:2260; 4061694.01 north; 594529.39 east; 36.70 lat; -121.94 lon; 50.75 depth; utmZone: 10 2024-11-06T03:04:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862270.93 2024-11-06T03:04:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:32Z- waiting for 1730862274.93, time is 1730862272.57 2024-11-06T03:04:32Z- AHRS_M2 msg: 1730862272.92 epoch sec; seqNo:2118 2024-11-06T03:04:32Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.50 psi 2024-11-06T03:04:32Z- handleDepth()- Depth_Keller msg: 1730862272.92 epoch sec; seqNo:2261; depth 50.77 2024-11-06T03:04:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862272.93 epoch sec; seqNo:2261; 4061694.01 north; 594529.39 east; 36.70 lat; -121.94 lon; 50.77 depth; utmZone: 10 2024-11-06T03:04:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862270.93 2024-11-06T03:04:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:33Z- waiting for 1730862274.93, time is 1730862272.93 2024-11-06T03:04:33Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:04:33Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:04:33Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:04:33Z- AHRS_M2 msg: 1730862273.31 epoch sec; seqNo:2119 2024-11-06T03:04:33Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.50 psi 2024-11-06T03:04:33Z- handleDepth()- Depth_Keller msg: 1730862273.35 epoch sec; seqNo:2262; depth 50.79 2024-11-06T03:04:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862273.36 epoch sec; seqNo:2262; 4061694.01 north; 594529.39 east; 36.70 lat; -121.94 lon; 50.79 depth; utmZone: 10 2024-11-06T03:04:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862270.93 2024-11-06T03:04:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:33Z- waiting for 1730862274.93, time is 1730862273.36 2024-11-06T03:04:33Z- AHRS_M2 msg: 1730862273.72 epoch sec; seqNo:2120 2024-11-06T03:04:33Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.50 psi 2024-11-06T03:04:33Z- handleDepth()- Depth_Keller msg: 1730862273.74 epoch sec; seqNo:2263; depth 50.81 2024-11-06T03:04:33Z- handleDVL() - DVL msg epochMillisec: 1730862273.76 2024-11-06T03:04:33Z- handleDvl() - RDI_Pathfinder msg: 1730862273.76 sec; seqNo:724; #beams:4 2024-11-06T03:04:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.07 , 32.79 , 34.66 , 34.66 2024-11-06T03:04:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.02 2024-11-06T03:04:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862273.77 epoch sec; seqNo:2263; 4061694.02 north; 594530.61 east; 36.70 lat; -121.94 lon; 50.81 depth; utmZone: 10 2024-11-06T03:04:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862270.93 2024-11-06T03:04:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:33Z- waiting for 1730862274.93, time is 1730862273.77 2024-11-06T03:04:34Z- AHRS_M2 msg: 1730862274.13 epoch sec; seqNo:2121 2024-11-06T03:04:34Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.50 psi 2024-11-06T03:04:34Z- handleDepth()- Depth_Keller msg: 1730862274.14 epoch sec; seqNo:2264; depth 50.83 2024-11-06T03:04:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862274.16 epoch sec; seqNo:2264; 4061694.02 north; 594530.61 east; 36.70 lat; -121.94 lon; 50.83 depth; utmZone: 10 2024-11-06T03:04:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862270.93 2024-11-06T03:04:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:34Z- waiting for 1730862274.93, time is 1730862274.16 2024-11-06T03:04:34Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:04:34Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:04:34Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:04:34Z- AHRS_M2 msg: 1730862274.53 epoch sec; seqNo:2122 2024-11-06T03:04:34Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.50 psi 2024-11-06T03:04:34Z- handleDepth()- Depth_Keller msg: 1730862274.56 epoch sec; seqNo:2265; depth 50.85 2024-11-06T03:04:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862274.57 epoch sec; seqNo:2265; 4061694.02 north; 594530.61 east; 36.70 lat; -121.94 lon; 50.85 depth; utmZone: 10 2024-11-06T03:04:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862270.93 2024-11-06T03:04:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:34Z- waiting for 1730862274.93, time is 1730862274.57 2024-11-06T03:04:34Z- AHRS_M2 msg: 1730862274.94 epoch sec; seqNo:2123 2024-11-06T03:04:34Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.50 psi 2024-11-06T03:04:34Z- handleDepth()- Depth_Keller msg: 1730862274.94 epoch sec; seqNo:2266; depth 50.85 2024-11-06T03:04:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862274.96 epoch sec; seqNo:2266; 4061694.02 north; 594530.61 east; 36.70 lat; -121.94 lon; 50.85 depth; utmZone: 10 2024-11-06T03:04:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862270.93 2024-11-06T03:04:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:35Z- waiting for 1730862274.93, time is 1730862274.96 2024-11-06T03:04:35Z- LcmTrn::updateTrn() >>>> pose time 1730862274.96, meas time 1730862273.76 <<<< 2024-11-06T03:04:35Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862274.96 2024-11-06T03:04:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862273.76 sec, ping # 0; waiting for INS data more recent than 1730862270.93... 2024-11-06T03:04:35Z- TerrainNav::motionUpdate() - currEstimate = (4061694, 594530.61, 50.85) 2024-11-06T03:04:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:04:35Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:04:35Z- TNavPF::Projecting Measurements 2024-11-06T03:04:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:04:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:04:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:04:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:04:35Z- TNavFilter::4 good beams. 2024-11-06T03:04:35Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:04:35Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:04:35Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:04:35Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:04:35Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:04:35Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:04:35Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:04:35Z- TerrainMapOctree: (vehN, vehE ) = (4061693.90, 594529.88). 2024-11-06T03:04:35Z- TerrainMapOctree: Min Distance = 227.15. 2024-11-06T03:04:35Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:04:35Z- Forcing Subcloud Comparison 2024-11-06T03:04:35Z- Weighting particles with subcloud comparison 2024-11-06T03:04:35Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:35Z- meanExpectedMeasDiff: 1.025839 delta_rms_squared: 1.834003 alpha: 0.989213 2024-11-06T03:04:35Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:35Z- meanExpectedMeasDiff: 0.522427 delta_rms_squared: 1.316062 alpha: 0.985031 2024-11-06T03:04:35Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:35Z- meanExpectedMeasDiff: 0.482947 delta_rms_squared: 1.477604 alpha: 0.986645 2024-11-06T03:04:35Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:35Z- meanExpectedMeasDiff: -1.040331 delta_rms_squared: 1.613177 alpha: 0.987754 2024-11-06T03:04:35Z- Subcloud NIS: 0.349039 2024-11-06T03:04:35Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:04:35Z- TNavPF:: sumWeights = 0.444957 2024-11-06T03:04:35Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:04:35Z- TNavPF::First Term of Map Covariance Matrix is 0.41 2024-11-06T03:04:35Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T03:04:35Z- TerrainNav::Resampling particle filter... 2024-11-06T03:04:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862273.76 sec. 2024-11-06T03:04:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:04:35Z- variances([0], [2], and psi_berg) = 0.720569 0.169037 0.000000 2024-11-06T03:04:35Z- publishEstimatesDecoder() - at 1730862274.96 sec 2024-11-06T03:04:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:04:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061726.68 594514.77 55.67 2024-11-06T03:04:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061726.34 594514.12 58.69 lat/lon: 36.70 -121.94 2024-11-06T03:04:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.72 0.17 0.67 0.00 2024-11-06T03:04:35Z- AHRS_M2 msg: 1730862275.34 epoch sec; seqNo:2124 2024-11-06T03:04:35Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.50 psi 2024-11-06T03:04:35Z- handleDepth()- Depth_Keller msg: 1730862275.35 epoch sec; seqNo:2267; depth 50.87 2024-11-06T03:04:35Z- handleDVL() - DVL msg epochMillisec: 1730862275.35 2024-11-06T03:04:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:04:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:04:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:04:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:04:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:04:35Z- handleDvl() - RDI_Pathfinder msg: 1730862275.35 sec; seqNo:725; #beams:0 2024-11-06T03:04:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:04:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.02 2024-11-06T03:04:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862275.36 epoch sec; seqNo:2267; 4061694.04 north; 594531.83 east; 36.70 lat; -121.94 lon; 50.87 depth; utmZone: 10 2024-11-06T03:04:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862274.96 2024-11-06T03:04:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:35Z- waiting for 1730862278.96, time is 1730862275.36 2024-11-06T03:04:35Z- AHRS_M2 msg: 1730862275.75 epoch sec; seqNo:2125 2024-11-06T03:04:35Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.50 psi 2024-11-06T03:04:35Z- handleDepth()- Depth_Keller msg: 1730862275.75 epoch sec; seqNo:2268; depth 50.88 2024-11-06T03:04:35Z- handleDVL() - DVL msg epochMillisec: 1730862275.77 2024-11-06T03:04:35Z- handleDvl() - RDI_Pathfinder msg: 1730862275.77 sec; seqNo:726; #beams:4 2024-11-06T03:04:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.24 , 32.79 , 34.83 , 35.00 2024-11-06T03:04:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.02 2024-11-06T03:04:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862275.79 epoch sec; seqNo:2268; 4061694.04 north; 594531.83 east; 36.70 lat; -121.94 lon; 50.88 depth; utmZone: 10 2024-11-06T03:04:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862274.96 2024-11-06T03:04:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:35Z- waiting for 1730862278.96, time is 1730862275.79 2024-11-06T03:04:36Z- AHRS_M2 msg: 1730862276.15 epoch sec; seqNo:2126 2024-11-06T03:04:36Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.50 psi 2024-11-06T03:04:36Z- handleDepth()- Depth_Keller msg: 1730862276.16 epoch sec; seqNo:2269; depth 50.89 2024-11-06T03:04:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862276.17 epoch sec; seqNo:2269; 4061694.04 north; 594531.83 east; 36.70 lat; -121.94 lon; 50.89 depth; utmZone: 10 2024-11-06T03:04:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862274.96 2024-11-06T03:04:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:36Z- waiting for 1730862278.96, time is 1730862276.17 2024-11-06T03:04:36Z- AHRS_M2 msg: 1730862276.55 epoch sec; seqNo:2127 2024-11-06T03:04:36Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.50 psi 2024-11-06T03:04:36Z- handleDepth()- Depth_Keller msg: 1730862276.56 epoch sec; seqNo:2270; depth 50.91 2024-11-06T03:04:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862276.58 epoch sec; seqNo:2270; 4061694.05 north; 594533.05 east; 36.70 lat; -121.94 lon; 50.91 depth; utmZone: 10 2024-11-06T03:04:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862274.96 2024-11-06T03:04:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:36Z- waiting for 1730862278.96, time is 1730862276.58 2024-11-06T03:04:36Z- AHRS_M2 msg: 1730862276.96 epoch sec; seqNo:2128 2024-11-06T03:04:36Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.50 psi 2024-11-06T03:04:36Z- handleDepth()- Depth_Keller msg: 1730862276.96 epoch sec; seqNo:2271; depth 50.93 2024-11-06T03:04:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862276.97 epoch sec; seqNo:2271; 4061694.43 north; 594533.04 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T03:04:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862274.96 2024-11-06T03:04:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:37Z- waiting for 1730862278.96, time is 1730862276.97 2024-11-06T03:04:37Z- AHRS_M2 msg: 1730862277.36 epoch sec; seqNo:2129 2024-11-06T03:04:37Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.50 psi 2024-11-06T03:04:37Z- handleDepth()- Depth_Keller msg: 1730862277.36 epoch sec; seqNo:2272; depth 50.92 2024-11-06T03:04:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862277.37 epoch sec; seqNo:2272; 4061694.43 north; 594533.04 east; 36.70 lat; -121.94 lon; 50.92 depth; utmZone: 10 2024-11-06T03:04:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862274.96 2024-11-06T03:04:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:37Z- waiting for 1730862278.96, time is 1730862277.37 2024-11-06T03:04:37Z- AHRS_M2 msg: 1730862277.76 epoch sec; seqNo:2130 2024-11-06T03:04:37Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.50 psi 2024-11-06T03:04:37Z- handleDepth()- Depth_Keller msg: 1730862277.77 epoch sec; seqNo:2273; depth 50.93 2024-11-06T03:04:37Z- handleDVL() - DVL msg epochMillisec: 1730862277.80 2024-11-06T03:04:37Z- handleDvl() - RDI_Pathfinder msg: 1730862277.80 sec; seqNo:727; #beams:4 2024-11-06T03:04:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.90 , 32.79 , 34.83 , 34.66 2024-11-06T03:04:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , 0.01 2024-11-06T03:04:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862277.81 epoch sec; seqNo:2273; 4061694.43 north; 594533.04 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T03:04:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862274.96 2024-11-06T03:04:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:37Z- waiting for 1730862278.96, time is 1730862277.81 2024-11-06T03:04:38Z- AHRS_M2 msg: 1730862278.17 epoch sec; seqNo:2131 2024-11-06T03:04:38Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.50 psi 2024-11-06T03:04:38Z- handleDepth()- Depth_Keller msg: 1730862278.19 epoch sec; seqNo:2274; depth 50.93 2024-11-06T03:04:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862278.20 epoch sec; seqNo:2274; 4061694.44 north; 594534.26 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T03:04:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862274.96 2024-11-06T03:04:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:38Z- waiting for 1730862278.96, time is 1730862278.20 2024-11-06T03:04:38Z- AHRS_M2 msg: 1730862278.57 epoch sec; seqNo:2132 2024-11-06T03:04:38Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T03:04:38Z- handleDepth()- Depth_Keller msg: 1730862278.58 epoch sec; seqNo:2275; depth 50.94 2024-11-06T03:04:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862278.59 epoch sec; seqNo:2275; 4061694.44 north; 594534.26 east; 36.70 lat; -121.94 lon; 50.94 depth; utmZone: 10 2024-11-06T03:04:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862274.96 2024-11-06T03:04:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:38Z- waiting for 1730862278.96, time is 1730862278.59 2024-11-06T03:04:38Z- AHRS_M2 msg: 1730862278.98 epoch sec; seqNo:2133 2024-11-06T03:04:38Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T03:04:38Z- handleDepth()- Depth_Keller msg: 1730862278.98 epoch sec; seqNo:2276; depth 50.95 2024-11-06T03:04:38Z- handleDVL() - DVL msg epochMillisec: 1730862278.98 2024-11-06T03:04:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:04:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:04:38Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:04:38Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:04:38Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:04:38Z- handleDvl() - RDI_Pathfinder msg: 1730862278.98 sec; seqNo:728; #beams:0 2024-11-06T03:04:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:04:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , 0.01 2024-11-06T03:04:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862278.99 epoch sec; seqNo:2276; 4061694.44 north; 594534.26 east; 36.70 lat; -121.94 lon; 50.95 depth; utmZone: 10 2024-11-06T03:04:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862274.96 2024-11-06T03:04:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:39Z- waiting for 1730862278.96, time is 1730862278.99 2024-11-06T03:04:39Z- LcmTrn::updateTrn() >>>> pose time 1730862278.99, meas time 1730862278.98 <<<< 2024-11-06T03:04:39Z- LcmTrn::updateTrn() equating pose 1730862278.98 and meas times 1730862278.98 2024-11-06T03:04:39Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862278.98 2024-11-06T03:04:39Z- TerrainNav::motionUpdate() - currEstimate = (4061694, 594534.26, 50.95) 2024-11-06T03:04:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:04:39Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:04:39Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:04:39Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:04:39Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:04:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:04:39Z- variances([0], [2], and psi_berg) = 0.723032 0.170772 0.000000 2024-11-06T03:04:39Z- publishEstimatesDecoder() - at 1730862278.98 sec 2024-11-06T03:04:39Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:04:39Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061727.17 594518.35 55.95 2024-11-06T03:04:39Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061726.76 594517.77 58.79 lat/lon: 36.70 -121.94 2024-11-06T03:04:39Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.72 0.17 0.91 0.00 2024-11-06T03:04:39Z- AHRS_M2 msg: 1730862279.38 epoch sec; seqNo:2134 2024-11-06T03:04:39Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.50 psi 2024-11-06T03:04:39Z- handleDepth()- Depth_Keller msg: 1730862279.39 epoch sec; seqNo:2277; depth 50.94 2024-11-06T03:04:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862279.41 epoch sec; seqNo:2277; 4061694.46 north; 594535.48 east; 36.70 lat; -121.94 lon; 50.94 depth; utmZone: 10 2024-11-06T03:04:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862278.98 2024-11-06T03:04:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:39Z- waiting for 1730862282.98, time is 1730862279.41 2024-11-06T03:04:39Z- AHRS_M2 msg: 1730862279.78 epoch sec; seqNo:2135 2024-11-06T03:04:39Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.50 psi 2024-11-06T03:04:39Z- handleDepth()- Depth_Keller msg: 1730862279.79 epoch sec; seqNo:2278; depth 50.94 2024-11-06T03:04:39Z- handleDVL() - DVL msg epochMillisec: 1730862279.82 2024-11-06T03:04:39Z- handleDvl() - RDI_Pathfinder msg: 1730862279.82 sec; seqNo:729; #beams:4 2024-11-06T03:04:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.56 , 32.79 , 35.00 , 34.32 2024-11-06T03:04:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , -0.01 2024-11-06T03:04:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862279.83 epoch sec; seqNo:2278; 4061694.46 north; 594535.48 east; 36.70 lat; -121.94 lon; 50.94 depth; utmZone: 10 2024-11-06T03:04:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862278.98 2024-11-06T03:04:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:39Z- waiting for 1730862282.98, time is 1730862279.83 2024-11-06T03:04:40Z- AHRS_M2 msg: 1730862280.19 epoch sec; seqNo:2136 2024-11-06T03:04:40Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.50 psi 2024-11-06T03:04:40Z- handleDepth()- Depth_Keller msg: 1730862280.20 epoch sec; seqNo:2279; depth 50.94 2024-11-06T03:04:40Z- handleDVL() - DVL msg epochMillisec: 1730862280.20 2024-11-06T03:04:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:04:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:04:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:04:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:04:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:04:40Z- handleDvl() - RDI_Pathfinder msg: 1730862280.20 sec; seqNo:730; #beams:0 2024-11-06T03:04:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:04:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , -0.01 2024-11-06T03:04:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862280.24 epoch sec; seqNo:2279; 4061694.46 north; 594535.48 east; 36.70 lat; -121.94 lon; 50.94 depth; utmZone: 10 2024-11-06T03:04:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862278.98 2024-11-06T03:04:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:40Z- waiting for 1730862282.98, time is 1730862280.24 2024-11-06T03:04:40Z- AHRS_M2 msg: 1730862280.59 epoch sec; seqNo:2137 2024-11-06T03:04:40Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.50 psi 2024-11-06T03:04:40Z- handleDepth()- Depth_Keller msg: 1730862280.60 epoch sec; seqNo:2280; depth 50.93 2024-11-06T03:04:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862280.61 epoch sec; seqNo:2280; 4061694.46 north; 594535.48 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T03:04:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862278.98 2024-11-06T03:04:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:40Z- waiting for 1730862282.98, time is 1730862280.61 2024-11-06T03:04:41Z- AHRS_M2 msg: 1730862281.00 epoch sec; seqNo:2138 2024-11-06T03:04:41Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.50 psi 2024-11-06T03:04:41Z- handleDepth()- Depth_Keller msg: 1730862281.01 epoch sec; seqNo:2281; depth 50.94 2024-11-06T03:04:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862281.02 epoch sec; seqNo:2281; 4061694.47 north; 594536.71 east; 36.70 lat; -121.94 lon; 50.94 depth; utmZone: 10 2024-11-06T03:04:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862278.98 2024-11-06T03:04:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:41Z- waiting for 1730862282.98, time is 1730862281.02 2024-11-06T03:04:41Z- AHRS_M2 msg: 1730862281.40 epoch sec; seqNo:2139 2024-11-06T03:04:41Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.49 psi 2024-11-06T03:04:41Z- handleDepth()- Depth_Keller msg: 1730862281.41 epoch sec; seqNo:2282; depth 50.93 2024-11-06T03:04:41Z- handleDVL() - DVL msg epochMillisec: 1730862281.41 2024-11-06T03:04:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:04:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:04:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:04:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:04:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:04:41Z- handleDvl() - RDI_Pathfinder msg: 1730862281.41 sec; seqNo:731; #beams:0 2024-11-06T03:04:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:04:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , -0.01 2024-11-06T03:04:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862281.43 epoch sec; seqNo:2282; 4061694.47 north; 594536.71 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T03:04:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862278.98 2024-11-06T03:04:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:41Z- waiting for 1730862282.98, time is 1730862281.43 2024-11-06T03:04:41Z- AHRS_M2 msg: 1730862281.80 epoch sec; seqNo:2140 2024-11-06T03:04:41Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.49 psi 2024-11-06T03:04:41Z- handleDepth()- Depth_Keller msg: 1730862281.81 epoch sec; seqNo:2283; depth 50.93 2024-11-06T03:04:41Z- handleDVL() - DVL msg epochMillisec: 1730862281.83 2024-11-06T03:04:41Z- handleDvl() - RDI_Pathfinder msg: 1730862281.83 sec; seqNo:732; #beams:4 2024-11-06T03:04:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.40 , 32.79 , 35.17 , 34.15 2024-11-06T03:04:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.02 2024-11-06T03:04:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862281.86 epoch sec; seqNo:2283; 4061694.47 north; 594536.71 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T03:04:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862278.98 2024-11-06T03:04:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:41Z- waiting for 1730862282.98, time is 1730862281.86 2024-11-06T03:04:42Z- AHRS_M2 msg: 1730862282.21 epoch sec; seqNo:2141 2024-11-06T03:04:42Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T03:04:42Z- handleDepth()- Depth_Keller msg: 1730862282.22 epoch sec; seqNo:2284; depth 50.93 2024-11-06T03:04:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862282.24 epoch sec; seqNo:2284; 4061694.85 north; 594536.70 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T03:04:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862278.98 2024-11-06T03:04:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:42Z- waiting for 1730862282.98, time is 1730862282.24 2024-11-06T03:04:42Z- AHRS_M2 msg: 1730862282.61 epoch sec; seqNo:2142 2024-11-06T03:04:42Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T03:04:42Z- handleDepth()- Depth_Keller msg: 1730862282.62 epoch sec; seqNo:2285; depth 50.91 2024-11-06T03:04:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862282.64 epoch sec; seqNo:2285; 4061694.86 north; 594537.92 east; 36.70 lat; -121.94 lon; 50.91 depth; utmZone: 10 2024-11-06T03:04:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862278.98 2024-11-06T03:04:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:42Z- waiting for 1730862282.98, time is 1730862282.64 2024-11-06T03:04:43Z- AHRS_M2 msg: 1730862283.02 epoch sec; seqNo:2143 2024-11-06T03:04:43Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T03:04:43Z- handleDepth()- Depth_Keller msg: 1730862283.02 epoch sec; seqNo:2286; depth 50.92 2024-11-06T03:04:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862283.03 epoch sec; seqNo:2286; 4061694.86 north; 594537.92 east; 36.70 lat; -121.94 lon; 50.92 depth; utmZone: 10 2024-11-06T03:04:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862278.98 2024-11-06T03:04:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:43Z- waiting for 1730862282.98, time is 1730862283.03 2024-11-06T03:04:43Z- LcmTrn::updateTrn() >>>> pose time 1730862283.03, meas time 1730862281.83 <<<< 2024-11-06T03:04:43Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862283.03 2024-11-06T03:04:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862281.83 sec, ping # 0; waiting for INS data more recent than 1730862278.98... 2024-11-06T03:04:43Z- TerrainNav::motionUpdate() - currEstimate = (4061695, 594537.92, 50.92) 2024-11-06T03:04:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:04:43Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:04:43Z- TNavPF::Projecting Measurements 2024-11-06T03:04:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:04:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:04:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:04:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:04:43Z- TNavFilter::4 good beams. 2024-11-06T03:04:43Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:04:43Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:04:43Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:04:43Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:04:43Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:04:43Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:04:43Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:04:43Z- TerrainMapOctree: (vehN, vehE ) = (4061694.74, 594536.84). 2024-11-06T03:04:43Z- TerrainMapOctree: Min Distance = 220.21. 2024-11-06T03:04:43Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:04:43Z- Forcing Subcloud Comparison 2024-11-06T03:04:43Z- Weighting particles with subcloud comparison 2024-11-06T03:04:43Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:43Z- meanExpectedMeasDiff: 0.533236 delta_rms_squared: 1.860281 alpha: 0.989363 2024-11-06T03:04:43Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:43Z- meanExpectedMeasDiff: 1.551178 delta_rms_squared: 1.349066 alpha: 0.985391 2024-11-06T03:04:43Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:43Z- meanExpectedMeasDiff: 0.470841 delta_rms_squared: 1.560609 alpha: 0.987347 2024-11-06T03:04:43Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:43Z- meanExpectedMeasDiff: -0.897442 delta_rms_squared: 1.592390 alpha: 0.987596 2024-11-06T03:04:43Z- Subcloud NIS: 0.347331 2024-11-06T03:04:43Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:04:43Z- TNavPF:: sumWeights = 0.381597 2024-11-06T03:04:43Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:04:43Z- TNavPF::First Term of Map Covariance Matrix is 0.28 2024-11-06T03:04:43Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T03:04:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862281.83 sec. 2024-11-06T03:04:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:04:43Z- variances([0], [2], and psi_berg) = 0.731090 0.171183 0.000000 2024-11-06T03:04:43Z- publishEstimatesDecoder() - at 1730862283.03 sec 2024-11-06T03:04:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:04:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061726.55 594520.80 58.18 2024-11-06T03:04:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061727.19 594521.43 58.54 lat/lon: 36.70 -121.94 2024-11-06T03:04:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.73 0.17 0.69 0.00 2024-11-06T03:04:43Z- AHRS_M2 msg: 1730862283.42 epoch sec; seqNo:2144 2024-11-06T03:04:43Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T03:04:43Z- handleDepth()- Depth_Keller msg: 1730862283.43 epoch sec; seqNo:2287; depth 50.92 2024-11-06T03:04:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862283.45 epoch sec; seqNo:2287; 4061694.86 north; 594537.92 east; 36.70 lat; -121.94 lon; 50.92 depth; utmZone: 10 2024-11-06T03:04:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862283.03 2024-11-06T03:04:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:43Z- waiting for 1730862287.03, time is 1730862283.45 2024-11-06T03:04:43Z- AHRS_M2 msg: 1730862283.82 epoch sec; seqNo:2145 2024-11-06T03:04:43Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.49 psi 2024-11-06T03:04:43Z- handleDepth()- Depth_Keller msg: 1730862283.83 epoch sec; seqNo:2288; depth 50.91 2024-11-06T03:04:43Z- handleDVL() - DVL msg epochMillisec: 1730862283.85 2024-11-06T03:04:43Z- handleDvl() - RDI_Pathfinder msg: 1730862283.85 sec; seqNo:733; #beams:4 2024-11-06T03:04:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.22 , 32.79 , 35.17 , 34.15 2024-11-06T03:04:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , -0.02 2024-11-06T03:04:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862283.86 epoch sec; seqNo:2288; 4061694.88 north; 594539.14 east; 36.70 lat; -121.94 lon; 50.91 depth; utmZone: 10 2024-11-06T03:04:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862283.03 2024-11-06T03:04:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:43Z- waiting for 1730862287.03, time is 1730862283.86 2024-11-06T03:04:44Z- AHRS_M2 msg: 1730862284.25 epoch sec; seqNo:2146 2024-11-06T03:04:44Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.49 psi 2024-11-06T03:04:44Z- handleDepth()- Depth_Keller msg: 1730862284.25 epoch sec; seqNo:2289; depth 50.90 2024-11-06T03:04:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862284.27 epoch sec; seqNo:2289; 4061694.88 north; 594539.14 east; 36.70 lat; -121.94 lon; 50.90 depth; utmZone: 10 2024-11-06T03:04:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862283.03 2024-11-06T03:04:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:44Z- waiting for 1730862287.03, time is 1730862284.27 2024-11-06T03:04:44Z- handleDepth()- Depth_Keller msg: 1730862284.66 epoch sec; seqNo:2290; depth 50.89 2024-11-06T03:04:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862284.67 epoch sec; seqNo:2290; 4061694.88 north; 594539.14 east; 36.70 lat; -121.94 lon; 50.89 depth; utmZone: 10 2024-11-06T03:04:44Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:04:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862283.03 2024-11-06T03:04:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:44Z- waiting for 1730862287.03, time is 1730862284.67 2024-11-06T03:04:45Z- AHRS_M2 msg: 1730862285.03 epoch sec; seqNo:2147 2024-11-06T03:04:45Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.48 psi 2024-11-06T03:04:45Z- handleDepth()- Depth_Keller msg: 1730862285.04 epoch sec; seqNo:2291; depth 50.89 2024-11-06T03:04:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862285.05 epoch sec; seqNo:2291; 4061694.88 north; 594539.14 east; 36.70 lat; -121.94 lon; 50.89 depth; utmZone: 10 2024-11-06T03:04:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862283.03 2024-11-06T03:04:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:45Z- waiting for 1730862287.03, time is 1730862285.05 2024-11-06T03:04:45Z- AHRS_M2 msg: 1730862285.44 epoch sec; seqNo:2148 2024-11-06T03:04:45Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.48 psi 2024-11-06T03:04:45Z- handleDepth()- Depth_Keller msg: 1730862285.44 epoch sec; seqNo:2292; depth 50.88 2024-11-06T03:04:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862285.46 epoch sec; seqNo:2292; 4061694.89 north; 594540.36 east; 36.70 lat; -121.94 lon; 50.88 depth; utmZone: 10 2024-11-06T03:04:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862283.03 2024-11-06T03:04:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:45Z- waiting for 1730862287.03, time is 1730862285.46 2024-11-06T03:04:45Z- AHRS_M2 msg: 1730862285.84 epoch sec; seqNo:2149 2024-11-06T03:04:45Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.48 psi 2024-11-06T03:04:45Z- handleDepth()- Depth_Keller msg: 1730862285.85 epoch sec; seqNo:2293; depth 50.90 2024-11-06T03:04:45Z- handleDVL() - DVL msg epochMillisec: 1730862285.88 2024-11-06T03:04:45Z- handleDvl() - RDI_Pathfinder msg: 1730862285.88 sec; seqNo:734; #beams:4 2024-11-06T03:04:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.56 , 32.79 , 35.17 , 34.66 2024-11-06T03:04:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , 0.00 2024-11-06T03:04:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862285.89 epoch sec; seqNo:2293; 4061694.89 north; 594540.36 east; 36.70 lat; -121.94 lon; 50.90 depth; utmZone: 10 2024-11-06T03:04:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862283.03 2024-11-06T03:04:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:46Z- waiting for 1730862287.03, time is 1730862285.89 2024-11-06T03:04:46Z- AHRS_M2 msg: 1730862286.26 epoch sec; seqNo:2150 2024-11-06T03:04:46Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.48 psi 2024-11-06T03:04:46Z- handleDepth()- Depth_Keller msg: 1730862286.26 epoch sec; seqNo:2294; depth 50.88 2024-11-06T03:04:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862286.27 epoch sec; seqNo:2294; 4061695.27 north; 594540.36 east; 36.70 lat; -121.94 lon; 50.88 depth; utmZone: 10 2024-11-06T03:04:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862283.03 2024-11-06T03:04:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:46Z- waiting for 1730862287.03, time is 1730862286.27 2024-11-06T03:04:46Z- AHRS_M2 msg: 1730862286.66 epoch sec; seqNo:2151 2024-11-06T03:04:46Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.48 psi 2024-11-06T03:04:46Z- handleDepth()- Depth_Keller msg: 1730862286.67 epoch sec; seqNo:2295; depth 50.88 2024-11-06T03:04:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862286.68 epoch sec; seqNo:2295; 4061695.27 north; 594540.36 east; 36.70 lat; -121.94 lon; 50.88 depth; utmZone: 10 2024-11-06T03:04:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862283.03 2024-11-06T03:04:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:46Z- waiting for 1730862287.03, time is 1730862286.68 2024-11-06T03:04:47Z- AHRS_M2 msg: 1730862287.05 epoch sec; seqNo:2152 2024-11-06T03:04:47Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.48 psi 2024-11-06T03:04:47Z- handleDepth()- Depth_Keller msg: 1730862287.06 epoch sec; seqNo:2296; depth 50.89 2024-11-06T03:04:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862287.07 epoch sec; seqNo:2296; 4061695.28 north; 594541.58 east; 36.70 lat; -121.94 lon; 50.89 depth; utmZone: 10 2024-11-06T03:04:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862283.03 2024-11-06T03:04:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:47Z- waiting for 1730862287.03, time is 1730862287.07 2024-11-06T03:04:47Z- LcmTrn::updateTrn() >>>> pose time 1730862287.07, meas time 1730862285.88 <<<< 2024-11-06T03:04:47Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862287.07 2024-11-06T03:04:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862285.88 sec, ping # 0; waiting for INS data more recent than 1730862283.03... 2024-11-06T03:04:47Z- TerrainNav::motionUpdate() - currEstimate = (4061695, 594541.58, 50.89) 2024-11-06T03:04:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:04:47Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:04:47Z- TNavPF::Projecting Measurements 2024-11-06T03:04:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:04:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:04:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:04:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:04:47Z- TNavFilter::4 good beams. 2024-11-06T03:04:47Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:04:47Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:04:47Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:04:47Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:04:47Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:04:47Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:04:47Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:04:47Z- TerrainMapOctree: (vehN, vehE ) = (4061695.16, 594540.50). 2024-11-06T03:04:47Z- TerrainMapOctree: Min Distance = 216.56. 2024-11-06T03:04:47Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:04:47Z- Forcing Subcloud Comparison 2024-11-06T03:04:47Z- Weighting particles with subcloud comparison 2024-11-06T03:04:47Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:47Z- meanExpectedMeasDiff: 0.463664 delta_rms_squared: 1.464473 alpha: 0.986527 2024-11-06T03:04:47Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:47Z- meanExpectedMeasDiff: 1.255632 delta_rms_squared: 1.070321 alpha: 0.981657 2024-11-06T03:04:47Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:47Z- meanExpectedMeasDiff: 0.063431 delta_rms_squared: 1.241581 alpha: 0.984147 2024-11-06T03:04:47Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:04:47Z- meanExpectedMeasDiff: -0.528533 delta_rms_squared: 1.247433 alpha: 0.984220 2024-11-06T03:04:47Z- Subcloud NIS: 0.340763 2024-11-06T03:04:47Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:04:47Z- TNavPF:: sumWeights = 0.504362 2024-11-06T03:04:47Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:04:47Z- TNavPF::First Term of Map Covariance Matrix is 0.33 2024-11-06T03:04:47Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.06 2024-11-06T03:04:47Z- TerrainNav::Resampling particle filter... 2024-11-06T03:04:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862285.88 sec. 2024-11-06T03:04:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:04:47Z- variances([0], [2], and psi_berg) = 0.734063 0.173470 0.000000 2024-11-06T03:04:47Z- publishEstimatesDecoder() - at 1730862287.07 sec 2024-11-06T03:04:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:04:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061727.87 594525.89 58.68 2024-11-06T03:04:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061727.62 594525.09 58.36 lat/lon: 36.70 -121.94 2024-11-06T03:04:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.73 0.17 0.64 0.00 2024-11-06T03:04:47Z- AHRS_M2 msg: 1730862287.46 epoch sec; seqNo:2153 2024-11-06T03:04:47Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.48 psi 2024-11-06T03:04:47Z- handleDepth()- Depth_Keller msg: 1730862287.47 epoch sec; seqNo:2297; depth 50.87 2024-11-06T03:04:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862287.52 epoch sec; seqNo:2297; 4061695.28 north; 594541.58 east; 36.70 lat; -121.94 lon; 50.87 depth; utmZone: 10 2024-11-06T03:04:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862287.07 2024-11-06T03:04:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:47Z- waiting for 1730862291.07, time is 1730862287.52 2024-11-06T03:04:47Z- AHRS_M2 msg: 1730862287.86 epoch sec; seqNo:2154 2024-11-06T03:04:47Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.48 psi 2024-11-06T03:04:47Z- handleDepth()- Depth_Keller msg: 1730862287.87 epoch sec; seqNo:2298; depth 50.88 2024-11-06T03:04:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862287.90 epoch sec; seqNo:2298; 4061695.28 north; 594541.58 east; 36.70 lat; -121.94 lon; 50.88 depth; utmZone: 10 2024-11-06T03:04:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862287.07 2024-11-06T03:04:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:48Z- waiting for 1730862291.07, time is 1730862287.90 2024-11-06T03:04:48Z- AHRS_M2 msg: 1730862288.27 epoch sec; seqNo:2155 2024-11-06T03:04:48Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.48 psi 2024-11-06T03:04:48Z- handleDepth()- Depth_Keller msg: 1730862288.28 epoch sec; seqNo:2299; depth 50.88 2024-11-06T03:04:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862288.29 epoch sec; seqNo:2299; 4061695.29 north; 594542.80 east; 36.70 lat; -121.94 lon; 50.88 depth; utmZone: 10 2024-11-06T03:04:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862287.07 2024-11-06T03:04:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:48Z- waiting for 1730862291.07, time is 1730862288.29 2024-11-06T03:04:48Z- handleDepth()- Depth_Keller msg: 1730862288.69 epoch sec; seqNo:2300; depth 50.88 2024-11-06T03:04:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862288.70 epoch sec; seqNo:2300; 4061695.29 north; 594542.80 east; 36.70 lat; -121.94 lon; 50.88 depth; utmZone: 10 2024-11-06T03:04:48Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:04:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862287.07 2024-11-06T03:04:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:48Z- waiting for 1730862291.07, time is 1730862288.70 2024-11-06T03:04:49Z- AHRS_M2 msg: 1730862289.08 epoch sec; seqNo:2156 2024-11-06T03:04:49Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.48 psi 2024-11-06T03:04:49Z- handleDepth()- Depth_Keller msg: 1730862289.08 epoch sec; seqNo:2301; depth 50.88 2024-11-06T03:04:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862289.09 epoch sec; seqNo:2301; 4061695.29 north; 594542.80 east; 36.70 lat; -121.94 lon; 50.88 depth; utmZone: 10 2024-11-06T03:04:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862287.07 2024-11-06T03:04:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:49Z- waiting for 1730862291.07, time is 1730862289.09 2024-11-06T03:04:49Z- AHRS_M2 msg: 1730862289.48 epoch sec; seqNo:2157 2024-11-06T03:04:49Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:04:49Z- handleDepth()- Depth_Keller msg: 1730862289.52 epoch sec; seqNo:2302; depth 50.88 2024-11-06T03:04:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862289.56 epoch sec; seqNo:2302; 4061695.29 north; 594542.80 east; 36.70 lat; -121.94 lon; 50.88 depth; utmZone: 10 2024-11-06T03:04:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862287.07 2024-11-06T03:04:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:49Z- waiting for 1730862291.07, time is 1730862289.56 2024-11-06T03:04:49Z- AHRS_M2 msg: 1730862289.89 epoch sec; seqNo:2158 2024-11-06T03:04:49Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:04:49Z- handleDepth()- Depth_Keller msg: 1730862289.89 epoch sec; seqNo:2303; depth 50.86 2024-11-06T03:04:49Z- handleDVL() - DVL msg epochMillisec: 1730862289.93 2024-11-06T03:04:49Z- handleDvl() - RDI_Pathfinder msg: 1730862289.93 sec; seqNo:735; #beams:4 2024-11-06T03:04:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.74 , 32.62 , 35.51 , 35.00 2024-11-06T03:04:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , -0.01 2024-11-06T03:04:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862289.94 epoch sec; seqNo:2303; 4061695.31 north; 594544.02 east; 36.70 lat; -121.94 lon; 50.86 depth; utmZone: 10 2024-11-06T03:04:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862287.07 2024-11-06T03:04:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:50Z- waiting for 1730862291.07, time is 1730862289.94 2024-11-06T03:04:50Z- AHRS_M2 msg: 1730862290.29 epoch sec; seqNo:2159 2024-11-06T03:04:50Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:04:50Z- handleDepth()- Depth_Keller msg: 1730862290.30 epoch sec; seqNo:2304; depth 50.87 2024-11-06T03:04:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862290.32 epoch sec; seqNo:2304; 4061695.69 north; 594544.01 east; 36.70 lat; -121.94 lon; 50.87 depth; utmZone: 10 2024-11-06T03:04:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862287.07 2024-11-06T03:04:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:50Z- waiting for 1730862291.07, time is 1730862290.32 2024-11-06T03:04:50Z- AHRS_M2 msg: 1730862290.69 epoch sec; seqNo:2160 2024-11-06T03:04:50Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:04:50Z- handleDepth()- Depth_Keller msg: 1730862290.70 epoch sec; seqNo:2305; depth 50.88 2024-11-06T03:04:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862290.71 epoch sec; seqNo:2305; 4061695.69 north; 594544.01 east; 36.70 lat; -121.94 lon; 50.88 depth; utmZone: 10 2024-11-06T03:04:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862287.07 2024-11-06T03:04:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:50Z- waiting for 1730862291.07, time is 1730862290.71 2024-11-06T03:04:51Z- AHRS_M2 msg: 1730862291.10 epoch sec; seqNo:2161 2024-11-06T03:04:51Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:04:51Z- handleDepth()- Depth_Keller msg: 1730862291.11 epoch sec; seqNo:2306; depth 50.87 2024-11-06T03:04:51Z- handleDVL() - DVL msg epochMillisec: 1730862291.11 2024-11-06T03:04:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:04:51Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:04:51Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:04:51Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:04:51Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:04:51Z- handleDvl() - RDI_Pathfinder msg: 1730862291.11 sec; seqNo:736; #beams:0 2024-11-06T03:04:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:04:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , -0.01 2024-11-06T03:04:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862291.12 epoch sec; seqNo:2306; 4061695.69 north; 594544.01 east; 36.70 lat; -121.94 lon; 50.87 depth; utmZone: 10 2024-11-06T03:04:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862287.07 2024-11-06T03:04:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:51Z- waiting for 1730862291.07, time is 1730862291.12 2024-11-06T03:04:51Z- LcmTrn::updateTrn() >>>> pose time 1730862291.12, meas time 1730862291.11 <<<< 2024-11-06T03:04:51Z- LcmTrn::updateTrn() equating pose 1730862291.11 and meas times 1730862291.11 2024-11-06T03:04:51Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862291.11 2024-11-06T03:04:51Z- TerrainNav::motionUpdate() - currEstimate = (4061696, 594544.01, 50.87) 2024-11-06T03:04:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:04:51Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:04:51Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:04:51Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:04:51Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:04:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:04:51Z- variances([0], [2], and psi_berg) = 0.735433 0.174549 0.000000 2024-11-06T03:04:51Z- publishEstimatesDecoder() - at 1730862291.11 sec 2024-11-06T03:04:51Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:04:51Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061728.29 594528.32 58.73 2024-11-06T03:04:51Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061728.02 594527.52 58.34 lat/lon: 36.70 -121.94 2024-11-06T03:04:51Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.74 0.17 0.90 0.00 2024-11-06T03:04:51Z- handleDepth()- Depth_Keller msg: 1730862291.56 epoch sec; seqNo:2307; depth 50.87 2024-11-06T03:04:51Z- handleDVL() - DVL msg epochMillisec: 1730862291.58 2024-11-06T03:04:51Z- handleDvl() - RDI_Pathfinder msg: 1730862291.58 sec; seqNo:737; #beams:4 2024-11-06T03:04:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.74 , 32.45 , 35.51 , 34.83 2024-11-06T03:04:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , -0.01 2024-11-06T03:04:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862291.61 epoch sec; seqNo:2307; 4061695.70 north; 594545.23 east; 36.70 lat; -121.94 lon; 50.87 depth; utmZone: 10 2024-11-06T03:04:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862291.11 2024-11-06T03:04:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:51Z- waiting for 1730862295.11, time is 1730862291.61 2024-11-06T03:04:51Z- AHRS_M2 msg: 1730862291.91 epoch sec; seqNo:2162 2024-11-06T03:04:51Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:04:51Z- handleDepth()- Depth_Keller msg: 1730862291.91 epoch sec; seqNo:2308; depth 50.86 2024-11-06T03:04:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862291.93 epoch sec; seqNo:2308; 4061695.70 north; 594545.23 east; 36.70 lat; -121.94 lon; 50.86 depth; utmZone: 10 2024-11-06T03:04:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862291.11 2024-11-06T03:04:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:52Z- waiting for 1730862295.11, time is 1730862291.93 2024-11-06T03:04:52Z- AHRS_M2 msg: 1730862292.31 epoch sec; seqNo:2163 2024-11-06T03:04:52Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:04:52Z- handleDepth()- Depth_Keller msg: 1730862292.32 epoch sec; seqNo:2309; depth 50.86 2024-11-06T03:04:52Z- handleDVL() - DVL msg epochMillisec: 1730862292.32 2024-11-06T03:04:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:04:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:04:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:04:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:04:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:04:52Z- handleDvl() - RDI_Pathfinder msg: 1730862292.32 sec; seqNo:738; #beams:0 2024-11-06T03:04:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:04:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , -0.01 2024-11-06T03:04:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862292.34 epoch sec; seqNo:2309; 4061695.70 north; 594545.23 east; 36.70 lat; -121.94 lon; 50.86 depth; utmZone: 10 2024-11-06T03:04:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862291.11 2024-11-06T03:04:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:52Z- waiting for 1730862295.11, time is 1730862292.34 2024-11-06T03:04:52Z- AHRS_M2 msg: 1730862292.71 epoch sec; seqNo:2164 2024-11-06T03:04:52Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:04:52Z- handleDepth()- Depth_Keller msg: 1730862292.71 epoch sec; seqNo:2310; depth 50.86 2024-11-06T03:04:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862292.73 epoch sec; seqNo:2310; 4061695.71 north; 594546.46 east; 36.70 lat; -121.94 lon; 50.86 depth; utmZone: 10 2024-11-06T03:04:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862291.11 2024-11-06T03:04:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:52Z- waiting for 1730862295.11, time is 1730862292.73 2024-11-06T03:04:53Z- AHRS_M2 msg: 1730862293.12 epoch sec; seqNo:2165 2024-11-06T03:04:53Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:04:53Z- handleDepth()- Depth_Keller msg: 1730862293.12 epoch sec; seqNo:2311; depth 50.85 2024-11-06T03:04:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862293.13 epoch sec; seqNo:2311; 4061695.71 north; 594546.46 east; 36.70 lat; -121.94 lon; 50.85 depth; utmZone: 10 2024-11-06T03:04:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862291.11 2024-11-06T03:04:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:53Z- waiting for 1730862295.11, time is 1730862293.13 2024-11-06T03:04:53Z- AHRS_M2 msg: 1730862293.56 epoch sec; seqNo:2166 2024-11-06T03:04:53Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:04:53Z- handleDepth()- Depth_Keller msg: 1730862293.57 epoch sec; seqNo:2312; depth 50.85 2024-11-06T03:04:53Z- handleDVL() - DVL msg epochMillisec: 1730862293.59 2024-11-06T03:04:53Z- handleDvl() - RDI_Pathfinder msg: 1730862293.59 sec; seqNo:739; #beams:4 2024-11-06T03:04:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.90 , 32.79 , 35.51 , 34.83 2024-11-06T03:04:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , 0.00 2024-11-06T03:04:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862293.60 epoch sec; seqNo:2312; 4061696.09 north; 594546.45 east; 36.70 lat; -121.94 lon; 50.85 depth; utmZone: 10 2024-11-06T03:04:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862291.11 2024-11-06T03:04:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:53Z- waiting for 1730862295.11, time is 1730862293.60 2024-11-06T03:04:53Z- AHRS_M2 msg: 1730862293.92 epoch sec; seqNo:2167 2024-11-06T03:04:53Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:04:53Z- handleDepth()- Depth_Keller msg: 1730862293.93 epoch sec; seqNo:2313; depth 50.86 2024-11-06T03:04:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862293.95 epoch sec; seqNo:2313; 4061696.09 north; 594546.45 east; 36.70 lat; -121.94 lon; 50.86 depth; utmZone: 10 2024-11-06T03:04:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862291.11 2024-11-06T03:04:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:54Z- waiting for 1730862295.11, time is 1730862293.95 2024-11-06T03:04:54Z- AHRS_M2 msg: 1730862294.33 epoch sec; seqNo:2168 2024-11-06T03:04:54Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:04:54Z- handleDepth()- Depth_Keller msg: 1730862294.33 epoch sec; seqNo:2314; depth 50.84 2024-11-06T03:04:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862294.36 epoch sec; seqNo:2314; 4061696.11 north; 594547.67 east; 36.70 lat; -121.94 lon; 50.84 depth; utmZone: 10 2024-11-06T03:04:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862291.11 2024-11-06T03:04:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:54Z- waiting for 1730862295.11, time is 1730862294.36 2024-11-06T03:04:54Z- AHRS_M2 msg: 1730862294.73 epoch sec; seqNo:2169 2024-11-06T03:04:54Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:04:54Z- handleDepth()- Depth_Keller msg: 1730862294.74 epoch sec; seqNo:2315; depth 50.85 2024-11-06T03:04:54Z- handleDVL() - DVL msg epochMillisec: 1730862294.74 2024-11-06T03:04:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:04:54Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:04:54Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:04:54Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:04:54Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:04:54Z- handleDvl() - RDI_Pathfinder msg: 1730862294.74 sec; seqNo:740; #beams:0 2024-11-06T03:04:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:04:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , 0.00 2024-11-06T03:04:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862294.75 epoch sec; seqNo:2315; 4061696.11 north; 594547.67 east; 36.70 lat; -121.94 lon; 50.85 depth; utmZone: 10 2024-11-06T03:04:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862291.11 2024-11-06T03:04:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:54Z- waiting for 1730862295.11, time is 1730862294.75 2024-11-06T03:04:55Z- AHRS_M2 msg: 1730862295.13 epoch sec; seqNo:2170 2024-11-06T03:04:55Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:04:55Z- handleDepth()- Depth_Keller msg: 1730862295.15 epoch sec; seqNo:2316; depth 50.85 2024-11-06T03:04:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862295.16 epoch sec; seqNo:2316; 4061696.11 north; 594547.67 east; 36.70 lat; -121.94 lon; 50.85 depth; utmZone: 10 2024-11-06T03:04:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862291.11 2024-11-06T03:04:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:55Z- waiting for 1730862295.11, time is 1730862295.16 2024-11-06T03:04:55Z- LcmTrn::updateTrn() >>>> pose time 1730862295.16, meas time 1730862294.74 <<<< 2024-11-06T03:04:55Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862295.16 2024-11-06T03:04:55Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:04:55Z- TerrainNav::motionUpdate() - currEstimate = (4061696, 594547.67, 50.85) 2024-11-06T03:04:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:04:55Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:04:55Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:04:55Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:04:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:04:55Z- variances([0], [2], and psi_berg) = 0.736474 0.177236 0.000000 2024-11-06T03:04:55Z- publishEstimatesDecoder() - at 1730862295.16 sec 2024-11-06T03:04:55Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:04:55Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061728.76 594531.85 58.22 2024-11-06T03:04:55Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061728.44 594531.18 58.33 lat/lon: 36.70 -121.94 2024-11-06T03:04:55Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.74 0.18 1.14 0.00 2024-11-06T03:04:55Z- AHRS_M2 msg: 1730862295.57 epoch sec; seqNo:2171 2024-11-06T03:04:55Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:04:55Z- handleDepth()- Depth_Keller msg: 1730862295.58 epoch sec; seqNo:2317; depth 50.82 2024-11-06T03:04:55Z- handleDVL() - DVL msg epochMillisec: 1730862295.60 2024-11-06T03:04:55Z- handleDvl() - RDI_Pathfinder msg: 1730862295.60 sec; seqNo:741; #beams:4 2024-11-06T03:04:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.90 , 32.62 , 35.68 , 35.00 2024-11-06T03:04:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , 0.02 2024-11-06T03:04:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862295.62 epoch sec; seqNo:2317; 4061696.11 north; 594547.67 east; 36.70 lat; -121.94 lon; 50.82 depth; utmZone: 10 2024-11-06T03:04:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862295.16 2024-11-06T03:04:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:55Z- waiting for 1730862299.16, time is 1730862295.62 2024-11-06T03:04:55Z- AHRS_M2 msg: 1730862295.94 epoch sec; seqNo:2172 2024-11-06T03:04:55Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:04:55Z- handleDepth()- Depth_Keller msg: 1730862295.95 epoch sec; seqNo:2318; depth 50.82 2024-11-06T03:04:55Z- handleDVL() - DVL msg epochMillisec: 1730862295.95 2024-11-06T03:04:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:04:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:04:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:04:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:04:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:04:55Z- handleDvl() - RDI_Pathfinder msg: 1730862295.95 sec; seqNo:742; #beams:0 2024-11-06T03:04:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:04:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , 0.02 2024-11-06T03:04:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862295.97 epoch sec; seqNo:2318; 4061696.12 north; 594548.89 east; 36.70 lat; -121.94 lon; 50.82 depth; utmZone: 10 2024-11-06T03:04:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862295.16 2024-11-06T03:04:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:56Z- waiting for 1730862299.16, time is 1730862295.97 2024-11-06T03:04:56Z- handleDepth()- Depth_Keller msg: 1730862296.37 epoch sec; seqNo:2319; depth 50.82 2024-11-06T03:04:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862296.38 epoch sec; seqNo:2319; 4061696.12 north; 594548.89 east; 36.70 lat; -121.94 lon; 50.82 depth; utmZone: 10 2024-11-06T03:04:56Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:04:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862295.16 2024-11-06T03:04:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:56Z- waiting for 1730862299.16, time is 1730862296.38 2024-11-06T03:04:56Z- handleDepth()- Depth_Keller msg: 1730862296.77 epoch sec; seqNo:2320; depth 50.81 2024-11-06T03:04:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862296.78 epoch sec; seqNo:2320; 4061696.12 north; 594548.89 east; 36.70 lat; -121.94 lon; 50.81 depth; utmZone: 10 2024-11-06T03:04:56Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:04:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862295.16 2024-11-06T03:04:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:56Z- waiting for 1730862299.16, time is 1730862296.78 2024-11-06T03:04:57Z- AHRS_M2 msg: 1730862297.15 epoch sec; seqNo:2173 2024-11-06T03:04:57Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.49 psi 2024-11-06T03:04:57Z- handleDepth()- Depth_Keller msg: 1730862297.17 epoch sec; seqNo:2321; depth 50.81 2024-11-06T03:04:57Z- handleDVL() - DVL msg epochMillisec: 1730862297.17 2024-11-06T03:04:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:04:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:04:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:04:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:04:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:04:57Z- handleDvl() - RDI_Pathfinder msg: 1730862297.17 sec; seqNo:743; #beams:0 2024-11-06T03:04:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:04:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , 0.02 2024-11-06T03:04:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862297.19 epoch sec; seqNo:2321; 4061696.51 north; 594550.11 east; 36.70 lat; -121.94 lon; 50.81 depth; utmZone: 10 2024-11-06T03:04:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862295.16 2024-11-06T03:04:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:57Z- waiting for 1730862299.16, time is 1730862297.19 2024-11-06T03:04:57Z- AHRS_M2 msg: 1730862297.58 epoch sec; seqNo:2174 2024-11-06T03:04:57Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.49 psi 2024-11-06T03:04:57Z- handleDepth()- Depth_Keller msg: 1730862297.58 epoch sec; seqNo:2322; depth 50.82 2024-11-06T03:04:57Z- handleDVL() - DVL msg epochMillisec: 1730862297.62 2024-11-06T03:04:57Z- handleDvl() - RDI_Pathfinder msg: 1730862297.62 sec; seqNo:744; #beams:4 2024-11-06T03:04:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.90 , 32.79 , 35.85 , 35.00 2024-11-06T03:04:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , 0.01 2024-11-06T03:04:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862297.64 epoch sec; seqNo:2322; 4061696.51 north; 594550.11 east; 36.70 lat; -121.94 lon; 50.82 depth; utmZone: 10 2024-11-06T03:04:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862295.16 2024-11-06T03:04:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:57Z- waiting for 1730862299.16, time is 1730862297.64 2024-11-06T03:04:57Z- AHRS_M2 msg: 1730862297.96 epoch sec; seqNo:2175 2024-11-06T03:04:57Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.49 psi 2024-11-06T03:04:57Z- handleDepth()- Depth_Keller msg: 1730862297.97 epoch sec; seqNo:2323; depth 50.80 2024-11-06T03:04:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862297.98 epoch sec; seqNo:2323; 4061696.51 north; 594550.11 east; 36.70 lat; -121.94 lon; 50.80 depth; utmZone: 10 2024-11-06T03:04:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862295.16 2024-11-06T03:04:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:58Z- waiting for 1730862299.16, time is 1730862297.98 2024-11-06T03:04:58Z- AHRS_M2 msg: 1730862298.37 epoch sec; seqNo:2176 2024-11-06T03:04:58Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.49 psi 2024-11-06T03:04:58Z- handleDepth()- Depth_Keller msg: 1730862298.38 epoch sec; seqNo:2324; depth 50.78 2024-11-06T03:04:58Z- handleDVL() - DVL msg epochMillisec: 1730862298.38 2024-11-06T03:04:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:04:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:04:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:04:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:04:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:04:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:04:58Z- handleDvl() - RDI_Pathfinder msg: 1730862298.38 sec; seqNo:745; #beams:0 2024-11-06T03:04:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:04:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , 0.01 2024-11-06T03:04:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862298.40 epoch sec; seqNo:2324; 4061696.51 north; 594550.11 east; 36.70 lat; -121.94 lon; 50.78 depth; utmZone: 10 2024-11-06T03:04:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862295.16 2024-11-06T03:04:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:58Z- waiting for 1730862299.16, time is 1730862298.40 2024-11-06T03:04:58Z- AHRS_M2 msg: 1730862298.77 epoch sec; seqNo:2177 2024-11-06T03:04:58Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T03:04:58Z- handleDepth()- Depth_Keller msg: 1730862298.78 epoch sec; seqNo:2325; depth 50.78 2024-11-06T03:04:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862298.79 epoch sec; seqNo:2325; 4061696.53 north; 594551.33 east; 36.70 lat; -121.94 lon; 50.78 depth; utmZone: 10 2024-11-06T03:04:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862295.16 2024-11-06T03:04:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:58Z- waiting for 1730862299.16, time is 1730862298.79 2024-11-06T03:04:59Z- AHRS_M2 msg: 1730862299.18 epoch sec; seqNo:2178 2024-11-06T03:04:59Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T03:04:59Z- handleDepth()- Depth_Keller msg: 1730862299.19 epoch sec; seqNo:2326; depth 50.78 2024-11-06T03:04:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862299.20 epoch sec; seqNo:2326; 4061696.53 north; 594551.33 east; 36.70 lat; -121.94 lon; 50.78 depth; utmZone: 10 2024-11-06T03:04:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:04:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862295.16 2024-11-06T03:04:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:59Z- waiting for 1730862299.16, time is 1730862299.20 2024-11-06T03:04:59Z- LcmTrn::updateTrn() >>>> pose time 1730862299.20, meas time 1730862298.38 <<<< 2024-11-06T03:04:59Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862299.20 2024-11-06T03:04:59Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:04:59Z- TerrainNav::motionUpdate() - currEstimate = (4061697, 594551.33, 50.78) 2024-11-06T03:04:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:04:59Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:04:59Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:04:59Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:04:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:04:59Z- variances([0], [2], and psi_berg) = 0.738584 0.178995 0.000000 2024-11-06T03:04:59Z- publishEstimatesDecoder() - at 1730862299.20 sec 2024-11-06T03:04:59Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:04:59Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061729.16 594535.49 57.65 2024-11-06T03:04:59Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061728.86 594534.84 58.25 lat/lon: 36.70 -121.94 2024-11-06T03:04:59Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.74 0.18 1.40 0.00 2024-11-06T03:04:59Z- AHRS_M2 msg: 1730862299.58 epoch sec; seqNo:2179 2024-11-06T03:04:59Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T03:04:59Z- handleDepth()- Depth_Keller msg: 1730862299.60 epoch sec; seqNo:2327; depth 50.74 2024-11-06T03:04:59Z- handleDVL() - DVL msg epochMillisec: 1730862299.62 2024-11-06T03:04:59Z- handleDvl() - RDI_Pathfinder msg: 1730862299.62 sec; seqNo:746; #beams:4 2024-11-06T03:04:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.74 , 32.62 , 36.02 , 34.83 2024-11-06T03:04:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , -0.03 2024-11-06T03:04:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862299.66 epoch sec; seqNo:2327; 4061696.53 north; 594551.33 east; 36.70 lat; -121.94 lon; 50.74 depth; utmZone: 10 2024-11-06T03:04:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:04:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862299.20 2024-11-06T03:04:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:04:59Z- waiting for 1730862303.20, time is 1730862299.66 2024-11-06T03:04:59Z- AHRS_M2 msg: 1730862299.98 epoch sec; seqNo:2180 2024-11-06T03:04:59Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T03:04:59Z- handleDepth()- Depth_Keller msg: 1730862299.99 epoch sec; seqNo:2328; depth 50.76 2024-11-06T03:05:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862300.02 epoch sec; seqNo:2328; 4061696.54 north; 594552.55 east; 36.70 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T03:05:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862299.20 2024-11-06T03:05:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:00Z- waiting for 1730862303.20, time is 1730862300.02 2024-11-06T03:05:00Z- AHRS_M2 msg: 1730862300.39 epoch sec; seqNo:2181 2024-11-06T03:05:00Z- AHRS_M2 msg: 0.16 phi; 0.03 theta; 1.50 psi 2024-11-06T03:05:00Z- handleDepth()- Depth_Keller msg: 1730862300.39 epoch sec; seqNo:2329; depth 50.74 2024-11-06T03:05:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862300.40 epoch sec; seqNo:2329; 4061696.54 north; 594552.55 east; 36.70 lat; -121.94 lon; 50.74 depth; utmZone: 10 2024-11-06T03:05:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862299.20 2024-11-06T03:05:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:00Z- waiting for 1730862303.20, time is 1730862300.40 2024-11-06T03:05:00Z- AHRS_M2 msg: 1730862300.79 epoch sec; seqNo:2182 2024-11-06T03:05:00Z- AHRS_M2 msg: 0.16 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:00Z- handleDepth()- Depth_Keller msg: 1730862300.80 epoch sec; seqNo:2330; depth 50.72 2024-11-06T03:05:00Z- handleDVL() - DVL msg epochMillisec: 1730862300.80 2024-11-06T03:05:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:00Z- handleDvl() - RDI_Pathfinder msg: 1730862300.80 sec; seqNo:747; #beams:0 2024-11-06T03:05:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , -0.03 2024-11-06T03:05:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862300.81 epoch sec; seqNo:2330; 4061696.54 north; 594552.55 east; 36.70 lat; -121.94 lon; 50.72 depth; utmZone: 10 2024-11-06T03:05:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862299.20 2024-11-06T03:05:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:00Z- waiting for 1730862303.20, time is 1730862300.81 2024-11-06T03:05:01Z- AHRS_M2 msg: 1730862301.19 epoch sec; seqNo:2183 2024-11-06T03:05:01Z- AHRS_M2 msg: 0.16 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:01Z- handleDepth()- Depth_Keller msg: 1730862301.21 epoch sec; seqNo:2331; depth 50.71 2024-11-06T03:05:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862301.22 epoch sec; seqNo:2331; 4061696.54 north; 594552.55 east; 36.70 lat; -121.94 lon; 50.71 depth; utmZone: 10 2024-11-06T03:05:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862299.20 2024-11-06T03:05:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:01Z- waiting for 1730862303.20, time is 1730862301.22 2024-11-06T03:05:01Z- AHRS_M2 msg: 1730862301.61 epoch sec; seqNo:2184 2024-11-06T03:05:01Z- AHRS_M2 msg: 0.16 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:01Z- handleDepth()- Depth_Keller msg: 1730862301.63 epoch sec; seqNo:2332; depth 50.69 2024-11-06T03:05:01Z- handleDVL() - DVL msg epochMillisec: 1730862301.68 2024-11-06T03:05:01Z- handleDvl() - RDI_Pathfinder msg: 1730862301.68 sec; seqNo:748; #beams:4 2024-11-06T03:05:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.90 , 32.62 , 36.19 , 34.66 2024-11-06T03:05:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.02 , -0.05 2024-11-06T03:05:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862301.69 epoch sec; seqNo:2332; 4061696.55 north; 594553.77 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:05:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862299.20 2024-11-06T03:05:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:01Z- waiting for 1730862303.20, time is 1730862301.69 2024-11-06T03:05:02Z- AHRS_M2 msg: 1730862302.00 epoch sec; seqNo:2185 2024-11-06T03:05:02Z- AHRS_M2 msg: 0.16 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:02Z- handleDepth()- Depth_Keller msg: 1730862302.01 epoch sec; seqNo:2333; depth 50.68 2024-11-06T03:05:02Z- handleDVL() - DVL msg epochMillisec: 1730862302.01 2024-11-06T03:05:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:02Z- handleDvl() - RDI_Pathfinder msg: 1730862302.01 sec; seqNo:749; #beams:0 2024-11-06T03:05:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.02 , -0.05 2024-11-06T03:05:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862302.02 epoch sec; seqNo:2333; 4061696.55 north; 594553.77 east; 36.70 lat; -121.94 lon; 50.68 depth; utmZone: 10 2024-11-06T03:05:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862299.20 2024-11-06T03:05:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:02Z- waiting for 1730862303.20, time is 1730862302.02 2024-11-06T03:05:02Z- AHRS_M2 msg: 1730862302.41 epoch sec; seqNo:2186 2024-11-06T03:05:02Z- AHRS_M2 msg: 0.16 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:02Z- handleDepth()- Depth_Keller msg: 1730862302.42 epoch sec; seqNo:2334; depth 50.67 2024-11-06T03:05:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862302.43 epoch sec; seqNo:2334; 4061696.55 north; 594553.77 east; 36.70 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T03:05:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862299.20 2024-11-06T03:05:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:02Z- waiting for 1730862303.20, time is 1730862302.43 2024-11-06T03:05:02Z- AHRS_M2 msg: 1730862302.81 epoch sec; seqNo:2187 2024-11-06T03:05:02Z- AHRS_M2 msg: 0.16 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:02Z- handleDepth()- Depth_Keller msg: 1730862302.82 epoch sec; seqNo:2335; depth 50.66 2024-11-06T03:05:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862302.84 epoch sec; seqNo:2335; 4061696.57 north; 594554.99 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T03:05:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862299.20 2024-11-06T03:05:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:02Z- waiting for 1730862303.20, time is 1730862302.84 2024-11-06T03:05:03Z- AHRS_M2 msg: 1730862303.21 epoch sec; seqNo:2188 2024-11-06T03:05:03Z- AHRS_M2 msg: 0.16 phi; 0.05 theta; 1.51 psi 2024-11-06T03:05:03Z- handleDepth()- Depth_Keller msg: 1730862303.22 epoch sec; seqNo:2336; depth 50.64 2024-11-06T03:05:03Z- handleDVL() - DVL msg epochMillisec: 1730862303.22 2024-11-06T03:05:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:03Z- handleDvl() - RDI_Pathfinder msg: 1730862303.22 sec; seqNo:750; #beams:0 2024-11-06T03:05:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.02 , -0.05 2024-11-06T03:05:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862303.23 epoch sec; seqNo:2336; 4061696.57 north; 594554.99 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:05:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862299.20 2024-11-06T03:05:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:03Z- waiting for 1730862303.20, time is 1730862303.23 2024-11-06T03:05:03Z- LcmTrn::updateTrn() >>>> pose time 1730862303.23, meas time 1730862303.22 <<<< 2024-11-06T03:05:03Z- LcmTrn::updateTrn() equating pose 1730862303.22 and meas times 1730862303.22 2024-11-06T03:05:03Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862303.22 2024-11-06T03:05:03Z- TerrainNav::motionUpdate() - currEstimate = (4061697, 594554.99, 50.64) 2024-11-06T03:05:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:05:03Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:05:03Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:05:03Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:05:03Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:05:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:05:03Z- variances([0], [2], and psi_berg) = 0.740290 0.181137 0.000000 2024-11-06T03:05:03Z- publishEstimatesDecoder() - at 1730862303.22 sec 2024-11-06T03:05:03Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:05:03Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061729.16 594539.07 57.08 2024-11-06T03:05:03Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061728.90 594538.50 58.11 lat/lon: 36.70 -121.94 2024-11-06T03:05:03Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.74 0.18 1.66 0.00 2024-11-06T03:05:03Z- AHRS_M2 msg: 1730862303.62 epoch sec; seqNo:2189 2024-11-06T03:05:03Z- AHRS_M2 msg: 0.15 phi; 0.05 theta; 1.51 psi 2024-11-06T03:05:03Z- handleDepth()- Depth_Keller msg: 1730862303.64 epoch sec; seqNo:2337; depth 50.64 2024-11-06T03:05:03Z- handleDVL() - DVL msg epochMillisec: 1730862303.67 2024-11-06T03:05:03Z- handleDvl() - RDI_Pathfinder msg: 1730862303.67 sec; seqNo:751; #beams:4 2024-11-06T03:05:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.24 , 32.62 , 36.36 , 34.66 2024-11-06T03:05:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , -0.02 2024-11-06T03:05:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862303.70 epoch sec; seqNo:2337; 4061696.94 north; 594554.98 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:05:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862303.22 2024-11-06T03:05:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:03Z- waiting for 1730862307.22, time is 1730862303.70 2024-11-06T03:05:04Z- AHRS_M2 msg: 1730862304.02 epoch sec; seqNo:2190 2024-11-06T03:05:04Z- AHRS_M2 msg: 0.15 phi; 0.05 theta; 1.51 psi 2024-11-06T03:05:04Z- handleDepth()- Depth_Keller msg: 1730862304.03 epoch sec; seqNo:2338; depth 50.62 2024-11-06T03:05:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862304.05 epoch sec; seqNo:2338; 4061696.94 north; 594554.98 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:05:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862303.22 2024-11-06T03:05:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:04Z- waiting for 1730862307.22, time is 1730862304.05 2024-11-06T03:05:04Z- AHRS_M2 msg: 1730862304.43 epoch sec; seqNo:2191 2024-11-06T03:05:04Z- AHRS_M2 msg: 0.15 phi; 0.05 theta; 1.50 psi 2024-11-06T03:05:04Z- handleDepth()- Depth_Keller msg: 1730862304.43 epoch sec; seqNo:2339; depth 50.60 2024-11-06T03:05:04Z- handleDVL() - DVL msg epochMillisec: 1730862304.44 2024-11-06T03:05:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:04Z- handleDvl() - RDI_Pathfinder msg: 1730862304.44 sec; seqNo:752; #beams:0 2024-11-06T03:05:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , -0.02 2024-11-06T03:05:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862304.44 epoch sec; seqNo:2339; 4061696.96 north; 594556.21 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:05:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862303.22 2024-11-06T03:05:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:04Z- waiting for 1730862307.22, time is 1730862304.44 2024-11-06T03:05:04Z- AHRS_M2 msg: 1730862304.83 epoch sec; seqNo:2192 2024-11-06T03:05:04Z- AHRS_M2 msg: 0.15 phi; 0.05 theta; 1.50 psi 2024-11-06T03:05:04Z- handleDepth()- Depth_Keller msg: 1730862304.84 epoch sec; seqNo:2340; depth 50.59 2024-11-06T03:05:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862304.86 epoch sec; seqNo:2340; 4061696.96 north; 594556.21 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:05:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862303.22 2024-11-06T03:05:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:04Z- waiting for 1730862307.22, time is 1730862304.86 2024-11-06T03:05:05Z- handleDepth()- Depth_Keller msg: 1730862305.26 epoch sec; seqNo:2341; depth 50.58 2024-11-06T03:05:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862305.27 epoch sec; seqNo:2341; 4061696.96 north; 594556.21 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T03:05:05Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:05:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862303.22 2024-11-06T03:05:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:05Z- waiting for 1730862307.22, time is 1730862305.27 2024-11-06T03:05:05Z- AHRS_M2 msg: 1730862305.65 epoch sec; seqNo:2193 2024-11-06T03:05:05Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.51 psi 2024-11-06T03:05:05Z- handleDepth()- Depth_Keller msg: 1730862305.66 epoch sec; seqNo:2342; depth 50.55 2024-11-06T03:05:05Z- handleDVL() - DVL msg epochMillisec: 1730862305.72 2024-11-06T03:05:05Z- handleDvl() - RDI_Pathfinder msg: 1730862305.72 sec; seqNo:753; #beams:4 2024-11-06T03:05:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.07 , 32.79 , 36.70 , 34.49 2024-11-06T03:05:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , 0.00 2024-11-06T03:05:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862305.75 epoch sec; seqNo:2342; 4061696.96 north; 594556.21 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:05:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862303.22 2024-11-06T03:05:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:05Z- waiting for 1730862307.22, time is 1730862305.75 2024-11-06T03:05:06Z- AHRS_M2 msg: 1730862306.04 epoch sec; seqNo:2194 2024-11-06T03:05:06Z- AHRS_M2 msg: 0.16 phi; 0.06 theta; 1.51 psi 2024-11-06T03:05:06Z- handleDepth()- Depth_Keller msg: 1730862306.05 epoch sec; seqNo:2343; depth 50.54 2024-11-06T03:05:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862306.07 epoch sec; seqNo:2343; 4061696.97 north; 594557.43 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:05:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862303.22 2024-11-06T03:05:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:06Z- waiting for 1730862307.22, time is 1730862306.07 2024-11-06T03:05:06Z- AHRS_M2 msg: 1730862306.45 epoch sec; seqNo:2195 2024-11-06T03:05:06Z- AHRS_M2 msg: 0.16 phi; 0.06 theta; 1.51 psi 2024-11-06T03:05:06Z- handleDepth()- Depth_Keller msg: 1730862306.45 epoch sec; seqNo:2344; depth 50.53 2024-11-06T03:05:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862306.46 epoch sec; seqNo:2344; 4061696.97 north; 594557.43 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:05:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862303.22 2024-11-06T03:05:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:06Z- waiting for 1730862307.22, time is 1730862306.46 2024-11-06T03:05:06Z- AHRS_M2 msg: 1730862306.86 epoch sec; seqNo:2196 2024-11-06T03:05:06Z- AHRS_M2 msg: 0.16 phi; 0.06 theta; 1.51 psi 2024-11-06T03:05:06Z- handleDepth()- Depth_Keller msg: 1730862306.86 epoch sec; seqNo:2345; depth 50.52 2024-11-06T03:05:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862306.87 epoch sec; seqNo:2345; 4061697.35 north; 594557.42 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:05:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862303.22 2024-11-06T03:05:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:07Z- waiting for 1730862307.22, time is 1730862306.87 2024-11-06T03:05:07Z- AHRS_M2 msg: 1730862307.25 epoch sec; seqNo:2197 2024-11-06T03:05:07Z- AHRS_M2 msg: 0.16 phi; 0.06 theta; 1.51 psi 2024-11-06T03:05:07Z- handleDepth()- Depth_Keller msg: 1730862307.27 epoch sec; seqNo:2346; depth 50.50 2024-11-06T03:05:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862307.29 epoch sec; seqNo:2346; 4061697.35 north; 594557.42 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:05:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862303.22 2024-11-06T03:05:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:07Z- waiting for 1730862307.22, time is 1730862307.29 2024-11-06T03:05:07Z- LcmTrn::updateTrn() >>>> pose time 1730862307.29, meas time 1730862305.72 <<<< 2024-11-06T03:05:07Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862307.29 2024-11-06T03:05:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862305.72 sec, ping # 0; waiting for INS data more recent than 1730862303.22... 2024-11-06T03:05:07Z- TerrainNav::motionUpdate() - currEstimate = (4061697, 594557.42, 50.50) 2024-11-06T03:05:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:05:07Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:05:07Z- TNavPF::Projecting Measurements 2024-11-06T03:05:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:05:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:05:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:05:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:05:07Z- TNavFilter::4 good beams. 2024-11-06T03:05:07Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:05:07Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:05:07Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:05:07Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:05:07Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:05:07Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:05:07Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:05:07Z- TerrainMapOctree: (vehN, vehE ) = (4061697.05, 594556.48). 2024-11-06T03:05:07Z- TerrainMapOctree: Min Distance = 200.63. 2024-11-06T03:05:07Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:05:07Z- Forcing Subcloud Comparison 2024-11-06T03:05:07Z- Weighting particles with subcloud comparison 2024-11-06T03:05:07Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:07Z- meanExpectedMeasDiff: -0.438169 delta_rms_squared: 3.139789 alpha: 0.993670 2024-11-06T03:05:07Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:07Z- meanExpectedMeasDiff: 0.958856 delta_rms_squared: 2.174067 alpha: 0.990885 2024-11-06T03:05:07Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:07Z- meanExpectedMeasDiff: -0.417963 delta_rms_squared: 2.769037 alpha: 0.992829 2024-11-06T03:05:07Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:07Z- meanExpectedMeasDiff: -0.286111 delta_rms_squared: 2.429533 alpha: 0.991835 2024-11-06T03:05:07Z- Subcloud NIS: 0.345705 2024-11-06T03:05:07Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:05:07Z- TNavPF:: sumWeights = 0.434615 2024-11-06T03:05:07Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:05:07Z- TNavPF::First Term of Map Covariance Matrix is 0.38 2024-11-06T03:05:07Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T03:05:07Z- TerrainNav::Resampling particle filter... 2024-11-06T03:05:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862305.72 sec. 2024-11-06T03:05:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:05:07Z- variances([0], [2], and psi_berg) = 0.738356 0.179948 0.000000 2024-11-06T03:05:07Z- publishEstimatesDecoder() - at 1730862307.29 sec 2024-11-06T03:05:07Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:05:07Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061729.90 594541.52 57.76 2024-11-06T03:05:07Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061729.70 594540.93 57.99 lat/lon: 36.70 -121.94 2024-11-06T03:05:07Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.74 0.18 0.74 0.00 2024-11-06T03:05:07Z- handleDepth()- Depth_Keller msg: 1730862307.68 epoch sec; seqNo:2347; depth 50.48 2024-11-06T03:05:07Z- handleDVL() - DVL msg epochMillisec: 1730862307.71 2024-11-06T03:05:07Z- handleDvl() - RDI_Pathfinder msg: 1730862307.71 sec; seqNo:754; #beams:4 2024-11-06T03:05:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.75 , 32.79 , 37.04 , 34.49 2024-11-06T03:05:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.01 2024-11-06T03:05:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862307.73 epoch sec; seqNo:2347; 4061697.36 north; 594558.64 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:05:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862307.29 2024-11-06T03:05:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:07Z- waiting for 1730862311.29, time is 1730862307.73 2024-11-06T03:05:08Z- AHRS_M2 msg: 1730862308.06 epoch sec; seqNo:2198 2024-11-06T03:05:08Z- AHRS_M2 msg: 0.16 phi; 0.06 theta; 1.51 psi 2024-11-06T03:05:08Z- handleDepth()- Depth_Keller msg: 1730862308.07 epoch sec; seqNo:2348; depth 50.47 2024-11-06T03:05:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862308.10 epoch sec; seqNo:2348; 4061697.36 north; 594558.64 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:05:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862307.29 2024-11-06T03:05:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:08Z- waiting for 1730862311.29, time is 1730862308.10 2024-11-06T03:05:08Z- AHRS_M2 msg: 1730862308.47 epoch sec; seqNo:2199 2024-11-06T03:05:08Z- AHRS_M2 msg: 0.16 phi; 0.07 theta; 1.51 psi 2024-11-06T03:05:08Z- handleDepth()- Depth_Keller msg: 1730862308.48 epoch sec; seqNo:2349; depth 50.45 2024-11-06T03:05:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862308.49 epoch sec; seqNo:2349; 4061697.36 north; 594558.64 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:05:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862307.29 2024-11-06T03:05:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:08Z- waiting for 1730862311.29, time is 1730862308.49 2024-11-06T03:05:08Z- AHRS_M2 msg: 1730862308.88 epoch sec; seqNo:2200 2024-11-06T03:05:08Z- AHRS_M2 msg: 0.15 phi; 0.07 theta; 1.51 psi 2024-11-06T03:05:08Z- handleDepth()- Depth_Keller msg: 1730862308.88 epoch sec; seqNo:2350; depth 50.44 2024-11-06T03:05:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862308.89 epoch sec; seqNo:2350; 4061697.38 north; 594559.86 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:05:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862307.29 2024-11-06T03:05:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:09Z- waiting for 1730862311.29, time is 1730862308.89 2024-11-06T03:05:09Z- AHRS_M2 msg: 1730862309.27 epoch sec; seqNo:2201 2024-11-06T03:05:09Z- AHRS_M2 msg: 0.15 phi; 0.07 theta; 1.51 psi 2024-11-06T03:05:09Z- handleDepth()- Depth_Keller msg: 1730862309.29 epoch sec; seqNo:2351; depth 50.42 2024-11-06T03:05:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862309.30 epoch sec; seqNo:2351; 4061697.38 north; 594559.86 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:05:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862307.29 2024-11-06T03:05:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:09Z- waiting for 1730862311.29, time is 1730862309.30 2024-11-06T03:05:09Z- AHRS_M2 msg: 1730862309.68 epoch sec; seqNo:2202 2024-11-06T03:05:09Z- AHRS_M2 msg: 0.15 phi; 0.07 theta; 1.51 psi 2024-11-06T03:05:09Z- handleDepth()- Depth_Keller msg: 1730862309.69 epoch sec; seqNo:2352; depth 50.40 2024-11-06T03:05:09Z- handleDVL() - DVL msg epochMillisec: 1730862309.70 2024-11-06T03:05:09Z- handleDvl() - RDI_Pathfinder msg: 1730862309.70 sec; seqNo:755; #beams:4 2024-11-06T03:05:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.75 , 32.96 , 37.38 , 34.66 2024-11-06T03:05:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.01 2024-11-06T03:05:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862309.74 epoch sec; seqNo:2352; 4061697.38 north; 594559.86 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:05:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862307.29 2024-11-06T03:05:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:09Z- waiting for 1730862311.29, time is 1730862309.74 2024-11-06T03:05:10Z- AHRS_M2 msg: 1730862310.09 epoch sec; seqNo:2203 2024-11-06T03:05:10Z- AHRS_M2 msg: 0.14 phi; 0.07 theta; 1.51 psi 2024-11-06T03:05:10Z- handleDepth()- Depth_Keller msg: 1730862310.10 epoch sec; seqNo:2353; depth 50.39 2024-11-06T03:05:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862310.11 epoch sec; seqNo:2353; 4061697.38 north; 594559.86 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:05:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862307.29 2024-11-06T03:05:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:10Z- waiting for 1730862311.29, time is 1730862310.11 2024-11-06T03:05:10Z- AHRS_M2 msg: 1730862310.49 epoch sec; seqNo:2204 2024-11-06T03:05:10Z- AHRS_M2 msg: 0.14 phi; 0.07 theta; 1.51 psi 2024-11-06T03:05:10Z- handleDepth()- Depth_Keller msg: 1730862310.50 epoch sec; seqNo:2354; depth 50.36 2024-11-06T03:05:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862310.51 epoch sec; seqNo:2354; 4061697.39 north; 594561.08 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:05:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862307.29 2024-11-06T03:05:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:10Z- waiting for 1730862311.29, time is 1730862310.51 2024-11-06T03:05:10Z- AHRS_M2 msg: 1730862310.89 epoch sec; seqNo:2205 2024-11-06T03:05:10Z- AHRS_M2 msg: 0.14 phi; 0.07 theta; 1.51 psi 2024-11-06T03:05:10Z- handleDepth()- Depth_Keller msg: 1730862310.90 epoch sec; seqNo:2355; depth 50.35 2024-11-06T03:05:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862310.91 epoch sec; seqNo:2355; 4061697.39 north; 594561.08 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:05:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862307.29 2024-11-06T03:05:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:11Z- waiting for 1730862311.29, time is 1730862310.91 2024-11-06T03:05:11Z- AHRS_M2 msg: 1730862311.31 epoch sec; seqNo:2206 2024-11-06T03:05:11Z- AHRS_M2 msg: 0.14 phi; 0.07 theta; 1.51 psi 2024-11-06T03:05:11Z- handleDepth()- Depth_Keller msg: 1730862311.31 epoch sec; seqNo:2356; depth 50.32 2024-11-06T03:05:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862311.32 epoch sec; seqNo:2356; 4061697.77 north; 594561.08 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:05:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862307.29 2024-11-06T03:05:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:11Z- waiting for 1730862311.29, time is 1730862311.32 2024-11-06T03:05:11Z- LcmTrn::updateTrn() >>>> pose time 1730862311.32, meas time 1730862309.70 <<<< 2024-11-06T03:05:11Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862311.32 2024-11-06T03:05:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862309.70 sec, ping # 0; waiting for INS data more recent than 1730862307.29... 2024-11-06T03:05:11Z- TerrainNav::motionUpdate() - currEstimate = (4061698, 594561.08, 50.32) 2024-11-06T03:05:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:05:11Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:05:11Z- TNavPF::Projecting Measurements 2024-11-06T03:05:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:05:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:05:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:05:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:05:11Z- TNavFilter::4 good beams. 2024-11-06T03:05:11Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:05:11Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:05:11Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:05:11Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:05:11Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:05:11Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:05:11Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:05:11Z- TerrainMapOctree: (vehN, vehE ) = (4061697.60, 594559.61). 2024-11-06T03:05:11Z- TerrainMapOctree: Min Distance = 197.47. 2024-11-06T03:05:11Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:05:11Z- Forcing Subcloud Comparison 2024-11-06T03:05:11Z- Weighting particles with subcloud comparison 2024-11-06T03:05:11Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:11Z- meanExpectedMeasDiff: 0.325160 delta_rms_squared: 1.594498 alpha: 0.987612 2024-11-06T03:05:11Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:11Z- meanExpectedMeasDiff: 0.858387 delta_rms_squared: 1.126013 alpha: 0.982548 2024-11-06T03:05:11Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:11Z- meanExpectedMeasDiff: -0.116996 delta_rms_squared: 1.440736 alpha: 0.986308 2024-11-06T03:05:11Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:11Z- meanExpectedMeasDiff: -0.027609 delta_rms_squared: 1.231609 alpha: 0.984021 2024-11-06T03:05:11Z- Subcloud NIS: 0.314274 2024-11-06T03:05:11Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:05:11Z- TNavPF:: sumWeights = 0.584716 2024-11-06T03:05:11Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:05:11Z- TNavPF::First Term of Map Covariance Matrix is 0.40 2024-11-06T03:05:11Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T03:05:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862309.70 sec. 2024-11-06T03:05:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:05:11Z- variances([0], [2], and psi_berg) = 0.738978 0.180844 0.000000 2024-11-06T03:05:11Z- publishEstimatesDecoder() - at 1730862311.32 sec 2024-11-06T03:05:11Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:05:11Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061729.70 594543.07 57.53 2024-11-06T03:05:11Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061730.12 594544.59 57.68 lat/lon: 36.70 -121.94 2024-11-06T03:05:11Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.74 0.18 0.66 0.00 2024-11-06T03:05:11Z- AHRS_M2 msg: 1730862311.70 epoch sec; seqNo:2207 2024-11-06T03:05:11Z- AHRS_M2 msg: 0.14 phi; 0.07 theta; 1.52 psi 2024-11-06T03:05:11Z- handleDepth()- Depth_Keller msg: 1730862311.72 epoch sec; seqNo:2357; depth 50.31 2024-11-06T03:05:11Z- handleDVL() - DVL msg epochMillisec: 1730862311.75 2024-11-06T03:05:11Z- handleDvl() - RDI_Pathfinder msg: 1730862311.75 sec; seqNo:756; #beams:4 2024-11-06T03:05:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.24 , 33.30 , 37.21 , 34.66 2024-11-06T03:05:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , -0.02 2024-11-06T03:05:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862311.78 epoch sec; seqNo:2357; 4061697.78 north; 594562.30 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:05:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862311.32 2024-11-06T03:05:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:11Z- waiting for 1730862315.32, time is 1730862311.78 2024-11-06T03:05:12Z- handleDepth()- Depth_Keller msg: 1730862312.13 epoch sec; seqNo:2358; depth 50.30 2024-11-06T03:05:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862312.14 epoch sec; seqNo:2358; 4061697.78 north; 594562.30 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:05:12Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:05:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862311.32 2024-11-06T03:05:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:12Z- waiting for 1730862315.32, time is 1730862312.14 2024-11-06T03:05:12Z- AHRS_M2 msg: 1730862312.51 epoch sec; seqNo:2208 2024-11-06T03:05:12Z- AHRS_M2 msg: 0.15 phi; 0.07 theta; 1.52 psi 2024-11-06T03:05:12Z- handleDepth()- Depth_Keller msg: 1730862312.52 epoch sec; seqNo:2359; depth 50.27 2024-11-06T03:05:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862312.53 epoch sec; seqNo:2359; 4061697.78 north; 594562.30 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:05:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862311.32 2024-11-06T03:05:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:12Z- waiting for 1730862315.32, time is 1730862312.53 2024-11-06T03:05:12Z- AHRS_M2 msg: 1730862312.91 epoch sec; seqNo:2209 2024-11-06T03:05:12Z- AHRS_M2 msg: 0.15 phi; 0.07 theta; 1.52 psi 2024-11-06T03:05:12Z- handleDepth()- Depth_Keller msg: 1730862312.92 epoch sec; seqNo:2360; depth 50.24 2024-11-06T03:05:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862312.93 epoch sec; seqNo:2360; 4061697.78 north; 594562.30 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:05:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862311.32 2024-11-06T03:05:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:13Z- waiting for 1730862315.32, time is 1730862312.93 2024-11-06T03:05:13Z- AHRS_M2 msg: 1730862313.32 epoch sec; seqNo:2210 2024-11-06T03:05:13Z- AHRS_M2 msg: 0.15 phi; 0.07 theta; 1.52 psi 2024-11-06T03:05:13Z- handleDepth()- Depth_Keller msg: 1730862313.32 epoch sec; seqNo:2361; depth 50.20 2024-11-06T03:05:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862313.33 epoch sec; seqNo:2361; 4061697.80 north; 594563.52 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:05:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862311.32 2024-11-06T03:05:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:13Z- waiting for 1730862315.32, time is 1730862313.33 2024-11-06T03:05:13Z- AHRS_M2 msg: 1730862313.73 epoch sec; seqNo:2211 2024-11-06T03:05:13Z- AHRS_M2 msg: 0.15 phi; 0.07 theta; 1.52 psi 2024-11-06T03:05:13Z- handleDepth()- Depth_Keller msg: 1730862313.73 epoch sec; seqNo:2362; depth 50.18 2024-11-06T03:05:13Z- handleDVL() - DVL msg epochMillisec: 1730862313.75 2024-11-06T03:05:13Z- handleDvl() - RDI_Pathfinder msg: 1730862313.75 sec; seqNo:757; #beams:4 2024-11-06T03:05:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.24 , 33.30 , 37.54 , 34.66 2024-11-06T03:05:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , -0.01 2024-11-06T03:05:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862313.79 epoch sec; seqNo:2362; 4061697.80 north; 594563.52 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:05:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862311.32 2024-11-06T03:05:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:13Z- waiting for 1730862315.32, time is 1730862313.79 2024-11-06T03:05:14Z- AHRS_M2 msg: 1730862314.15 epoch sec; seqNo:2212 2024-11-06T03:05:14Z- AHRS_M2 msg: 0.15 phi; 0.07 theta; 1.52 psi 2024-11-06T03:05:14Z- handleDepth()- Depth_Keller msg: 1730862314.16 epoch sec; seqNo:2363; depth 50.18 2024-11-06T03:05:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862314.19 epoch sec; seqNo:2363; 4061697.80 north; 594563.52 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:05:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862311.32 2024-11-06T03:05:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:14Z- waiting for 1730862315.32, time is 1730862314.19 2024-11-06T03:05:14Z- AHRS_M2 msg: 1730862314.55 epoch sec; seqNo:2213 2024-11-06T03:05:14Z- AHRS_M2 msg: 0.15 phi; 0.07 theta; 1.52 psi 2024-11-06T03:05:14Z- handleDepth()- Depth_Keller msg: 1730862314.56 epoch sec; seqNo:2364; depth 50.15 2024-11-06T03:05:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862314.57 epoch sec; seqNo:2364; 4061697.80 north; 594563.52 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:05:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862311.32 2024-11-06T03:05:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:14Z- waiting for 1730862315.32, time is 1730862314.57 2024-11-06T03:05:14Z- AHRS_M2 msg: 1730862314.96 epoch sec; seqNo:2214 2024-11-06T03:05:14Z- AHRS_M2 msg: 0.15 phi; 0.07 theta; 1.53 psi 2024-11-06T03:05:14Z- handleDepth()- Depth_Keller msg: 1730862314.96 epoch sec; seqNo:2365; depth 50.11 2024-11-06T03:05:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862314.97 epoch sec; seqNo:2365; 4061697.81 north; 594564.74 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:05:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862311.32 2024-11-06T03:05:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:15Z- waiting for 1730862315.32, time is 1730862314.97 2024-11-06T03:05:15Z- AHRS_M2 msg: 1730862315.36 epoch sec; seqNo:2215 2024-11-06T03:05:15Z- AHRS_M2 msg: 0.16 phi; 0.06 theta; 1.53 psi 2024-11-06T03:05:15Z- handleDepth()- Depth_Keller msg: 1730862315.39 epoch sec; seqNo:2366; depth 50.10 2024-11-06T03:05:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862315.40 epoch sec; seqNo:2366; 4061697.81 north; 594564.74 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:05:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862311.32 2024-11-06T03:05:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:15Z- waiting for 1730862315.32, time is 1730862315.40 2024-11-06T03:05:15Z- LcmTrn::updateTrn() >>>> pose time 1730862315.40, meas time 1730862313.75 <<<< 2024-11-06T03:05:15Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862315.40 2024-11-06T03:05:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862313.75 sec, ping # 0; waiting for INS data more recent than 1730862311.32... 2024-11-06T03:05:15Z- TerrainNav::motionUpdate() - currEstimate = (4061698, 594564.74, 50.10) 2024-11-06T03:05:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:05:15Z- TerrainNav:: SubcloudNIS is 0.31, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:05:15Z- TNavPF::Projecting Measurements 2024-11-06T03:05:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:05:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:05:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:05:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:05:15Z- TNavFilter::4 good beams. 2024-11-06T03:05:15Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:05:15Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:05:15Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:05:15Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:05:15Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:05:15Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:05:15Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:05:15Z- TerrainMapOctree: (vehN, vehE ) = (4061697.79, 594563.26). 2024-11-06T03:05:15Z- TerrainMapOctree: Min Distance = 193.90. 2024-11-06T03:05:15Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:05:15Z- Forcing Subcloud Comparison 2024-11-06T03:05:15Z- Weighting particles with subcloud comparison 2024-11-06T03:05:15Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:15Z- meanExpectedMeasDiff: -0.674917 delta_rms_squared: 1.500269 alpha: 0.986844 2024-11-06T03:05:15Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:15Z- meanExpectedMeasDiff: 0.885944 delta_rms_squared: 1.044898 alpha: 0.981219 2024-11-06T03:05:15Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:15Z- meanExpectedMeasDiff: -0.355521 delta_rms_squared: 1.351405 alpha: 0.985416 2024-11-06T03:05:15Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:15Z- meanExpectedMeasDiff: -0.428778 delta_rms_squared: 1.156700 alpha: 0.983003 2024-11-06T03:05:15Z- Subcloud NIS: 0.292547 2024-11-06T03:05:15Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:05:15Z- TNavPF:: sumWeights = 0.540647 2024-11-06T03:05:15Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:05:15Z- TNavPF::First Term of Map Covariance Matrix is 0.40 2024-11-06T03:05:15Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T03:05:15Z- TerrainNav::Resampling particle filter... 2024-11-06T03:05:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862313.75 sec. 2024-11-06T03:05:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:05:15Z- variances([0], [2], and psi_berg) = 0.730088 0.180749 0.000000 2024-11-06T03:05:15Z- publishEstimatesDecoder() - at 1730862315.40 sec 2024-11-06T03:05:15Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:05:15Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061730.35 594548.58 57.67 2024-11-06T03:05:15Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061730.17 594548.24 57.53 lat/lon: 36.70 -121.94 2024-11-06T03:05:15Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.73 0.18 0.63 0.00 2024-11-06T03:05:15Z- AHRS_M2 msg: 1730862315.77 epoch sec; seqNo:2216 2024-11-06T03:05:15Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.53 psi 2024-11-06T03:05:15Z- handleDepth()- Depth_Keller msg: 1730862315.78 epoch sec; seqNo:2367; depth 50.06 2024-11-06T03:05:15Z- handleDVL() - DVL msg epochMillisec: 1730862315.84 2024-11-06T03:05:15Z- handleDvl() - RDI_Pathfinder msg: 1730862315.84 sec; seqNo:758; #beams:4 2024-11-06T03:05:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.92 , 33.30 , 37.38 , 34.83 2024-11-06T03:05:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , -0.00 2024-11-06T03:05:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862315.85 epoch sec; seqNo:2367; 4061697.81 north; 594564.74 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:05:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862315.40 2024-11-06T03:05:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:15Z- waiting for 1730862319.40, time is 1730862315.85 2024-11-06T03:05:16Z- AHRS_M2 msg: 1730862316.17 epoch sec; seqNo:2217 2024-11-06T03:05:16Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.53 psi 2024-11-06T03:05:16Z- handleDepth()- Depth_Keller msg: 1730862316.18 epoch sec; seqNo:2368; depth 50.04 2024-11-06T03:05:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862316.20 epoch sec; seqNo:2368; 4061697.82 north; 594565.96 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:05:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862315.40 2024-11-06T03:05:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:16Z- waiting for 1730862319.40, time is 1730862316.20 2024-11-06T03:05:16Z- AHRS_M2 msg: 1730862316.57 epoch sec; seqNo:2218 2024-11-06T03:05:16Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.53 psi 2024-11-06T03:05:16Z- handleDepth()- Depth_Keller msg: 1730862316.59 epoch sec; seqNo:2369; depth 50.02 2024-11-06T03:05:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862316.60 epoch sec; seqNo:2369; 4061697.82 north; 594565.96 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:05:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862315.40 2024-11-06T03:05:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:16Z- waiting for 1730862319.40, time is 1730862316.60 2024-11-06T03:05:16Z- AHRS_M2 msg: 1730862316.98 epoch sec; seqNo:2219 2024-11-06T03:05:16Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.53 psi 2024-11-06T03:05:16Z- handleDepth()- Depth_Keller msg: 1730862316.98 epoch sec; seqNo:2370; depth 50.00 2024-11-06T03:05:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862316.99 epoch sec; seqNo:2370; 4061697.82 north; 594565.96 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:05:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862315.40 2024-11-06T03:05:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:17Z- waiting for 1730862319.40, time is 1730862316.99 2024-11-06T03:05:17Z- AHRS_M2 msg: 1730862317.39 epoch sec; seqNo:2220 2024-11-06T03:05:17Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.53 psi 2024-11-06T03:05:17Z- handleDepth()- Depth_Keller msg: 1730862317.39 epoch sec; seqNo:2371; depth 49.97 2024-11-06T03:05:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862317.41 epoch sec; seqNo:2371; 4061697.82 north; 594565.96 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:05:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862315.40 2024-11-06T03:05:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:17Z- waiting for 1730862319.40, time is 1730862317.41 2024-11-06T03:05:17Z- AHRS_M2 msg: 1730862317.79 epoch sec; seqNo:2221 2024-11-06T03:05:17Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 1.53 psi 2024-11-06T03:05:17Z- handleDepth()- Depth_Keller msg: 1730862317.80 epoch sec; seqNo:2372; depth 49.94 2024-11-06T03:05:17Z- handleDVL() - DVL msg epochMillisec: 1730862317.82 2024-11-06T03:05:17Z- handleDvl() - RDI_Pathfinder msg: 1730862317.82 sec; seqNo:759; #beams:4 2024-11-06T03:05:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.92 , 33.30 , 37.21 , 35.17 2024-11-06T03:05:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , -0.03 2024-11-06T03:05:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862317.84 epoch sec; seqNo:2372; 4061697.84 north; 594567.18 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:05:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862315.40 2024-11-06T03:05:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:17Z- waiting for 1730862319.40, time is 1730862317.84 2024-11-06T03:05:18Z- AHRS_M2 msg: 1730862318.19 epoch sec; seqNo:2222 2024-11-06T03:05:18Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 1.53 psi 2024-11-06T03:05:18Z- handleDepth()- Depth_Keller msg: 1730862318.19 epoch sec; seqNo:2373; depth 49.92 2024-11-06T03:05:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862318.21 epoch sec; seqNo:2373; 4061697.84 north; 594567.18 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:05:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862315.40 2024-11-06T03:05:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:18Z- waiting for 1730862319.40, time is 1730862318.21 2024-11-06T03:05:18Z- AHRS_M2 msg: 1730862318.60 epoch sec; seqNo:2223 2024-11-06T03:05:18Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 1.53 psi 2024-11-06T03:05:18Z- handleDepth()- Depth_Keller msg: 1730862318.60 epoch sec; seqNo:2374; depth 49.91 2024-11-06T03:05:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862318.61 epoch sec; seqNo:2374; 4061697.84 north; 594567.18 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:05:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862315.40 2024-11-06T03:05:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:18Z- waiting for 1730862319.40, time is 1730862318.61 2024-11-06T03:05:19Z- AHRS_M2 msg: 1730862319.00 epoch sec; seqNo:2224 2024-11-06T03:05:19Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 1.53 psi 2024-11-06T03:05:19Z- handleDepth()- Depth_Keller msg: 1730862319.00 epoch sec; seqNo:2375; depth 49.88 2024-11-06T03:05:19Z- handleDVL() - DVL msg epochMillisec: 1730862319.01 2024-11-06T03:05:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:19Z- handleDvl() - RDI_Pathfinder msg: 1730862319.01 sec; seqNo:760; #beams:0 2024-11-06T03:05:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , -0.03 2024-11-06T03:05:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862319.02 epoch sec; seqNo:2375; 4061698.23 north; 594568.40 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:05:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862315.40 2024-11-06T03:05:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:19Z- waiting for 1730862319.40, time is 1730862319.02 2024-11-06T03:05:19Z- AHRS_M2 msg: 1730862319.40 epoch sec; seqNo:2225 2024-11-06T03:05:19Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 1.53 psi 2024-11-06T03:05:19Z- handleDepth()- Depth_Keller msg: 1730862319.41 epoch sec; seqNo:2376; depth 49.85 2024-11-06T03:05:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862319.42 epoch sec; seqNo:2376; 4061698.23 north; 594568.40 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:05:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862315.40 2024-11-06T03:05:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:19Z- waiting for 1730862319.40, time is 1730862319.42 2024-11-06T03:05:19Z- LcmTrn::updateTrn() >>>> pose time 1730862319.42, meas time 1730862319.01 <<<< 2024-11-06T03:05:19Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862319.42 2024-11-06T03:05:19Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:05:19Z- TerrainNav::motionUpdate() - currEstimate = (4061698, 594568.40, 49.85) 2024-11-06T03:05:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:05:19Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:05:19Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:05:19Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:05:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:05:19Z- variances([0], [2], and psi_berg) = 0.732864 0.182238 0.000000 2024-11-06T03:05:19Z- publishEstimatesDecoder() - at 1730862319.42 sec 2024-11-06T03:05:19Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:05:19Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061730.78 594552.24 58.57 2024-11-06T03:05:19Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061730.59 594551.90 57.28 lat/lon: 36.70 -121.94 2024-11-06T03:05:19Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.73 0.18 0.88 0.00 2024-11-06T03:05:19Z- AHRS_M2 msg: 1730862319.81 epoch sec; seqNo:2226 2024-11-06T03:05:19Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 1.53 psi 2024-11-06T03:05:19Z- handleDepth()- Depth_Keller msg: 1730862319.82 epoch sec; seqNo:2377; depth 49.83 2024-11-06T03:05:19Z- handleDVL() - DVL msg epochMillisec: 1730862319.84 2024-11-06T03:05:19Z- handleDvl() - RDI_Pathfinder msg: 1730862319.84 sec; seqNo:761; #beams:4 2024-11-06T03:05:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.41 , 33.47 , 37.38 , 35.00 2024-11-06T03:05:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.06 2024-11-06T03:05:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862319.85 epoch sec; seqNo:2377; 4061698.23 north; 594568.40 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:05:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862319.42 2024-11-06T03:05:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:19Z- waiting for 1730862323.42, time is 1730862319.85 2024-11-06T03:05:20Z- AHRS_M2 msg: 1730862320.21 epoch sec; seqNo:2227 2024-11-06T03:05:20Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 1.53 psi 2024-11-06T03:05:20Z- handleDepth()- Depth_Keller msg: 1730862320.23 epoch sec; seqNo:2378; depth 49.82 2024-11-06T03:05:20Z- handleDVL() - DVL msg epochMillisec: 1730862320.23 2024-11-06T03:05:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:20Z- handleDvl() - RDI_Pathfinder msg: 1730862320.23 sec; seqNo:762; #beams:0 2024-11-06T03:05:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.06 2024-11-06T03:05:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862320.25 epoch sec; seqNo:2378; 4061698.23 north; 594568.40 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:05:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862319.42 2024-11-06T03:05:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:20Z- waiting for 1730862323.42, time is 1730862320.25 2024-11-06T03:05:20Z- AHRS_M2 msg: 1730862320.62 epoch sec; seqNo:2228 2024-11-06T03:05:20Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 1.53 psi 2024-11-06T03:05:20Z- handleDepth()- Depth_Keller msg: 1730862320.62 epoch sec; seqNo:2379; depth 49.80 2024-11-06T03:05:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862320.64 epoch sec; seqNo:2379; 4061698.24 north; 594569.62 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:05:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862319.42 2024-11-06T03:05:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:20Z- waiting for 1730862323.42, time is 1730862320.64 2024-11-06T03:05:21Z- AHRS_M2 msg: 1730862321.03 epoch sec; seqNo:2229 2024-11-06T03:05:21Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 1.52 psi 2024-11-06T03:05:21Z- handleDepth()- Depth_Keller msg: 1730862321.03 epoch sec; seqNo:2380; depth 49.79 2024-11-06T03:05:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862321.04 epoch sec; seqNo:2380; 4061698.24 north; 594569.62 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:05:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862319.42 2024-11-06T03:05:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:21Z- waiting for 1730862323.42, time is 1730862321.04 2024-11-06T03:05:21Z- AHRS_M2 msg: 1730862321.42 epoch sec; seqNo:2230 2024-11-06T03:05:21Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 1.52 psi 2024-11-06T03:05:21Z- handleDepth()- Depth_Keller msg: 1730862321.45 epoch sec; seqNo:2381; depth 49.76 2024-11-06T03:05:21Z- handleDVL() - DVL msg epochMillisec: 1730862321.45 2024-11-06T03:05:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:21Z- handleDvl() - RDI_Pathfinder msg: 1730862321.45 sec; seqNo:763; #beams:0 2024-11-06T03:05:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.06 2024-11-06T03:05:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862321.46 epoch sec; seqNo:2381; 4061698.24 north; 594569.62 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:05:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862319.42 2024-11-06T03:05:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:21Z- waiting for 1730862323.42, time is 1730862321.46 2024-11-06T03:05:21Z- AHRS_M2 msg: 1730862321.83 epoch sec; seqNo:2231 2024-11-06T03:05:21Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 1.52 psi 2024-11-06T03:05:21Z- handleDepth()- Depth_Keller msg: 1730862321.83 epoch sec; seqNo:2382; depth 49.77 2024-11-06T03:05:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862321.85 epoch sec; seqNo:2382; 4061698.26 north; 594570.84 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:05:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862319.42 2024-11-06T03:05:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:21Z- waiting for 1730862323.42, time is 1730862321.85 2024-11-06T03:05:22Z- AHRS_M2 msg: 1730862322.23 epoch sec; seqNo:2232 2024-11-06T03:05:22Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.52 psi 2024-11-06T03:05:22Z- handleDepth()- Depth_Keller msg: 1730862322.24 epoch sec; seqNo:2383; depth 49.75 2024-11-06T03:05:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862322.24 epoch sec; seqNo:2383; 4061698.26 north; 594570.84 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:05:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862319.42 2024-11-06T03:05:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:22Z- waiting for 1730862323.42, time is 1730862322.24 2024-11-06T03:05:22Z- AHRS_M2 msg: 1730862322.64 epoch sec; seqNo:2233 2024-11-06T03:05:22Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.52 psi 2024-11-06T03:05:22Z- handleDepth()- Depth_Keller msg: 1730862322.65 epoch sec; seqNo:2384; depth 49.74 2024-11-06T03:05:22Z- handleDVL() - DVL msg epochMillisec: 1730862322.66 2024-11-06T03:05:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:22Z- handleDvl() - RDI_Pathfinder msg: 1730862322.66 sec; seqNo:764; #beams:0 2024-11-06T03:05:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.06 2024-11-06T03:05:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862322.66 epoch sec; seqNo:2384; 4061698.26 north; 594570.84 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:05:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862319.42 2024-11-06T03:05:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:22Z- waiting for 1730862323.42, time is 1730862322.66 2024-11-06T03:05:23Z- AHRS_M2 msg: 1730862323.04 epoch sec; seqNo:2234 2024-11-06T03:05:23Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.52 psi 2024-11-06T03:05:23Z- handleDepth()- Depth_Keller msg: 1730862323.05 epoch sec; seqNo:2385; depth 49.73 2024-11-06T03:05:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862323.08 epoch sec; seqNo:2385; 4061698.26 north; 594570.84 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:05:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862319.42 2024-11-06T03:05:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:23Z- waiting for 1730862323.42, time is 1730862323.08 2024-11-06T03:05:23Z- AHRS_M2 msg: 1730862323.44 epoch sec; seqNo:2235 2024-11-06T03:05:23Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.52 psi 2024-11-06T03:05:23Z- handleDepth()- Depth_Keller msg: 1730862323.45 epoch sec; seqNo:2386; depth 49.72 2024-11-06T03:05:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862323.46 epoch sec; seqNo:2386; 4061698.65 north; 594572.05 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:05:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862319.42 2024-11-06T03:05:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:23Z- waiting for 1730862323.42, time is 1730862323.46 2024-11-06T03:05:23Z- LcmTrn::updateTrn() >>>> pose time 1730862323.46, meas time 1730862322.66 <<<< 2024-11-06T03:05:23Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862323.46 2024-11-06T03:05:23Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:05:23Z- TerrainNav::motionUpdate() - currEstimate = (4061699, 594572.05, 49.72) 2024-11-06T03:05:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:05:23Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:05:23Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:05:23Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:05:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:05:23Z- variances([0], [2], and psi_berg) = 0.734955 0.184922 0.000000 2024-11-06T03:05:23Z- publishEstimatesDecoder() - at 1730862323.46 sec 2024-11-06T03:05:23Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:05:23Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061731.22 594555.89 58.77 2024-11-06T03:05:23Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061731.01 594555.56 57.15 lat/lon: 36.70 -121.94 2024-11-06T03:05:23Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.73 0.18 1.15 0.00 2024-11-06T03:05:23Z- AHRS_M2 msg: 1730862323.85 epoch sec; seqNo:2236 2024-11-06T03:05:23Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:23Z- handleDepth()- Depth_Keller msg: 1730862323.87 epoch sec; seqNo:2387; depth 49.72 2024-11-06T03:05:23Z- handleDVL() - DVL msg epochMillisec: 1730862323.89 2024-11-06T03:05:23Z- handleDvl() - RDI_Pathfinder msg: 1730862323.89 sec; seqNo:765; #beams:4 2024-11-06T03:05:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.41 , 33.64 , 37.04 , 35.34 2024-11-06T03:05:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , -0.00 2024-11-06T03:05:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862323.90 epoch sec; seqNo:2387; 4061698.65 north; 594572.05 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:05:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862323.46 2024-11-06T03:05:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:24Z- waiting for 1730862327.46, time is 1730862323.90 2024-11-06T03:05:24Z- AHRS_M2 msg: 1730862324.26 epoch sec; seqNo:2237 2024-11-06T03:05:24Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:24Z- handleDepth()- Depth_Keller msg: 1730862324.26 epoch sec; seqNo:2388; depth 49.71 2024-11-06T03:05:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862324.28 epoch sec; seqNo:2388; 4061698.65 north; 594572.05 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:05:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862323.46 2024-11-06T03:05:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:24Z- waiting for 1730862327.46, time is 1730862324.28 2024-11-06T03:05:24Z- AHRS_M2 msg: 1730862324.68 epoch sec; seqNo:2238 2024-11-06T03:05:24Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:24Z- handleDepth()- Depth_Keller msg: 1730862324.68 epoch sec; seqNo:2389; depth 49.69 2024-11-06T03:05:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862324.69 epoch sec; seqNo:2389; 4061698.65 north; 594572.05 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:05:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862323.46 2024-11-06T03:05:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:24Z- waiting for 1730862327.46, time is 1730862324.69 2024-11-06T03:05:25Z- AHRS_M2 msg: 1730862325.06 epoch sec; seqNo:2239 2024-11-06T03:05:25Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:25Z- handleDepth()- Depth_Keller msg: 1730862325.06 epoch sec; seqNo:2390; depth 49.69 2024-11-06T03:05:25Z- handleDVL() - DVL msg epochMillisec: 1730862325.06 2024-11-06T03:05:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:25Z- handleDvl() - RDI_Pathfinder msg: 1730862325.06 sec; seqNo:766; #beams:0 2024-11-06T03:05:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , -0.00 2024-11-06T03:05:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862325.09 epoch sec; seqNo:2390; 4061698.66 north; 594573.27 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:05:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862323.46 2024-11-06T03:05:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:25Z- waiting for 1730862327.46, time is 1730862325.09 2024-11-06T03:05:25Z- AHRS_M2 msg: 1730862325.46 epoch sec; seqNo:2240 2024-11-06T03:05:25Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:25Z- handleDepth()- Depth_Keller msg: 1730862325.47 epoch sec; seqNo:2391; depth 49.68 2024-11-06T03:05:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862325.48 epoch sec; seqNo:2391; 4061698.66 north; 594573.27 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:05:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862323.46 2024-11-06T03:05:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:25Z- waiting for 1730862327.46, time is 1730862325.48 2024-11-06T03:05:25Z- AHRS_M2 msg: 1730862325.87 epoch sec; seqNo:2241 2024-11-06T03:05:25Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:25Z- handleDepth()- Depth_Keller msg: 1730862325.88 epoch sec; seqNo:2392; depth 49.68 2024-11-06T03:05:25Z- handleDVL() - DVL msg epochMillisec: 1730862325.90 2024-11-06T03:05:25Z- handleDvl() - RDI_Pathfinder msg: 1730862325.90 sec; seqNo:767; #beams:4 2024-11-06T03:05:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.75 , 33.47 , 37.04 , 35.51 2024-11-06T03:05:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.03 2024-11-06T03:05:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862325.92 epoch sec; seqNo:2392; 4061698.66 north; 594573.27 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:05:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862323.46 2024-11-06T03:05:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:26Z- waiting for 1730862327.46, time is 1730862325.92 2024-11-06T03:05:26Z- AHRS_M2 msg: 1730862326.27 epoch sec; seqNo:2242 2024-11-06T03:05:26Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:26Z- handleDepth()- Depth_Keller msg: 1730862326.28 epoch sec; seqNo:2393; depth 49.68 2024-11-06T03:05:26Z- handleDVL() - DVL msg epochMillisec: 1730862326.28 2024-11-06T03:05:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:26Z- handleDvl() - RDI_Pathfinder msg: 1730862326.28 sec; seqNo:768; #beams:0 2024-11-06T03:05:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.03 2024-11-06T03:05:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862326.30 epoch sec; seqNo:2393; 4061698.68 north; 594574.49 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:05:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862323.46 2024-11-06T03:05:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:26Z- waiting for 1730862327.46, time is 1730862326.30 2024-11-06T03:05:26Z- AHRS_M2 msg: 1730862326.67 epoch sec; seqNo:2243 2024-11-06T03:05:26Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:26Z- handleDepth()- Depth_Keller msg: 1730862326.68 epoch sec; seqNo:2394; depth 49.66 2024-11-06T03:05:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862326.69 epoch sec; seqNo:2394; 4061698.68 north; 594574.49 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:05:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862323.46 2024-11-06T03:05:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:26Z- waiting for 1730862327.46, time is 1730862326.69 2024-11-06T03:05:27Z- AHRS_M2 msg: 1730862327.09 epoch sec; seqNo:2244 2024-11-06T03:05:27Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:27Z- handleDepth()- Depth_Keller msg: 1730862327.10 epoch sec; seqNo:2395; depth 49.66 2024-11-06T03:05:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862327.12 epoch sec; seqNo:2395; 4061698.68 north; 594574.49 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:05:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862323.46 2024-11-06T03:05:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:27Z- waiting for 1730862327.46, time is 1730862327.12 2024-11-06T03:05:27Z- AHRS_M2 msg: 1730862327.48 epoch sec; seqNo:2245 2024-11-06T03:05:27Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:27Z- handleDepth()- Depth_Keller msg: 1730862327.50 epoch sec; seqNo:2396; depth 49.65 2024-11-06T03:05:27Z- handleDVL() - DVL msg epochMillisec: 1730862327.51 2024-11-06T03:05:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:27Z- handleDvl() - RDI_Pathfinder msg: 1730862327.51 sec; seqNo:769; #beams:0 2024-11-06T03:05:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.03 2024-11-06T03:05:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862327.59 epoch sec; seqNo:2396; 4061698.68 north; 594574.49 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:05:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862323.46 2024-11-06T03:05:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:27Z- waiting for 1730862327.46, time is 1730862327.59 2024-11-06T03:05:27Z- LcmTrn::updateTrn() >>>> pose time 1730862327.59, meas time 1730862327.51 <<<< 2024-11-06T03:05:27Z- LcmTrn::updateTrn() equating pose 1730862327.51 and meas times 1730862327.51 2024-11-06T03:05:27Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862327.51 2024-11-06T03:05:27Z- TerrainNav::motionUpdate() - currEstimate = (4061699, 594574.49, 49.65) 2024-11-06T03:05:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:05:27Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:05:27Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:05:27Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:05:27Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:05:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:05:27Z- variances([0], [2], and psi_berg) = 0.736042 0.186121 0.000000 2024-11-06T03:05:27Z- publishEstimatesDecoder() - at 1730862327.51 sec 2024-11-06T03:05:27Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:05:27Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061731.26 594558.34 58.60 2024-11-06T03:05:27Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061731.04 594558.00 57.09 lat/lon: 36.70 -121.94 2024-11-06T03:05:27Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.74 0.19 1.42 0.00 2024-11-06T03:05:27Z- AHRS_M2 msg: 1730862327.89 epoch sec; seqNo:2246 2024-11-06T03:05:27Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:27Z- handleDepth()- Depth_Keller msg: 1730862327.90 epoch sec; seqNo:2397; depth 49.62 2024-11-06T03:05:27Z- handleDVL() - DVL msg epochMillisec: 1730862327.92 2024-11-06T03:05:27Z- handleDvl() - RDI_Pathfinder msg: 1730862327.92 sec; seqNo:770; #beams:4 2024-11-06T03:05:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.92 , 33.81 , 37.04 , 35.85 2024-11-06T03:05:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.02 2024-11-06T03:05:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862327.93 epoch sec; seqNo:2397; 4061698.69 north; 594575.71 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:05:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862327.51 2024-11-06T03:05:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:28Z- waiting for 1730862331.51, time is 1730862327.93 2024-11-06T03:05:28Z- AHRS_M2 msg: 1730862328.29 epoch sec; seqNo:2247 2024-11-06T03:05:28Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:28Z- handleDepth()- Depth_Keller msg: 1730862328.30 epoch sec; seqNo:2398; depth 49.63 2024-11-06T03:05:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862328.32 epoch sec; seqNo:2398; 4061699.07 north; 594575.71 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:05:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862327.51 2024-11-06T03:05:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:28Z- waiting for 1730862331.51, time is 1730862328.32 2024-11-06T03:05:28Z- AHRS_M2 msg: 1730862328.69 epoch sec; seqNo:2248 2024-11-06T03:05:28Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:28Z- handleDepth()- Depth_Keller msg: 1730862328.71 epoch sec; seqNo:2399; depth 49.62 2024-11-06T03:05:28Z- handleDVL() - DVL msg epochMillisec: 1730862328.71 2024-11-06T03:05:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:28Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:28Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:28Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:28Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:28Z- handleDvl() - RDI_Pathfinder msg: 1730862328.71 sec; seqNo:771; #beams:0 2024-11-06T03:05:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.02 2024-11-06T03:05:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862328.72 epoch sec; seqNo:2399; 4061699.07 north; 594575.71 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:05:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862327.51 2024-11-06T03:05:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:28Z- waiting for 1730862331.51, time is 1730862328.72 2024-11-06T03:05:29Z- AHRS_M2 msg: 1730862329.10 epoch sec; seqNo:2249 2024-11-06T03:05:29Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:29Z- handleDepth()- Depth_Keller msg: 1730862329.11 epoch sec; seqNo:2400; depth 49.60 2024-11-06T03:05:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862329.12 epoch sec; seqNo:2400; 4061699.08 north; 594576.93 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:05:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862327.51 2024-11-06T03:05:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:29Z- waiting for 1730862331.51, time is 1730862329.12 2024-11-06T03:05:29Z- AHRS_M2 msg: 1730862329.54 epoch sec; seqNo:2250 2024-11-06T03:05:29Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.51 psi 2024-11-06T03:05:29Z- handleDepth()- Depth_Keller msg: 1730862329.56 epoch sec; seqNo:2401; depth 49.59 2024-11-06T03:05:29Z- handleDVL() - DVL msg epochMillisec: 1730862329.58 2024-11-06T03:05:29Z- handleDvl() - RDI_Pathfinder msg: 1730862329.58 sec; seqNo:772; #beams:4 2024-11-06T03:05:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.58 , 34.15 , 37.21 , 35.68 2024-11-06T03:05:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , -0.02 2024-11-06T03:05:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862329.59 epoch sec; seqNo:2401; 4061699.08 north; 594576.93 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:05:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862327.51 2024-11-06T03:05:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:29Z- waiting for 1730862331.51, time is 1730862329.59 2024-11-06T03:05:29Z- AHRS_M2 msg: 1730862329.91 epoch sec; seqNo:2251 2024-11-06T03:05:29Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T03:05:29Z- handleDepth()- Depth_Keller msg: 1730862329.92 epoch sec; seqNo:2402; depth 49.55 2024-11-06T03:05:29Z- handleDVL() - DVL msg epochMillisec: 1730862329.92 2024-11-06T03:05:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:29Z- handleDvl() - RDI_Pathfinder msg: 1730862329.92 sec; seqNo:773; #beams:0 2024-11-06T03:05:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , -0.02 2024-11-06T03:05:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862329.94 epoch sec; seqNo:2402; 4061699.08 north; 594576.93 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:05:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862327.51 2024-11-06T03:05:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:30Z- waiting for 1730862331.51, time is 1730862329.94 2024-11-06T03:05:30Z- AHRS_M2 msg: 1730862330.31 epoch sec; seqNo:2252 2024-11-06T03:05:30Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T03:05:30Z- handleDepth()- Depth_Keller msg: 1730862330.32 epoch sec; seqNo:2403; depth 49.56 2024-11-06T03:05:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862330.33 epoch sec; seqNo:2403; 4061699.08 north; 594576.93 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:05:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862327.51 2024-11-06T03:05:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:30Z- waiting for 1730862331.51, time is 1730862330.33 2024-11-06T03:05:30Z- AHRS_M2 msg: 1730862330.72 epoch sec; seqNo:2253 2024-11-06T03:05:30Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T03:05:30Z- handleDepth()- Depth_Keller msg: 1730862330.72 epoch sec; seqNo:2404; depth 49.53 2024-11-06T03:05:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862330.73 epoch sec; seqNo:2404; 4061699.10 north; 594578.15 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T03:05:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862327.51 2024-11-06T03:05:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:30Z- waiting for 1730862331.51, time is 1730862330.73 2024-11-06T03:05:31Z- AHRS_M2 msg: 1730862331.12 epoch sec; seqNo:2254 2024-11-06T03:05:31Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T03:05:31Z- handleDepth()- Depth_Keller msg: 1730862331.13 epoch sec; seqNo:2405; depth 49.52 2024-11-06T03:05:31Z- handleDVL() - DVL msg epochMillisec: 1730862331.14 2024-11-06T03:05:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:31Z- handleDvl() - RDI_Pathfinder msg: 1730862331.14 sec; seqNo:774; #beams:0 2024-11-06T03:05:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , -0.02 2024-11-06T03:05:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862331.15 epoch sec; seqNo:2405; 4061699.10 north; 594578.15 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T03:05:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862327.51 2024-11-06T03:05:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:31Z- waiting for 1730862331.51, time is 1730862331.15 2024-11-06T03:05:31Z- AHRS_M2 msg: 1730862331.56 epoch sec; seqNo:2255 2024-11-06T03:05:31Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T03:05:31Z- handleDepth()- Depth_Keller msg: 1730862331.56 epoch sec; seqNo:2406; depth 49.50 2024-11-06T03:05:31Z- handleDVL() - DVL msg epochMillisec: 1730862331.59 2024-11-06T03:05:31Z- handleDvl() - RDI_Pathfinder msg: 1730862331.59 sec; seqNo:775; #beams:4 2024-11-06T03:05:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.75 , 33.81 , 37.04 , 35.85 2024-11-06T03:05:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.06 2024-11-06T03:05:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862331.63 epoch sec; seqNo:2406; 4061699.10 north; 594578.15 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:05:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862327.51 2024-11-06T03:05:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:31Z- waiting for 1730862331.51, time is 1730862331.63 2024-11-06T03:05:31Z- LcmTrn::updateTrn() >>>> pose time 1730862331.63, meas time 1730862331.59 <<<< 2024-11-06T03:05:31Z- LcmTrn::updateTrn() equating pose 1730862331.59 and meas times 1730862331.59 2024-11-06T03:05:31Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862331.59 2024-11-06T03:05:31Z- TerrainNav::motionUpdate() - currEstimate = (4061699, 594578.15, 49.50) 2024-11-06T03:05:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:05:31Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:05:31Z- TNavPF::Projecting Measurements 2024-11-06T03:05:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:05:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:05:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:05:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:05:31Z- TNavFilter::4 good beams. 2024-11-06T03:05:31Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:05:31Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:05:31Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:05:31Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:05:31Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:05:31Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:05:31Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:05:31Z- TerrainMapOctree: (vehN, vehE ) = (4061699.10, 594578.15). 2024-11-06T03:05:31Z- TerrainMapOctree: Min Distance = 179.23. 2024-11-06T03:05:31Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:05:31Z- Forcing Subcloud Comparison 2024-11-06T03:05:31Z- Weighting particles with subcloud comparison 2024-11-06T03:05:31Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:31Z- meanExpectedMeasDiff: -0.621742 delta_rms_squared: 2.656528 alpha: 0.992528 2024-11-06T03:05:31Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:31Z- meanExpectedMeasDiff: 1.749374 delta_rms_squared: 1.904788 alpha: 0.989609 2024-11-06T03:05:31Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:31Z- meanExpectedMeasDiff: -0.589163 delta_rms_squared: 2.305591 alpha: 0.991400 2024-11-06T03:05:31Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:31Z- meanExpectedMeasDiff: -0.627812 delta_rms_squared: 2.166043 alpha: 0.990851 2024-11-06T03:05:31Z- Subcloud NIS: 0.285336 2024-11-06T03:05:31Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:05:31Z- TNavPF:: sumWeights = 0.312794 2024-11-06T03:05:31Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:05:31Z- TNavPF::First Term of Map Covariance Matrix is 0.31 2024-11-06T03:05:31Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T03:05:31Z- TerrainNav::Resampling particle filter... 2024-11-06T03:05:31Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862331.59 sec, ping # 0. 2024-11-06T03:05:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:05:31Z- variances([0], [2], and psi_berg) = 0.748525 0.191038 0.000000 2024-11-06T03:05:31Z- publishEstimatesDecoder() - at 1730862331.59 sec 2024-11-06T03:05:31Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:05:31Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061731.05 594561.83 56.72 2024-11-06T03:05:31Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061731.45 594561.65 56.95 lat/lon: 36.70 -121.94 2024-11-06T03:05:31Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.75 0.19 0.50 0.00 2024-11-06T03:05:31Z- AHRS_M2 msg: 1730862331.93 epoch sec; seqNo:2256 2024-11-06T03:05:31Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.51 psi 2024-11-06T03:05:31Z- handleDepth()- Depth_Keller msg: 1730862331.93 epoch sec; seqNo:2407; depth 49.47 2024-11-06T03:05:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862331.95 epoch sec; seqNo:2407; 4061699.11 north; 594579.37 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:05:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862331.59 2024-11-06T03:05:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:32Z- waiting for 1730862335.59, time is 1730862331.95 2024-11-06T03:05:32Z- AHRS_M2 msg: 1730862332.33 epoch sec; seqNo:2257 2024-11-06T03:05:32Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.51 psi 2024-11-06T03:05:32Z- handleDepth()- Depth_Keller msg: 1730862332.34 epoch sec; seqNo:2408; depth 49.47 2024-11-06T03:05:32Z- handleDVL() - DVL msg epochMillisec: 1730862332.34 2024-11-06T03:05:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:32Z- handleDvl() - RDI_Pathfinder msg: 1730862332.34 sec; seqNo:776; #beams:0 2024-11-06T03:05:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.06 2024-11-06T03:05:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862332.35 epoch sec; seqNo:2408; 4061699.11 north; 594579.37 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:05:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862331.59 2024-11-06T03:05:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:32Z- waiting for 1730862335.59, time is 1730862332.35 2024-11-06T03:05:32Z- AHRS_M2 msg: 1730862332.74 epoch sec; seqNo:2258 2024-11-06T03:05:32Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.51 psi 2024-11-06T03:05:32Z- handleDepth()- Depth_Keller msg: 1730862332.74 epoch sec; seqNo:2409; depth 49.45 2024-11-06T03:05:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862332.75 epoch sec; seqNo:2409; 4061699.11 north; 594579.37 east; 36.70 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:05:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862331.59 2024-11-06T03:05:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:32Z- waiting for 1730862335.59, time is 1730862332.75 2024-11-06T03:05:33Z- AHRS_M2 msg: 1730862333.14 epoch sec; seqNo:2259 2024-11-06T03:05:33Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.51 psi 2024-11-06T03:05:33Z- handleDepth()- Depth_Keller msg: 1730862333.14 epoch sec; seqNo:2410; depth 49.44 2024-11-06T03:05:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862333.15 epoch sec; seqNo:2410; 4061699.12 north; 594580.59 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:05:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862331.59 2024-11-06T03:05:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:33Z- waiting for 1730862335.59, time is 1730862333.15 2024-11-06T03:05:33Z- AHRS_M2 msg: 1730862333.56 epoch sec; seqNo:2260 2024-11-06T03:05:33Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.51 psi 2024-11-06T03:05:33Z- handleDepth()- Depth_Keller msg: 1730862333.57 epoch sec; seqNo:2411; depth 49.42 2024-11-06T03:05:33Z- handleDVL() - DVL msg epochMillisec: 1730862333.59 2024-11-06T03:05:33Z- handleDvl() - RDI_Pathfinder msg: 1730862333.59 sec; seqNo:777; #beams:4 2024-11-06T03:05:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.75 , 33.81 , 36.53 , 35.85 2024-11-06T03:05:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , -0.06 2024-11-06T03:05:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862333.61 epoch sec; seqNo:2411; 4061699.50 north; 594580.59 east; 36.70 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T03:05:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862331.59 2024-11-06T03:05:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:33Z- waiting for 1730862335.59, time is 1730862333.61 2024-11-06T03:05:33Z- AHRS_M2 msg: 1730862333.95 epoch sec; seqNo:2261 2024-11-06T03:05:33Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.51 psi 2024-11-06T03:05:33Z- handleDepth()- Depth_Keller msg: 1730862333.96 epoch sec; seqNo:2412; depth 49.42 2024-11-06T03:05:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862333.98 epoch sec; seqNo:2412; 4061699.50 north; 594580.59 east; 36.70 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T03:05:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862331.59 2024-11-06T03:05:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:34Z- waiting for 1730862335.59, time is 1730862333.98 2024-11-06T03:05:34Z- AHRS_M2 msg: 1730862334.35 epoch sec; seqNo:2262 2024-11-06T03:05:34Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.51 psi 2024-11-06T03:05:34Z- handleDepth()- Depth_Keller msg: 1730862334.36 epoch sec; seqNo:2413; depth 49.40 2024-11-06T03:05:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862334.37 epoch sec; seqNo:2413; 4061699.50 north; 594580.59 east; 36.70 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T03:05:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862331.59 2024-11-06T03:05:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:34Z- waiting for 1730862335.59, time is 1730862334.37 2024-11-06T03:05:34Z- AHRS_M2 msg: 1730862334.75 epoch sec; seqNo:2263 2024-11-06T03:05:34Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.51 psi 2024-11-06T03:05:34Z- handleDepth()- Depth_Keller msg: 1730862334.76 epoch sec; seqNo:2414; depth 49.39 2024-11-06T03:05:34Z- handleDVL() - DVL msg epochMillisec: 1730862334.76 2024-11-06T03:05:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:34Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:34Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:34Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:34Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:34Z- handleDvl() - RDI_Pathfinder msg: 1730862334.76 sec; seqNo:778; #beams:0 2024-11-06T03:05:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.08 , -0.06 2024-11-06T03:05:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862334.77 epoch sec; seqNo:2414; 4061699.51 north; 594581.81 east; 36.70 lat; -121.94 lon; 49.39 depth; utmZone: 10 2024-11-06T03:05:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862331.59 2024-11-06T03:05:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:34Z- waiting for 1730862335.59, time is 1730862334.77 2024-11-06T03:05:35Z- AHRS_M2 msg: 1730862335.16 epoch sec; seqNo:2264 2024-11-06T03:05:35Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.51 psi 2024-11-06T03:05:35Z- handleDepth()- Depth_Keller msg: 1730862335.16 epoch sec; seqNo:2415; depth 49.36 2024-11-06T03:05:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862335.17 epoch sec; seqNo:2415; 4061699.51 north; 594581.81 east; 36.70 lat; -121.94 lon; 49.36 depth; utmZone: 10 2024-11-06T03:05:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862331.59 2024-11-06T03:05:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:35Z- waiting for 1730862335.59, time is 1730862335.17 2024-11-06T03:05:35Z- AHRS_M2 msg: 1730862335.57 epoch sec; seqNo:2265 2024-11-06T03:05:35Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:05:35Z- handleDepth()- Depth_Keller msg: 1730862335.57 epoch sec; seqNo:2416; depth 49.37 2024-11-06T03:05:35Z- handleDVL() - DVL msg epochMillisec: 1730862335.60 2024-11-06T03:05:35Z- handleDvl() - RDI_Pathfinder msg: 1730862335.60 sec; seqNo:779; #beams:4 2024-11-06T03:05:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.75 , 33.64 , 36.53 , 36.19 2024-11-06T03:05:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.09 , -0.02 2024-11-06T03:05:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862335.61 epoch sec; seqNo:2416; 4061699.51 north; 594581.81 east; 36.70 lat; -121.94 lon; 49.37 depth; utmZone: 10 2024-11-06T03:05:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862331.59 2024-11-06T03:05:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:35Z- waiting for 1730862335.59, time is 1730862335.61 2024-11-06T03:05:35Z- LcmTrn::updateTrn() >>>> pose time 1730862335.61, meas time 1730862335.60 <<<< 2024-11-06T03:05:35Z- LcmTrn::updateTrn() equating pose 1730862335.60 and meas times 1730862335.60 2024-11-06T03:05:35Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862335.60 2024-11-06T03:05:35Z- TerrainNav::motionUpdate() - currEstimate = (4061700, 594581.81, 49.37) 2024-11-06T03:05:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:05:35Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:05:35Z- TNavPF::Projecting Measurements 2024-11-06T03:05:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:05:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:05:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:05:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:05:35Z- TNavFilter::4 good beams. 2024-11-06T03:05:35Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:05:35Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:05:35Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:05:35Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:05:35Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:05:35Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:05:35Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:05:35Z- TerrainMapOctree: (vehN, vehE ) = (4061699.51, 594581.81). 2024-11-06T03:05:35Z- TerrainMapOctree: Min Distance = 175.61. 2024-11-06T03:05:35Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:05:35Z- Forcing Subcloud Comparison 2024-11-06T03:05:35Z- Weighting particles with subcloud comparison 2024-11-06T03:05:35Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:35Z- meanExpectedMeasDiff: -0.814609 delta_rms_squared: 1.205896 alpha: 0.983685 2024-11-06T03:05:35Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:35Z- meanExpectedMeasDiff: 1.493194 delta_rms_squared: 0.858651 alpha: 0.977238 2024-11-06T03:05:35Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:35Z- meanExpectedMeasDiff: -0.799022 delta_rms_squared: 1.019240 alpha: 0.980755 2024-11-06T03:05:35Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:35Z- meanExpectedMeasDiff: -0.831274 delta_rms_squared: 1.004526 alpha: 0.980479 2024-11-06T03:05:35Z- Subcloud NIS: 0.268811 2024-11-06T03:05:35Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:05:35Z- TNavPF:: sumWeights = 0.408259 2024-11-06T03:05:35Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:05:35Z- TNavPF::First Term of Map Covariance Matrix is 0.31 2024-11-06T03:05:35Z- TNavPF::Calculated NIS Value : 0.27 numBeams normalized NIS: 0.07 2024-11-06T03:05:35Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862335.60 sec, ping # 0. 2024-11-06T03:05:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:05:35Z- variances([0], [2], and psi_berg) = 0.747224 0.194523 0.000000 2024-11-06T03:05:35Z- publishEstimatesDecoder() - at 1730862335.60 sec 2024-11-06T03:05:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:05:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061732.06 594565.77 57.03 2024-11-06T03:05:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061731.87 594565.31 56.93 lat/lon: 36.70 -121.94 2024-11-06T03:05:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.75 0.19 0.37 0.00 2024-11-06T03:05:35Z- AHRS_M2 msg: 1730862335.97 epoch sec; seqNo:2266 2024-11-06T03:05:35Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:05:35Z- handleDepth()- Depth_Keller msg: 1730862335.98 epoch sec; seqNo:2417; depth 49.35 2024-11-06T03:05:35Z- handleDVL() - DVL msg epochMillisec: 1730862335.98 2024-11-06T03:05:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:35Z- handleDvl() - RDI_Pathfinder msg: 1730862335.98 sec; seqNo:780; #beams:0 2024-11-06T03:05:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.09 , -0.02 2024-11-06T03:05:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862336.00 epoch sec; seqNo:2417; 4061699.53 north; 594583.03 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:05:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862335.60 2024-11-06T03:05:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:36Z- waiting for 1730862339.60, time is 1730862336.00 2024-11-06T03:05:36Z- handleDepth()- Depth_Keller msg: 1730862336.39 epoch sec; seqNo:2418; depth 49.35 2024-11-06T03:05:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862336.40 epoch sec; seqNo:2418; 4061699.91 north; 594583.02 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:05:36Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:05:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862335.60 2024-11-06T03:05:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:36Z- waiting for 1730862339.60, time is 1730862336.40 2024-11-06T03:05:36Z- AHRS_M2 msg: 1730862336.77 epoch sec; seqNo:2267 2024-11-06T03:05:36Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.50 psi 2024-11-06T03:05:36Z- handleDepth()- Depth_Keller msg: 1730862336.78 epoch sec; seqNo:2419; depth 49.34 2024-11-06T03:05:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862336.79 epoch sec; seqNo:2419; 4061699.91 north; 594583.02 east; 36.70 lat; -121.94 lon; 49.34 depth; utmZone: 10 2024-11-06T03:05:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862335.60 2024-11-06T03:05:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:36Z- waiting for 1730862339.60, time is 1730862336.79 2024-11-06T03:05:37Z- AHRS_M2 msg: 1730862337.18 epoch sec; seqNo:2268 2024-11-06T03:05:37Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.50 psi 2024-11-06T03:05:37Z- handleDepth()- Depth_Keller msg: 1730862337.19 epoch sec; seqNo:2420; depth 49.35 2024-11-06T03:05:37Z- handleDVL() - DVL msg epochMillisec: 1730862337.19 2024-11-06T03:05:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:37Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:37Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:37Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:37Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:37Z- handleDvl() - RDI_Pathfinder msg: 1730862337.19 sec; seqNo:781; #beams:0 2024-11-06T03:05:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.09 , -0.02 2024-11-06T03:05:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862337.21 epoch sec; seqNo:2420; 4061699.91 north; 594583.02 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:05:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862335.60 2024-11-06T03:05:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:37Z- waiting for 1730862339.60, time is 1730862337.21 2024-11-06T03:05:37Z- AHRS_M2 msg: 1730862337.60 epoch sec; seqNo:2269 2024-11-06T03:05:37Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.49 psi 2024-11-06T03:05:37Z- handleDepth()- Depth_Keller msg: 1730862337.60 epoch sec; seqNo:2421; depth 49.35 2024-11-06T03:05:37Z- handleDVL() - DVL msg epochMillisec: 1730862337.64 2024-11-06T03:05:37Z- handleDvl() - RDI_Pathfinder msg: 1730862337.64 sec; seqNo:782; #beams:4 2024-11-06T03:05:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.75 , 33.64 , 36.19 , 36.36 2024-11-06T03:05:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.06 , 0.02 2024-11-06T03:05:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862337.66 epoch sec; seqNo:2421; 4061699.92 north; 594584.24 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:05:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862335.60 2024-11-06T03:05:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:37Z- waiting for 1730862339.60, time is 1730862337.66 2024-11-06T03:05:37Z- AHRS_M2 msg: 1730862337.99 epoch sec; seqNo:2270 2024-11-06T03:05:37Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.49 psi 2024-11-06T03:05:37Z- handleDepth()- Depth_Keller msg: 1730862337.99 epoch sec; seqNo:2422; depth 49.36 2024-11-06T03:05:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862338.01 epoch sec; seqNo:2422; 4061699.92 north; 594584.24 east; 36.70 lat; -121.94 lon; 49.36 depth; utmZone: 10 2024-11-06T03:05:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862335.60 2024-11-06T03:05:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:38Z- waiting for 1730862339.60, time is 1730862338.01 2024-11-06T03:05:38Z- AHRS_M2 msg: 1730862338.39 epoch sec; seqNo:2271 2024-11-06T03:05:38Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.49 psi 2024-11-06T03:05:38Z- handleDepth()- Depth_Keller msg: 1730862338.40 epoch sec; seqNo:2423; depth 49.36 2024-11-06T03:05:38Z- handleDVL() - DVL msg epochMillisec: 1730862338.40 2024-11-06T03:05:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:38Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:38Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:38Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:38Z- handleDvl() - RDI_Pathfinder msg: 1730862338.40 sec; seqNo:783; #beams:0 2024-11-06T03:05:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.06 , 0.02 2024-11-06T03:05:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862338.42 epoch sec; seqNo:2423; 4061699.92 north; 594584.24 east; 36.70 lat; -121.94 lon; 49.36 depth; utmZone: 10 2024-11-06T03:05:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862335.60 2024-11-06T03:05:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:38Z- waiting for 1730862339.60, time is 1730862338.42 2024-11-06T03:05:38Z- AHRS_M2 msg: 1730862338.80 epoch sec; seqNo:2272 2024-11-06T03:05:38Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.49 psi 2024-11-06T03:05:38Z- handleDepth()- Depth_Keller msg: 1730862338.80 epoch sec; seqNo:2424; depth 49.35 2024-11-06T03:05:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862338.81 epoch sec; seqNo:2424; 4061699.92 north; 594584.24 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:05:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862335.60 2024-11-06T03:05:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:38Z- waiting for 1730862339.60, time is 1730862338.81 2024-11-06T03:05:39Z- AHRS_M2 msg: 1730862339.21 epoch sec; seqNo:2273 2024-11-06T03:05:39Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.49 psi 2024-11-06T03:05:39Z- handleDepth()- Depth_Keller msg: 1730862339.22 epoch sec; seqNo:2425; depth 49.35 2024-11-06T03:05:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862339.22 epoch sec; seqNo:2425; 4061700.31 north; 594585.46 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:05:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862335.60 2024-11-06T03:05:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:39Z- waiting for 1730862339.60, time is 1730862339.22 2024-11-06T03:05:39Z- AHRS_M2 msg: 1730862339.60 epoch sec; seqNo:2274 2024-11-06T03:05:39Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T03:05:39Z- handleDepth()- Depth_Keller msg: 1730862339.61 epoch sec; seqNo:2426; depth 49.35 2024-11-06T03:05:39Z- handleDVL() - DVL msg epochMillisec: 1730862339.64 2024-11-06T03:05:39Z- handleDvl() - RDI_Pathfinder msg: 1730862339.64 sec; seqNo:784; #beams:4 2024-11-06T03:05:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.41 , 33.81 , 35.85 , 36.53 2024-11-06T03:05:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.03 2024-11-06T03:05:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862339.67 epoch sec; seqNo:2426; 4061700.31 north; 594585.46 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:05:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862335.60 2024-11-06T03:05:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:39Z- waiting for 1730862339.60, time is 1730862339.67 2024-11-06T03:05:39Z- LcmTrn::updateTrn() >>>> pose time 1730862339.67, meas time 1730862339.64 <<<< 2024-11-06T03:05:39Z- LcmTrn::updateTrn() equating pose 1730862339.64 and meas times 1730862339.64 2024-11-06T03:05:39Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862339.64 2024-11-06T03:05:39Z- TerrainNav::motionUpdate() - currEstimate = (4061700, 594585.46, 49.35) 2024-11-06T03:05:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:05:39Z- TerrainNav:: SubcloudNIS is 0.27, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:05:39Z- TNavPF::Projecting Measurements 2024-11-06T03:05:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:05:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:05:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:05:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:05:39Z- TNavFilter::4 good beams. 2024-11-06T03:05:39Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:05:39Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:05:39Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:05:39Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:05:39Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:05:39Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:05:39Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:05:39Z- TerrainMapOctree: (vehN, vehE ) = (4061700.31, 594585.46). 2024-11-06T03:05:39Z- TerrainMapOctree: Min Distance = 171.88. 2024-11-06T03:05:39Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:05:39Z- Forcing Subcloud Comparison 2024-11-06T03:05:39Z- Weighting particles with subcloud comparison 2024-11-06T03:05:39Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:39Z- meanExpectedMeasDiff: -0.777915 delta_rms_squared: 0.992379 alpha: 0.980245 2024-11-06T03:05:39Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:39Z- meanExpectedMeasDiff: 1.634394 delta_rms_squared: 0.721505 alpha: 0.973028 2024-11-06T03:05:39Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:39Z- meanExpectedMeasDiff: -1.003883 delta_rms_squared: 0.832610 alpha: 0.976543 2024-11-06T03:05:39Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:39Z- meanExpectedMeasDiff: -0.637007 delta_rms_squared: 0.993723 alpha: 0.980271 2024-11-06T03:05:39Z- Subcloud NIS: 0.263009 2024-11-06T03:05:39Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:05:39Z- TNavPF:: sumWeights = 0.393900 2024-11-06T03:05:39Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:05:39Z- TNavPF::First Term of Map Covariance Matrix is 0.27 2024-11-06T03:05:39Z- TNavPF::Calculated NIS Value : 0.27 numBeams normalized NIS: 0.07 2024-11-06T03:05:39Z- TerrainNav::Resampling particle filter... 2024-11-06T03:05:39Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862339.64 sec, ping # 0. 2024-11-06T03:05:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:05:39Z- variances([0], [2], and psi_berg) = 0.741119 0.197087 0.000000 2024-11-06T03:05:39Z- publishEstimatesDecoder() - at 1730862339.64 sec 2024-11-06T03:05:39Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:05:39Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061732.16 594569.11 56.69 2024-11-06T03:05:39Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061732.67 594568.96 56.99 lat/lon: 36.70 -121.94 2024-11-06T03:05:39Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.74 0.20 0.33 0.00 2024-11-06T03:05:40Z- AHRS_M2 msg: 1730862340.01 epoch sec; seqNo:2275 2024-11-06T03:05:40Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T03:05:40Z- handleDepth()- Depth_Keller msg: 1730862340.02 epoch sec; seqNo:2427; depth 49.35 2024-11-06T03:05:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862340.05 epoch sec; seqNo:2427; 4061700.31 north; 594585.46 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:05:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862339.64 2024-11-06T03:05:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:40Z- waiting for 1730862343.64, time is 1730862340.05 2024-11-06T03:05:40Z- AHRS_M2 msg: 1730862340.41 epoch sec; seqNo:2276 2024-11-06T03:05:40Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T03:05:40Z- handleDepth()- Depth_Keller msg: 1730862340.41 epoch sec; seqNo:2428; depth 49.37 2024-11-06T03:05:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862340.43 epoch sec; seqNo:2428; 4061700.33 north; 594586.68 east; 36.70 lat; -121.94 lon; 49.37 depth; utmZone: 10 2024-11-06T03:05:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862339.64 2024-11-06T03:05:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:40Z- waiting for 1730862343.64, time is 1730862340.43 2024-11-06T03:05:40Z- AHRS_M2 msg: 1730862340.82 epoch sec; seqNo:2277 2024-11-06T03:05:40Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.49 psi 2024-11-06T03:05:40Z- handleDepth()- Depth_Keller msg: 1730862340.82 epoch sec; seqNo:2429; depth 49.38 2024-11-06T03:05:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862340.84 epoch sec; seqNo:2429; 4061700.33 north; 594586.68 east; 36.70 lat; -121.94 lon; 49.38 depth; utmZone: 10 2024-11-06T03:05:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862339.64 2024-11-06T03:05:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:40Z- waiting for 1730862343.64, time is 1730862340.84 2024-11-06T03:05:41Z- AHRS_M2 msg: 1730862341.22 epoch sec; seqNo:2278 2024-11-06T03:05:41Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.49 psi 2024-11-06T03:05:41Z- handleDepth()- Depth_Keller msg: 1730862341.22 epoch sec; seqNo:2430; depth 49.38 2024-11-06T03:05:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862341.25 epoch sec; seqNo:2430; 4061700.33 north; 594586.68 east; 36.70 lat; -121.94 lon; 49.38 depth; utmZone: 10 2024-11-06T03:05:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862339.64 2024-11-06T03:05:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:41Z- waiting for 1730862343.64, time is 1730862341.25 2024-11-06T03:05:41Z- AHRS_M2 msg: 1730862341.63 epoch sec; seqNo:2279 2024-11-06T03:05:41Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.49 psi 2024-11-06T03:05:41Z- handleDepth()- Depth_Keller msg: 1730862341.67 epoch sec; seqNo:2431; depth 49.38 2024-11-06T03:05:41Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:05:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862339.64 2024-11-06T03:05:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:41Z- waiting for 1730862343.64, time is 1730862341.63 2024-11-06T03:05:41Z- handleDVL() - DVL msg epochMillisec: 1730862341.77 2024-11-06T03:05:41Z- handleDvl() - RDI_Pathfinder msg: 1730862341.77 sec; seqNo:785; #beams:4 2024-11-06T03:05:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.92 , 33.64 , 35.85 , 37.04 2024-11-06T03:05:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , 0.01 2024-11-06T03:05:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862341.88 epoch sec; seqNo:2431; 4061700.34 north; 594587.90 east; 36.70 lat; -121.94 lon; 49.38 depth; utmZone: 10 2024-11-06T03:05:41Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:05:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862339.64 2024-11-06T03:05:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:41Z- waiting for 1730862343.64, time is 1730862341.88 2024-11-06T03:05:42Z- AHRS_M2 msg: 1730862342.03 epoch sec; seqNo:2280 2024-11-06T03:05:42Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.49 psi 2024-11-06T03:05:42Z- handleDepth()- Depth_Keller msg: 1730862342.04 epoch sec; seqNo:2432; depth 49.39 2024-11-06T03:05:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862342.05 epoch sec; seqNo:2432; 4061700.34 north; 594587.90 east; 36.70 lat; -121.94 lon; 49.39 depth; utmZone: 10 2024-11-06T03:05:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862339.64 2024-11-06T03:05:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:42Z- waiting for 1730862343.64, time is 1730862342.05 2024-11-06T03:05:42Z- AHRS_M2 msg: 1730862342.43 epoch sec; seqNo:2281 2024-11-06T03:05:42Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.49 psi 2024-11-06T03:05:42Z- handleDepth()- Depth_Keller msg: 1730862342.44 epoch sec; seqNo:2433; depth 49.39 2024-11-06T03:05:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862342.45 epoch sec; seqNo:2433; 4061700.34 north; 594587.90 east; 36.70 lat; -121.94 lon; 49.39 depth; utmZone: 10 2024-11-06T03:05:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862339.64 2024-11-06T03:05:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:42Z- waiting for 1730862343.64, time is 1730862342.45 2024-11-06T03:05:42Z- AHRS_M2 msg: 1730862342.83 epoch sec; seqNo:2282 2024-11-06T03:05:42Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T03:05:42Z- handleDepth()- Depth_Keller msg: 1730862342.85 epoch sec; seqNo:2434; depth 49.39 2024-11-06T03:05:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862342.87 epoch sec; seqNo:2434; 4061700.34 north; 594587.90 east; 36.70 lat; -121.94 lon; 49.39 depth; utmZone: 10 2024-11-06T03:05:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862339.64 2024-11-06T03:05:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:42Z- waiting for 1730862343.64, time is 1730862342.87 2024-11-06T03:05:43Z- AHRS_M2 msg: 1730862343.24 epoch sec; seqNo:2283 2024-11-06T03:05:43Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T03:05:43Z- handleDepth()- Depth_Keller msg: 1730862343.25 epoch sec; seqNo:2435; depth 49.41 2024-11-06T03:05:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862343.25 epoch sec; seqNo:2435; 4061700.35 north; 594589.12 east; 36.70 lat; -121.94 lon; 49.41 depth; utmZone: 10 2024-11-06T03:05:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862339.64 2024-11-06T03:05:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:43Z- waiting for 1730862343.64, time is 1730862343.25 2024-11-06T03:05:43Z- AHRS_M2 msg: 1730862343.65 epoch sec; seqNo:2284 2024-11-06T03:05:43Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T03:05:43Z- handleDepth()- Depth_Keller msg: 1730862343.65 epoch sec; seqNo:2436; depth 49.42 2024-11-06T03:05:43Z- handleDVL() - DVL msg epochMillisec: 1730862343.67 2024-11-06T03:05:43Z- handleDvl() - RDI_Pathfinder msg: 1730862343.67 sec; seqNo:786; #beams:4 2024-11-06T03:05:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.92 , 33.13 , 35.51 , 37.04 2024-11-06T03:05:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , -0.00 2024-11-06T03:05:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862343.70 epoch sec; seqNo:2436; 4061700.35 north; 594589.12 east; 36.70 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T03:05:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862339.64 2024-11-06T03:05:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:43Z- waiting for 1730862343.64, time is 1730862343.70 2024-11-06T03:05:43Z- LcmTrn::updateTrn() >>>> pose time 1730862343.70, meas time 1730862343.67 <<<< 2024-11-06T03:05:43Z- LcmTrn::updateTrn() equating pose 1730862343.67 and meas times 1730862343.67 2024-11-06T03:05:43Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862343.67 2024-11-06T03:05:43Z- TerrainNav::motionUpdate() - currEstimate = (4061700, 594589.12, 49.42) 2024-11-06T03:05:43Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:05:43Z- TerrainNav:: SubcloudNIS is 0.26, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:05:43Z- TNavPF::Projecting Measurements 2024-11-06T03:05:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:05:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:05:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:05:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:05:43Z- TNavFilter::4 good beams. 2024-11-06T03:05:43Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:05:43Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:05:43Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:05:43Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:05:43Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:05:43Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:05:43Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:05:43Z- TerrainMapOctree: (vehN, vehE ) = (4061700.35, 594589.12). 2024-11-06T03:05:43Z- TerrainMapOctree: Min Distance = 168.37. 2024-11-06T03:05:43Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:05:43Z- Forcing Subcloud Comparison 2024-11-06T03:05:43Z- Weighting particles with subcloud comparison 2024-11-06T03:05:43Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:43Z- meanExpectedMeasDiff: -0.858054 delta_rms_squared: 1.759596 alpha: 0.988761 2024-11-06T03:05:43Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:43Z- meanExpectedMeasDiff: 1.256204 delta_rms_squared: 0.618609 alpha: 0.968682 2024-11-06T03:05:43Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:43Z- meanExpectedMeasDiff: -0.794068 delta_rms_squared: 0.738888 alpha: 0.973646 2024-11-06T03:05:43Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:43Z- meanExpectedMeasDiff: -0.526366 delta_rms_squared: 0.795704 alpha: 0.975481 2024-11-06T03:05:43Z- Subcloud NIS: 0.259775 2024-11-06T03:05:43Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:05:43Z- TNavPF:: sumWeights = 0.473238 2024-11-06T03:05:43Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:05:43Z- TNavPF::First Term of Map Covariance Matrix is 0.50 2024-11-06T03:05:43Z- TNavPF::Calculated NIS Value : 0.28 numBeams normalized NIS: 0.07 2024-11-06T03:05:43Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862343.67 sec, ping # 0. 2024-11-06T03:05:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:05:43Z- variances([0], [2], and psi_berg) = 0.764143 0.203892 0.000000 2024-11-06T03:05:43Z- publishEstimatesDecoder() - at 1730862343.67 sec 2024-11-06T03:05:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:05:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061734.28 594572.90 57.63 2024-11-06T03:05:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061732.63 594572.61 57.17 lat/lon: 36.70 -121.94 2024-11-06T03:05:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.76 0.20 0.31 0.00 2024-11-06T03:05:44Z- AHRS_M2 msg: 1730862344.05 epoch sec; seqNo:2285 2024-11-06T03:05:44Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T03:05:44Z- handleDepth()- Depth_Keller msg: 1730862344.06 epoch sec; seqNo:2437; depth 49.42 2024-11-06T03:05:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862344.07 epoch sec; seqNo:2437; 4061700.35 north; 594589.12 east; 36.70 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T03:05:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862343.67 2024-11-06T03:05:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:44Z- waiting for 1730862347.67, time is 1730862344.07 2024-11-06T03:05:44Z- AHRS_M2 msg: 1730862344.45 epoch sec; seqNo:2286 2024-11-06T03:05:44Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T03:05:44Z- handleDepth()- Depth_Keller msg: 1730862344.46 epoch sec; seqNo:2438; depth 49.43 2024-11-06T03:05:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862344.48 epoch sec; seqNo:2438; 4061700.73 north; 594589.12 east; 36.70 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T03:05:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862343.67 2024-11-06T03:05:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:44Z- waiting for 1730862347.67, time is 1730862344.48 2024-11-06T03:05:44Z- AHRS_M2 msg: 1730862344.85 epoch sec; seqNo:2287 2024-11-06T03:05:44Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T03:05:44Z- handleDepth()- Depth_Keller msg: 1730862344.86 epoch sec; seqNo:2439; depth 49.42 2024-11-06T03:05:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862344.87 epoch sec; seqNo:2439; 4061700.75 north; 594590.34 east; 36.70 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T03:05:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862343.67 2024-11-06T03:05:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:45Z- waiting for 1730862347.67, time is 1730862344.87 2024-11-06T03:05:45Z- AHRS_M2 msg: 1730862345.26 epoch sec; seqNo:2288 2024-11-06T03:05:45Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T03:05:45Z- handleDepth()- Depth_Keller msg: 1730862345.27 epoch sec; seqNo:2440; depth 49.44 2024-11-06T03:05:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862345.27 epoch sec; seqNo:2440; 4061700.75 north; 594590.34 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:05:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862343.67 2024-11-06T03:05:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:45Z- waiting for 1730862347.67, time is 1730862345.27 2024-11-06T03:05:45Z- AHRS_M2 msg: 1730862345.66 epoch sec; seqNo:2289 2024-11-06T03:05:45Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T03:05:45Z- handleDepth()- Depth_Keller msg: 1730862345.68 epoch sec; seqNo:2441; depth 49.44 2024-11-06T03:05:45Z- handleDVL() - DVL msg epochMillisec: 1730862345.70 2024-11-06T03:05:45Z- handleDvl() - RDI_Pathfinder msg: 1730862345.70 sec; seqNo:787; #beams:4 2024-11-06T03:05:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.75 , 32.96 , 35.34 , 37.38 2024-11-06T03:05:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.03 2024-11-06T03:05:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862345.71 epoch sec; seqNo:2441; 4061700.75 north; 594590.34 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:05:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862343.67 2024-11-06T03:05:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:45Z- waiting for 1730862347.67, time is 1730862345.71 2024-11-06T03:05:46Z- AHRS_M2 msg: 1730862346.07 epoch sec; seqNo:2290 2024-11-06T03:05:46Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T03:05:46Z- handleDepth()- Depth_Keller msg: 1730862346.08 epoch sec; seqNo:2442; depth 49.47 2024-11-06T03:05:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862346.10 epoch sec; seqNo:2442; 4061700.76 north; 594591.56 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:05:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862343.67 2024-11-06T03:05:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:46Z- waiting for 1730862347.67, time is 1730862346.10 2024-11-06T03:05:46Z- AHRS_M2 msg: 1730862346.47 epoch sec; seqNo:2291 2024-11-06T03:05:46Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T03:05:46Z- handleDepth()- Depth_Keller msg: 1730862346.48 epoch sec; seqNo:2443; depth 49.46 2024-11-06T03:05:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862346.49 epoch sec; seqNo:2443; 4061700.76 north; 594591.56 east; 36.70 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T03:05:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862343.67 2024-11-06T03:05:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:46Z- waiting for 1730862347.67, time is 1730862346.49 2024-11-06T03:05:46Z- AHRS_M2 msg: 1730862346.87 epoch sec; seqNo:2292 2024-11-06T03:05:46Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.50 psi 2024-11-06T03:05:46Z- handleDepth()- Depth_Keller msg: 1730862346.88 epoch sec; seqNo:2444; depth 49.48 2024-11-06T03:05:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862346.90 epoch sec; seqNo:2444; 4061700.76 north; 594591.56 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:05:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862343.67 2024-11-06T03:05:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:47Z- waiting for 1730862347.67, time is 1730862346.90 2024-11-06T03:05:47Z- AHRS_M2 msg: 1730862347.28 epoch sec; seqNo:2293 2024-11-06T03:05:47Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.50 psi 2024-11-06T03:05:47Z- handleDepth()- Depth_Keller msg: 1730862347.28 epoch sec; seqNo:2445; depth 49.48 2024-11-06T03:05:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862347.29 epoch sec; seqNo:2445; 4061700.77 north; 594592.78 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:05:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862343.67 2024-11-06T03:05:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:47Z- waiting for 1730862347.67, time is 1730862347.29 2024-11-06T03:05:47Z- AHRS_M2 msg: 1730862347.69 epoch sec; seqNo:2294 2024-11-06T03:05:47Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.50 psi 2024-11-06T03:05:47Z- handleDepth()- Depth_Keller msg: 1730862347.70 epoch sec; seqNo:2446; depth 49.50 2024-11-06T03:05:47Z- handleDVL() - DVL msg epochMillisec: 1730862347.72 2024-11-06T03:05:47Z- handleDvl() - RDI_Pathfinder msg: 1730862347.72 sec; seqNo:788; #beams:4 2024-11-06T03:05:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.92 , 32.96 , 35.17 , 37.21 2024-11-06T03:05:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.07 , -0.03 2024-11-06T03:05:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862347.73 epoch sec; seqNo:2446; 4061700.77 north; 594592.78 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:05:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862343.67 2024-11-06T03:05:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:47Z- waiting for 1730862347.67, time is 1730862347.73 2024-11-06T03:05:47Z- LcmTrn::updateTrn() >>>> pose time 1730862347.73, meas time 1730862347.72 <<<< 2024-11-06T03:05:47Z- LcmTrn::updateTrn() equating pose 1730862347.72 and meas times 1730862347.72 2024-11-06T03:05:47Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862347.72 2024-11-06T03:05:47Z- TerrainNav::motionUpdate() - currEstimate = (4061701, 594592.78, 49.50) 2024-11-06T03:05:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:05:47Z- TerrainNav:: SubcloudNIS is 0.26, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:05:47Z- TNavPF::Projecting Measurements 2024-11-06T03:05:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:05:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:05:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:05:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:05:47Z- TNavFilter::4 good beams. 2024-11-06T03:05:47Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:05:47Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:05:47Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:05:47Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:05:47Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:05:47Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:05:47Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:05:47Z- TerrainMapOctree: (vehN, vehE ) = (4061700.77, 594592.78). 2024-11-06T03:05:47Z- TerrainMapOctree: Min Distance = 164.75. 2024-11-06T03:05:47Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:05:47Z- Forcing Subcloud Comparison 2024-11-06T03:05:47Z- Weighting particles with subcloud comparison 2024-11-06T03:05:47Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:47Z- meanExpectedMeasDiff: -0.547960 delta_rms_squared: 0.957466 alpha: 0.979539 2024-11-06T03:05:47Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:47Z- meanExpectedMeasDiff: 1.202004 delta_rms_squared: 0.561458 alpha: 0.965604 2024-11-06T03:05:47Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:47Z- meanExpectedMeasDiff: -1.021184 delta_rms_squared: 1.383319 alpha: 0.985748 2024-11-06T03:05:47Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:47Z- meanExpectedMeasDiff: -0.475162 delta_rms_squared: 0.809891 alpha: 0.975900 2024-11-06T03:05:47Z- Subcloud NIS: 0.259967 2024-11-06T03:05:47Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:05:47Z- TNavPF:: sumWeights = 0.495635 2024-11-06T03:05:47Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:05:47Z- TNavPF::First Term of Map Covariance Matrix is 0.28 2024-11-06T03:05:47Z- TNavPF::Calculated NIS Value : 0.26 numBeams normalized NIS: 0.07 2024-11-06T03:05:47Z- TerrainNav::Resampling particle filter... 2024-11-06T03:05:47Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862347.72 sec, ping # 0. 2024-11-06T03:05:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:05:47Z- variances([0], [2], and psi_berg) = 0.772391 0.196840 0.000000 2024-11-06T03:05:47Z- publishEstimatesDecoder() - at 1730862347.72 sec 2024-11-06T03:05:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:05:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061732.96 594576.48 57.11 2024-11-06T03:05:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061733.16 594576.24 57.40 lat/lon: 36.70 -121.94 2024-11-06T03:05:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.77 0.20 0.32 0.00 2024-11-06T03:05:48Z- AHRS_M2 msg: 1730862348.09 epoch sec; seqNo:2295 2024-11-06T03:05:48Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.50 psi 2024-11-06T03:05:48Z- handleDepth()- Depth_Keller msg: 1730862348.10 epoch sec; seqNo:2447; depth 49.50 2024-11-06T03:05:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862348.12 epoch sec; seqNo:2447; 4061700.77 north; 594592.78 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:05:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862347.72 2024-11-06T03:05:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:48Z- waiting for 1730862351.72, time is 1730862348.12 2024-11-06T03:05:48Z- AHRS_M2 msg: 1730862348.50 epoch sec; seqNo:2296 2024-11-06T03:05:48Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T03:05:48Z- handleDepth()- Depth_Keller msg: 1730862348.51 epoch sec; seqNo:2448; depth 49.50 2024-11-06T03:05:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862348.54 epoch sec; seqNo:2448; 4061701.15 north; 594592.77 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:05:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862347.72 2024-11-06T03:05:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:48Z- waiting for 1730862351.72, time is 1730862348.54 2024-11-06T03:05:48Z- AHRS_M2 msg: 1730862348.90 epoch sec; seqNo:2297 2024-11-06T03:05:48Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T03:05:48Z- handleDepth()- Depth_Keller msg: 1730862348.91 epoch sec; seqNo:2449; depth 49.53 2024-11-06T03:05:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862348.92 epoch sec; seqNo:2449; 4061701.16 north; 594593.99 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T03:05:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862347.72 2024-11-06T03:05:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:49Z- waiting for 1730862351.72, time is 1730862348.92 2024-11-06T03:05:49Z- AHRS_M2 msg: 1730862349.30 epoch sec; seqNo:2298 2024-11-06T03:05:49Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T03:05:49Z- handleDepth()- Depth_Keller msg: 1730862349.32 epoch sec; seqNo:2450; depth 49.54 2024-11-06T03:05:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862349.32 epoch sec; seqNo:2450; 4061701.16 north; 594593.99 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T03:05:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862347.72 2024-11-06T03:05:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:49Z- waiting for 1730862351.72, time is 1730862349.32 2024-11-06T03:05:49Z- AHRS_M2 msg: 1730862349.70 epoch sec; seqNo:2299 2024-11-06T03:05:49Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:49Z- handleDepth()- Depth_Keller msg: 1730862349.72 epoch sec; seqNo:2451; depth 49.56 2024-11-06T03:05:49Z- handleDVL() - DVL msg epochMillisec: 1730862349.74 2024-11-06T03:05:49Z- handleDvl() - RDI_Pathfinder msg: 1730862349.74 sec; seqNo:789; #beams:4 2024-11-06T03:05:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.75 , 32.96 , 34.83 , 37.04 2024-11-06T03:05:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.07 , -0.02 2024-11-06T03:05:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862349.76 epoch sec; seqNo:2451; 4061701.16 north; 594593.99 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:05:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862347.72 2024-11-06T03:05:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:49Z- waiting for 1730862351.72, time is 1730862349.76 2024-11-06T03:05:50Z- AHRS_M2 msg: 1730862350.11 epoch sec; seqNo:2300 2024-11-06T03:05:50Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:50Z- handleDepth()- Depth_Keller msg: 1730862350.12 epoch sec; seqNo:2452; depth 49.58 2024-11-06T03:05:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862350.13 epoch sec; seqNo:2452; 4061701.18 north; 594595.21 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:05:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862347.72 2024-11-06T03:05:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:50Z- waiting for 1730862351.72, time is 1730862350.13 2024-11-06T03:05:50Z- AHRS_M2 msg: 1730862350.52 epoch sec; seqNo:2301 2024-11-06T03:05:50Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:50Z- handleDepth()- Depth_Keller msg: 1730862350.54 epoch sec; seqNo:2453; depth 49.58 2024-11-06T03:05:50Z- handleDVL() - DVL msg epochMillisec: 1730862350.55 2024-11-06T03:05:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:50Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:50Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:50Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:50Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:50Z- handleDvl() - RDI_Pathfinder msg: 1730862350.55 sec; seqNo:790; #beams:0 2024-11-06T03:05:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.07 , -0.02 2024-11-06T03:05:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862350.55 epoch sec; seqNo:2453; 4061701.18 north; 594595.21 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:05:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862347.72 2024-11-06T03:05:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:50Z- waiting for 1730862351.72, time is 1730862350.55 2024-11-06T03:05:50Z- AHRS_M2 msg: 1730862350.91 epoch sec; seqNo:2302 2024-11-06T03:05:50Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:50Z- handleDepth()- Depth_Keller msg: 1730862350.92 epoch sec; seqNo:2454; depth 49.59 2024-11-06T03:05:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862350.93 epoch sec; seqNo:2454; 4061701.18 north; 594595.21 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:05:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862347.72 2024-11-06T03:05:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:51Z- waiting for 1730862351.72, time is 1730862350.93 2024-11-06T03:05:51Z- AHRS_M2 msg: 1730862351.32 epoch sec; seqNo:2303 2024-11-06T03:05:51Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:51Z- handleDepth()- Depth_Keller msg: 1730862351.33 epoch sec; seqNo:2455; depth 49.60 2024-11-06T03:05:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862351.34 epoch sec; seqNo:2455; 4061701.18 north; 594595.21 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:05:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862347.72 2024-11-06T03:05:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:51Z- waiting for 1730862351.72, time is 1730862351.34 2024-11-06T03:05:51Z- AHRS_M2 msg: 1730862351.72 epoch sec; seqNo:2304 2024-11-06T03:05:51Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:51Z- handleDepth()- Depth_Keller msg: 1730862351.73 epoch sec; seqNo:2456; depth 49.63 2024-11-06T03:05:51Z- handleDVL() - DVL msg epochMillisec: 1730862351.76 2024-11-06T03:05:51Z- handleDvl() - RDI_Pathfinder msg: 1730862351.76 sec; seqNo:791; #beams:4 2024-11-06T03:05:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.58 , 32.96 , 34.66 , 37.21 2024-11-06T03:05:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , -0.00 2024-11-06T03:05:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862351.77 epoch sec; seqNo:2456; 4061701.57 north; 594596.43 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:05:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862347.72 2024-11-06T03:05:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:51Z- waiting for 1730862351.72, time is 1730862351.77 2024-11-06T03:05:51Z- LcmTrn::updateTrn() >>>> pose time 1730862351.77, meas time 1730862351.76 <<<< 2024-11-06T03:05:51Z- LcmTrn::updateTrn() equating pose 1730862351.76 and meas times 1730862351.76 2024-11-06T03:05:51Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862351.76 2024-11-06T03:05:51Z- TerrainNav::motionUpdate() - currEstimate = (4061702, 594596.43, 49.63) 2024-11-06T03:05:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:05:51Z- TerrainNav:: SubcloudNIS is 0.26, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:05:51Z- TNavPF::Projecting Measurements 2024-11-06T03:05:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:05:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:05:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:05:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:05:51Z- TNavFilter::4 good beams. 2024-11-06T03:05:51Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:05:51Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:05:51Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:05:51Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:05:51Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:05:51Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:05:51Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:05:51Z- TerrainMapOctree: (vehN, vehE ) = (4061701.57, 594596.43). 2024-11-06T03:05:51Z- TerrainMapOctree: Min Distance = 161.03. 2024-11-06T03:05:51Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:05:51Z- Forcing Subcloud Comparison 2024-11-06T03:05:51Z- Weighting particles with subcloud comparison 2024-11-06T03:05:51Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:51Z- meanExpectedMeasDiff: -0.492482 delta_rms_squared: 0.939657 alpha: 0.979159 2024-11-06T03:05:51Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:51Z- meanExpectedMeasDiff: 0.439506 delta_rms_squared: 0.871008 alpha: 0.977554 2024-11-06T03:05:51Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:51Z- meanExpectedMeasDiff: -1.173633 delta_rms_squared: 0.583417 alpha: 0.966855 2024-11-06T03:05:51Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:51Z- meanExpectedMeasDiff: -0.189621 delta_rms_squared: 0.816854 alpha: 0.976101 2024-11-06T03:05:51Z- Subcloud NIS: 0.246394 2024-11-06T03:05:52Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:05:52Z- TNavPF:: sumWeights = 0.589377 2024-11-06T03:05:52Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:05:52Z- TNavPF::First Term of Map Covariance Matrix is 0.35 2024-11-06T03:05:52Z- TNavPF::Calculated NIS Value : 0.19 numBeams normalized NIS: 0.05 2024-11-06T03:05:52Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862351.76 sec, ping # 0. 2024-11-06T03:05:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:05:52Z- variances([0], [2], and psi_berg) = 0.697604 0.171467 0.000000 2024-11-06T03:05:52Z- publishEstimatesDecoder() - at 1730862351.76 sec 2024-11-06T03:05:52Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:05:52Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061734.43 594580.35 57.96 2024-11-06T03:05:52Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061734.08 594579.96 57.67 lat/lon: 36.70 -121.94 2024-11-06T03:05:52Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.17 0.35 0.00 2024-11-06T03:05:52Z- AHRS_M2 msg: 1730862352.13 epoch sec; seqNo:2305 2024-11-06T03:05:52Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:52Z- handleDepth()- Depth_Keller msg: 1730862352.14 epoch sec; seqNo:2457; depth 49.64 2024-11-06T03:05:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862352.16 epoch sec; seqNo:2457; 4061701.57 north; 594596.43 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:05:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862351.76 2024-11-06T03:05:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:52Z- waiting for 1730862355.76, time is 1730862352.16 2024-11-06T03:05:52Z- AHRS_M2 msg: 1730862352.55 epoch sec; seqNo:2306 2024-11-06T03:05:52Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:52Z- handleDepth()- Depth_Keller msg: 1730862352.55 epoch sec; seqNo:2458; depth 49.66 2024-11-06T03:05:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862352.56 epoch sec; seqNo:2458; 4061701.57 north; 594596.43 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:05:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862351.76 2024-11-06T03:05:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:52Z- waiting for 1730862355.76, time is 1730862352.56 2024-11-06T03:05:52Z- AHRS_M2 msg: 1730862352.93 epoch sec; seqNo:2307 2024-11-06T03:05:52Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:52Z- handleDepth()- Depth_Keller msg: 1730862352.94 epoch sec; seqNo:2459; depth 49.66 2024-11-06T03:05:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862352.95 epoch sec; seqNo:2459; 4061701.58 north; 594597.65 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:05:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862351.76 2024-11-06T03:05:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:53Z- waiting for 1730862355.76, time is 1730862352.95 2024-11-06T03:05:53Z- AHRS_M2 msg: 1730862353.34 epoch sec; seqNo:2308 2024-11-06T03:05:53Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:53Z- handleDepth()- Depth_Keller msg: 1730862353.34 epoch sec; seqNo:2460; depth 49.68 2024-11-06T03:05:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862353.35 epoch sec; seqNo:2460; 4061701.58 north; 594597.65 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:05:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862351.76 2024-11-06T03:05:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:53Z- waiting for 1730862355.76, time is 1730862353.35 2024-11-06T03:05:53Z- AHRS_M2 msg: 1730862353.74 epoch sec; seqNo:2309 2024-11-06T03:05:53Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:53Z- handleDepth()- Depth_Keller msg: 1730862353.75 epoch sec; seqNo:2461; depth 49.71 2024-11-06T03:05:53Z- handleDVL() - DVL msg epochMillisec: 1730862353.78 2024-11-06T03:05:53Z- handleDvl() - RDI_Pathfinder msg: 1730862353.78 sec; seqNo:792; #beams:4 2024-11-06T03:05:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.58 , 32.62 , 34.66 , 37.04 2024-11-06T03:05:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , 0.00 2024-11-06T03:05:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862353.80 epoch sec; seqNo:2461; 4061701.58 north; 594597.65 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:05:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862351.76 2024-11-06T03:05:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:53Z- waiting for 1730862355.76, time is 1730862353.80 2024-11-06T03:05:54Z- AHRS_M2 msg: 1730862354.16 epoch sec; seqNo:2310 2024-11-06T03:05:54Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:54Z- handleDepth()- Depth_Keller msg: 1730862354.17 epoch sec; seqNo:2462; depth 49.72 2024-11-06T03:05:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862354.18 epoch sec; seqNo:2462; 4061701.58 north; 594597.65 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:05:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862351.76 2024-11-06T03:05:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:54Z- waiting for 1730862355.76, time is 1730862354.18 2024-11-06T03:05:54Z- AHRS_M2 msg: 1730862354.56 epoch sec; seqNo:2311 2024-11-06T03:05:54Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:54Z- handleDepth()- Depth_Keller msg: 1730862354.59 epoch sec; seqNo:2463; depth 49.72 2024-11-06T03:05:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862354.60 epoch sec; seqNo:2463; 4061701.60 north; 594598.87 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:05:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862351.76 2024-11-06T03:05:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:54Z- waiting for 1730862355.76, time is 1730862354.60 2024-11-06T03:05:54Z- AHRS_M2 msg: 1730862354.95 epoch sec; seqNo:2312 2024-11-06T03:05:54Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:54Z- handleDepth()- Depth_Keller msg: 1730862354.96 epoch sec; seqNo:2464; depth 49.73 2024-11-06T03:05:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862354.97 epoch sec; seqNo:2464; 4061701.60 north; 594598.87 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:05:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862351.76 2024-11-06T03:05:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:55Z- waiting for 1730862355.76, time is 1730862354.97 2024-11-06T03:05:55Z- AHRS_M2 msg: 1730862355.36 epoch sec; seqNo:2313 2024-11-06T03:05:55Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:55Z- handleDepth()- Depth_Keller msg: 1730862355.37 epoch sec; seqNo:2465; depth 49.75 2024-11-06T03:05:55Z- handleDVL() - DVL msg epochMillisec: 1730862355.37 2024-11-06T03:05:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:05:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:05:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:05:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:05:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:05:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:05:55Z- handleDvl() - RDI_Pathfinder msg: 1730862355.37 sec; seqNo:793; #beams:0 2024-11-06T03:05:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:05:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , 0.00 2024-11-06T03:05:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862355.38 epoch sec; seqNo:2465; 4061701.60 north; 594598.87 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:05:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862351.76 2024-11-06T03:05:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:55Z- waiting for 1730862355.76, time is 1730862355.38 2024-11-06T03:05:55Z- AHRS_M2 msg: 1730862355.76 epoch sec; seqNo:2314 2024-11-06T03:05:55Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:55Z- handleDepth()- Depth_Keller msg: 1730862355.77 epoch sec; seqNo:2466; depth 49.77 2024-11-06T03:05:55Z- handleDVL() - DVL msg epochMillisec: 1730862355.79 2024-11-06T03:05:55Z- handleDvl() - RDI_Pathfinder msg: 1730862355.79 sec; seqNo:794; #beams:4 2024-11-06T03:05:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.07 , 32.96 , 34.49 , 37.04 2024-11-06T03:05:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , -0.01 2024-11-06T03:05:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862355.80 epoch sec; seqNo:2466; 4061701.61 north; 594600.09 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:05:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862351.76 2024-11-06T03:05:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:55Z- waiting for 1730862355.76, time is 1730862355.80 2024-11-06T03:05:55Z- LcmTrn::updateTrn() >>>> pose time 1730862355.80, meas time 1730862355.79 <<<< 2024-11-06T03:05:55Z- LcmTrn::updateTrn() equating pose 1730862355.79 and meas times 1730862355.79 2024-11-06T03:05:55Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862355.79 2024-11-06T03:05:55Z- TerrainNav::motionUpdate() - currEstimate = (4061702, 594600.09, 49.77) 2024-11-06T03:05:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:05:55Z- TerrainNav:: SubcloudNIS is 0.25, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:05:55Z- TNavPF::Projecting Measurements 2024-11-06T03:05:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:05:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:05:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:05:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:05:55Z- TNavFilter::4 good beams. 2024-11-06T03:05:55Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:05:55Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:05:55Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:05:55Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:05:55Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:05:55Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:05:55Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:05:55Z- TerrainMapOctree: (vehN, vehE ) = (4061701.61, 594600.09). 2024-11-06T03:05:55Z- TerrainMapOctree: Min Distance = 157.53. 2024-11-06T03:05:55Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:05:56Z- Forcing Subcloud Comparison 2024-11-06T03:05:56Z- Weighting particles with subcloud comparison 2024-11-06T03:05:56Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:56Z- meanExpectedMeasDiff: -0.337155 delta_rms_squared: 0.963841 alpha: 0.979672 2024-11-06T03:05:56Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:56Z- meanExpectedMeasDiff: 0.915978 delta_rms_squared: 0.601151 alpha: 0.967802 2024-11-06T03:05:56Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:56Z- meanExpectedMeasDiff: -0.673791 delta_rms_squared: 0.765963 alpha: 0.974554 2024-11-06T03:05:56Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:05:56Z- meanExpectedMeasDiff: 0.020032 delta_rms_squared: 0.850007 alpha: 0.977012 2024-11-06T03:05:56Z- Subcloud NIS: 0.231266 2024-11-06T03:05:56Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:05:56Z- TNavPF:: sumWeights = 0.610311 2024-11-06T03:05:56Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:05:56Z- TNavPF::First Term of Map Covariance Matrix is 0.35 2024-11-06T03:05:56Z- TNavPF::Calculated NIS Value : 0.25 numBeams normalized NIS: 0.06 2024-11-06T03:05:56Z- TerrainNav::Resampling particle filter... 2024-11-06T03:05:56Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862355.79 sec, ping # 0. 2024-11-06T03:05:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:05:56Z- variances([0], [2], and psi_berg) = 0.629317 0.172157 0.000000 2024-11-06T03:05:56Z- publishEstimatesDecoder() - at 1730862355.79 sec 2024-11-06T03:05:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:05:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061733.76 594583.86 58.55 2024-11-06T03:05:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061734.20 594583.62 57.83 lat/lon: 36.70 -121.94 2024-11-06T03:05:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.63 0.17 0.34 0.00 2024-11-06T03:05:56Z- AHRS_M2 msg: 1730862356.17 epoch sec; seqNo:2315 2024-11-06T03:05:56Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:56Z- handleDepth()- Depth_Keller msg: 1730862356.17 epoch sec; seqNo:2467; depth 49.78 2024-11-06T03:05:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862356.22 epoch sec; seqNo:2467; 4061701.99 north; 594600.09 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:05:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862355.79 2024-11-06T03:05:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:56Z- waiting for 1730862359.79, time is 1730862356.22 2024-11-06T03:05:56Z- AHRS_M2 msg: 1730862356.57 epoch sec; seqNo:2316 2024-11-06T03:05:56Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:56Z- handleDepth()- Depth_Keller msg: 1730862356.58 epoch sec; seqNo:2468; depth 49.81 2024-11-06T03:05:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862356.58 epoch sec; seqNo:2468; 4061701.99 north; 594600.09 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:05:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862355.79 2024-11-06T03:05:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:56Z- waiting for 1730862359.79, time is 1730862356.58 2024-11-06T03:05:56Z- AHRS_M2 msg: 1730862356.98 epoch sec; seqNo:2317 2024-11-06T03:05:56Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T03:05:56Z- handleDepth()- Depth_Keller msg: 1730862356.98 epoch sec; seqNo:2469; depth 49.81 2024-11-06T03:05:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862356.99 epoch sec; seqNo:2469; 4061701.99 north; 594600.09 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:05:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862355.79 2024-11-06T03:05:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:57Z- waiting for 1730862359.79, time is 1730862356.99 2024-11-06T03:05:57Z- AHRS_M2 msg: 1730862357.38 epoch sec; seqNo:2318 2024-11-06T03:05:57Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T03:05:57Z- handleDepth()- Depth_Keller msg: 1730862357.39 epoch sec; seqNo:2470; depth 49.83 2024-11-06T03:05:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862357.41 epoch sec; seqNo:2470; 4061702.00 north; 594601.31 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:05:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862355.79 2024-11-06T03:05:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:57Z- waiting for 1730862359.79, time is 1730862357.41 2024-11-06T03:05:57Z- AHRS_M2 msg: 1730862357.78 epoch sec; seqNo:2319 2024-11-06T03:05:57Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.52 psi 2024-11-06T03:05:57Z- handleDepth()- Depth_Keller msg: 1730862357.79 epoch sec; seqNo:2471; depth 49.83 2024-11-06T03:05:57Z- handleDVL() - DVL msg epochMillisec: 1730862357.82 2024-11-06T03:05:57Z- handleDvl() - RDI_Pathfinder msg: 1730862357.82 sec; seqNo:795; #beams:4 2024-11-06T03:05:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.07 , 32.79 , 34.15 , 37.04 2024-11-06T03:05:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , -0.01 2024-11-06T03:05:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862357.84 epoch sec; seqNo:2471; 4061702.00 north; 594601.31 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:05:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862355.79 2024-11-06T03:05:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:57Z- waiting for 1730862359.79, time is 1730862357.84 2024-11-06T03:05:58Z- AHRS_M2 msg: 1730862358.19 epoch sec; seqNo:2320 2024-11-06T03:05:58Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:58Z- handleDepth()- Depth_Keller msg: 1730862358.21 epoch sec; seqNo:2472; depth 49.85 2024-11-06T03:05:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862358.23 epoch sec; seqNo:2472; 4061702.00 north; 594601.31 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:05:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862355.79 2024-11-06T03:05:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:58Z- waiting for 1730862359.79, time is 1730862358.23 2024-11-06T03:05:58Z- AHRS_M2 msg: 1730862358.60 epoch sec; seqNo:2321 2024-11-06T03:05:58Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:58Z- handleDepth()- Depth_Keller msg: 1730862358.60 epoch sec; seqNo:2473; depth 49.86 2024-11-06T03:05:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862358.63 epoch sec; seqNo:2473; 4061702.02 north; 594602.53 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:05:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862355.79 2024-11-06T03:05:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:58Z- waiting for 1730862359.79, time is 1730862358.63 2024-11-06T03:05:59Z- AHRS_M2 msg: 1730862359.00 epoch sec; seqNo:2322 2024-11-06T03:05:59Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:59Z- handleDepth()- Depth_Keller msg: 1730862359.00 epoch sec; seqNo:2474; depth 49.88 2024-11-06T03:05:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862359.01 epoch sec; seqNo:2474; 4061702.02 north; 594602.53 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:05:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862355.79 2024-11-06T03:05:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:59Z- waiting for 1730862359.79, time is 1730862359.01 2024-11-06T03:05:59Z- AHRS_M2 msg: 1730862359.40 epoch sec; seqNo:2323 2024-11-06T03:05:59Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:59Z- handleDepth()- Depth_Keller msg: 1730862359.41 epoch sec; seqNo:2475; depth 49.88 2024-11-06T03:05:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862359.42 epoch sec; seqNo:2475; 4061702.02 north; 594602.53 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:05:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:05:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862355.79 2024-11-06T03:05:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:59Z- waiting for 1730862359.79, time is 1730862359.42 2024-11-06T03:05:59Z- AHRS_M2 msg: 1730862359.80 epoch sec; seqNo:2324 2024-11-06T03:05:59Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T03:05:59Z- handleDepth()- Depth_Keller msg: 1730862359.81 epoch sec; seqNo:2476; depth 49.90 2024-11-06T03:05:59Z- handleDVL() - DVL msg epochMillisec: 1730862359.84 2024-11-06T03:05:59Z- handleDvl() - RDI_Pathfinder msg: 1730862359.84 sec; seqNo:796; #beams:4 2024-11-06T03:05:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.41 , 32.28 , 34.32 , 36.87 2024-11-06T03:05:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.01 2024-11-06T03:05:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862359.85 epoch sec; seqNo:2476; 4061702.02 north; 594602.53 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:05:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:05:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862355.79 2024-11-06T03:05:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:05:59Z- waiting for 1730862359.79, time is 1730862359.85 2024-11-06T03:05:59Z- LcmTrn::updateTrn() >>>> pose time 1730862359.85, meas time 1730862359.84 <<<< 2024-11-06T03:05:59Z- LcmTrn::updateTrn() equating pose 1730862359.84 and meas times 1730862359.84 2024-11-06T03:05:59Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862359.84 2024-11-06T03:05:59Z- TerrainNav::motionUpdate() - currEstimate = (4061702, 594602.53, 49.90) 2024-11-06T03:05:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:05:59Z- TerrainNav:: SubcloudNIS is 0.23, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:05:59Z- TNavPF::Projecting Measurements 2024-11-06T03:05:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:05:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:05:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:05:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:05:59Z- TNavFilter::4 good beams. 2024-11-06T03:05:59Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:05:59Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:05:59Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:05:59Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:05:59Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:05:59Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:05:59Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:05:59Z- TerrainMapOctree: (vehN, vehE ) = (4061702.02, 594602.53). 2024-11-06T03:05:59Z- TerrainMapOctree: Min Distance = 155.08. 2024-11-06T03:05:59Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:06:00Z- Forcing Subcloud Comparison 2024-11-06T03:06:00Z- Weighting particles with subcloud comparison 2024-11-06T03:06:00Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:00Z- meanExpectedMeasDiff: -1.112697 delta_rms_squared: 1.506485 alpha: 0.986898 2024-11-06T03:06:00Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:00Z- meanExpectedMeasDiff: -0.361691 delta_rms_squared: 0.687887 alpha: 0.971747 2024-11-06T03:06:00Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:00Z- meanExpectedMeasDiff: -0.793790 delta_rms_squared: 0.765090 alpha: 0.974525 2024-11-06T03:06:00Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:00Z- meanExpectedMeasDiff: 0.096107 delta_rms_squared: 0.840072 alpha: 0.976746 2024-11-06T03:06:00Z- Subcloud NIS: 0.219048 2024-11-06T03:06:00Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:06:00Z- TNavPF:: sumWeights = 0.583359 2024-11-06T03:06:00Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:06:00Z- TNavPF::First Term of Map Covariance Matrix is 0.59 2024-11-06T03:06:00Z- TNavPF::Calculated NIS Value : 0.14 numBeams normalized NIS: 0.03 2024-11-06T03:06:00Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862359.84 sec, ping # 0. 2024-11-06T03:06:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:06:00Z- variances([0], [2], and psi_berg) = 0.655347 0.172755 0.000000 2024-11-06T03:06:00Z- publishEstimatesDecoder() - at 1730862359.84 sec 2024-11-06T03:06:00Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:06:00Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061733.02 594587.42 58.08 2024-11-06T03:06:00Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061734.61 594586.07 58.18 lat/lon: 36.70 -121.94 2024-11-06T03:06:00Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.66 0.17 0.32 0.00 2024-11-06T03:06:00Z- AHRS_M2 msg: 1730862360.21 epoch sec; seqNo:2325 2024-11-06T03:06:00Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:06:00Z- handleDepth()- Depth_Keller msg: 1730862360.22 epoch sec; seqNo:2477; depth 49.92 2024-11-06T03:06:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862360.23 epoch sec; seqNo:2477; 4061702.03 north; 594603.75 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:06:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862359.84 2024-11-06T03:06:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:00Z- waiting for 1730862363.84, time is 1730862360.23 2024-11-06T03:06:00Z- AHRS_M2 msg: 1730862360.61 epoch sec; seqNo:2326 2024-11-06T03:06:00Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:06:00Z- handleDepth()- Depth_Keller msg: 1730862360.63 epoch sec; seqNo:2478; depth 49.92 2024-11-06T03:06:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862360.65 epoch sec; seqNo:2478; 4061702.03 north; 594603.75 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:06:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862359.84 2024-11-06T03:06:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:00Z- waiting for 1730862363.84, time is 1730862360.65 2024-11-06T03:06:01Z- AHRS_M2 msg: 1730862361.01 epoch sec; seqNo:2327 2024-11-06T03:06:01Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:06:01Z- handleDepth()- Depth_Keller msg: 1730862361.03 epoch sec; seqNo:2479; depth 49.95 2024-11-06T03:06:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862361.04 epoch sec; seqNo:2479; 4061702.03 north; 594603.75 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:06:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862359.84 2024-11-06T03:06:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:01Z- waiting for 1730862363.84, time is 1730862361.04 2024-11-06T03:06:01Z- AHRS_M2 msg: 1730862361.42 epoch sec; seqNo:2328 2024-11-06T03:06:01Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:06:01Z- handleDepth()- Depth_Keller msg: 1730862361.43 epoch sec; seqNo:2480; depth 49.94 2024-11-06T03:06:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862361.45 epoch sec; seqNo:2480; 4061702.04 north; 594604.97 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:06:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862359.84 2024-11-06T03:06:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:01Z- waiting for 1730862363.84, time is 1730862361.45 2024-11-06T03:06:01Z- AHRS_M2 msg: 1730862361.82 epoch sec; seqNo:2329 2024-11-06T03:06:01Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:06:01Z- handleDepth()- Depth_Keller msg: 1730862361.83 epoch sec; seqNo:2481; depth 49.96 2024-11-06T03:06:01Z- handleDVL() - DVL msg epochMillisec: 1730862361.85 2024-11-06T03:06:01Z- handleDvl() - RDI_Pathfinder msg: 1730862361.85 sec; seqNo:797; #beams:4 2024-11-06T03:06:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.24 , 32.11 , 34.32 , 36.70 2024-11-06T03:06:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , -0.01 2024-11-06T03:06:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862361.87 epoch sec; seqNo:2481; 4061702.42 north; 594604.96 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:06:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862359.84 2024-11-06T03:06:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:01Z- waiting for 1730862363.84, time is 1730862361.87 2024-11-06T03:06:02Z- AHRS_M2 msg: 1730862362.23 epoch sec; seqNo:2330 2024-11-06T03:06:02Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:06:02Z- handleDepth()- Depth_Keller msg: 1730862362.24 epoch sec; seqNo:2482; depth 49.97 2024-11-06T03:06:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862362.26 epoch sec; seqNo:2482; 4061702.42 north; 594604.96 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:06:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862359.84 2024-11-06T03:06:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:02Z- waiting for 1730862363.84, time is 1730862362.26 2024-11-06T03:06:02Z- AHRS_M2 msg: 1730862362.65 epoch sec; seqNo:2331 2024-11-06T03:06:02Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:06:02Z- handleDepth()- Depth_Keller msg: 1730862362.67 epoch sec; seqNo:2483; depth 49.99 2024-11-06T03:06:02Z- handleDVL() - DVL msg epochMillisec: 1730862362.67 2024-11-06T03:06:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:06:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:06:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:06:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:06:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:06:02Z- handleDvl() - RDI_Pathfinder msg: 1730862362.67 sec; seqNo:798; #beams:0 2024-11-06T03:06:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:06:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , -0.01 2024-11-06T03:06:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862362.68 epoch sec; seqNo:2483; 4061702.42 north; 594604.96 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:06:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862359.84 2024-11-06T03:06:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:02Z- waiting for 1730862363.84, time is 1730862362.68 2024-11-06T03:06:03Z- AHRS_M2 msg: 1730862363.04 epoch sec; seqNo:2332 2024-11-06T03:06:03Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T03:06:03Z- handleDepth()- Depth_Keller msg: 1730862363.04 epoch sec; seqNo:2484; depth 50.00 2024-11-06T03:06:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862363.05 epoch sec; seqNo:2484; 4061702.44 north; 594606.18 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:06:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862359.84 2024-11-06T03:06:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:03Z- waiting for 1730862363.84, time is 1730862363.05 2024-11-06T03:06:03Z- AHRS_M2 msg: 1730862363.44 epoch sec; seqNo:2333 2024-11-06T03:06:03Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T03:06:03Z- handleDepth()- Depth_Keller msg: 1730862363.45 epoch sec; seqNo:2485; depth 50.02 2024-11-06T03:06:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862363.45 epoch sec; seqNo:2485; 4061702.44 north; 594606.18 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:06:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862359.84 2024-11-06T03:06:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:03Z- waiting for 1730862363.84, time is 1730862363.45 2024-11-06T03:06:03Z- AHRS_M2 msg: 1730862363.84 epoch sec; seqNo:2334 2024-11-06T03:06:03Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T03:06:03Z- handleDepth()- Depth_Keller msg: 1730862363.86 epoch sec; seqNo:2486; depth 50.03 2024-11-06T03:06:03Z- handleDVL() - DVL msg epochMillisec: 1730862363.88 2024-11-06T03:06:03Z- handleDvl() - RDI_Pathfinder msg: 1730862363.88 sec; seqNo:799; #beams:4 2024-11-06T03:06:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.90 , 31.94 , 33.98 , 36.53 2024-11-06T03:06:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.00 2024-11-06T03:06:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862363.89 epoch sec; seqNo:2486; 4061702.44 north; 594606.18 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:06:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862359.84 2024-11-06T03:06:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:04Z- waiting for 1730862363.84, time is 1730862363.89 2024-11-06T03:06:04Z- LcmTrn::updateTrn() >>>> pose time 1730862363.89, meas time 1730862363.88 <<<< 2024-11-06T03:06:04Z- LcmTrn::updateTrn() equating pose 1730862363.88 and meas times 1730862363.88 2024-11-06T03:06:04Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862363.88 2024-11-06T03:06:04Z- TerrainNav::motionUpdate() - currEstimate = (4061702, 594606.18, 50.03) 2024-11-06T03:06:04Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:06:04Z- TerrainNav:: SubcloudNIS is 0.22, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:06:04Z- TNavPF::Projecting Measurements 2024-11-06T03:06:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:06:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:06:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:06:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:06:04Z- TNavFilter::4 good beams. 2024-11-06T03:06:04Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:06:04Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:06:04Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:06:04Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:06:04Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:06:04Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:06:04Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:06:04Z- TerrainMapOctree: (vehN, vehE ) = (4061702.44, 594606.18). 2024-11-06T03:06:04Z- TerrainMapOctree: Min Distance = 151.48. 2024-11-06T03:06:04Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:06:04Z- Forcing Subcloud Comparison 2024-11-06T03:06:04Z- Weighting particles with subcloud comparison 2024-11-06T03:06:04Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:04Z- meanExpectedMeasDiff: -1.027473 delta_rms_squared: 1.275267 alpha: 0.984559 2024-11-06T03:06:04Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:04Z- meanExpectedMeasDiff: -0.639281 delta_rms_squared: 0.650870 alpha: 0.970188 2024-11-06T03:06:04Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:04Z- meanExpectedMeasDiff: -0.830546 delta_rms_squared: 0.744703 alpha: 0.973846 2024-11-06T03:06:04Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:04Z- meanExpectedMeasDiff: 0.279722 delta_rms_squared: 0.808335 alpha: 0.975855 2024-11-06T03:06:04Z- Subcloud NIS: 0.213138 2024-11-06T03:06:04Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:06:04Z- TNavPF:: sumWeights = 0.580676 2024-11-06T03:06:04Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:06:04Z- TNavPF::First Term of Map Covariance Matrix is 0.50 2024-11-06T03:06:04Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.04 2024-11-06T03:06:04Z- TerrainNav::Resampling particle filter... 2024-11-06T03:06:04Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862363.88 sec, ping # 0. 2024-11-06T03:06:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:06:04Z- variances([0], [2], and psi_berg) = 0.631250 0.178217 0.000000 2024-11-06T03:06:04Z- publishEstimatesDecoder() - at 1730862363.88 sec 2024-11-06T03:06:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:06:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061734.59 594589.86 59.67 2024-11-06T03:06:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061735.04 594589.71 58.52 lat/lon: 36.70 -121.94 2024-11-06T03:06:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.63 0.18 0.31 0.00 2024-11-06T03:06:04Z- AHRS_M2 msg: 1730862364.25 epoch sec; seqNo:2335 2024-11-06T03:06:04Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.51 psi 2024-11-06T03:06:04Z- handleDepth()- Depth_Keller msg: 1730862364.25 epoch sec; seqNo:2487; depth 50.03 2024-11-06T03:06:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862364.29 epoch sec; seqNo:2487; 4061702.45 north; 594607.40 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:06:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862363.88 2024-11-06T03:06:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:04Z- waiting for 1730862367.88, time is 1730862364.29 2024-11-06T03:06:04Z- AHRS_M2 msg: 1730862364.66 epoch sec; seqNo:2336 2024-11-06T03:06:04Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T03:06:04Z- handleDepth()- Depth_Keller msg: 1730862364.66 epoch sec; seqNo:2488; depth 50.05 2024-11-06T03:06:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862364.67 epoch sec; seqNo:2488; 4061702.45 north; 594607.40 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:06:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862363.88 2024-11-06T03:06:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:04Z- waiting for 1730862367.88, time is 1730862364.67 2024-11-06T03:06:05Z- AHRS_M2 msg: 1730862365.05 epoch sec; seqNo:2337 2024-11-06T03:06:05Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T03:06:05Z- handleDepth()- Depth_Keller msg: 1730862365.07 epoch sec; seqNo:2489; depth 50.05 2024-11-06T03:06:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862365.08 epoch sec; seqNo:2489; 4061702.45 north; 594607.40 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:06:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862363.88 2024-11-06T03:06:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:05Z- waiting for 1730862367.88, time is 1730862365.08 2024-11-06T03:06:05Z- AHRS_M2 msg: 1730862365.46 epoch sec; seqNo:2338 2024-11-06T03:06:05Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T03:06:05Z- handleDepth()- Depth_Keller msg: 1730862365.46 epoch sec; seqNo:2490; depth 50.06 2024-11-06T03:06:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862365.48 epoch sec; seqNo:2490; 4061702.83 north; 594607.40 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:06:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862363.88 2024-11-06T03:06:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:05Z- waiting for 1730862367.88, time is 1730862365.48 2024-11-06T03:06:05Z- AHRS_M2 msg: 1730862365.86 epoch sec; seqNo:2339 2024-11-06T03:06:05Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T03:06:05Z- handleDepth()- Depth_Keller msg: 1730862365.87 epoch sec; seqNo:2491; depth 50.06 2024-11-06T03:06:05Z- handleDVL() - DVL msg epochMillisec: 1730862365.90 2024-11-06T03:06:05Z- handleDvl() - RDI_Pathfinder msg: 1730862365.90 sec; seqNo:800; #beams:4 2024-11-06T03:06:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.90 , 31.60 , 34.15 , 36.36 2024-11-06T03:06:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.01 2024-11-06T03:06:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862365.91 epoch sec; seqNo:2491; 4061702.84 north; 594608.62 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:06:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862363.88 2024-11-06T03:06:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:06Z- waiting for 1730862367.88, time is 1730862365.91 2024-11-06T03:06:06Z- AHRS_M2 msg: 1730862366.27 epoch sec; seqNo:2340 2024-11-06T03:06:06Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T03:06:06Z- handleDepth()- Depth_Keller msg: 1730862366.28 epoch sec; seqNo:2492; depth 50.08 2024-11-06T03:06:06Z- handleDVL() - DVL msg epochMillisec: 1730862366.28 2024-11-06T03:06:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:06:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:06:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:06:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:06:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:06:06Z- handleDvl() - RDI_Pathfinder msg: 1730862366.28 sec; seqNo:801; #beams:0 2024-11-06T03:06:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:06:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.01 2024-11-06T03:06:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862366.30 epoch sec; seqNo:2492; 4061702.84 north; 594608.62 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:06:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862363.88 2024-11-06T03:06:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:06Z- waiting for 1730862367.88, time is 1730862366.30 2024-11-06T03:06:06Z- AHRS_M2 msg: 1730862366.68 epoch sec; seqNo:2341 2024-11-06T03:06:06Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T03:06:06Z- handleDepth()- Depth_Keller msg: 1730862366.69 epoch sec; seqNo:2493; depth 50.09 2024-11-06T03:06:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862366.70 epoch sec; seqNo:2493; 4061702.84 north; 594608.62 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:06:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862363.88 2024-11-06T03:06:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:06Z- waiting for 1730862367.88, time is 1730862366.70 2024-11-06T03:06:07Z- AHRS_M2 msg: 1730862367.07 epoch sec; seqNo:2342 2024-11-06T03:06:07Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T03:06:07Z- handleDepth()- Depth_Keller msg: 1730862367.08 epoch sec; seqNo:2494; depth 50.08 2024-11-06T03:06:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862367.10 epoch sec; seqNo:2494; 4061702.86 north; 594609.84 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:06:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862363.88 2024-11-06T03:06:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:07Z- waiting for 1730862367.88, time is 1730862367.10 2024-11-06T03:06:07Z- AHRS_M2 msg: 1730862367.48 epoch sec; seqNo:2343 2024-11-06T03:06:07Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T03:06:07Z- handleDepth()- Depth_Keller msg: 1730862367.50 epoch sec; seqNo:2495; depth 50.08 2024-11-06T03:06:07Z- handleDVL() - DVL msg epochMillisec: 1730862367.53 2024-11-06T03:06:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:06:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:06:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:06:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:06:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:06:07Z- handleDvl() - RDI_Pathfinder msg: 1730862367.53 sec; seqNo:802; #beams:0 2024-11-06T03:06:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:06:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.01 2024-11-06T03:06:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862367.56 epoch sec; seqNo:2495; 4061702.86 north; 594609.84 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:06:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862363.88 2024-11-06T03:06:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:07Z- waiting for 1730862367.88, time is 1730862367.56 2024-11-06T03:06:07Z- AHRS_M2 msg: 1730862367.88 epoch sec; seqNo:2344 2024-11-06T03:06:07Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T03:06:07Z- handleDepth()- Depth_Keller msg: 1730862367.89 epoch sec; seqNo:2496; depth 50.10 2024-11-06T03:06:07Z- handleDVL() - DVL msg epochMillisec: 1730862367.91 2024-11-06T03:06:07Z- handleDvl() - RDI_Pathfinder msg: 1730862367.91 sec; seqNo:803; #beams:4 2024-11-06T03:06:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.56 , 31.09 , 33.81 , 36.19 2024-11-06T03:06:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , 0.01 2024-11-06T03:06:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862367.92 epoch sec; seqNo:2496; 4061702.86 north; 594609.84 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:06:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862363.88 2024-11-06T03:06:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:08Z- waiting for 1730862367.88, time is 1730862367.92 2024-11-06T03:06:08Z- LcmTrn::updateTrn() >>>> pose time 1730862367.92, meas time 1730862367.91 <<<< 2024-11-06T03:06:08Z- LcmTrn::updateTrn() equating pose 1730862367.91 and meas times 1730862367.91 2024-11-06T03:06:08Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862367.91 2024-11-06T03:06:08Z- TerrainNav::motionUpdate() - currEstimate = (4061703, 594609.84, 50.10) 2024-11-06T03:06:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:06:08Z- TerrainNav:: SubcloudNIS is 0.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:06:08Z- TNavPF::Projecting Measurements 2024-11-06T03:06:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:06:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:06:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:06:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:06:08Z- TNavFilter::4 good beams. 2024-11-06T03:06:08Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:06:08Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:06:08Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:06:08Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:06:08Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:06:08Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:06:08Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:06:08Z- TerrainMapOctree: (vehN, vehE ) = (4061702.86, 594609.84). 2024-11-06T03:06:08Z- TerrainMapOctree: Min Distance = 147.88. 2024-11-06T03:06:08Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:06:08Z- Forcing Subcloud Comparison 2024-11-06T03:06:08Z- Weighting particles with subcloud comparison 2024-11-06T03:06:08Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:08Z- meanExpectedMeasDiff: -1.342906 delta_rms_squared: 0.932347 alpha: 0.978999 2024-11-06T03:06:08Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:08Z- meanExpectedMeasDiff: -1.200062 delta_rms_squared: 0.648609 alpha: 0.970087 2024-11-06T03:06:08Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:08Z- meanExpectedMeasDiff: -0.759077 delta_rms_squared: 0.744771 alpha: 0.973848 2024-11-06T03:06:08Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:08Z- meanExpectedMeasDiff: 0.382804 delta_rms_squared: 0.799718 alpha: 0.975601 2024-11-06T03:06:08Z- Subcloud NIS: 0.214190 2024-11-06T03:06:08Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:06:08Z- TNavPF:: sumWeights = 0.497852 2024-11-06T03:06:08Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:06:08Z- TNavPF::First Term of Map Covariance Matrix is 0.38 2024-11-06T03:06:08Z- TNavPF::Calculated NIS Value : 0.25 numBeams normalized NIS: 0.06 2024-11-06T03:06:08Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862367.91 sec, ping # 0. 2024-11-06T03:06:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:06:08Z- variances([0], [2], and psi_berg) = 0.604650 0.178068 0.000000 2024-11-06T03:06:08Z- publishEstimatesDecoder() - at 1730862367.91 sec 2024-11-06T03:06:08Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:06:08Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061734.90 594593.20 59.22 2024-11-06T03:06:08Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061735.49 594593.37 58.87 lat/lon: 36.70 -121.94 2024-11-06T03:06:08Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.60 0.18 0.31 0.00 2024-11-06T03:06:08Z- AHRS_M2 msg: 1730862368.29 epoch sec; seqNo:2345 2024-11-06T03:06:08Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T03:06:08Z- handleDepth()- Depth_Keller msg: 1730862368.30 epoch sec; seqNo:2497; depth 50.09 2024-11-06T03:06:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862368.32 epoch sec; seqNo:2497; 4061702.87 north; 594611.06 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:06:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862367.91 2024-11-06T03:06:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:08Z- waiting for 1730862371.91, time is 1730862368.32 2024-11-06T03:06:08Z- AHRS_M2 msg: 1730862368.70 epoch sec; seqNo:2346 2024-11-06T03:06:08Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T03:06:08Z- handleDepth()- Depth_Keller msg: 1730862368.70 epoch sec; seqNo:2498; depth 50.09 2024-11-06T03:06:08Z- handleDVL() - DVL msg epochMillisec: 1730862368.70 2024-11-06T03:06:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:06:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:06:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:06:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:06:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:06:08Z- handleDvl() - RDI_Pathfinder msg: 1730862368.70 sec; seqNo:804; #beams:0 2024-11-06T03:06:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:06:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , 0.01 2024-11-06T03:06:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862368.71 epoch sec; seqNo:2498; 4061702.87 north; 594611.06 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:06:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862367.91 2024-11-06T03:06:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:08Z- waiting for 1730862371.91, time is 1730862368.71 2024-11-06T03:06:09Z- AHRS_M2 msg: 1730862369.10 epoch sec; seqNo:2347 2024-11-06T03:06:09Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T03:06:09Z- handleDepth()- Depth_Keller msg: 1730862369.10 epoch sec; seqNo:2499; depth 50.11 2024-11-06T03:06:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862369.11 epoch sec; seqNo:2499; 4061702.87 north; 594611.06 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:06:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862367.91 2024-11-06T03:06:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:09Z- waiting for 1730862371.91, time is 1730862369.11 2024-11-06T03:06:09Z- AHRS_M2 msg: 1730862369.54 epoch sec; seqNo:2348 2024-11-06T03:06:09Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T03:06:09Z- handleDepth()- Depth_Keller msg: 1730862369.55 epoch sec; seqNo:2500; depth 50.10 2024-11-06T03:06:09Z- handleDVL() - DVL msg epochMillisec: 1730862369.58 2024-11-06T03:06:09Z- handleDvl() - RDI_Pathfinder msg: 1730862369.58 sec; seqNo:805; #beams:4 2024-11-06T03:06:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.56 , 30.92 , 33.97 , 36.18 2024-11-06T03:06:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , -0.00 2024-11-06T03:06:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862369.59 epoch sec; seqNo:2500; 4061703.25 north; 594611.06 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:06:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862367.91 2024-11-06T03:06:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:09Z- waiting for 1730862371.91, time is 1730862369.59 2024-11-06T03:06:09Z- AHRS_M2 msg: 1730862369.90 epoch sec; seqNo:2349 2024-11-06T03:06:09Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T03:06:09Z- handleDepth()- Depth_Keller msg: 1730862369.91 epoch sec; seqNo:2501; depth 50.09 2024-11-06T03:06:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862369.94 epoch sec; seqNo:2501; 4061703.26 north; 594612.28 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:06:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862367.91 2024-11-06T03:06:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:10Z- waiting for 1730862371.91, time is 1730862369.94 2024-11-06T03:06:10Z- AHRS_M2 msg: 1730862370.31 epoch sec; seqNo:2350 2024-11-06T03:06:10Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.51 psi 2024-11-06T03:06:10Z- handleDepth()- Depth_Keller msg: 1730862370.32 epoch sec; seqNo:2502; depth 50.10 2024-11-06T03:06:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862370.33 epoch sec; seqNo:2502; 4061703.26 north; 594612.28 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:06:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862367.91 2024-11-06T03:06:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:10Z- waiting for 1730862371.91, time is 1730862370.33 2024-11-06T03:06:10Z- AHRS_M2 msg: 1730862370.71 epoch sec; seqNo:2351 2024-11-06T03:06:10Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.51 psi 2024-11-06T03:06:10Z- handleDepth()- Depth_Keller msg: 1730862370.72 epoch sec; seqNo:2503; depth 50.11 2024-11-06T03:06:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862370.73 epoch sec; seqNo:2503; 4061703.26 north; 594612.28 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:06:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862367.91 2024-11-06T03:06:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:10Z- waiting for 1730862371.91, time is 1730862370.73 2024-11-06T03:06:11Z- AHRS_M2 msg: 1730862371.12 epoch sec; seqNo:2352 2024-11-06T03:06:11Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.51 psi 2024-11-06T03:06:11Z- handleDepth()- Depth_Keller msg: 1730862371.12 epoch sec; seqNo:2504; depth 50.10 2024-11-06T03:06:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862371.14 epoch sec; seqNo:2504; 4061703.27 north; 594613.50 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:06:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862367.91 2024-11-06T03:06:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:11Z- waiting for 1730862371.91, time is 1730862371.14 2024-11-06T03:06:11Z- AHRS_M2 msg: 1730862371.56 epoch sec; seqNo:2353 2024-11-06T03:06:11Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.51 psi 2024-11-06T03:06:11Z- handleDepth()- Depth_Keller msg: 1730862371.56 epoch sec; seqNo:2505; depth 50.11 2024-11-06T03:06:11Z- handleDVL() - DVL msg epochMillisec: 1730862371.58 2024-11-06T03:06:11Z- handleDvl() - RDI_Pathfinder msg: 1730862371.58 sec; seqNo:806; #beams:4 2024-11-06T03:06:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.40 , 30.58 , 33.98 , 36.19 2024-11-06T03:06:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , -0.02 2024-11-06T03:06:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862371.62 epoch sec; seqNo:2505; 4061703.27 north; 594613.50 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:06:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862367.91 2024-11-06T03:06:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:11Z- waiting for 1730862371.91, time is 1730862371.62 2024-11-06T03:06:11Z- AHRS_M2 msg: 1730862371.92 epoch sec; seqNo:2354 2024-11-06T03:06:11Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.51 psi 2024-11-06T03:06:11Z- handleDepth()- Depth_Keller msg: 1730862371.93 epoch sec; seqNo:2506; depth 50.11 2024-11-06T03:06:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862371.94 epoch sec; seqNo:2506; 4061703.27 north; 594613.50 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:06:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862367.91 2024-11-06T03:06:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:12Z- waiting for 1730862371.91, time is 1730862371.94 2024-11-06T03:06:12Z- LcmTrn::updateTrn() >>>> pose time 1730862371.94, meas time 1730862371.58 <<<< 2024-11-06T03:06:12Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862371.94 2024-11-06T03:06:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862371.58 sec, ping # 0; waiting for INS data more recent than 1730862367.91... 2024-11-06T03:06:12Z- TerrainNav::motionUpdate() - currEstimate = (4061703, 594613.50, 50.11) 2024-11-06T03:06:12Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:06:12Z- TerrainNav:: SubcloudNIS is 0.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:06:12Z- TNavPF::Projecting Measurements 2024-11-06T03:06:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:06:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:06:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:06:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:06:12Z- TNavFilter::4 good beams. 2024-11-06T03:06:12Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:06:12Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:06:12Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:06:12Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:06:12Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:06:12Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:06:12Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:06:12Z- TerrainMapOctree: (vehN, vehE ) = (4061703.24, 594613.17). 2024-11-06T03:06:12Z- TerrainMapOctree: Min Distance = 144.60. 2024-11-06T03:06:12Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:06:12Z- Forcing Subcloud Comparison 2024-11-06T03:06:12Z- Weighting particles with subcloud comparison 2024-11-06T03:06:12Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:12Z- meanExpectedMeasDiff: -0.946207 delta_rms_squared: 1.600755 alpha: 0.987660 2024-11-06T03:06:12Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:12Z- meanExpectedMeasDiff: -1.362442 delta_rms_squared: 0.646732 alpha: 0.970003 2024-11-06T03:06:12Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:12Z- meanExpectedMeasDiff: -0.203483 delta_rms_squared: 0.742027 alpha: 0.973754 2024-11-06T03:06:12Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:12Z- meanExpectedMeasDiff: 0.641746 delta_rms_squared: 0.803187 alpha: 0.975704 2024-11-06T03:06:12Z- Subcloud NIS: 0.212195 2024-11-06T03:06:12Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:06:12Z- TNavPF:: sumWeights = 0.472721 2024-11-06T03:06:12Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:06:12Z- TNavPF::First Term of Map Covariance Matrix is 0.51 2024-11-06T03:06:12Z- TNavPF::Calculated NIS Value : 0.26 numBeams normalized NIS: 0.07 2024-11-06T03:06:12Z- TerrainNav::Resampling particle filter... 2024-11-06T03:06:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862371.58 sec. 2024-11-06T03:06:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:06:12Z- variances([0], [2], and psi_berg) = 0.628687 0.180275 0.000000 2024-11-06T03:06:12Z- publishEstimatesDecoder() - at 1730862371.94 sec 2024-11-06T03:06:12Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:06:12Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061735.42 594597.01 59.15 2024-11-06T03:06:12Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061735.90 594597.03 59.07 lat/lon: 36.70 -121.94 2024-11-06T03:06:12Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.63 0.18 0.55 0.00 2024-11-06T03:06:12Z- handleDepth()- Depth_Keller msg: 1730862372.35 epoch sec; seqNo:2507; depth 50.11 2024-11-06T03:06:12Z- handleDVL() - DVL msg epochMillisec: 1730862372.35 2024-11-06T03:06:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:06:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:06:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:06:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:06:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:06:12Z- handleDvl() - RDI_Pathfinder msg: 1730862372.35 sec; seqNo:807; #beams:0 2024-11-06T03:06:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:06:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , -0.02 2024-11-06T03:06:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862372.36 epoch sec; seqNo:2507; 4061703.27 north; 594613.50 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:06:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862371.94 2024-11-06T03:06:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:12Z- waiting for 1730862375.94, time is 1730862372.36 2024-11-06T03:06:12Z- AHRS_M2 msg: 1730862372.73 epoch sec; seqNo:2355 2024-11-06T03:06:12Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.51 psi 2024-11-06T03:06:12Z- handleDepth()- Depth_Keller msg: 1730862372.74 epoch sec; seqNo:2508; depth 50.11 2024-11-06T03:06:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862372.77 epoch sec; seqNo:2508; 4061703.29 north; 594614.72 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:06:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862371.94 2024-11-06T03:06:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:12Z- waiting for 1730862375.94, time is 1730862372.77 2024-11-06T03:06:13Z- AHRS_M2 msg: 1730862373.13 epoch sec; seqNo:2356 2024-11-06T03:06:13Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.51 psi 2024-11-06T03:06:13Z- handleDepth()- Depth_Keller msg: 1730862373.14 epoch sec; seqNo:2509; depth 50.11 2024-11-06T03:06:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862373.15 epoch sec; seqNo:2509; 4061703.29 north; 594614.72 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:06:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862371.94 2024-11-06T03:06:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:13Z- waiting for 1730862375.94, time is 1730862373.15 2024-11-06T03:06:13Z- AHRS_M2 msg: 1730862373.56 epoch sec; seqNo:2357 2024-11-06T03:06:13Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.51 psi 2024-11-06T03:06:13Z- handleDepth()- Depth_Keller msg: 1730862373.57 epoch sec; seqNo:2510; depth 50.11 2024-11-06T03:06:13Z- handleDVL() - DVL msg epochMillisec: 1730862373.60 2024-11-06T03:06:13Z- handleDvl() - RDI_Pathfinder msg: 1730862373.60 sec; seqNo:808; #beams:4 2024-11-06T03:06:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.56 , 30.58 , 33.64 , 36.53 2024-11-06T03:06:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , -0.01 2024-11-06T03:06:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862373.61 epoch sec; seqNo:2510; 4061703.29 north; 594614.72 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:06:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862371.94 2024-11-06T03:06:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:13Z- waiting for 1730862375.94, time is 1730862373.61 2024-11-06T03:06:13Z- AHRS_M2 msg: 1730862373.94 epoch sec; seqNo:2358 2024-11-06T03:06:13Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T03:06:13Z- handleDepth()- Depth_Keller msg: 1730862373.95 epoch sec; seqNo:2511; depth 50.13 2024-11-06T03:06:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862373.97 epoch sec; seqNo:2511; 4061703.30 north; 594615.94 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:06:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862371.94 2024-11-06T03:06:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:14Z- waiting for 1730862375.94, time is 1730862373.97 2024-11-06T03:06:14Z- AHRS_M2 msg: 1730862374.35 epoch sec; seqNo:2359 2024-11-06T03:06:14Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T03:06:14Z- handleDepth()- Depth_Keller msg: 1730862374.35 epoch sec; seqNo:2512; depth 50.14 2024-11-06T03:06:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862374.36 epoch sec; seqNo:2512; 4061703.68 north; 594615.93 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:06:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862371.94 2024-11-06T03:06:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:14Z- waiting for 1730862375.94, time is 1730862374.36 2024-11-06T03:06:14Z- AHRS_M2 msg: 1730862374.75 epoch sec; seqNo:2360 2024-11-06T03:06:14Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T03:06:14Z- handleDepth()- Depth_Keller msg: 1730862374.76 epoch sec; seqNo:2513; depth 50.14 2024-11-06T03:06:14Z- handleDVL() - DVL msg epochMillisec: 1730862374.76 2024-11-06T03:06:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:06:14Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:06:14Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:06:14Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:06:14Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:06:14Z- handleDvl() - RDI_Pathfinder msg: 1730862374.76 sec; seqNo:809; #beams:0 2024-11-06T03:06:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:06:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , -0.01 2024-11-06T03:06:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862374.77 epoch sec; seqNo:2513; 4061703.68 north; 594615.93 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:06:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862371.94 2024-11-06T03:06:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:14Z- waiting for 1730862375.94, time is 1730862374.77 2024-11-06T03:06:15Z- AHRS_M2 msg: 1730862375.16 epoch sec; seqNo:2361 2024-11-06T03:06:15Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T03:06:15Z- handleDepth()- Depth_Keller msg: 1730862375.16 epoch sec; seqNo:2514; depth 50.15 2024-11-06T03:06:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862375.18 epoch sec; seqNo:2514; 4061703.68 north; 594615.93 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:06:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862371.94 2024-11-06T03:06:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:15Z- waiting for 1730862375.94, time is 1730862375.18 2024-11-06T03:06:15Z- AHRS_M2 msg: 1730862375.58 epoch sec; seqNo:2362 2024-11-06T03:06:15Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T03:06:15Z- handleDepth()- Depth_Keller msg: 1730862375.58 epoch sec; seqNo:2515; depth 50.14 2024-11-06T03:06:15Z- handleDVL() - DVL msg epochMillisec: 1730862375.60 2024-11-06T03:06:15Z- handleDvl() - RDI_Pathfinder msg: 1730862375.60 sec; seqNo:810; #beams:4 2024-11-06T03:06:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.40 , 30.58 , 33.47 , 36.36 2024-11-06T03:06:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.01 2024-11-06T03:06:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862375.62 epoch sec; seqNo:2515; 4061703.69 north; 594617.15 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:06:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862371.94 2024-11-06T03:06:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:15Z- waiting for 1730862375.94, time is 1730862375.62 2024-11-06T03:06:15Z- AHRS_M2 msg: 1730862375.97 epoch sec; seqNo:2363 2024-11-06T03:06:15Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T03:06:15Z- handleDepth()- Depth_Keller msg: 1730862375.97 epoch sec; seqNo:2516; depth 50.17 2024-11-06T03:06:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862375.98 epoch sec; seqNo:2516; 4061703.69 north; 594617.15 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:06:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862371.94 2024-11-06T03:06:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:16Z- waiting for 1730862375.94, time is 1730862375.98 2024-11-06T03:06:16Z- LcmTrn::updateTrn() >>>> pose time 1730862375.98, meas time 1730862375.60 <<<< 2024-11-06T03:06:16Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862375.98 2024-11-06T03:06:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862375.60 sec, ping # 0; waiting for INS data more recent than 1730862371.94... 2024-11-06T03:06:16Z- TerrainNav::motionUpdate() - currEstimate = (4061704, 594617.15, 50.17) 2024-11-06T03:06:16Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:06:16Z- TerrainNav:: SubcloudNIS is 0.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:06:16Z- TNavPF::Projecting Measurements 2024-11-06T03:06:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:06:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:06:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:06:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:06:16Z- TNavFilter::4 good beams. 2024-11-06T03:06:16Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:06:16Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:06:16Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:06:16Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:06:16Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:06:16Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:06:16Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:06:16Z- TerrainMapOctree: (vehN, vehE ) = (4061703.65, 594616.81). 2024-11-06T03:06:16Z- TerrainMapOctree: Min Distance = 141.02. 2024-11-06T03:06:16Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:06:16Z- Forcing Subcloud Comparison 2024-11-06T03:06:16Z- Weighting particles with subcloud comparison 2024-11-06T03:06:16Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:16Z- meanExpectedMeasDiff: 0.044722 delta_rms_squared: 1.823753 alpha: 0.989153 2024-11-06T03:06:16Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:16Z- meanExpectedMeasDiff: -1.163030 delta_rms_squared: 0.915578 alpha: 0.978623 2024-11-06T03:06:16Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:16Z- meanExpectedMeasDiff: -0.346497 delta_rms_squared: 1.040321 alpha: 0.981138 2024-11-06T03:06:16Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:16Z- meanExpectedMeasDiff: 1.020950 delta_rms_squared: 1.136905 alpha: 0.982713 2024-11-06T03:06:16Z- Subcloud NIS: 0.206180 2024-11-06T03:06:16Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:06:16Z- TNavPF:: sumWeights = 0.455759 2024-11-06T03:06:16Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:06:16Z- TNavPF::First Term of Map Covariance Matrix is 0.42 2024-11-06T03:06:16Z- TNavPF::Calculated NIS Value : 0.25 numBeams normalized NIS: 0.06 2024-11-06T03:06:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862375.60 sec. 2024-11-06T03:06:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:06:16Z- variances([0], [2], and psi_berg) = 0.637972 0.182066 0.000000 2024-11-06T03:06:16Z- publishEstimatesDecoder() - at 1730862375.98 sec 2024-11-06T03:06:16Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:06:16Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061734.49 594600.66 59.16 2024-11-06T03:06:16Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061736.29 594600.68 59.19 lat/lon: 36.70 -121.94 2024-11-06T03:06:16Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.64 0.18 0.62 0.00 2024-11-06T03:06:16Z- AHRS_M2 msg: 1730862376.37 epoch sec; seqNo:2364 2024-11-06T03:06:16Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T03:06:16Z- handleDepth()- Depth_Keller msg: 1730862376.38 epoch sec; seqNo:2517; depth 50.17 2024-11-06T03:06:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862376.39 epoch sec; seqNo:2517; 4061703.69 north; 594617.15 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:06:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862375.98 2024-11-06T03:06:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:16Z- waiting for 1730862379.98, time is 1730862376.39 2024-11-06T03:06:16Z- AHRS_M2 msg: 1730862376.77 epoch sec; seqNo:2365 2024-11-06T03:06:16Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T03:06:16Z- handleDepth()- Depth_Keller msg: 1730862376.78 epoch sec; seqNo:2518; depth 50.19 2024-11-06T03:06:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862376.80 epoch sec; seqNo:2518; 4061703.71 north; 594618.37 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:06:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862375.98 2024-11-06T03:06:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:16Z- waiting for 1730862379.98, time is 1730862376.80 2024-11-06T03:06:17Z- AHRS_M2 msg: 1730862377.18 epoch sec; seqNo:2366 2024-11-06T03:06:17Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.52 psi 2024-11-06T03:06:17Z- handleDepth()- Depth_Keller msg: 1730862377.18 epoch sec; seqNo:2519; depth 50.20 2024-11-06T03:06:17Z- handleDVL() - DVL msg epochMillisec: 1730862377.18 2024-11-06T03:06:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:06:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:06:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:06:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:06:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:06:17Z- handleDvl() - RDI_Pathfinder msg: 1730862377.18 sec; seqNo:811; #beams:0 2024-11-06T03:06:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:06:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.01 2024-11-06T03:06:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862377.19 epoch sec; seqNo:2519; 4061703.71 north; 594618.37 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:06:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862375.98 2024-11-06T03:06:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:17Z- waiting for 1730862379.98, time is 1730862377.19 2024-11-06T03:06:17Z- AHRS_M2 msg: 1730862377.58 epoch sec; seqNo:2367 2024-11-06T03:06:17Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.52 psi 2024-11-06T03:06:17Z- handleDepth()- Depth_Keller msg: 1730862377.59 epoch sec; seqNo:2520; depth 50.21 2024-11-06T03:06:17Z- handleDVL() - DVL msg epochMillisec: 1730862377.65 2024-11-06T03:06:17Z- handleDvl() - RDI_Pathfinder msg: 1730862377.65 sec; seqNo:812; #beams:4 2024-11-06T03:06:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.56 , 30.41 , 33.47 , 36.19 2024-11-06T03:06:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.02 2024-11-06T03:06:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862377.67 epoch sec; seqNo:2520; 4061703.71 north; 594618.37 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:06:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862375.98 2024-11-06T03:06:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:17Z- waiting for 1730862379.98, time is 1730862377.67 2024-11-06T03:06:17Z- AHRS_M2 msg: 1730862377.98 epoch sec; seqNo:2368 2024-11-06T03:06:17Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.52 psi 2024-11-06T03:06:17Z- handleDepth()- Depth_Keller msg: 1730862377.99 epoch sec; seqNo:2521; depth 50.23 2024-11-06T03:06:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862378.01 epoch sec; seqNo:2521; 4061703.71 north; 594618.37 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:06:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862375.98 2024-11-06T03:06:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:18Z- waiting for 1730862379.98, time is 1730862378.01 2024-11-06T03:06:18Z- AHRS_M2 msg: 1730862378.39 epoch sec; seqNo:2369 2024-11-06T03:06:18Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.52 psi 2024-11-06T03:06:18Z- handleDepth()- Depth_Keller msg: 1730862378.40 epoch sec; seqNo:2522; depth 50.24 2024-11-06T03:06:18Z- handleDVL() - DVL msg epochMillisec: 1730862378.40 2024-11-06T03:06:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:06:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:06:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:06:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:06:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:06:18Z- handleDvl() - RDI_Pathfinder msg: 1730862378.40 sec; seqNo:813; #beams:0 2024-11-06T03:06:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:06:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.02 2024-11-06T03:06:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862378.41 epoch sec; seqNo:2522; 4061703.72 north; 594619.60 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:06:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862375.98 2024-11-06T03:06:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:18Z- waiting for 1730862379.98, time is 1730862378.41 2024-11-06T03:06:18Z- AHRS_M2 msg: 1730862378.79 epoch sec; seqNo:2370 2024-11-06T03:06:18Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.52 psi 2024-11-06T03:06:18Z- handleDepth()- Depth_Keller msg: 1730862378.80 epoch sec; seqNo:2523; depth 50.25 2024-11-06T03:06:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862378.81 epoch sec; seqNo:2523; 4061704.10 north; 594619.59 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:06:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862375.98 2024-11-06T03:06:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:18Z- waiting for 1730862379.98, time is 1730862378.81 2024-11-06T03:06:19Z- AHRS_M2 msg: 1730862379.19 epoch sec; seqNo:2371 2024-11-06T03:06:19Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.52 psi 2024-11-06T03:06:19Z- handleDepth()- Depth_Keller msg: 1730862379.21 epoch sec; seqNo:2524; depth 50.27 2024-11-06T03:06:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862379.23 epoch sec; seqNo:2524; 4061704.10 north; 594619.59 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:06:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862375.98 2024-11-06T03:06:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:19Z- waiting for 1730862379.98, time is 1730862379.23 2024-11-06T03:06:19Z- AHRS_M2 msg: 1730862379.60 epoch sec; seqNo:2372 2024-11-06T03:06:19Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.53 psi 2024-11-06T03:06:19Z- handleDepth()- Depth_Keller msg: 1730862379.60 epoch sec; seqNo:2525; depth 50.29 2024-11-06T03:06:19Z- handleDVL() - DVL msg epochMillisec: 1730862379.63 2024-11-06T03:06:19Z- handleDvl() - RDI_Pathfinder msg: 1730862379.63 sec; seqNo:814; #beams:4 2024-11-06T03:06:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.40 , 30.75 , 33.47 , 36.02 2024-11-06T03:06:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , 0.01 2024-11-06T03:06:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862379.66 epoch sec; seqNo:2525; 4061704.11 north; 594620.81 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:06:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862375.98 2024-11-06T03:06:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:19Z- waiting for 1730862379.98, time is 1730862379.66 2024-11-06T03:06:20Z- AHRS_M2 msg: 1730862380.01 epoch sec; seqNo:2373 2024-11-06T03:06:20Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.53 psi 2024-11-06T03:06:20Z- handleDepth()- Depth_Keller msg: 1730862380.01 epoch sec; seqNo:2526; depth 50.30 2024-11-06T03:06:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862380.03 epoch sec; seqNo:2526; 4061704.11 north; 594620.81 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:06:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862375.98 2024-11-06T03:06:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:20Z- waiting for 1730862379.98, time is 1730862380.03 2024-11-06T03:06:20Z- LcmTrn::updateTrn() >>>> pose time 1730862380.03, meas time 1730862379.63 <<<< 2024-11-06T03:06:20Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862380.03 2024-11-06T03:06:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862379.63 sec, ping # 0; waiting for INS data more recent than 1730862375.98... 2024-11-06T03:06:20Z- TerrainNav::motionUpdate() - currEstimate = (4061704, 594620.81, 50.30) 2024-11-06T03:06:20Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:06:20Z- TerrainNav:: SubcloudNIS is 0.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:06:20Z- TNavPF::Projecting Measurements 2024-11-06T03:06:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:06:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:06:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:06:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:06:20Z- TNavFilter::4 good beams. 2024-11-06T03:06:20Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:06:20Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:06:20Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:06:20Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:06:20Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:06:20Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:06:20Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:06:20Z- TerrainMapOctree: (vehN, vehE ) = (4061704.07, 594620.45). 2024-11-06T03:06:20Z- TerrainMapOctree: Min Distance = 137.45. 2024-11-06T03:06:20Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:06:20Z- Forcing Subcloud Comparison 2024-11-06T03:06:20Z- Weighting particles with subcloud comparison 2024-11-06T03:06:20Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:20Z- meanExpectedMeasDiff: -0.617338 delta_rms_squared: 1.419716 alpha: 0.986108 2024-11-06T03:06:20Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:20Z- meanExpectedMeasDiff: -0.767094 delta_rms_squared: 0.987719 alpha: 0.980153 2024-11-06T03:06:20Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:20Z- meanExpectedMeasDiff: -0.400408 delta_rms_squared: 1.142141 alpha: 0.982790 2024-11-06T03:06:20Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:20Z- meanExpectedMeasDiff: 1.182620 delta_rms_squared: 1.208729 alpha: 0.983723 2024-11-06T03:06:20Z- Subcloud NIS: 0.208049 2024-11-06T03:06:20Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:06:20Z- TNavPF:: sumWeights = 0.476538 2024-11-06T03:06:20Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:06:20Z- TNavPF::First Term of Map Covariance Matrix is 0.33 2024-11-06T03:06:20Z- TNavPF::Calculated NIS Value : 0.24 numBeams normalized NIS: 0.06 2024-11-06T03:06:20Z- TerrainNav::Resampling particle filter... 2024-11-06T03:06:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862379.63 sec. 2024-11-06T03:06:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:06:20Z- variances([0], [2], and psi_berg) = 0.633727 0.182892 0.000000 2024-11-06T03:06:20Z- publishEstimatesDecoder() - at 1730862380.03 sec 2024-11-06T03:06:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:06:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061736.31 594604.42 59.08 2024-11-06T03:06:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061736.72 594604.34 59.39 lat/lon: 36.70 -121.94 2024-11-06T03:06:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.63 0.18 0.63 0.00 2024-11-06T03:06:20Z- AHRS_M2 msg: 1730862380.41 epoch sec; seqNo:2374 2024-11-06T03:06:20Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.53 psi 2024-11-06T03:06:20Z- handleDepth()- Depth_Keller msg: 1730862380.41 epoch sec; seqNo:2527; depth 50.30 2024-11-06T03:06:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862380.43 epoch sec; seqNo:2527; 4061704.11 north; 594620.81 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:06:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862380.03 2024-11-06T03:06:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:20Z- waiting for 1730862384.03, time is 1730862380.43 2024-11-06T03:06:20Z- AHRS_M2 msg: 1730862380.82 epoch sec; seqNo:2375 2024-11-06T03:06:20Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.53 psi 2024-11-06T03:06:20Z- handleDepth()- Depth_Keller msg: 1730862380.82 epoch sec; seqNo:2528; depth 50.31 2024-11-06T03:06:20Z- handleDVL() - DVL msg epochMillisec: 1730862380.82 2024-11-06T03:06:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:06:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:06:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:06:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:06:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:06:20Z- handleDvl() - RDI_Pathfinder msg: 1730862380.82 sec; seqNo:815; #beams:0 2024-11-06T03:06:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:06:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , 0.01 2024-11-06T03:06:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862380.83 epoch sec; seqNo:2528; 4061704.11 north; 594620.81 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:06:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862380.03 2024-11-06T03:06:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:20Z- waiting for 1730862384.03, time is 1730862380.83 2024-11-06T03:06:21Z- AHRS_M2 msg: 1730862381.22 epoch sec; seqNo:2376 2024-11-06T03:06:21Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.53 psi 2024-11-06T03:06:21Z- handleDepth()- Depth_Keller msg: 1730862381.23 epoch sec; seqNo:2529; depth 50.31 2024-11-06T03:06:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862381.23 epoch sec; seqNo:2529; 4061704.13 north; 594622.03 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:06:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862380.03 2024-11-06T03:06:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:21Z- waiting for 1730862384.03, time is 1730862381.23 2024-11-06T03:06:21Z- AHRS_M2 msg: 1730862381.62 epoch sec; seqNo:2377 2024-11-06T03:06:21Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.53 psi 2024-11-06T03:06:21Z- handleDepth()- Depth_Keller msg: 1730862381.63 epoch sec; seqNo:2530; depth 50.32 2024-11-06T03:06:21Z- handleDVL() - DVL msg epochMillisec: 1730862381.66 2024-11-06T03:06:21Z- handleDvl() - RDI_Pathfinder msg: 1730862381.66 sec; seqNo:816; #beams:4 2024-11-06T03:06:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.22 , 31.09 , 33.47 , 35.51 2024-11-06T03:06:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , 0.01 2024-11-06T03:06:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862381.69 epoch sec; seqNo:2530; 4061704.13 north; 594622.03 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:06:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862380.03 2024-11-06T03:06:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:21Z- waiting for 1730862384.03, time is 1730862381.69 2024-11-06T03:06:22Z- AHRS_M2 msg: 1730862382.03 epoch sec; seqNo:2378 2024-11-06T03:06:22Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.53 psi 2024-11-06T03:06:22Z- handleDepth()- Depth_Keller msg: 1730862382.04 epoch sec; seqNo:2531; depth 50.33 2024-11-06T03:06:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862382.06 epoch sec; seqNo:2531; 4061704.13 north; 594622.03 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:06:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862380.03 2024-11-06T03:06:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:22Z- waiting for 1730862384.03, time is 1730862382.06 2024-11-06T03:06:22Z- AHRS_M2 msg: 1730862382.43 epoch sec; seqNo:2379 2024-11-06T03:06:22Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.53 psi 2024-11-06T03:06:22Z- handleDepth()- Depth_Keller msg: 1730862382.44 epoch sec; seqNo:2532; depth 50.33 2024-11-06T03:06:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862382.45 epoch sec; seqNo:2532; 4061704.14 north; 594623.25 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:06:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862380.03 2024-11-06T03:06:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:22Z- waiting for 1730862384.03, time is 1730862382.45 2024-11-06T03:06:22Z- AHRS_M2 msg: 1730862382.83 epoch sec; seqNo:2380 2024-11-06T03:06:22Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.53 psi 2024-11-06T03:06:22Z- handleDepth()- Depth_Keller msg: 1730862382.84 epoch sec; seqNo:2533; depth 50.34 2024-11-06T03:06:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862382.85 epoch sec; seqNo:2533; 4061704.14 north; 594623.25 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:06:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862380.03 2024-11-06T03:06:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:22Z- waiting for 1730862384.03, time is 1730862382.85 2024-11-06T03:06:23Z- AHRS_M2 msg: 1730862383.24 epoch sec; seqNo:2381 2024-11-06T03:06:23Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.54 psi 2024-11-06T03:06:23Z- handleDepth()- Depth_Keller msg: 1730862383.24 epoch sec; seqNo:2534; depth 50.34 2024-11-06T03:06:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862383.25 epoch sec; seqNo:2534; 4061704.52 north; 594623.25 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:06:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862380.03 2024-11-06T03:06:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:23Z- waiting for 1730862384.03, time is 1730862383.25 2024-11-06T03:06:23Z- AHRS_M2 msg: 1730862383.64 epoch sec; seqNo:2382 2024-11-06T03:06:23Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 1.54 psi 2024-11-06T03:06:23Z- handleDepth()- Depth_Keller msg: 1730862383.66 epoch sec; seqNo:2535; depth 50.35 2024-11-06T03:06:23Z- handleDVL() - DVL msg epochMillisec: 1730862383.68 2024-11-06T03:06:23Z- handleDvl() - RDI_Pathfinder msg: 1730862383.68 sec; seqNo:817; #beams:4 2024-11-06T03:06:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.06 , 31.09 , 33.47 , 35.51 2024-11-06T03:06:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , 0.02 2024-11-06T03:06:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862383.69 epoch sec; seqNo:2535; 4061704.53 north; 594624.47 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:06:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862380.03 2024-11-06T03:06:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:23Z- waiting for 1730862384.03, time is 1730862383.69 2024-11-06T03:06:24Z- AHRS_M2 msg: 1730862384.05 epoch sec; seqNo:2383 2024-11-06T03:06:24Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 1.54 psi 2024-11-06T03:06:24Z- handleDepth()- Depth_Keller msg: 1730862384.05 epoch sec; seqNo:2536; depth 50.34 2024-11-06T03:06:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862384.06 epoch sec; seqNo:2536; 4061704.53 north; 594624.47 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:06:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862380.03 2024-11-06T03:06:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:24Z- waiting for 1730862384.03, time is 1730862384.06 2024-11-06T03:06:24Z- LcmTrn::updateTrn() >>>> pose time 1730862384.06, meas time 1730862383.68 <<<< 2024-11-06T03:06:24Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862384.06 2024-11-06T03:06:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862383.68 sec, ping # 0; waiting for INS data more recent than 1730862380.03... 2024-11-06T03:06:24Z- TerrainNav::motionUpdate() - currEstimate = (4061705, 594624.47, 50.34) 2024-11-06T03:06:24Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:06:24Z- TerrainNav:: SubcloudNIS is 0.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:06:24Z- TNavPF::Projecting Measurements 2024-11-06T03:06:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:06:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:06:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:06:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:06:24Z- TNavFilter::4 good beams. 2024-11-06T03:06:24Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:06:24Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:06:24Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:06:24Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:06:24Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:06:24Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:06:24Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:06:24Z- TerrainMapOctree: (vehN, vehE ) = (4061704.49, 594624.12). 2024-11-06T03:06:24Z- TerrainMapOctree: Min Distance = 133.85. 2024-11-06T03:06:24Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:06:24Z- Forcing Subcloud Comparison 2024-11-06T03:06:24Z- Weighting particles with subcloud comparison 2024-11-06T03:06:24Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:24Z- meanExpectedMeasDiff: -0.926894 delta_rms_squared: 1.460847 alpha: 0.986494 2024-11-06T03:06:24Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:24Z- meanExpectedMeasDiff: -0.226762 delta_rms_squared: 0.996135 alpha: 0.980318 2024-11-06T03:06:24Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:24Z- meanExpectedMeasDiff: -0.479590 delta_rms_squared: 1.172280 alpha: 0.983225 2024-11-06T03:06:24Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:24Z- meanExpectedMeasDiff: 1.027904 delta_rms_squared: 1.209591 alpha: 0.983734 2024-11-06T03:06:24Z- Subcloud NIS: 0.206662 2024-11-06T03:06:24Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:06:24Z- TNavPF:: sumWeights = 0.499731 2024-11-06T03:06:24Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:06:24Z- TNavPF::First Term of Map Covariance Matrix is 0.40 2024-11-06T03:06:24Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.06 2024-11-06T03:06:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862383.68 sec. 2024-11-06T03:06:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:06:24Z- variances([0], [2], and psi_berg) = 0.627150 0.183356 0.000000 2024-11-06T03:06:24Z- publishEstimatesDecoder() - at 1730862384.06 sec 2024-11-06T03:06:24Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:06:24Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061737.32 594608.38 59.12 2024-11-06T03:06:24Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061737.14 594608.00 59.51 lat/lon: 36.70 -121.94 2024-11-06T03:06:24Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.63 0.18 0.64 0.00 2024-11-06T03:06:24Z- AHRS_M2 msg: 1730862384.45 epoch sec; seqNo:2384 2024-11-06T03:06:24Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 1.54 psi 2024-11-06T03:06:24Z- handleDepth()- Depth_Keller msg: 1730862384.46 epoch sec; seqNo:2537; depth 50.35 2024-11-06T03:06:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862384.47 epoch sec; seqNo:2537; 4061704.53 north; 594624.47 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:06:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862384.06 2024-11-06T03:06:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:24Z- waiting for 1730862388.06, time is 1730862384.47 2024-11-06T03:06:24Z- AHRS_M2 msg: 1730862384.85 epoch sec; seqNo:2385 2024-11-06T03:06:24Z- AHRS_M2 msg: 0.17 phi; -0.01 theta; 1.54 psi 2024-11-06T03:06:24Z- handleDepth()- Depth_Keller msg: 1730862384.87 epoch sec; seqNo:2538; depth 50.34 2024-11-06T03:06:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862384.88 epoch sec; seqNo:2538; 4061704.53 north; 594624.47 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:06:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862384.06 2024-11-06T03:06:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:25Z- waiting for 1730862388.06, time is 1730862384.88 2024-11-06T03:06:25Z- AHRS_M2 msg: 1730862385.26 epoch sec; seqNo:2386 2024-11-06T03:06:25Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 1.55 psi 2024-11-06T03:06:25Z- handleDepth()- Depth_Keller msg: 1730862385.26 epoch sec; seqNo:2539; depth 50.35 2024-11-06T03:06:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862385.27 epoch sec; seqNo:2539; 4061704.55 north; 594625.69 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:06:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862384.06 2024-11-06T03:06:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:25Z- waiting for 1730862388.06, time is 1730862385.27 2024-11-06T03:06:25Z- AHRS_M2 msg: 1730862385.66 epoch sec; seqNo:2387 2024-11-06T03:06:25Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 1.55 psi 2024-11-06T03:06:25Z- handleDepth()- Depth_Keller msg: 1730862385.67 epoch sec; seqNo:2540; depth 50.34 2024-11-06T03:06:25Z- handleDVL() - DVL msg epochMillisec: 1730862385.70 2024-11-06T03:06:25Z- handleDvl() - RDI_Pathfinder msg: 1730862385.70 sec; seqNo:818; #beams:4 2024-11-06T03:06:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.22 , 31.09 , 33.64 , 35.17 2024-11-06T03:06:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , 0.00 2024-11-06T03:06:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862385.74 epoch sec; seqNo:2540; 4061704.55 north; 594625.69 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:06:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862384.06 2024-11-06T03:06:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:25Z- waiting for 1730862388.06, time is 1730862385.74 2024-11-06T03:06:26Z- AHRS_M2 msg: 1730862386.08 epoch sec; seqNo:2388 2024-11-06T03:06:26Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 1.55 psi 2024-11-06T03:06:26Z- handleDepth()- Depth_Keller msg: 1730862386.09 epoch sec; seqNo:2541; depth 50.32 2024-11-06T03:06:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862386.12 epoch sec; seqNo:2541; 4061704.55 north; 594625.69 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:06:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862384.06 2024-11-06T03:06:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:26Z- waiting for 1730862388.06, time is 1730862386.12 2024-11-06T03:06:26Z- AHRS_M2 msg: 1730862386.47 epoch sec; seqNo:2389 2024-11-06T03:06:26Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.55 psi 2024-11-06T03:06:26Z- handleDepth()- Depth_Keller msg: 1730862386.47 epoch sec; seqNo:2542; depth 50.31 2024-11-06T03:06:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862386.48 epoch sec; seqNo:2542; 4061704.56 north; 594626.91 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:06:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862384.06 2024-11-06T03:06:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:26Z- waiting for 1730862388.06, time is 1730862386.48 2024-11-06T03:06:26Z- AHRS_M2 msg: 1730862386.88 epoch sec; seqNo:2390 2024-11-06T03:06:26Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.54 psi 2024-11-06T03:06:26Z- handleDepth()- Depth_Keller msg: 1730862386.88 epoch sec; seqNo:2543; depth 50.30 2024-11-06T03:06:26Z- handleDVL() - DVL msg epochMillisec: 1730862386.88 2024-11-06T03:06:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:06:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:06:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:06:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:06:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:06:26Z- handleDvl() - RDI_Pathfinder msg: 1730862386.88 sec; seqNo:819; #beams:0 2024-11-06T03:06:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:06:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , 0.00 2024-11-06T03:06:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862386.89 epoch sec; seqNo:2543; 4061704.56 north; 594626.91 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:06:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862384.06 2024-11-06T03:06:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:27Z- waiting for 1730862388.06, time is 1730862386.89 2024-11-06T03:06:27Z- AHRS_M2 msg: 1730862387.28 epoch sec; seqNo:2391 2024-11-06T03:06:27Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.54 psi 2024-11-06T03:06:27Z- handleDepth()- Depth_Keller msg: 1730862387.29 epoch sec; seqNo:2544; depth 50.29 2024-11-06T03:06:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862387.30 epoch sec; seqNo:2544; 4061704.56 north; 594626.91 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:06:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862384.06 2024-11-06T03:06:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:27Z- waiting for 1730862388.06, time is 1730862387.30 2024-11-06T03:06:27Z- AHRS_M2 msg: 1730862387.69 epoch sec; seqNo:2392 2024-11-06T03:06:27Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.54 psi 2024-11-06T03:06:27Z- handleDepth()- Depth_Keller msg: 1730862387.70 epoch sec; seqNo:2545; depth 50.27 2024-11-06T03:06:27Z- handleDVL() - DVL msg epochMillisec: 1730862387.72 2024-11-06T03:06:27Z- handleDvl() - RDI_Pathfinder msg: 1730862387.72 sec; seqNo:820; #beams:4 2024-11-06T03:06:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.06 , 31.43 , 33.98 , 34.66 2024-11-06T03:06:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , -0.03 2024-11-06T03:06:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862387.73 epoch sec; seqNo:2545; 4061704.57 north; 594628.13 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:06:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862384.06 2024-11-06T03:06:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:27Z- waiting for 1730862388.06, time is 1730862387.73 2024-11-06T03:06:28Z- AHRS_M2 msg: 1730862388.09 epoch sec; seqNo:2393 2024-11-06T03:06:28Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.54 psi 2024-11-06T03:06:28Z- handleDepth()- Depth_Keller msg: 1730862388.09 epoch sec; seqNo:2546; depth 50.27 2024-11-06T03:06:28Z- handleDVL() - DVL msg epochMillisec: 1730862388.10 2024-11-06T03:06:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:06:28Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:06:28Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:06:28Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:06:28Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:06:28Z- handleDvl() - RDI_Pathfinder msg: 1730862388.10 sec; seqNo:821; #beams:0 2024-11-06T03:06:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:06:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , -0.03 2024-11-06T03:06:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862388.12 epoch sec; seqNo:2546; 4061704.57 north; 594628.13 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:06:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862384.06 2024-11-06T03:06:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:28Z- waiting for 1730862388.06, time is 1730862388.12 2024-11-06T03:06:28Z- LcmTrn::updateTrn() >>>> pose time 1730862388.12, meas time 1730862388.10 <<<< 2024-11-06T03:06:28Z- LcmTrn::updateTrn() equating pose 1730862388.10 and meas times 1730862388.10 2024-11-06T03:06:28Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862388.10 2024-11-06T03:06:28Z- TerrainNav::motionUpdate() - currEstimate = (4061705, 594628.13, 50.27) 2024-11-06T03:06:28Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:06:28Z- TerrainNav:: SubcloudNIS is 0.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:06:28Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:06:28Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:06:28Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:06:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:06:28Z- variances([0], [2], and psi_berg) = 0.629419 0.185055 0.000000 2024-11-06T03:06:28Z- publishEstimatesDecoder() - at 1730862388.10 sec 2024-11-06T03:06:28Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:06:28Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061737.37 594612.06 59.31 2024-11-06T03:06:28Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061737.18 594611.66 59.44 lat/lon: 36.70 -121.94 2024-11-06T03:06:28Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.63 0.19 0.89 0.00 2024-11-06T03:06:28Z- AHRS_M2 msg: 1730862388.49 epoch sec; seqNo:2394 2024-11-06T03:06:28Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.53 psi 2024-11-06T03:06:28Z- handleDepth()- Depth_Keller msg: 1730862388.50 epoch sec; seqNo:2547; depth 50.25 2024-11-06T03:06:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862388.51 epoch sec; seqNo:2547; 4061704.57 north; 594628.13 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:06:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862388.10 2024-11-06T03:06:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:28Z- waiting for 1730862392.10, time is 1730862388.51 2024-11-06T03:06:28Z- AHRS_M2 msg: 1730862388.90 epoch sec; seqNo:2395 2024-11-06T03:06:28Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.53 psi 2024-11-06T03:06:28Z- handleDepth()- Depth_Keller msg: 1730862388.90 epoch sec; seqNo:2548; depth 50.23 2024-11-06T03:06:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862388.91 epoch sec; seqNo:2548; 4061704.57 north; 594628.13 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:06:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862388.10 2024-11-06T03:06:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:29Z- waiting for 1730862392.10, time is 1730862388.91 2024-11-06T03:06:29Z- handleDepth()- Depth_Keller msg: 1730862389.32 epoch sec; seqNo:2549; depth 50.22 2024-11-06T03:06:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862389.33 epoch sec; seqNo:2549; 4061704.59 north; 594629.35 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:06:29Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:06:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862388.10 2024-11-06T03:06:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:29Z- waiting for 1730862392.10, time is 1730862389.33 2024-11-06T03:06:29Z- handleDepth()- Depth_Keller msg: 1730862389.73 epoch sec; seqNo:2550; depth 50.20 2024-11-06T03:06:29Z- handleDVL() - DVL msg epochMillisec: 1730862389.75 2024-11-06T03:06:29Z- handleDvl() - RDI_Pathfinder msg: 1730862389.75 sec; seqNo:822; #beams:4 2024-11-06T03:06:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.04 , 31.60 , 33.98 , 34.32 2024-11-06T03:06:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , -0.04 2024-11-06T03:06:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862389.76 epoch sec; seqNo:2550; 4061704.59 north; 594629.35 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:06:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862388.10 2024-11-06T03:06:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:29Z- waiting for 1730862392.10, time is 1730862389.76 2024-11-06T03:06:30Z- AHRS_M2 msg: 1730862390.11 epoch sec; seqNo:2396 2024-11-06T03:06:30Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.53 psi 2024-11-06T03:06:30Z- handleDepth()- Depth_Keller msg: 1730862390.12 epoch sec; seqNo:2551; depth 50.18 2024-11-06T03:06:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862390.13 epoch sec; seqNo:2551; 4061704.59 north; 594629.35 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:06:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862388.10 2024-11-06T03:06:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:30Z- waiting for 1730862392.10, time is 1730862390.13 2024-11-06T03:06:30Z- AHRS_M2 msg: 1730862390.51 epoch sec; seqNo:2397 2024-11-06T03:06:30Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.53 psi 2024-11-06T03:06:30Z- handleDepth()- Depth_Keller msg: 1730862390.52 epoch sec; seqNo:2552; depth 50.16 2024-11-06T03:06:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862390.52 epoch sec; seqNo:2552; 4061704.60 north; 594630.57 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:06:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862388.10 2024-11-06T03:06:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:30Z- waiting for 1730862392.10, time is 1730862390.52 2024-11-06T03:06:30Z- AHRS_M2 msg: 1730862390.92 epoch sec; seqNo:2398 2024-11-06T03:06:30Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.53 psi 2024-11-06T03:06:30Z- handleDepth()- Depth_Keller msg: 1730862390.92 epoch sec; seqNo:2553; depth 50.16 2024-11-06T03:06:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862390.93 epoch sec; seqNo:2553; 4061704.60 north; 594630.57 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:06:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862388.10 2024-11-06T03:06:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:31Z- waiting for 1730862392.10, time is 1730862390.93 2024-11-06T03:06:31Z- AHRS_M2 msg: 1730862391.32 epoch sec; seqNo:2399 2024-11-06T03:06:31Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.53 psi 2024-11-06T03:06:31Z- handleDepth()- Depth_Keller msg: 1730862391.33 epoch sec; seqNo:2554; depth 50.13 2024-11-06T03:06:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862391.35 epoch sec; seqNo:2554; 4061704.60 north; 594630.57 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:06:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862388.10 2024-11-06T03:06:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:31Z- waiting for 1730862392.10, time is 1730862391.35 2024-11-06T03:06:31Z- AHRS_M2 msg: 1730862391.73 epoch sec; seqNo:2400 2024-11-06T03:06:31Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.53 psi 2024-11-06T03:06:31Z- handleDepth()- Depth_Keller msg: 1730862391.73 epoch sec; seqNo:2555; depth 50.13 2024-11-06T03:06:31Z- handleDVL() - DVL msg epochMillisec: 1730862391.76 2024-11-06T03:06:31Z- handleDvl() - RDI_Pathfinder msg: 1730862391.76 sec; seqNo:823; #beams:4 2024-11-06T03:06:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.00 , 31.34 , 34.10 , 34.25 2024-11-06T03:06:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.04 , -0.01 2024-11-06T03:06:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862391.77 epoch sec; seqNo:2555; 4061704.60 north; 594630.57 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:06:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862388.10 2024-11-06T03:06:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:31Z- waiting for 1730862392.10, time is 1730862391.77 2024-11-06T03:06:32Z- AHRS_M2 msg: 1730862392.13 epoch sec; seqNo:2401 2024-11-06T03:06:32Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.53 psi 2024-11-06T03:06:32Z- handleDepth()- Depth_Keller msg: 1730862392.13 epoch sec; seqNo:2556; depth 50.11 2024-11-06T03:06:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862392.16 epoch sec; seqNo:2556; 4061704.61 north; 594631.79 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:06:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862388.10 2024-11-06T03:06:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:32Z- waiting for 1730862392.10, time is 1730862392.16 2024-11-06T03:06:32Z- LcmTrn::updateTrn() >>>> pose time 1730862392.16, meas time 1730862391.76 <<<< 2024-11-06T03:06:32Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862392.16 2024-11-06T03:06:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862391.76 sec, ping # 0; waiting for INS data more recent than 1730862388.10... 2024-11-06T03:06:32Z- TerrainNav::motionUpdate() - currEstimate = (4061705, 594631.79, 50.11) 2024-11-06T03:06:32Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:06:32Z- TerrainNav:: SubcloudNIS is 0.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:06:32Z- TNavPF::Projecting Measurements 2024-11-06T03:06:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:06:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:06:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:06:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:06:32Z- TNavFilter::4 good beams. 2024-11-06T03:06:32Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:06:32Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:06:32Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:06:32Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:06:32Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:06:32Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:06:32Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:06:32Z- TerrainMapOctree: (vehN, vehE ) = (4061704.61, 594631.43). 2024-11-06T03:06:32Z- TerrainMapOctree: Min Distance = 126.96. 2024-11-06T03:06:32Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:06:32Z- Forcing Subcloud Comparison 2024-11-06T03:06:32Z- Weighting particles with subcloud comparison 2024-11-06T03:06:32Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:32Z- meanExpectedMeasDiff: -1.591341 delta_rms_squared: 1.829104 alpha: 0.989184 2024-11-06T03:06:32Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:32Z- meanExpectedMeasDiff: -0.330449 delta_rms_squared: 1.314860 alpha: 0.985017 2024-11-06T03:06:32Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:32Z- meanExpectedMeasDiff: -0.606337 delta_rms_squared: 1.580356 alpha: 0.987503 2024-11-06T03:06:32Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:32Z- meanExpectedMeasDiff: 0.405905 delta_rms_squared: 1.505025 alpha: 0.986885 2024-11-06T03:06:32Z- Subcloud NIS: 0.210801 2024-11-06T03:06:32Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:06:32Z- TNavPF:: sumWeights = 0.446294 2024-11-06T03:06:32Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:06:32Z- TNavPF::First Term of Map Covariance Matrix is 0.51 2024-11-06T03:06:32Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T03:06:32Z- TerrainNav::Resampling particle filter... 2024-11-06T03:06:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862391.76 sec. 2024-11-06T03:06:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:06:32Z- variances([0], [2], and psi_berg) = 0.625396 0.188842 0.000000 2024-11-06T03:06:32Z- publishEstimatesDecoder() - at 1730862392.16 sec 2024-11-06T03:06:32Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:06:32Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061736.87 594615.37 59.72 2024-11-06T03:06:32Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061737.22 594615.32 59.57 lat/lon: 36.70 -121.94 2024-11-06T03:06:32Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.63 0.19 0.67 0.00 2024-11-06T03:06:32Z- AHRS_M2 msg: 1730862392.53 epoch sec; seqNo:2402 2024-11-06T03:06:32Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.53 psi 2024-11-06T03:06:32Z- handleDepth()- Depth_Keller msg: 1730862392.54 epoch sec; seqNo:2557; depth 50.12 2024-11-06T03:06:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862392.57 epoch sec; seqNo:2557; 4061704.61 north; 594631.79 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:06:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862392.16 2024-11-06T03:06:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:32Z- waiting for 1730862396.16, time is 1730862392.57 2024-11-06T03:06:32Z- AHRS_M2 msg: 1730862392.94 epoch sec; seqNo:2403 2024-11-06T03:06:32Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.52 psi 2024-11-06T03:06:32Z- handleDepth()- Depth_Keller msg: 1730862392.96 epoch sec; seqNo:2558; depth 50.11 2024-11-06T03:06:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862392.98 epoch sec; seqNo:2558; 4061704.61 north; 594631.79 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:06:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862392.16 2024-11-06T03:06:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:33Z- waiting for 1730862396.16, time is 1730862392.98 2024-11-06T03:06:33Z- AHRS_M2 msg: 1730862393.34 epoch sec; seqNo:2404 2024-11-06T03:06:33Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.52 psi 2024-11-06T03:06:33Z- handleDepth()- Depth_Keller msg: 1730862393.35 epoch sec; seqNo:2559; depth 50.10 2024-11-06T03:06:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862393.36 epoch sec; seqNo:2559; 4061704.99 north; 594631.79 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:06:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862392.16 2024-11-06T03:06:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:33Z- waiting for 1730862396.16, time is 1730862393.36 2024-11-06T03:06:33Z- AHRS_M2 msg: 1730862393.74 epoch sec; seqNo:2405 2024-11-06T03:06:33Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.52 psi 2024-11-06T03:06:33Z- handleDepth()- Depth_Keller msg: 1730862393.75 epoch sec; seqNo:2560; depth 50.10 2024-11-06T03:06:33Z- handleDVL() - DVL msg epochMillisec: 1730862393.77 2024-11-06T03:06:33Z- handleDvl() - RDI_Pathfinder msg: 1730862393.77 sec; seqNo:824; #beams:4 2024-11-06T03:06:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.46 , 31.34 , 34.25 , 34.25 2024-11-06T03:06:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , 0.04 2024-11-06T03:06:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862393.79 epoch sec; seqNo:2560; 4061705.01 north; 594633.01 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:06:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862392.16 2024-11-06T03:06:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:33Z- waiting for 1730862396.16, time is 1730862393.79 2024-11-06T03:06:34Z- AHRS_M2 msg: 1730862394.15 epoch sec; seqNo:2406 2024-11-06T03:06:34Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.52 psi 2024-11-06T03:06:34Z- handleDepth()- Depth_Keller msg: 1730862394.15 epoch sec; seqNo:2561; depth 50.11 2024-11-06T03:06:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862394.17 epoch sec; seqNo:2561; 4061705.01 north; 594633.01 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:06:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862392.16 2024-11-06T03:06:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:34Z- waiting for 1730862396.16, time is 1730862394.17 2024-11-06T03:06:34Z- AHRS_M2 msg: 1730862394.55 epoch sec; seqNo:2407 2024-11-06T03:06:34Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.52 psi 2024-11-06T03:06:34Z- handleDepth()- Depth_Keller msg: 1730862394.56 epoch sec; seqNo:2562; depth 50.10 2024-11-06T03:06:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862394.57 epoch sec; seqNo:2562; 4061705.01 north; 594633.01 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:06:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862392.16 2024-11-06T03:06:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:34Z- waiting for 1730862396.16, time is 1730862394.57 2024-11-06T03:06:34Z- AHRS_M2 msg: 1730862394.96 epoch sec; seqNo:2408 2024-11-06T03:06:34Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.52 psi 2024-11-06T03:06:34Z- handleDepth()- Depth_Keller msg: 1730862394.97 epoch sec; seqNo:2563; depth 50.10 2024-11-06T03:06:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862394.97 epoch sec; seqNo:2563; 4061705.02 north; 594634.23 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:06:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862392.16 2024-11-06T03:06:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:35Z- waiting for 1730862396.16, time is 1730862394.97 2024-11-06T03:06:35Z- AHRS_M2 msg: 1730862395.36 epoch sec; seqNo:2409 2024-11-06T03:06:35Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.52 psi 2024-11-06T03:06:35Z- handleDepth()- Depth_Keller msg: 1730862395.36 epoch sec; seqNo:2564; depth 50.09 2024-11-06T03:06:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862395.38 epoch sec; seqNo:2564; 4061705.02 north; 594634.23 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:06:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862392.16 2024-11-06T03:06:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:35Z- waiting for 1730862396.16, time is 1730862395.38 2024-11-06T03:06:35Z- AHRS_M2 msg: 1730862395.76 epoch sec; seqNo:2410 2024-11-06T03:06:35Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.52 psi 2024-11-06T03:06:35Z- handleDepth()- Depth_Keller msg: 1730862395.77 epoch sec; seqNo:2565; depth 50.09 2024-11-06T03:06:35Z- handleDVL() - DVL msg epochMillisec: 1730862395.80 2024-11-06T03:06:35Z- handleDvl() - RDI_Pathfinder msg: 1730862395.80 sec; seqNo:825; #beams:4 2024-11-06T03:06:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.77 , 31.19 , 33.94 , 34.40 2024-11-06T03:06:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.06 2024-11-06T03:06:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862395.81 epoch sec; seqNo:2565; 4061705.02 north; 594634.23 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:06:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862392.16 2024-11-06T03:06:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:35Z- waiting for 1730862396.16, time is 1730862395.81 2024-11-06T03:06:36Z- AHRS_M2 msg: 1730862396.17 epoch sec; seqNo:2411 2024-11-06T03:06:36Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.52 psi 2024-11-06T03:06:36Z- handleDepth()- Depth_Keller msg: 1730862396.18 epoch sec; seqNo:2566; depth 50.09 2024-11-06T03:06:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862396.19 epoch sec; seqNo:2566; 4061705.02 north; 594634.23 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:06:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862392.16 2024-11-06T03:06:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:36Z- waiting for 1730862396.16, time is 1730862396.19 2024-11-06T03:06:36Z- LcmTrn::updateTrn() >>>> pose time 1730862396.19, meas time 1730862395.80 <<<< 2024-11-06T03:06:36Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862396.19 2024-11-06T03:06:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862395.80 sec, ping # 0; waiting for INS data more recent than 1730862392.16... 2024-11-06T03:06:36Z- TerrainNav::motionUpdate() - currEstimate = (4061705, 594634.23, 50.09) 2024-11-06T03:06:36Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:06:36Z- TerrainNav:: SubcloudNIS is 0.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:06:36Z- TNavPF::Projecting Measurements 2024-11-06T03:06:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:06:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:06:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:06:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:06:36Z- TNavFilter::4 good beams. 2024-11-06T03:06:36Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:06:36Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:06:36Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:06:36Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:06:36Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:06:36Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:06:36Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:06:36Z- TerrainMapOctree: (vehN, vehE ) = (4061704.98, 594633.99). 2024-11-06T03:06:36Z- TerrainMapOctree: Min Distance = 124.44. 2024-11-06T03:06:36Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:06:36Z- Forcing Subcloud Comparison 2024-11-06T03:06:36Z- Weighting particles with subcloud comparison 2024-11-06T03:06:36Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:36Z- meanExpectedMeasDiff: -0.485439 delta_rms_squared: 1.468342 alpha: 0.986562 2024-11-06T03:06:36Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:36Z- meanExpectedMeasDiff: -0.231419 delta_rms_squared: 1.057267 alpha: 0.981435 2024-11-06T03:06:36Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:36Z- meanExpectedMeasDiff: -0.369439 delta_rms_squared: 1.262690 alpha: 0.984408 2024-11-06T03:06:36Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:36Z- meanExpectedMeasDiff: 0.756304 delta_rms_squared: 1.213638 alpha: 0.983788 2024-11-06T03:06:36Z- Subcloud NIS: 0.201096 2024-11-06T03:06:36Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:06:36Z- TNavPF:: sumWeights = 0.570715 2024-11-06T03:06:36Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:06:36Z- TNavPF::First Term of Map Covariance Matrix is 0.38 2024-11-06T03:06:36Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T03:06:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862395.80 sec. 2024-11-06T03:06:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:06:36Z- variances([0], [2], and psi_berg) = 0.620401 0.191091 0.000000 2024-11-06T03:06:36Z- publishEstimatesDecoder() - at 1730862396.19 sec 2024-11-06T03:06:36Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:06:36Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061738.11 594617.80 59.88 2024-11-06T03:06:36Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061737.62 594617.76 59.57 lat/lon: 36.70 -121.94 2024-11-06T03:06:36Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.62 0.19 0.62 0.00 2024-11-06T03:06:36Z- AHRS_M2 msg: 1730862396.57 epoch sec; seqNo:2412 2024-11-06T03:06:36Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.52 psi 2024-11-06T03:06:36Z- handleDepth()- Depth_Keller msg: 1730862396.58 epoch sec; seqNo:2567; depth 50.10 2024-11-06T03:06:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862396.59 epoch sec; seqNo:2567; 4061705.03 north; 594635.45 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:06:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862396.19 2024-11-06T03:06:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:36Z- waiting for 1730862400.19, time is 1730862396.59 2024-11-06T03:06:36Z- AHRS_M2 msg: 1730862396.98 epoch sec; seqNo:2413 2024-11-06T03:06:36Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.51 psi 2024-11-06T03:06:36Z- handleDepth()- Depth_Keller msg: 1730862396.99 epoch sec; seqNo:2568; depth 50.10 2024-11-06T03:06:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862397.00 epoch sec; seqNo:2568; 4061705.03 north; 594635.45 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:06:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862396.19 2024-11-06T03:06:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:37Z- waiting for 1730862400.19, time is 1730862397.00 2024-11-06T03:06:37Z- AHRS_M2 msg: 1730862397.38 epoch sec; seqNo:2414 2024-11-06T03:06:37Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.51 psi 2024-11-06T03:06:37Z- handleDepth()- Depth_Keller msg: 1730862397.39 epoch sec; seqNo:2569; depth 50.09 2024-11-06T03:06:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862397.39 epoch sec; seqNo:2569; 4061705.03 north; 594635.45 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:06:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862396.19 2024-11-06T03:06:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:37Z- waiting for 1730862400.19, time is 1730862397.39 2024-11-06T03:06:37Z- AHRS_M2 msg: 1730862397.78 epoch sec; seqNo:2415 2024-11-06T03:06:37Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.51 psi 2024-11-06T03:06:37Z- handleDepth()- Depth_Keller msg: 1730862397.79 epoch sec; seqNo:2570; depth 50.07 2024-11-06T03:06:37Z- handleDVL() - DVL msg epochMillisec: 1730862397.81 2024-11-06T03:06:37Z- handleDvl() - RDI_Pathfinder msg: 1730862397.81 sec; seqNo:826; #beams:4 2024-11-06T03:06:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.46 , 31.50 , 34.10 , 34.71 2024-11-06T03:06:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , 0.05 2024-11-06T03:06:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862397.84 epoch sec; seqNo:2570; 4061705.42 north; 594636.66 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:06:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862396.19 2024-11-06T03:06:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:37Z- waiting for 1730862400.19, time is 1730862397.84 2024-11-06T03:06:38Z- AHRS_M2 msg: 1730862398.19 epoch sec; seqNo:2416 2024-11-06T03:06:38Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.51 psi 2024-11-06T03:06:38Z- handleDepth()- Depth_Keller msg: 1730862398.20 epoch sec; seqNo:2571; depth 50.07 2024-11-06T03:06:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862398.22 epoch sec; seqNo:2571; 4061705.42 north; 594636.66 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:06:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862396.19 2024-11-06T03:06:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:38Z- waiting for 1730862400.19, time is 1730862398.22 2024-11-06T03:06:38Z- AHRS_M2 msg: 1730862398.59 epoch sec; seqNo:2417 2024-11-06T03:06:38Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.51 psi 2024-11-06T03:06:38Z- handleDepth()- Depth_Keller msg: 1730862398.60 epoch sec; seqNo:2572; depth 50.07 2024-11-06T03:06:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862398.60 epoch sec; seqNo:2572; 4061705.42 north; 594636.66 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:06:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862396.19 2024-11-06T03:06:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:38Z- waiting for 1730862400.19, time is 1730862398.60 2024-11-06T03:06:39Z- AHRS_M2 msg: 1730862398.99 epoch sec; seqNo:2418 2024-11-06T03:06:39Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.51 psi 2024-11-06T03:06:39Z- handleDepth()- Depth_Keller msg: 1730862399.00 epoch sec; seqNo:2573; depth 50.05 2024-11-06T03:06:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862399.01 epoch sec; seqNo:2573; 4061705.42 north; 594636.66 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:06:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862396.19 2024-11-06T03:06:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:39Z- waiting for 1730862400.19, time is 1730862399.01 2024-11-06T03:06:39Z- AHRS_M2 msg: 1730862399.40 epoch sec; seqNo:2419 2024-11-06T03:06:39Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.51 psi 2024-11-06T03:06:39Z- handleDepth()- Depth_Keller msg: 1730862399.40 epoch sec; seqNo:2574; depth 50.06 2024-11-06T03:06:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862399.41 epoch sec; seqNo:2574; 4061705.44 north; 594637.88 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:06:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862396.19 2024-11-06T03:06:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:39Z- waiting for 1730862400.19, time is 1730862399.41 2024-11-06T03:06:39Z- AHRS_M2 msg: 1730862399.80 epoch sec; seqNo:2420 2024-11-06T03:06:39Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.51 psi 2024-11-06T03:06:39Z- handleDepth()- Depth_Keller msg: 1730862399.81 epoch sec; seqNo:2575; depth 50.03 2024-11-06T03:06:39Z- handleDVL() - DVL msg epochMillisec: 1730862399.83 2024-11-06T03:06:39Z- handleDvl() - RDI_Pathfinder msg: 1730862399.83 sec; seqNo:827; #beams:4 2024-11-06T03:06:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.87 , 31.77 , 33.98 , 34.49 2024-11-06T03:06:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.03 , -0.00 2024-11-06T03:06:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862399.85 epoch sec; seqNo:2575; 4061705.44 north; 594637.88 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:06:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862396.19 2024-11-06T03:06:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:39Z- waiting for 1730862400.19, time is 1730862399.85 2024-11-06T03:06:40Z- AHRS_M2 msg: 1730862400.21 epoch sec; seqNo:2421 2024-11-06T03:06:40Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.51 psi 2024-11-06T03:06:40Z- handleDepth()- Depth_Keller msg: 1730862400.21 epoch sec; seqNo:2576; depth 50.03 2024-11-06T03:06:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862400.24 epoch sec; seqNo:2576; 4061705.44 north; 594637.88 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:06:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862396.19 2024-11-06T03:06:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:40Z- waiting for 1730862400.19, time is 1730862400.24 2024-11-06T03:06:40Z- LcmTrn::updateTrn() >>>> pose time 1730862400.24, meas time 1730862399.83 <<<< 2024-11-06T03:06:40Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862400.24 2024-11-06T03:06:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862399.83 sec, ping # 0; waiting for INS data more recent than 1730862396.19... 2024-11-06T03:06:40Z- TerrainNav::motionUpdate() - currEstimate = (4061705, 594637.88, 50.03) 2024-11-06T03:06:40Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:06:40Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:06:40Z- TNavPF::Projecting Measurements 2024-11-06T03:06:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:06:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:06:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:06:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:06:40Z- TNavFilter::4 good beams. 2024-11-06T03:06:40Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:06:40Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:06:40Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:06:40Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:06:40Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:06:40Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:06:40Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:06:40Z- TerrainMapOctree: (vehN, vehE ) = (4061705.40, 594637.51). 2024-11-06T03:06:40Z- TerrainMapOctree: Min Distance = 121.01. 2024-11-06T03:06:40Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:06:40Z- Forcing Subcloud Comparison 2024-11-06T03:06:40Z- Weighting particles with subcloud comparison 2024-11-06T03:06:40Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:40Z- meanExpectedMeasDiff: -0.886539 delta_rms_squared: 1.359948 alpha: 0.985507 2024-11-06T03:06:40Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:40Z- meanExpectedMeasDiff: 0.083394 delta_rms_squared: 1.022347 alpha: 0.980813 2024-11-06T03:06:40Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:40Z- meanExpectedMeasDiff: -0.230307 delta_rms_squared: 1.193274 alpha: 0.983516 2024-11-06T03:06:40Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:40Z- meanExpectedMeasDiff: 0.844300 delta_rms_squared: 1.153813 alpha: 0.982962 2024-11-06T03:06:40Z- Subcloud NIS: 0.198883 2024-11-06T03:06:40Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:06:40Z- TNavPF:: sumWeights = 0.540691 2024-11-06T03:06:40Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:06:40Z- TNavPF::First Term of Map Covariance Matrix is 0.40 2024-11-06T03:06:40Z- TNavPF::Calculated NIS Value : 0.21 numBeams normalized NIS: 0.05 2024-11-06T03:06:40Z- TerrainNav::Resampling particle filter... 2024-11-06T03:06:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862399.83 sec. 2024-11-06T03:06:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:06:40Z- variances([0], [2], and psi_berg) = 0.626075 0.193960 0.000000 2024-11-06T03:06:40Z- publishEstimatesDecoder() - at 1730862400.24 sec 2024-11-06T03:06:40Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:06:40Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061737.67 594621.48 59.77 2024-11-06T03:06:40Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061738.03 594621.42 59.54 lat/lon: 36.70 -121.94 2024-11-06T03:06:40Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.63 0.19 0.63 0.00 2024-11-06T03:06:40Z- AHRS_M2 msg: 1730862400.61 epoch sec; seqNo:2422 2024-11-06T03:06:40Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.51 psi 2024-11-06T03:06:40Z- handleDepth()- Depth_Keller msg: 1730862400.62 epoch sec; seqNo:2577; depth 50.02 2024-11-06T03:06:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862400.64 epoch sec; seqNo:2577; 4061705.45 north; 594639.10 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:06:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862400.24 2024-11-06T03:06:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:40Z- waiting for 1730862404.24, time is 1730862400.64 2024-11-06T03:06:41Z- AHRS_M2 msg: 1730862401.02 epoch sec; seqNo:2423 2024-11-06T03:06:41Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.51 psi 2024-11-06T03:06:41Z- handleDepth()- Depth_Keller msg: 1730862401.02 epoch sec; seqNo:2578; depth 49.99 2024-11-06T03:06:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862401.03 epoch sec; seqNo:2578; 4061705.45 north; 594639.10 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:06:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862400.24 2024-11-06T03:06:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:41Z- waiting for 1730862404.24, time is 1730862401.03 2024-11-06T03:06:41Z- AHRS_M2 msg: 1730862401.42 epoch sec; seqNo:2424 2024-11-06T03:06:41Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.51 psi 2024-11-06T03:06:41Z- handleDepth()- Depth_Keller msg: 1730862401.43 epoch sec; seqNo:2579; depth 49.96 2024-11-06T03:06:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862401.44 epoch sec; seqNo:2579; 4061705.45 north; 594639.10 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:06:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862400.24 2024-11-06T03:06:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:41Z- waiting for 1730862404.24, time is 1730862401.44 2024-11-06T03:06:41Z- AHRS_M2 msg: 1730862401.82 epoch sec; seqNo:2425 2024-11-06T03:06:41Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.51 psi 2024-11-06T03:06:41Z- handleDepth()- Depth_Keller msg: 1730862401.83 epoch sec; seqNo:2580; depth 49.95 2024-11-06T03:06:41Z- handleDVL() - DVL msg epochMillisec: 1730862401.86 2024-11-06T03:06:41Z- handleDvl() - RDI_Pathfinder msg: 1730862401.86 sec; seqNo:828; #beams:4 2024-11-06T03:06:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.70 , 31.80 , 34.10 , 34.25 2024-11-06T03:06:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , -0.06 2024-11-06T03:06:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862401.87 epoch sec; seqNo:2580; 4061705.47 north; 594640.32 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:06:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862400.24 2024-11-06T03:06:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:41Z- waiting for 1730862404.24, time is 1730862401.87 2024-11-06T03:06:42Z- AHRS_M2 msg: 1730862402.23 epoch sec; seqNo:2426 2024-11-06T03:06:42Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.51 psi 2024-11-06T03:06:42Z- handleDepth()- Depth_Keller msg: 1730862402.23 epoch sec; seqNo:2581; depth 49.92 2024-11-06T03:06:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862402.24 epoch sec; seqNo:2581; 4061705.47 north; 594640.32 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:06:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862400.24 2024-11-06T03:06:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:42Z- waiting for 1730862404.24, time is 1730862402.24 2024-11-06T03:06:42Z- AHRS_M2 msg: 1730862402.66 epoch sec; seqNo:2427 2024-11-06T03:06:42Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.51 psi 2024-11-06T03:06:42Z- handleDepth()- Depth_Keller msg: 1730862402.67 epoch sec; seqNo:2582; depth 49.90 2024-11-06T03:06:42Z- handleDVL() - DVL msg epochMillisec: 1730862402.67 2024-11-06T03:06:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:06:42Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:06:42Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:06:42Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:06:42Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:06:42Z- handleDvl() - RDI_Pathfinder msg: 1730862402.67 sec; seqNo:829; #beams:0 2024-11-06T03:06:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:06:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , -0.06 2024-11-06T03:06:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862402.68 epoch sec; seqNo:2582; 4061705.47 north; 594640.32 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:06:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862400.24 2024-11-06T03:06:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:42Z- waiting for 1730862404.24, time is 1730862402.68 2024-11-06T03:06:43Z- AHRS_M2 msg: 1730862403.04 epoch sec; seqNo:2428 2024-11-06T03:06:43Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.51 psi 2024-11-06T03:06:43Z- handleDepth()- Depth_Keller msg: 1730862403.04 epoch sec; seqNo:2583; depth 49.89 2024-11-06T03:06:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862403.06 epoch sec; seqNo:2583; 4061705.48 north; 594641.54 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:06:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862400.24 2024-11-06T03:06:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:43Z- waiting for 1730862404.24, time is 1730862403.06 2024-11-06T03:06:43Z- AHRS_M2 msg: 1730862403.44 epoch sec; seqNo:2429 2024-11-06T03:06:43Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.51 psi 2024-11-06T03:06:43Z- handleDepth()- Depth_Keller msg: 1730862403.45 epoch sec; seqNo:2584; depth 49.87 2024-11-06T03:06:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862403.48 epoch sec; seqNo:2584; 4061705.48 north; 594641.54 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:06:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862400.24 2024-11-06T03:06:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:43Z- waiting for 1730862404.24, time is 1730862403.48 2024-11-06T03:06:43Z- AHRS_M2 msg: 1730862403.84 epoch sec; seqNo:2430 2024-11-06T03:06:43Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.50 psi 2024-11-06T03:06:43Z- handleDepth()- Depth_Keller msg: 1730862403.85 epoch sec; seqNo:2585; depth 49.84 2024-11-06T03:06:43Z- handleDVL() - DVL msg epochMillisec: 1730862403.87 2024-11-06T03:06:43Z- handleDvl() - RDI_Pathfinder msg: 1730862403.87 sec; seqNo:830; #beams:4 2024-11-06T03:06:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.69 , 31.34 , 33.94 , 34.25 2024-11-06T03:06:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.07 2024-11-06T03:06:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862403.90 epoch sec; seqNo:2585; 4061705.48 north; 594641.54 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:06:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862400.24 2024-11-06T03:06:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:44Z- waiting for 1730862404.24, time is 1730862403.90 2024-11-06T03:06:44Z- AHRS_M2 msg: 1730862404.24 epoch sec; seqNo:2431 2024-11-06T03:06:44Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.50 psi 2024-11-06T03:06:44Z- handleDepth()- Depth_Keller msg: 1730862404.26 epoch sec; seqNo:2586; depth 49.83 2024-11-06T03:06:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862404.27 epoch sec; seqNo:2586; 4061705.48 north; 594641.54 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:06:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862400.24 2024-11-06T03:06:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:44Z- waiting for 1730862404.24, time is 1730862404.27 2024-11-06T03:06:44Z- LcmTrn::updateTrn() >>>> pose time 1730862404.27, meas time 1730862403.87 <<<< 2024-11-06T03:06:44Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862404.27 2024-11-06T03:06:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862403.87 sec, ping # 0; waiting for INS data more recent than 1730862400.24... 2024-11-06T03:06:44Z- TerrainNav::motionUpdate() - currEstimate = (4061705, 594641.54, 49.83) 2024-11-06T03:06:44Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:06:44Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:06:44Z- TNavPF::Projecting Measurements 2024-11-06T03:06:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:06:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:06:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:06:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:06:44Z- TNavFilter::4 good beams. 2024-11-06T03:06:44Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:06:44Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:06:44Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:06:44Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:06:44Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:06:44Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:06:44Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:06:44Z- TerrainMapOctree: (vehN, vehE ) = (4061705.47, 594641.18). 2024-11-06T03:06:44Z- TerrainMapOctree: Min Distance = 117.58. 2024-11-06T03:06:44Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:06:44Z- Forcing Subcloud Comparison 2024-11-06T03:06:44Z- Weighting particles with subcloud comparison 2024-11-06T03:06:44Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:44Z- meanExpectedMeasDiff: -1.371603 delta_rms_squared: 1.408634 alpha: 0.986001 2024-11-06T03:06:44Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:44Z- meanExpectedMeasDiff: -0.457341 delta_rms_squared: 1.048756 alpha: 0.981287 2024-11-06T03:06:44Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:44Z- meanExpectedMeasDiff: -0.396302 delta_rms_squared: 1.224509 alpha: 0.983929 2024-11-06T03:06:44Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:44Z- meanExpectedMeasDiff: 0.339107 delta_rms_squared: 1.193824 alpha: 0.983523 2024-11-06T03:06:44Z- Subcloud NIS: 0.201481 2024-11-06T03:06:44Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:06:44Z- TNavPF:: sumWeights = 0.514995 2024-11-06T03:06:44Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:06:44Z- TNavPF::First Term of Map Covariance Matrix is 0.45 2024-11-06T03:06:44Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.05 2024-11-06T03:06:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862403.87 sec. 2024-11-06T03:06:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:06:44Z- variances([0], [2], and psi_berg) = 0.636090 0.195594 0.000000 2024-11-06T03:06:44Z- publishEstimatesDecoder() - at 1730862404.27 sec 2024-11-06T03:06:44Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:06:44Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061736.19 594624.90 59.90 2024-11-06T03:06:44Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061738.07 594625.08 59.58 lat/lon: 36.70 -121.94 2024-11-06T03:06:44Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.64 0.20 0.63 0.00 2024-11-06T03:06:44Z- AHRS_M2 msg: 1730862404.65 epoch sec; seqNo:2432 2024-11-06T03:06:44Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.50 psi 2024-11-06T03:06:44Z- handleDepth()- Depth_Keller msg: 1730862404.66 epoch sec; seqNo:2587; depth 49.81 2024-11-06T03:06:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862404.68 epoch sec; seqNo:2587; 4061705.49 north; 594642.76 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:06:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862404.27 2024-11-06T03:06:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:44Z- waiting for 1730862408.27, time is 1730862404.68 2024-11-06T03:06:45Z- AHRS_M2 msg: 1730862405.05 epoch sec; seqNo:2433 2024-11-06T03:06:45Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.49 psi 2024-11-06T03:06:45Z- handleDepth()- Depth_Keller msg: 1730862405.06 epoch sec; seqNo:2588; depth 49.80 2024-11-06T03:06:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862405.07 epoch sec; seqNo:2588; 4061705.49 north; 594642.76 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:06:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862404.27 2024-11-06T03:06:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:45Z- waiting for 1730862408.27, time is 1730862405.07 2024-11-06T03:06:45Z- AHRS_M2 msg: 1730862405.46 epoch sec; seqNo:2434 2024-11-06T03:06:45Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.49 psi 2024-11-06T03:06:45Z- handleDepth()- Depth_Keller msg: 1730862405.48 epoch sec; seqNo:2589; depth 49.78 2024-11-06T03:06:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862405.52 epoch sec; seqNo:2589; 4061705.49 north; 594642.76 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:06:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862404.27 2024-11-06T03:06:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:45Z- waiting for 1730862408.27, time is 1730862405.52 2024-11-06T03:06:45Z- AHRS_M2 msg: 1730862405.86 epoch sec; seqNo:2435 2024-11-06T03:06:45Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.49 psi 2024-11-06T03:06:45Z- handleDepth()- Depth_Keller msg: 1730862405.87 epoch sec; seqNo:2590; depth 49.77 2024-11-06T03:06:45Z- handleDVL() - DVL msg epochMillisec: 1730862405.89 2024-11-06T03:06:45Z- handleDvl() - RDI_Pathfinder msg: 1730862405.89 sec; seqNo:831; #beams:4 2024-11-06T03:06:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.54 , 31.34 , 33.64 , 34.10 2024-11-06T03:06:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.06 , -0.03 2024-11-06T03:06:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862405.90 epoch sec; seqNo:2590; 4061705.87 north; 594642.76 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:06:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862404.27 2024-11-06T03:06:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:46Z- waiting for 1730862408.27, time is 1730862405.90 2024-11-06T03:06:46Z- AHRS_M2 msg: 1730862406.27 epoch sec; seqNo:2436 2024-11-06T03:06:46Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.49 psi 2024-11-06T03:06:46Z- handleDepth()- Depth_Keller msg: 1730862406.27 epoch sec; seqNo:2591; depth 49.76 2024-11-06T03:06:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862406.30 epoch sec; seqNo:2591; 4061705.88 north; 594643.98 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:06:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862404.27 2024-11-06T03:06:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:46Z- waiting for 1730862408.27, time is 1730862406.30 2024-11-06T03:06:46Z- AHRS_M2 msg: 1730862406.67 epoch sec; seqNo:2437 2024-11-06T03:06:46Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.48 psi 2024-11-06T03:06:46Z- handleDepth()- Depth_Keller msg: 1730862406.68 epoch sec; seqNo:2592; depth 49.77 2024-11-06T03:06:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862406.69 epoch sec; seqNo:2592; 4061705.88 north; 594643.98 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:06:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862404.27 2024-11-06T03:06:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:46Z- waiting for 1730862408.27, time is 1730862406.69 2024-11-06T03:06:47Z- AHRS_M2 msg: 1730862407.08 epoch sec; seqNo:2438 2024-11-06T03:06:47Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.48 psi 2024-11-06T03:06:47Z- handleDepth()- Depth_Keller msg: 1730862407.08 epoch sec; seqNo:2593; depth 49.76 2024-11-06T03:06:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862407.09 epoch sec; seqNo:2593; 4061705.88 north; 594643.98 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:06:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862404.27 2024-11-06T03:06:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:47Z- waiting for 1730862408.27, time is 1730862407.09 2024-11-06T03:06:47Z- AHRS_M2 msg: 1730862407.51 epoch sec; seqNo:2439 2024-11-06T03:06:47Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.48 psi 2024-11-06T03:06:47Z- handleDepth()- Depth_Keller msg: 1730862407.55 epoch sec; seqNo:2594; depth 49.76 2024-11-06T03:06:47Z- handleDVL() - DVL msg epochMillisec: 1730862407.58 2024-11-06T03:06:47Z- handleDvl() - RDI_Pathfinder msg: 1730862407.58 sec; seqNo:832; #beams:4 2024-11-06T03:06:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.24 , 31.34 , 33.64 , 34.25 2024-11-06T03:06:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , 0.04 2024-11-06T03:06:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862407.60 epoch sec; seqNo:2594; 4061705.90 north; 594645.20 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:06:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862404.27 2024-11-06T03:06:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:47Z- waiting for 1730862408.27, time is 1730862407.60 2024-11-06T03:06:47Z- handleDepth()- Depth_Keller msg: 1730862407.90 epoch sec; seqNo:2595; depth 49.76 2024-11-06T03:06:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862407.91 epoch sec; seqNo:2595; 4061705.90 north; 594645.20 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:06:48Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:06:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862404.27 2024-11-06T03:06:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:48Z- waiting for 1730862408.27, time is 1730862407.91 2024-11-06T03:06:48Z- AHRS_M2 msg: 1730862408.29 epoch sec; seqNo:2440 2024-11-06T03:06:48Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.48 psi 2024-11-06T03:06:48Z- handleDepth()- Depth_Keller msg: 1730862408.29 epoch sec; seqNo:2596; depth 49.76 2024-11-06T03:06:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862408.30 epoch sec; seqNo:2596; 4061705.90 north; 594645.20 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:06:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862404.27 2024-11-06T03:06:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:48Z- waiting for 1730862408.27, time is 1730862408.30 2024-11-06T03:06:48Z- LcmTrn::updateTrn() >>>> pose time 1730862408.30, meas time 1730862407.58 <<<< 2024-11-06T03:06:48Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862408.30 2024-11-06T03:06:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862407.58 sec, ping # 0; waiting for INS data more recent than 1730862404.27... 2024-11-06T03:06:48Z- TerrainNav::motionUpdate() - currEstimate = (4061706, 594645.20, 49.76) 2024-11-06T03:06:48Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:06:48Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:06:48Z- TNavPF::Projecting Measurements 2024-11-06T03:06:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:06:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:06:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:06:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:06:48Z- TNavFilter::4 good beams. 2024-11-06T03:06:48Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:06:48Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:06:48Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:06:48Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:06:48Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:06:48Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:06:48Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:06:48Z- TerrainMapOctree: (vehN, vehE ) = (4061705.82, 594644.55). 2024-11-06T03:06:48Z- TerrainMapOctree: Min Distance = 114.33. 2024-11-06T03:06:48Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:06:48Z- Forcing Subcloud Comparison 2024-11-06T03:06:48Z- Weighting particles with subcloud comparison 2024-11-06T03:06:48Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:48Z- meanExpectedMeasDiff: -1.642415 delta_rms_squared: 1.378369 alpha: 0.985698 2024-11-06T03:06:48Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:48Z- meanExpectedMeasDiff: -0.274309 delta_rms_squared: 1.024829 alpha: 0.980858 2024-11-06T03:06:48Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:48Z- meanExpectedMeasDiff: -0.483472 delta_rms_squared: 1.195165 alpha: 0.983541 2024-11-06T03:06:48Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:48Z- meanExpectedMeasDiff: 0.494001 delta_rms_squared: 1.169417 alpha: 0.983185 2024-11-06T03:06:48Z- Subcloud NIS: 0.205190 2024-11-06T03:06:48Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:06:48Z- TNavPF:: sumWeights = 0.461192 2024-11-06T03:06:48Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:06:48Z- TNavPF::First Term of Map Covariance Matrix is 0.52 2024-11-06T03:06:48Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.05 2024-11-06T03:06:48Z- TerrainNav::Resampling particle filter... 2024-11-06T03:06:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862407.58 sec. 2024-11-06T03:06:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:06:48Z- variances([0], [2], and psi_berg) = 0.640888 0.194840 0.000000 2024-11-06T03:06:48Z- publishEstimatesDecoder() - at 1730862408.30 sec 2024-11-06T03:06:48Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:06:48Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061738.10 594628.77 59.82 2024-11-06T03:06:48Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061738.49 594628.73 59.75 lat/lon: 36.70 -121.94 2024-11-06T03:06:48Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.64 0.19 0.64 0.00 2024-11-06T03:06:48Z- AHRS_M2 msg: 1730862408.69 epoch sec; seqNo:2441 2024-11-06T03:06:48Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.47 psi 2024-11-06T03:06:48Z- handleDepth()- Depth_Keller msg: 1730862408.70 epoch sec; seqNo:2597; depth 49.75 2024-11-06T03:06:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862408.71 epoch sec; seqNo:2597; 4061706.28 north; 594645.20 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:06:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862408.30 2024-11-06T03:06:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:48Z- waiting for 1730862412.30, time is 1730862408.71 2024-11-06T03:06:49Z- AHRS_M2 msg: 1730862409.09 epoch sec; seqNo:2442 2024-11-06T03:06:49Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.47 psi 2024-11-06T03:06:49Z- handleDepth()- Depth_Keller msg: 1730862409.11 epoch sec; seqNo:2598; depth 49.76 2024-11-06T03:06:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862409.13 epoch sec; seqNo:2598; 4061706.29 north; 594646.42 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:06:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862408.30 2024-11-06T03:06:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:49Z- waiting for 1730862412.30, time is 1730862409.13 2024-11-06T03:06:49Z- handleDepth()- Depth_Keller msg: 1730862409.57 epoch sec; seqNo:2599; depth 49.79 2024-11-06T03:06:49Z- handleDVL() - DVL msg epochMillisec: 1730862409.60 2024-11-06T03:06:49Z- handleDvl() - RDI_Pathfinder msg: 1730862409.60 sec; seqNo:833; #beams:4 2024-11-06T03:06:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.08 , 31.50 , 33.94 , 34.56 2024-11-06T03:06:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , 0.06 2024-11-06T03:06:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862409.61 epoch sec; seqNo:2599; 4061706.29 north; 594646.42 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:06:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862408.30 2024-11-06T03:06:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:49Z- waiting for 1730862412.30, time is 1730862409.61 2024-11-06T03:06:49Z- AHRS_M2 msg: 1730862409.91 epoch sec; seqNo:2443 2024-11-06T03:06:49Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.47 psi 2024-11-06T03:06:49Z- handleDepth()- Depth_Keller msg: 1730862409.92 epoch sec; seqNo:2600; depth 49.79 2024-11-06T03:06:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862409.93 epoch sec; seqNo:2600; 4061706.29 north; 594646.42 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:06:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862408.30 2024-11-06T03:06:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:50Z- waiting for 1730862412.30, time is 1730862409.93 2024-11-06T03:06:50Z- AHRS_M2 msg: 1730862410.31 epoch sec; seqNo:2444 2024-11-06T03:06:50Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.47 psi 2024-11-06T03:06:50Z- handleDepth()- Depth_Keller msg: 1730862410.32 epoch sec; seqNo:2601; depth 49.76 2024-11-06T03:06:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862410.33 epoch sec; seqNo:2601; 4061706.29 north; 594646.42 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:06:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862408.30 2024-11-06T03:06:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:50Z- waiting for 1730862412.30, time is 1730862410.33 2024-11-06T03:06:50Z- AHRS_M2 msg: 1730862410.71 epoch sec; seqNo:2445 2024-11-06T03:06:50Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.47 psi 2024-11-06T03:06:50Z- handleDepth()- Depth_Keller msg: 1730862410.71 epoch sec; seqNo:2602; depth 49.78 2024-11-06T03:06:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862410.72 epoch sec; seqNo:2602; 4061706.30 north; 594647.64 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:06:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862408.30 2024-11-06T03:06:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:50Z- waiting for 1730862412.30, time is 1730862410.72 2024-11-06T03:06:51Z- AHRS_M2 msg: 1730862411.11 epoch sec; seqNo:2446 2024-11-06T03:06:51Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.47 psi 2024-11-06T03:06:51Z- handleDepth()- Depth_Keller msg: 1730862411.12 epoch sec; seqNo:2603; depth 49.78 2024-11-06T03:06:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862411.13 epoch sec; seqNo:2603; 4061706.30 north; 594647.64 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:06:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862408.30 2024-11-06T03:06:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:51Z- waiting for 1730862412.30, time is 1730862411.13 2024-11-06T03:06:51Z- AHRS_M2 msg: 1730862411.56 epoch sec; seqNo:2447 2024-11-06T03:06:51Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.47 psi 2024-11-06T03:06:51Z- handleDepth()- Depth_Keller msg: 1730862411.56 epoch sec; seqNo:2604; depth 49.79 2024-11-06T03:06:51Z- handleDVL() - DVL msg epochMillisec: 1730862411.59 2024-11-06T03:06:51Z- handleDvl() - RDI_Pathfinder msg: 1730862411.59 sec; seqNo:834; #beams:4 2024-11-06T03:06:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 30.73 , 33.94 , 34.40 2024-11-06T03:06:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.04 2024-11-06T03:06:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862411.60 epoch sec; seqNo:2604; 4061706.68 north; 594647.63 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:06:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862408.30 2024-11-06T03:06:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:51Z- waiting for 1730862412.30, time is 1730862411.60 2024-11-06T03:06:51Z- AHRS_M2 msg: 1730862411.92 epoch sec; seqNo:2448 2024-11-06T03:06:51Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.47 psi 2024-11-06T03:06:51Z- handleDepth()- Depth_Keller msg: 1730862411.93 epoch sec; seqNo:2605; depth 49.79 2024-11-06T03:06:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862411.95 epoch sec; seqNo:2605; 4061706.70 north; 594648.85 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:06:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862408.30 2024-11-06T03:06:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:52Z- waiting for 1730862412.30, time is 1730862411.95 2024-11-06T03:06:52Z- AHRS_M2 msg: 1730862412.33 epoch sec; seqNo:2449 2024-11-06T03:06:52Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.48 psi 2024-11-06T03:06:52Z- handleDepth()- Depth_Keller msg: 1730862412.33 epoch sec; seqNo:2606; depth 49.78 2024-11-06T03:06:52Z- handleDVL() - DVL msg epochMillisec: 1730862412.34 2024-11-06T03:06:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:06:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:06:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:06:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:06:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:06:52Z- handleDvl() - RDI_Pathfinder msg: 1730862412.34 sec; seqNo:835; #beams:0 2024-11-06T03:06:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:06:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.04 2024-11-06T03:06:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862412.35 epoch sec; seqNo:2606; 4061706.70 north; 594648.85 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:06:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862408.30 2024-11-06T03:06:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:52Z- waiting for 1730862412.30, time is 1730862412.35 2024-11-06T03:06:52Z- LcmTrn::updateTrn() >>>> pose time 1730862412.35, meas time 1730862412.34 <<<< 2024-11-06T03:06:52Z- LcmTrn::updateTrn() equating pose 1730862412.34 and meas times 1730862412.34 2024-11-06T03:06:52Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862412.34 2024-11-06T03:06:52Z- TerrainNav::motionUpdate() - currEstimate = (4061707, 594648.85, 49.78) 2024-11-06T03:06:52Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:06:52Z- TerrainNav:: SubcloudNIS is 0.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:06:52Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:06:52Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:06:52Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:06:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:06:52Z- variances([0], [2], and psi_berg) = 0.643596 0.197501 0.000000 2024-11-06T03:06:52Z- publishEstimatesDecoder() - at 1730862412.34 sec 2024-11-06T03:06:52Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:06:52Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061738.87 594632.41 59.80 2024-11-06T03:06:52Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061739.29 594632.39 59.78 lat/lon: 36.70 -121.94 2024-11-06T03:06:52Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.64 0.20 0.90 0.00 2024-11-06T03:06:52Z- handleDepth()- Depth_Keller msg: 1730862412.76 epoch sec; seqNo:2607; depth 49.77 2024-11-06T03:06:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862412.77 epoch sec; seqNo:2607; 4061706.70 north; 594648.85 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:06:52Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:06:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862412.34 2024-11-06T03:06:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:52Z- waiting for 1730862416.34, time is 1730862412.77 2024-11-06T03:06:53Z- AHRS_M2 msg: 1730862413.13 epoch sec; seqNo:2450 2024-11-06T03:06:53Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:06:53Z- handleDepth()- Depth_Keller msg: 1730862413.15 epoch sec; seqNo:2608; depth 49.77 2024-11-06T03:06:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862413.16 epoch sec; seqNo:2608; 4061706.70 north; 594648.85 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:06:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862412.34 2024-11-06T03:06:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:53Z- waiting for 1730862416.34, time is 1730862413.16 2024-11-06T03:06:53Z- AHRS_M2 msg: 1730862413.56 epoch sec; seqNo:2451 2024-11-06T03:06:53Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:06:53Z- handleDepth()- Depth_Keller msg: 1730862413.57 epoch sec; seqNo:2609; depth 49.76 2024-11-06T03:06:53Z- handleDVL() - DVL msg epochMillisec: 1730862413.60 2024-11-06T03:06:53Z- handleDvl() - RDI_Pathfinder msg: 1730862413.60 sec; seqNo:836; #beams:4 2024-11-06T03:06:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 30.73 , 33.94 , 34.56 2024-11-06T03:06:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , 0.00 2024-11-06T03:06:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862413.63 epoch sec; seqNo:2609; 4061706.71 north; 594650.07 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:06:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862412.34 2024-11-06T03:06:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:53Z- waiting for 1730862416.34, time is 1730862413.63 2024-11-06T03:06:53Z- AHRS_M2 msg: 1730862413.94 epoch sec; seqNo:2452 2024-11-06T03:06:53Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:06:53Z- handleDepth()- Depth_Keller msg: 1730862413.95 epoch sec; seqNo:2610; depth 49.76 2024-11-06T03:06:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862413.97 epoch sec; seqNo:2610; 4061706.71 north; 594650.07 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:06:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862412.34 2024-11-06T03:06:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:54Z- waiting for 1730862416.34, time is 1730862413.97 2024-11-06T03:06:54Z- AHRS_M2 msg: 1730862414.35 epoch sec; seqNo:2453 2024-11-06T03:06:54Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:06:54Z- handleDepth()- Depth_Keller msg: 1730862414.35 epoch sec; seqNo:2611; depth 49.75 2024-11-06T03:06:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862414.36 epoch sec; seqNo:2611; 4061706.71 north; 594650.07 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:06:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862412.34 2024-11-06T03:06:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:54Z- waiting for 1730862416.34, time is 1730862414.36 2024-11-06T03:06:54Z- AHRS_M2 msg: 1730862414.75 epoch sec; seqNo:2454 2024-11-06T03:06:54Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:06:54Z- handleDepth()- Depth_Keller msg: 1730862414.76 epoch sec; seqNo:2612; depth 49.73 2024-11-06T03:06:54Z- handleDVL() - DVL msg epochMillisec: 1730862414.76 2024-11-06T03:06:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:06:54Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:06:54Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:06:54Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:06:54Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:06:54Z- handleDvl() - RDI_Pathfinder msg: 1730862414.76 sec; seqNo:837; #beams:0 2024-11-06T03:06:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:06:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , 0.00 2024-11-06T03:06:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862414.77 epoch sec; seqNo:2612; 4061706.72 north; 594651.29 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:06:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862412.34 2024-11-06T03:06:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:54Z- waiting for 1730862416.34, time is 1730862414.77 2024-11-06T03:06:55Z- AHRS_M2 msg: 1730862415.15 epoch sec; seqNo:2455 2024-11-06T03:06:55Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:06:55Z- handleDepth()- Depth_Keller msg: 1730862415.16 epoch sec; seqNo:2613; depth 49.72 2024-11-06T03:06:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862415.17 epoch sec; seqNo:2613; 4061706.72 north; 594651.29 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:06:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862412.34 2024-11-06T03:06:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:55Z- waiting for 1730862416.34, time is 1730862415.17 2024-11-06T03:06:55Z- AHRS_M2 msg: 1730862415.56 epoch sec; seqNo:2456 2024-11-06T03:06:55Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.48 psi 2024-11-06T03:06:55Z- handleDepth()- Depth_Keller msg: 1730862415.56 epoch sec; seqNo:2614; depth 49.70 2024-11-06T03:06:55Z- handleDVL() - DVL msg epochMillisec: 1730862415.59 2024-11-06T03:06:55Z- handleDvl() - RDI_Pathfinder msg: 1730862415.59 sec; seqNo:838; #beams:4 2024-11-06T03:06:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.63 , 29.97 , 34.10 , 34.40 2024-11-06T03:06:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , -0.02 2024-11-06T03:06:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862415.63 epoch sec; seqNo:2614; 4061707.10 north; 594651.29 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:06:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862412.34 2024-11-06T03:06:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:55Z- waiting for 1730862416.34, time is 1730862415.63 2024-11-06T03:06:55Z- AHRS_M2 msg: 1730862415.96 epoch sec; seqNo:2457 2024-11-06T03:06:55Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:06:55Z- handleDepth()- Depth_Keller msg: 1730862415.97 epoch sec; seqNo:2615; depth 49.69 2024-11-06T03:06:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862415.99 epoch sec; seqNo:2615; 4061707.12 north; 594652.51 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:06:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862412.34 2024-11-06T03:06:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:56Z- waiting for 1730862416.34, time is 1730862415.99 2024-11-06T03:06:56Z- AHRS_M2 msg: 1730862416.37 epoch sec; seqNo:2458 2024-11-06T03:06:56Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:06:56Z- handleDepth()- Depth_Keller msg: 1730862416.39 epoch sec; seqNo:2616; depth 49.68 2024-11-06T03:06:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862416.46 epoch sec; seqNo:2616; 4061707.12 north; 594652.51 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:06:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862412.34 2024-11-06T03:06:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:56Z- waiting for 1730862416.34, time is 1730862416.46 2024-11-06T03:06:56Z- LcmTrn::updateTrn() >>>> pose time 1730862416.46, meas time 1730862415.59 <<<< 2024-11-06T03:06:56Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862416.46 2024-11-06T03:06:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862415.59 sec, ping # 0; waiting for INS data more recent than 1730862412.34... 2024-11-06T03:06:56Z- TerrainNav::motionUpdate() - currEstimate = (4061707, 594652.51, 49.68) 2024-11-06T03:06:56Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:06:56Z- TerrainNav:: SubcloudNIS is 0.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:06:56Z- TNavPF::Projecting Measurements 2024-11-06T03:06:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:06:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:06:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:06:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:06:56Z- TNavFilter::4 good beams. 2024-11-06T03:06:56Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:06:56Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:06:56Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:06:56Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:06:56Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:06:56Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:06:56Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:06:56Z- TerrainMapOctree: (vehN, vehE ) = (4061707.03, 594651.73). 2024-11-06T03:06:56Z- TerrainMapOctree: Min Distance = 107.25. 2024-11-06T03:06:56Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:06:56Z- Forcing Subcloud Comparison 2024-11-06T03:06:56Z- Weighting particles with subcloud comparison 2024-11-06T03:06:56Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:56Z- meanExpectedMeasDiff: -2.794325 delta_rms_squared: 1.875003 alpha: 0.989446 2024-11-06T03:06:56Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:56Z- meanExpectedMeasDiff: -1.140863 delta_rms_squared: 1.312579 alpha: 0.984992 2024-11-06T03:06:56Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:56Z- meanExpectedMeasDiff: 0.106189 delta_rms_squared: 1.568979 alpha: 0.987413 2024-11-06T03:06:56Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:06:56Z- meanExpectedMeasDiff: 0.676761 delta_rms_squared: 1.544402 alpha: 0.987216 2024-11-06T03:06:56Z- Subcloud NIS: 0.219036 2024-11-06T03:06:56Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:06:56Z- TNavPF:: sumWeights = 0.208686 2024-11-06T03:06:56Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:06:56Z- TNavPF::First Term of Map Covariance Matrix is 0.79 2024-11-06T03:06:56Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T03:06:56Z- TerrainNav::Resampling particle filter... 2024-11-06T03:06:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862415.59 sec. 2024-11-06T03:06:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:06:56Z- variances([0], [2], and psi_berg) = 0.657029 0.202414 0.000000 2024-11-06T03:06:56Z- publishEstimatesDecoder() - at 1730862416.46 sec 2024-11-06T03:06:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:06:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061739.29 594636.12 59.45 2024-11-06T03:06:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061739.70 594636.04 60.12 lat/lon: 36.70 -121.94 2024-11-06T03:06:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.66 0.20 0.66 0.00 2024-11-06T03:06:56Z- AHRS_M2 msg: 1730862416.77 epoch sec; seqNo:2459 2024-11-06T03:06:56Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.48 psi 2024-11-06T03:06:56Z- handleDepth()- Depth_Keller msg: 1730862416.78 epoch sec; seqNo:2617; depth 49.66 2024-11-06T03:06:56Z- handleDVL() - DVL msg epochMillisec: 1730862416.83 2024-11-06T03:06:56Z- handleDvl() - RDI_Pathfinder msg: 1730862416.83 sec; seqNo:839; #beams:4 2024-11-06T03:06:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.78 , 29.51 , 33.94 , 34.40 2024-11-06T03:06:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.02 2024-11-06T03:06:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862416.85 epoch sec; seqNo:2617; 4061707.12 north; 594652.51 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:06:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862416.46 2024-11-06T03:06:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:56Z- waiting for 1730862420.46, time is 1730862416.85 2024-11-06T03:06:57Z- AHRS_M2 msg: 1730862417.17 epoch sec; seqNo:2460 2024-11-06T03:06:57Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.48 psi 2024-11-06T03:06:57Z- handleDepth()- Depth_Keller msg: 1730862417.18 epoch sec; seqNo:2618; depth 49.65 2024-11-06T03:06:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862417.21 epoch sec; seqNo:2618; 4061707.12 north; 594652.51 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:06:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862416.46 2024-11-06T03:06:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:57Z- waiting for 1730862420.46, time is 1730862417.21 2024-11-06T03:06:57Z- AHRS_M2 msg: 1730862417.58 epoch sec; seqNo:2461 2024-11-06T03:06:57Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.48 psi 2024-11-06T03:06:57Z- handleDepth()- Depth_Keller msg: 1730862417.59 epoch sec; seqNo:2619; depth 49.63 2024-11-06T03:06:57Z- handleDVL() - DVL msg epochMillisec: 1730862417.62 2024-11-06T03:06:57Z- handleDvl() - RDI_Pathfinder msg: 1730862417.62 sec; seqNo:840; #beams:4 2024-11-06T03:06:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.78 , 29.97 , 33.79 , 34.40 2024-11-06T03:06:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.02 2024-11-06T03:06:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862417.66 epoch sec; seqNo:2619; 4061707.13 north; 594653.73 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:06:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862416.46 2024-11-06T03:06:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:57Z- waiting for 1730862420.46, time is 1730862417.66 2024-11-06T03:06:57Z- AHRS_M2 msg: 1730862417.99 epoch sec; seqNo:2462 2024-11-06T03:06:58Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.48 psi 2024-11-06T03:06:58Z- handleDepth()- Depth_Keller msg: 1730862417.99 epoch sec; seqNo:2620; depth 49.61 2024-11-06T03:06:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862418.01 epoch sec; seqNo:2620; 4061707.13 north; 594653.73 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:06:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862416.46 2024-11-06T03:06:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:58Z- waiting for 1730862420.46, time is 1730862418.01 2024-11-06T03:06:58Z- AHRS_M2 msg: 1730862418.40 epoch sec; seqNo:2463 2024-11-06T03:06:58Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.48 psi 2024-11-06T03:06:58Z- handleDepth()- Depth_Keller msg: 1730862418.41 epoch sec; seqNo:2621; depth 49.61 2024-11-06T03:06:58Z- handleDVL() - DVL msg epochMillisec: 1730862418.42 2024-11-06T03:06:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:06:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:06:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:06:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:06:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:06:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:06:58Z- handleDvl() - RDI_Pathfinder msg: 1730862418.42 sec; seqNo:841; #beams:0 2024-11-06T03:06:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:06:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.02 2024-11-06T03:06:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862418.48 epoch sec; seqNo:2621; 4061707.13 north; 594653.73 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:06:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862416.46 2024-11-06T03:06:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:58Z- waiting for 1730862420.46, time is 1730862418.48 2024-11-06T03:06:58Z- AHRS_M2 msg: 1730862418.79 epoch sec; seqNo:2464 2024-11-06T03:06:58Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.48 psi 2024-11-06T03:06:58Z- handleDepth()- Depth_Keller msg: 1730862418.79 epoch sec; seqNo:2622; depth 49.59 2024-11-06T03:06:58Z- handleDVL() - DVL msg epochMillisec: 1730862418.83 2024-11-06T03:06:58Z- handleDvl() - RDI_Pathfinder msg: 1730862418.83 sec; seqNo:842; #beams:4 2024-11-06T03:06:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.24 , 29.66 , 33.64 , 34.40 2024-11-06T03:06:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.03 2024-11-06T03:06:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862418.84 epoch sec; seqNo:2622; 4061707.14 north; 594654.95 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:06:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862416.46 2024-11-06T03:06:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:58Z- waiting for 1730862420.46, time is 1730862418.84 2024-11-06T03:06:59Z- AHRS_M2 msg: 1730862419.20 epoch sec; seqNo:2465 2024-11-06T03:06:59Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.48 psi 2024-11-06T03:06:59Z- handleDepth()- Depth_Keller msg: 1730862419.20 epoch sec; seqNo:2623; depth 49.58 2024-11-06T03:06:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862419.22 epoch sec; seqNo:2623; 4061707.14 north; 594654.95 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:06:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:06:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862416.46 2024-11-06T03:06:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:59Z- waiting for 1730862420.46, time is 1730862419.22 2024-11-06T03:06:59Z- AHRS_M2 msg: 1730862419.60 epoch sec; seqNo:2466 2024-11-06T03:06:59Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.48 psi 2024-11-06T03:06:59Z- handleDepth()- Depth_Keller msg: 1730862419.61 epoch sec; seqNo:2624; depth 49.55 2024-11-06T03:06:59Z- handleDVL() - DVL msg epochMillisec: 1730862419.64 2024-11-06T03:06:59Z- handleDvl() - RDI_Pathfinder msg: 1730862419.64 sec; seqNo:843; #beams:4 2024-11-06T03:06:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.39 , 29.66 , 33.49 , 34.40 2024-11-06T03:06:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.00 , -0.02 2024-11-06T03:06:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862419.68 epoch sec; seqNo:2624; 4061707.14 north; 594654.95 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:06:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:06:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862416.46 2024-11-06T03:06:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:06:59Z- waiting for 1730862420.46, time is 1730862419.68 2024-11-06T03:07:00Z- AHRS_M2 msg: 1730862420.00 epoch sec; seqNo:2467 2024-11-06T03:07:00Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.48 psi 2024-11-06T03:07:00Z- handleDepth()- Depth_Keller msg: 1730862420.02 epoch sec; seqNo:2625; depth 49.55 2024-11-06T03:07:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862420.04 epoch sec; seqNo:2625; 4061707.52 north; 594654.95 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:07:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862416.46 2024-11-06T03:07:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:00Z- waiting for 1730862420.46, time is 1730862420.04 2024-11-06T03:07:00Z- AHRS_M2 msg: 1730862420.41 epoch sec; seqNo:2468 2024-11-06T03:07:00Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.48 psi 2024-11-06T03:07:00Z- handleDepth()- Depth_Keller msg: 1730862420.41 epoch sec; seqNo:2626; depth 49.54 2024-11-06T03:07:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862420.43 epoch sec; seqNo:2626; 4061707.53 north; 594656.17 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T03:07:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862416.46 2024-11-06T03:07:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:00Z- waiting for 1730862420.46, time is 1730862420.43 2024-11-06T03:07:00Z- AHRS_M2 msg: 1730862420.81 epoch sec; seqNo:2469 2024-11-06T03:07:00Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.48 psi 2024-11-06T03:07:00Z- handleDepth()- Depth_Keller msg: 1730862420.83 epoch sec; seqNo:2627; depth 49.53 2024-11-06T03:07:00Z- handleDVL() - DVL msg epochMillisec: 1730862420.85 2024-11-06T03:07:00Z- handleDvl() - RDI_Pathfinder msg: 1730862420.85 sec; seqNo:844; #beams:4 2024-11-06T03:07:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.54 , 29.81 , 33.33 , 34.40 2024-11-06T03:07:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , -0.02 2024-11-06T03:07:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862420.88 epoch sec; seqNo:2627; 4061707.53 north; 594656.17 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T03:07:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862416.46 2024-11-06T03:07:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:00Z- waiting for 1730862420.46, time is 1730862420.88 2024-11-06T03:07:00Z- LcmTrn::updateTrn() >>>> pose time 1730862420.88, meas time 1730862420.85 <<<< 2024-11-06T03:07:00Z- LcmTrn::updateTrn() equating pose 1730862420.85 and meas times 1730862420.85 2024-11-06T03:07:00Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862420.85 2024-11-06T03:07:00Z- TerrainNav::motionUpdate() - currEstimate = (4061708, 594656.17, 49.53) 2024-11-06T03:07:00Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:07:00Z- TerrainNav:: SubcloudNIS is 0.22, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:07:00Z- TNavPF::Projecting Measurements 2024-11-06T03:07:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:07:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:07:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:07:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:07:00Z- TNavFilter::4 good beams. 2024-11-06T03:07:00Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:07:00Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:07:00Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:07:00Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:07:00Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:07:00Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:07:00Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:07:00Z- TerrainMapOctree: (vehN, vehE ) = (4061707.53, 594656.17). 2024-11-06T03:07:00Z- TerrainMapOctree: Min Distance = 102.99. 2024-11-06T03:07:00Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:07:01Z- Forcing Subcloud Comparison 2024-11-06T03:07:01Z- Weighting particles with subcloud comparison 2024-11-06T03:07:01Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:01Z- meanExpectedMeasDiff: -1.501379 delta_rms_squared: 1.463195 alpha: 0.986516 2024-11-06T03:07:01Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:01Z- meanExpectedMeasDiff: -1.014571 delta_rms_squared: 1.026002 alpha: 0.980880 2024-11-06T03:07:01Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:01Z- meanExpectedMeasDiff: -0.074155 delta_rms_squared: 1.206424 alpha: 0.983692 2024-11-06T03:07:01Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:01Z- meanExpectedMeasDiff: 0.735486 delta_rms_squared: 1.225460 alpha: 0.983942 2024-11-06T03:07:01Z- Subcloud NIS: 0.214583 2024-11-06T03:07:01Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:07:01Z- TNavPF:: sumWeights = 0.413033 2024-11-06T03:07:01Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:07:01Z- TNavPF::First Term of Map Covariance Matrix is 0.46 2024-11-06T03:07:01Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.06 2024-11-06T03:07:01Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862420.85 sec, ping # 0. 2024-11-06T03:07:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:07:01Z- variances([0], [2], and psi_berg) = 0.656089 0.204473 0.000000 2024-11-06T03:07:01Z- publishEstimatesDecoder() - at 1730862420.85 sec 2024-11-06T03:07:01Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:07:01Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061740.04 594640.14 60.38 2024-11-06T03:07:01Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061740.13 594639.70 60.21 lat/lon: 36.70 -121.94 2024-11-06T03:07:01Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.66 0.20 0.38 0.00 2024-11-06T03:07:01Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:07:01Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:07:01Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:07:01Z- AHRS_M2 msg: 1730862421.21 epoch sec; seqNo:2470 2024-11-06T03:07:01Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.48 psi 2024-11-06T03:07:01Z- handleDepth()- Depth_Keller msg: 1730862421.24 epoch sec; seqNo:2628; depth 49.51 2024-11-06T03:07:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862421.25 epoch sec; seqNo:2628; 4061707.53 north; 594656.17 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T03:07:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862420.85 2024-11-06T03:07:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:01Z- waiting for 1730862424.85, time is 1730862421.25 2024-11-06T03:07:01Z- AHRS_M2 msg: 1730862421.62 epoch sec; seqNo:2471 2024-11-06T03:07:01Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.48 psi 2024-11-06T03:07:01Z- handleDepth()- Depth_Keller msg: 1730862421.63 epoch sec; seqNo:2629; depth 49.51 2024-11-06T03:07:01Z- handleDVL() - DVL msg epochMillisec: 1730862421.66 2024-11-06T03:07:01Z- handleDvl() - RDI_Pathfinder msg: 1730862421.66 sec; seqNo:845; #beams:4 2024-11-06T03:07:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.54 , 29.97 , 33.33 , 34.55 2024-11-06T03:07:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , -0.01 2024-11-06T03:07:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862421.68 epoch sec; seqNo:2629; 4061707.55 north; 594657.39 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T03:07:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862420.85 2024-11-06T03:07:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:01Z- waiting for 1730862424.85, time is 1730862421.68 2024-11-06T03:07:02Z- AHRS_M2 msg: 1730862422.02 epoch sec; seqNo:2472 2024-11-06T03:07:02Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.48 psi 2024-11-06T03:07:02Z- handleDepth()- Depth_Keller msg: 1730862422.03 epoch sec; seqNo:2630; depth 49.52 2024-11-06T03:07:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862422.04 epoch sec; seqNo:2630; 4061707.55 north; 594657.39 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T03:07:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862420.85 2024-11-06T03:07:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:02Z- waiting for 1730862424.85, time is 1730862422.04 2024-11-06T03:07:02Z- AHRS_M2 msg: 1730862422.43 epoch sec; seqNo:2473 2024-11-06T03:07:02Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.48 psi 2024-11-06T03:07:02Z- handleDepth()- Depth_Keller msg: 1730862422.43 epoch sec; seqNo:2631; depth 49.51 2024-11-06T03:07:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862422.44 epoch sec; seqNo:2631; 4061707.55 north; 594657.39 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T03:07:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862420.85 2024-11-06T03:07:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:02Z- waiting for 1730862424.85, time is 1730862422.44 2024-11-06T03:07:02Z- AHRS_M2 msg: 1730862422.84 epoch sec; seqNo:2474 2024-11-06T03:07:02Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.48 psi 2024-11-06T03:07:02Z- handleDepth()- Depth_Keller msg: 1730862422.85 epoch sec; seqNo:2632; depth 49.52 2024-11-06T03:07:02Z- handleDVL() - DVL msg epochMillisec: 1730862422.87 2024-11-06T03:07:02Z- handleDvl() - RDI_Pathfinder msg: 1730862422.87 sec; seqNo:846; #beams:4 2024-11-06T03:07:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.39 , 30.27 , 33.03 , 34.56 2024-11-06T03:07:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , -0.00 2024-11-06T03:07:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862422.89 epoch sec; seqNo:2632; 4061707.55 north; 594657.39 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T03:07:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862420.85 2024-11-06T03:07:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:02Z- waiting for 1730862424.85, time is 1730862422.89 2024-11-06T03:07:03Z- AHRS_M2 msg: 1730862423.23 epoch sec; seqNo:2475 2024-11-06T03:07:03Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.47 psi 2024-11-06T03:07:03Z- handleDepth()- Depth_Keller msg: 1730862423.24 epoch sec; seqNo:2633; depth 49.52 2024-11-06T03:07:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862423.26 epoch sec; seqNo:2633; 4061707.56 north; 594658.61 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T03:07:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862420.85 2024-11-06T03:07:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:03Z- waiting for 1730862424.85, time is 1730862423.26 2024-11-06T03:07:03Z- AHRS_M2 msg: 1730862423.65 epoch sec; seqNo:2476 2024-11-06T03:07:03Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.47 psi 2024-11-06T03:07:03Z- handleDepth()- Depth_Keller msg: 1730862423.65 epoch sec; seqNo:2634; depth 49.53 2024-11-06T03:07:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862423.67 epoch sec; seqNo:2634; 4061707.56 north; 594658.61 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T03:07:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862420.85 2024-11-06T03:07:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:03Z- waiting for 1730862424.85, time is 1730862423.67 2024-11-06T03:07:04Z- handleDepth()- Depth_Keller msg: 1730862424.07 epoch sec; seqNo:2635; depth 49.54 2024-11-06T03:07:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862424.08 epoch sec; seqNo:2635; 4061707.94 north; 594658.60 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T03:07:04Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:07:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862420.85 2024-11-06T03:07:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:04Z- waiting for 1730862424.85, time is 1730862424.08 2024-11-06T03:07:04Z- AHRS_M2 msg: 1730862424.45 epoch sec; seqNo:2477 2024-11-06T03:07:04Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.47 psi 2024-11-06T03:07:04Z- handleDepth()- Depth_Keller msg: 1730862424.45 epoch sec; seqNo:2636; depth 49.53 2024-11-06T03:07:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862424.46 epoch sec; seqNo:2636; 4061707.94 north; 594658.60 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T03:07:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862420.85 2024-11-06T03:07:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:04Z- waiting for 1730862424.85, time is 1730862424.46 2024-11-06T03:07:04Z- AHRS_M2 msg: 1730862424.86 epoch sec; seqNo:2478 2024-11-06T03:07:04Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.47 psi 2024-11-06T03:07:04Z- handleDepth()- Depth_Keller msg: 1730862424.87 epoch sec; seqNo:2637; depth 49.55 2024-11-06T03:07:04Z- handleDVL() - DVL msg epochMillisec: 1730862424.88 2024-11-06T03:07:04Z- handleDvl() - RDI_Pathfinder msg: 1730862424.88 sec; seqNo:847; #beams:4 2024-11-06T03:07:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.15 , 30.42 , 32.72 , 34.55 2024-11-06T03:07:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , 0.03 2024-11-06T03:07:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862424.91 epoch sec; seqNo:2637; 4061707.95 north; 594659.82 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:07:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862420.85 2024-11-06T03:07:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:05Z- waiting for 1730862424.85, time is 1730862424.91 2024-11-06T03:07:05Z- LcmTrn::updateTrn() >>>> pose time 1730862424.91, meas time 1730862424.88 <<<< 2024-11-06T03:07:05Z- LcmTrn::updateTrn() equating pose 1730862424.88 and meas times 1730862424.88 2024-11-06T03:07:05Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862424.88 2024-11-06T03:07:05Z- TerrainNav::motionUpdate() - currEstimate = (4061708, 594659.82, 49.55) 2024-11-06T03:07:05Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:07:05Z- TerrainNav:: SubcloudNIS is 0.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:07:05Z- TNavPF::Projecting Measurements 2024-11-06T03:07:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:07:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:07:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:07:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:07:05Z- TNavFilter::4 good beams. 2024-11-06T03:07:05Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:07:05Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:07:05Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:07:05Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:07:05Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:07:05Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:07:05Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:07:05Z- TerrainMapOctree: (vehN, vehE ) = (4061707.95, 594659.82). 2024-11-06T03:07:05Z- TerrainMapOctree: Min Distance = 99.50. 2024-11-06T03:07:05Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:07:05Z- Forcing Subcloud Comparison 2024-11-06T03:07:05Z- Weighting particles with subcloud comparison 2024-11-06T03:07:05Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:05Z- meanExpectedMeasDiff: -0.532482 delta_rms_squared: 1.010830 alpha: 0.980598 2024-11-06T03:07:05Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:05Z- meanExpectedMeasDiff: -0.152635 delta_rms_squared: 0.710162 alpha: 0.972609 2024-11-06T03:07:05Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:05Z- meanExpectedMeasDiff: -0.122342 delta_rms_squared: 0.820691 alpha: 0.976210 2024-11-06T03:07:05Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:05Z- meanExpectedMeasDiff: 0.907785 delta_rms_squared: 0.861910 alpha: 0.977322 2024-11-06T03:07:05Z- Subcloud NIS: 0.204542 2024-11-06T03:07:05Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:07:05Z- TNavPF:: sumWeights = 0.618113 2024-11-06T03:07:05Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:07:05Z- TNavPF::First Term of Map Covariance Matrix is 0.35 2024-11-06T03:07:05Z- TNavPF::Calculated NIS Value : 0.21 numBeams normalized NIS: 0.05 2024-11-06T03:07:05Z- TerrainNav::Resampling particle filter... 2024-11-06T03:07:05Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862424.88 sec, ping # 0. 2024-11-06T03:07:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:07:05Z- variances([0], [2], and psi_berg) = 0.661164 0.206297 0.000000 2024-11-06T03:07:05Z- publishEstimatesDecoder() - at 1730862424.88 sec 2024-11-06T03:07:05Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:07:05Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061740.12 594643.35 60.72 2024-11-06T03:07:05Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061740.55 594643.36 60.21 lat/lon: 36.70 -121.94 2024-11-06T03:07:05Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.66 0.21 0.32 0.00 2024-11-06T03:07:05Z- AHRS_M2 msg: 1730862425.27 epoch sec; seqNo:2479 2024-11-06T03:07:05Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.47 psi 2024-11-06T03:07:05Z- handleDepth()- Depth_Keller msg: 1730862425.28 epoch sec; seqNo:2638; depth 49.55 2024-11-06T03:07:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862425.30 epoch sec; seqNo:2638; 4061707.95 north; 594659.82 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:07:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862424.88 2024-11-06T03:07:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:05Z- waiting for 1730862428.88, time is 1730862425.30 2024-11-06T03:07:05Z- AHRS_M2 msg: 1730862425.67 epoch sec; seqNo:2480 2024-11-06T03:07:05Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.47 psi 2024-11-06T03:07:05Z- handleDepth()- Depth_Keller msg: 1730862425.67 epoch sec; seqNo:2639; depth 49.56 2024-11-06T03:07:05Z- handleDVL() - DVL msg epochMillisec: 1730862425.67 2024-11-06T03:07:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:05Z- handleDvl() - RDI_Pathfinder msg: 1730862425.67 sec; seqNo:848; #beams:0 2024-11-06T03:07:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , 0.03 2024-11-06T03:07:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862425.70 epoch sec; seqNo:2639; 4061707.95 north; 594659.82 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:07:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862424.88 2024-11-06T03:07:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:05Z- waiting for 1730862428.88, time is 1730862425.70 2024-11-06T03:07:06Z- AHRS_M2 msg: 1730862426.07 epoch sec; seqNo:2481 2024-11-06T03:07:06Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.47 psi 2024-11-06T03:07:06Z- handleDepth()- Depth_Keller msg: 1730862426.08 epoch sec; seqNo:2640; depth 49.57 2024-11-06T03:07:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862426.09 epoch sec; seqNo:2640; 4061707.95 north; 594659.82 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:07:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862424.88 2024-11-06T03:07:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:06Z- waiting for 1730862428.88, time is 1730862426.09 2024-11-06T03:07:06Z- AHRS_M2 msg: 1730862426.52 epoch sec; seqNo:2482 2024-11-06T03:07:06Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.47 psi 2024-11-06T03:07:06Z- handleDepth()- Depth_Keller msg: 1730862426.54 epoch sec; seqNo:2641; depth 49.58 2024-11-06T03:07:06Z- handleDVL() - DVL msg epochMillisec: 1730862426.56 2024-11-06T03:07:06Z- handleDvl() - RDI_Pathfinder msg: 1730862426.56 sec; seqNo:849; #beams:4 2024-11-06T03:07:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.30 , 29.97 , 32.56 , 34.86 2024-11-06T03:07:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.05 2024-11-06T03:07:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862426.59 epoch sec; seqNo:2641; 4061707.97 north; 594661.04 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:07:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862424.88 2024-11-06T03:07:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:06Z- waiting for 1730862428.88, time is 1730862426.59 2024-11-06T03:07:06Z- AHRS_M2 msg: 1730862426.89 epoch sec; seqNo:2483 2024-11-06T03:07:06Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.47 psi 2024-11-06T03:07:06Z- handleDepth()- Depth_Keller msg: 1730862426.89 epoch sec; seqNo:2642; depth 49.59 2024-11-06T03:07:06Z- handleDVL() - DVL msg epochMillisec: 1730862426.90 2024-11-06T03:07:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:06Z- handleDvl() - RDI_Pathfinder msg: 1730862426.90 sec; seqNo:850; #beams:0 2024-11-06T03:07:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.05 2024-11-06T03:07:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862426.91 epoch sec; seqNo:2642; 4061708.35 north; 594661.04 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:07:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862424.88 2024-11-06T03:07:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:07Z- waiting for 1730862428.88, time is 1730862426.91 2024-11-06T03:07:07Z- AHRS_M2 msg: 1730862427.29 epoch sec; seqNo:2484 2024-11-06T03:07:07Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.47 psi 2024-11-06T03:07:07Z- handleDepth()- Depth_Keller msg: 1730862427.29 epoch sec; seqNo:2643; depth 49.60 2024-11-06T03:07:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862427.32 epoch sec; seqNo:2643; 4061708.35 north; 594661.04 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:07:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862424.88 2024-11-06T03:07:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:07Z- waiting for 1730862428.88, time is 1730862427.32 2024-11-06T03:07:07Z- handleDepth()- Depth_Keller msg: 1730862427.72 epoch sec; seqNo:2644; depth 49.62 2024-11-06T03:07:07Z- handleDVL() - DVL msg epochMillisec: 1730862427.74 2024-11-06T03:07:07Z- handleDvl() - RDI_Pathfinder msg: 1730862427.74 sec; seqNo:851; #beams:4 2024-11-06T03:07:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.46 , 29.97 , 32.41 , 35.16 2024-11-06T03:07:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.04 2024-11-06T03:07:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862427.76 epoch sec; seqNo:2644; 4061708.36 north; 594662.26 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:07:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862424.88 2024-11-06T03:07:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:07Z- waiting for 1730862428.88, time is 1730862427.76 2024-11-06T03:07:08Z- AHRS_M2 msg: 1730862428.09 epoch sec; seqNo:2485 2024-11-06T03:07:08Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.47 psi 2024-11-06T03:07:08Z- handleDepth()- Depth_Keller msg: 1730862428.10 epoch sec; seqNo:2645; depth 49.61 2024-11-06T03:07:08Z- handleDVL() - DVL msg epochMillisec: 1730862428.10 2024-11-06T03:07:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:08Z- handleDvl() - RDI_Pathfinder msg: 1730862428.10 sec; seqNo:852; #beams:0 2024-11-06T03:07:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.04 2024-11-06T03:07:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862428.12 epoch sec; seqNo:2645; 4061708.36 north; 594662.26 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:07:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862424.88 2024-11-06T03:07:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:08Z- waiting for 1730862428.88, time is 1730862428.12 2024-11-06T03:07:08Z- AHRS_M2 msg: 1730862428.49 epoch sec; seqNo:2486 2024-11-06T03:07:08Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.47 psi 2024-11-06T03:07:08Z- handleDepth()- Depth_Keller msg: 1730862428.50 epoch sec; seqNo:2646; depth 49.63 2024-11-06T03:07:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862428.52 epoch sec; seqNo:2646; 4061708.36 north; 594662.26 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:07:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862424.88 2024-11-06T03:07:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:08Z- waiting for 1730862428.88, time is 1730862428.52 2024-11-06T03:07:08Z- AHRS_M2 msg: 1730862428.90 epoch sec; seqNo:2487 2024-11-06T03:07:08Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.47 psi 2024-11-06T03:07:08Z- handleDepth()- Depth_Keller msg: 1730862428.91 epoch sec; seqNo:2647; depth 49.64 2024-11-06T03:07:08Z- handleDVL() - DVL msg epochMillisec: 1730862428.93 2024-11-06T03:07:08Z- handleDvl() - RDI_Pathfinder msg: 1730862428.93 sec; seqNo:853; #beams:4 2024-11-06T03:07:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.85 , 30.12 , 32.41 , 35.01 2024-11-06T03:07:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , 0.03 2024-11-06T03:07:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862428.95 epoch sec; seqNo:2647; 4061708.37 north; 594663.48 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:07:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862424.88 2024-11-06T03:07:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:09Z- waiting for 1730862428.88, time is 1730862428.95 2024-11-06T03:07:09Z- LcmTrn::updateTrn() >>>> pose time 1730862428.95, meas time 1730862428.93 <<<< 2024-11-06T03:07:09Z- LcmTrn::updateTrn() equating pose 1730862428.93 and meas times 1730862428.93 2024-11-06T03:07:09Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862428.93 2024-11-06T03:07:09Z- TerrainNav::motionUpdate() - currEstimate = (4061708, 594663.48, 49.64) 2024-11-06T03:07:09Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:07:09Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:07:09Z- TNavPF::Projecting Measurements 2024-11-06T03:07:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:07:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:07:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:07:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:07:09Z- TNavFilter::4 good beams. 2024-11-06T03:07:09Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:07:09Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:07:09Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:07:09Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:07:09Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:07:09Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:07:09Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:07:09Z- TerrainMapOctree: (vehN, vehE ) = (4061708.37, 594663.48). 2024-11-06T03:07:09Z- TerrainMapOctree: Min Distance = 96.01. 2024-11-06T03:07:09Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:07:09Z- Forcing Subcloud Comparison 2024-11-06T03:07:09Z- Weighting particles with subcloud comparison 2024-11-06T03:07:09Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:09Z- meanExpectedMeasDiff: -0.354505 delta_rms_squared: 0.897772 alpha: 0.978208 2024-11-06T03:07:09Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:09Z- meanExpectedMeasDiff: -0.538420 delta_rms_squared: 0.650741 alpha: 0.970182 2024-11-06T03:07:09Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:09Z- meanExpectedMeasDiff: -0.092720 delta_rms_squared: 0.732770 alpha: 0.973431 2024-11-06T03:07:09Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:09Z- meanExpectedMeasDiff: 1.321132 delta_rms_squared: 0.787923 alpha: 0.975245 2024-11-06T03:07:09Z- Subcloud NIS: 0.198584 2024-11-06T03:07:09Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:07:09Z- TNavPF:: sumWeights = 0.549816 2024-11-06T03:07:09Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:07:09Z- TNavPF::First Term of Map Covariance Matrix is 0.29 2024-11-06T03:07:09Z- TNavPF::Calculated NIS Value : 0.31 numBeams normalized NIS: 0.08 2024-11-06T03:07:09Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862428.93 sec, ping # 0. 2024-11-06T03:07:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:07:09Z- variances([0], [2], and psi_berg) = 0.662323 0.207499 0.000000 2024-11-06T03:07:09Z- publishEstimatesDecoder() - at 1730862428.93 sec 2024-11-06T03:07:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:07:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061742.12 594647.16 60.24 2024-11-06T03:07:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061740.97 594647.01 60.28 lat/lon: 36.70 -121.94 2024-11-06T03:07:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.66 0.21 0.31 0.00 2024-11-06T03:07:09Z- AHRS_M2 msg: 1730862429.30 epoch sec; seqNo:2488 2024-11-06T03:07:09Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.47 psi 2024-11-06T03:07:09Z- handleDepth()- Depth_Keller msg: 1730862429.32 epoch sec; seqNo:2648; depth 49.66 2024-11-06T03:07:09Z- handleDVL() - DVL msg epochMillisec: 1730862429.32 2024-11-06T03:07:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:09Z- handleDvl() - RDI_Pathfinder msg: 1730862429.32 sec; seqNo:854; #beams:0 2024-11-06T03:07:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , 0.03 2024-11-06T03:07:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862429.33 epoch sec; seqNo:2648; 4061708.37 north; 594663.48 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:07:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862428.93 2024-11-06T03:07:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:09Z- waiting for 1730862432.93, time is 1730862429.33 2024-11-06T03:07:09Z- AHRS_M2 msg: 1730862429.71 epoch sec; seqNo:2489 2024-11-06T03:07:09Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.47 psi 2024-11-06T03:07:09Z- handleDepth()- Depth_Keller msg: 1730862429.72 epoch sec; seqNo:2649; depth 49.65 2024-11-06T03:07:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862429.74 epoch sec; seqNo:2649; 4061708.37 north; 594663.48 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:07:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862428.93 2024-11-06T03:07:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:09Z- waiting for 1730862432.93, time is 1730862429.74 2024-11-06T03:07:10Z- AHRS_M2 msg: 1730862430.11 epoch sec; seqNo:2490 2024-11-06T03:07:10Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.47 psi 2024-11-06T03:07:10Z- handleDepth()- Depth_Keller msg: 1730862430.12 epoch sec; seqNo:2650; depth 49.65 2024-11-06T03:07:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862430.13 epoch sec; seqNo:2650; 4061708.37 north; 594663.48 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:07:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862428.93 2024-11-06T03:07:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:10Z- waiting for 1730862432.93, time is 1730862430.13 2024-11-06T03:07:10Z- AHRS_M2 msg: 1730862430.52 epoch sec; seqNo:2491 2024-11-06T03:07:10Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.47 psi 2024-11-06T03:07:10Z- handleDepth()- Depth_Keller msg: 1730862430.52 epoch sec; seqNo:2651; depth 49.67 2024-11-06T03:07:10Z- handleDVL() - DVL msg epochMillisec: 1730862430.52 2024-11-06T03:07:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:10Z- handleDvl() - RDI_Pathfinder msg: 1730862430.52 sec; seqNo:855; #beams:0 2024-11-06T03:07:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , 0.03 2024-11-06T03:07:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862430.53 epoch sec; seqNo:2651; 4061708.77 north; 594664.70 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:07:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862428.93 2024-11-06T03:07:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:10Z- waiting for 1730862432.93, time is 1730862430.53 2024-11-06T03:07:10Z- AHRS_M2 msg: 1730862430.92 epoch sec; seqNo:2492 2024-11-06T03:07:10Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.47 psi 2024-11-06T03:07:10Z- handleDepth()- Depth_Keller msg: 1730862430.93 epoch sec; seqNo:2652; depth 49.68 2024-11-06T03:07:10Z- handleDVL() - DVL msg epochMillisec: 1730862430.95 2024-11-06T03:07:10Z- handleDvl() - RDI_Pathfinder msg: 1730862430.95 sec; seqNo:856; #beams:4 2024-11-06T03:07:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.85 , 30.27 , 32.11 , 35.01 2024-11-06T03:07:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , -0.01 2024-11-06T03:07:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862430.96 epoch sec; seqNo:2652; 4061708.77 north; 594664.70 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:07:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862428.93 2024-11-06T03:07:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:11Z- waiting for 1730862432.93, time is 1730862430.96 2024-11-06T03:07:11Z- AHRS_M2 msg: 1730862431.32 epoch sec; seqNo:2493 2024-11-06T03:07:11Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.47 psi 2024-11-06T03:07:11Z- handleDepth()- Depth_Keller msg: 1730862431.33 epoch sec; seqNo:2653; depth 49.68 2024-11-06T03:07:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862431.35 epoch sec; seqNo:2653; 4061708.77 north; 594664.70 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:07:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862428.93 2024-11-06T03:07:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:11Z- waiting for 1730862432.93, time is 1730862431.35 2024-11-06T03:07:11Z- AHRS_M2 msg: 1730862431.73 epoch sec; seqNo:2494 2024-11-06T03:07:11Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T03:07:11Z- handleDepth()- Depth_Keller msg: 1730862431.73 epoch sec; seqNo:2654; depth 49.68 2024-11-06T03:07:11Z- handleDVL() - DVL msg epochMillisec: 1730862431.74 2024-11-06T03:07:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:11Z- handleDvl() - RDI_Pathfinder msg: 1730862431.74 sec; seqNo:857; #beams:0 2024-11-06T03:07:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , -0.01 2024-11-06T03:07:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862431.75 epoch sec; seqNo:2654; 4061708.78 north; 594665.92 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:07:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862428.93 2024-11-06T03:07:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:11Z- waiting for 1730862432.93, time is 1730862431.75 2024-11-06T03:07:12Z- AHRS_M2 msg: 1730862432.13 epoch sec; seqNo:2495 2024-11-06T03:07:12Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T03:07:12Z- handleDepth()- Depth_Keller msg: 1730862432.14 epoch sec; seqNo:2655; depth 49.68 2024-11-06T03:07:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862432.15 epoch sec; seqNo:2655; 4061708.78 north; 594665.92 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:07:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862428.93 2024-11-06T03:07:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:12Z- waiting for 1730862432.93, time is 1730862432.15 2024-11-06T03:07:12Z- AHRS_M2 msg: 1730862432.57 epoch sec; seqNo:2496 2024-11-06T03:07:12Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.47 psi 2024-11-06T03:07:12Z- handleDepth()- Depth_Keller msg: 1730862432.57 epoch sec; seqNo:2656; depth 49.70 2024-11-06T03:07:12Z- handleDVL() - DVL msg epochMillisec: 1730862432.59 2024-11-06T03:07:12Z- handleDvl() - RDI_Pathfinder msg: 1730862432.59 sec; seqNo:858; #beams:4 2024-11-06T03:07:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.53 , 29.97 , 31.96 , 34.86 2024-11-06T03:07:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , -0.02 2024-11-06T03:07:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862432.60 epoch sec; seqNo:2656; 4061708.78 north; 594665.92 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:07:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862428.93 2024-11-06T03:07:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:12Z- waiting for 1730862432.93, time is 1730862432.60 2024-11-06T03:07:12Z- AHRS_M2 msg: 1730862432.94 epoch sec; seqNo:2497 2024-11-06T03:07:12Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.47 psi 2024-11-06T03:07:12Z- handleDepth()- Depth_Keller msg: 1730862432.95 epoch sec; seqNo:2657; depth 49.69 2024-11-06T03:07:12Z- handleDVL() - DVL msg epochMillisec: 1730862432.95 2024-11-06T03:07:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:12Z- handleDvl() - RDI_Pathfinder msg: 1730862432.95 sec; seqNo:859; #beams:0 2024-11-06T03:07:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , -0.02 2024-11-06T03:07:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862432.97 epoch sec; seqNo:2657; 4061708.78 north; 594665.92 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:07:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862428.93 2024-11-06T03:07:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:13Z- waiting for 1730862432.93, time is 1730862432.97 2024-11-06T03:07:13Z- LcmTrn::updateTrn() >>>> pose time 1730862432.97, meas time 1730862432.95 <<<< 2024-11-06T03:07:13Z- LcmTrn::updateTrn() equating pose 1730862432.95 and meas times 1730862432.95 2024-11-06T03:07:13Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862432.95 2024-11-06T03:07:13Z- TerrainNav::motionUpdate() - currEstimate = (4061709, 594665.92, 49.69) 2024-11-06T03:07:13Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:07:13Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:07:13Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:07:13Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:07:13Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:07:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:07:13Z- variances([0], [2], and psi_berg) = 0.663126 0.208584 0.000000 2024-11-06T03:07:13Z- publishEstimatesDecoder() - at 1730862432.95 sec 2024-11-06T03:07:13Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:07:13Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061742.48 594649.60 59.86 2024-11-06T03:07:13Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061741.38 594649.45 60.33 lat/lon: 36.70 -121.94 2024-11-06T03:07:13Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.66 0.21 0.54 0.00 2024-11-06T03:07:13Z- AHRS_M2 msg: 1730862433.34 epoch sec; seqNo:2498 2024-11-06T03:07:13Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.47 psi 2024-11-06T03:07:13Z- handleDepth()- Depth_Keller msg: 1730862433.35 epoch sec; seqNo:2658; depth 49.70 2024-11-06T03:07:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862433.37 epoch sec; seqNo:2658; 4061708.79 north; 594667.14 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:07:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862432.95 2024-11-06T03:07:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:13Z- waiting for 1730862436.95, time is 1730862433.37 2024-11-06T03:07:13Z- AHRS_M2 msg: 1730862433.75 epoch sec; seqNo:2499 2024-11-06T03:07:13Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.47 psi 2024-11-06T03:07:13Z- handleDepth()- Depth_Keller msg: 1730862433.75 epoch sec; seqNo:2659; depth 49.70 2024-11-06T03:07:13Z- handleDVL() - DVL msg epochMillisec: 1730862433.77 2024-11-06T03:07:13Z- handleDvl() - RDI_Pathfinder msg: 1730862433.77 sec; seqNo:860; #beams:4 2024-11-06T03:07:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.68 , 29.81 , 31.95 , 35.01 2024-11-06T03:07:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , -0.02 2024-11-06T03:07:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862433.80 epoch sec; seqNo:2659; 4061708.79 north; 594667.14 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:07:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862432.95 2024-11-06T03:07:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:13Z- waiting for 1730862436.95, time is 1730862433.80 2024-11-06T03:07:14Z- AHRS_M2 msg: 1730862434.15 epoch sec; seqNo:2500 2024-11-06T03:07:14Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T03:07:14Z- handleDepth()- Depth_Keller msg: 1730862434.17 epoch sec; seqNo:2660; depth 49.70 2024-11-06T03:07:14Z- handleDVL() - DVL msg epochMillisec: 1730862434.18 2024-11-06T03:07:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:14Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:14Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:14Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:14Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:14Z- handleDvl() - RDI_Pathfinder msg: 1730862434.18 sec; seqNo:861; #beams:0 2024-11-06T03:07:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.00 , -0.02 2024-11-06T03:07:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862434.20 epoch sec; seqNo:2660; 4061708.79 north; 594667.14 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:07:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862432.95 2024-11-06T03:07:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:14Z- waiting for 1730862436.95, time is 1730862434.20 2024-11-06T03:07:14Z- AHRS_M2 msg: 1730862434.59 epoch sec; seqNo:2501 2024-11-06T03:07:14Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.47 psi 2024-11-06T03:07:14Z- handleDepth()- Depth_Keller msg: 1730862434.60 epoch sec; seqNo:2661; depth 49.72 2024-11-06T03:07:14Z- handleDVL() - DVL msg epochMillisec: 1730862434.63 2024-11-06T03:07:14Z- handleDvl() - RDI_Pathfinder msg: 1730862434.63 sec; seqNo:862; #beams:4 2024-11-06T03:07:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.75 , 29.81 , 31.80 , 35.01 2024-11-06T03:07:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.00 2024-11-06T03:07:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862434.65 epoch sec; seqNo:2661; 4061708.81 north; 594668.36 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:07:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862432.95 2024-11-06T03:07:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:14Z- waiting for 1730862436.95, time is 1730862434.65 2024-11-06T03:07:14Z- AHRS_M2 msg: 1730862434.96 epoch sec; seqNo:2502 2024-11-06T03:07:14Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.47 psi 2024-11-06T03:07:14Z- handleDepth()- Depth_Keller msg: 1730862434.97 epoch sec; seqNo:2662; depth 49.71 2024-11-06T03:07:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862434.99 epoch sec; seqNo:2662; 4061709.19 north; 594668.35 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:07:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862432.95 2024-11-06T03:07:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:15Z- waiting for 1730862436.95, time is 1730862434.99 2024-11-06T03:07:15Z- AHRS_M2 msg: 1730862435.36 epoch sec; seqNo:2503 2024-11-06T03:07:15Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.47 psi 2024-11-06T03:07:15Z- handleDepth()- Depth_Keller msg: 1730862435.37 epoch sec; seqNo:2663; depth 49.73 2024-11-06T03:07:15Z- handleDVL() - DVL msg epochMillisec: 1730862435.37 2024-11-06T03:07:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:15Z- handleDvl() - RDI_Pathfinder msg: 1730862435.37 sec; seqNo:863; #beams:0 2024-11-06T03:07:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.00 2024-11-06T03:07:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862435.38 epoch sec; seqNo:2663; 4061709.19 north; 594668.35 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:07:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862432.95 2024-11-06T03:07:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:15Z- waiting for 1730862436.95, time is 1730862435.38 2024-11-06T03:07:15Z- AHRS_M2 msg: 1730862435.78 epoch sec; seqNo:2504 2024-11-06T03:07:15Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.48 psi 2024-11-06T03:07:15Z- handleDepth()- Depth_Keller msg: 1730862435.78 epoch sec; seqNo:2664; depth 49.74 2024-11-06T03:07:15Z- handleDVL() - DVL msg epochMillisec: 1730862435.80 2024-11-06T03:07:15Z- handleDvl() - RDI_Pathfinder msg: 1730862435.80 sec; seqNo:864; #beams:4 2024-11-06T03:07:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.75 , 29.66 , 31.65 , 34.86 2024-11-06T03:07:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , 0.00 2024-11-06T03:07:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862435.84 epoch sec; seqNo:2664; 4061709.19 north; 594668.35 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:07:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862432.95 2024-11-06T03:07:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:15Z- waiting for 1730862436.95, time is 1730862435.84 2024-11-06T03:07:16Z- handleDepth()- Depth_Keller msg: 1730862436.19 epoch sec; seqNo:2665; depth 49.76 2024-11-06T03:07:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862436.20 epoch sec; seqNo:2665; 4061709.20 north; 594669.57 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:07:16Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:07:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862432.95 2024-11-06T03:07:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:16Z- waiting for 1730862436.95, time is 1730862436.20 2024-11-06T03:07:16Z- AHRS_M2 msg: 1730862436.61 epoch sec; seqNo:2505 2024-11-06T03:07:16Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.48 psi 2024-11-06T03:07:16Z- handleDepth()- Depth_Keller msg: 1730862436.63 epoch sec; seqNo:2666; depth 49.75 2024-11-06T03:07:16Z- handleDVL() - DVL msg epochMillisec: 1730862436.66 2024-11-06T03:07:16Z- handleDvl() - RDI_Pathfinder msg: 1730862436.66 sec; seqNo:865; #beams:4 2024-11-06T03:07:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.84 , 29.66 , 31.80 , 34.86 2024-11-06T03:07:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.01 2024-11-06T03:07:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862436.68 epoch sec; seqNo:2666; 4061709.20 north; 594669.57 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:07:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862432.95 2024-11-06T03:07:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:16Z- waiting for 1730862436.95, time is 1730862436.68 2024-11-06T03:07:16Z- AHRS_M2 msg: 1730862436.98 epoch sec; seqNo:2506 2024-11-06T03:07:16Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.48 psi 2024-11-06T03:07:16Z- handleDepth()- Depth_Keller msg: 1730862436.99 epoch sec; seqNo:2667; depth 49.76 2024-11-06T03:07:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862437.00 epoch sec; seqNo:2667; 4061709.20 north; 594669.57 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:07:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862432.95 2024-11-06T03:07:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:17Z- waiting for 1730862436.95, time is 1730862437.00 2024-11-06T03:07:17Z- LcmTrn::updateTrn() >>>> pose time 1730862437.00, meas time 1730862436.66 <<<< 2024-11-06T03:07:17Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862437.00 2024-11-06T03:07:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862436.66 sec, ping # 0; waiting for INS data more recent than 1730862432.95... 2024-11-06T03:07:17Z- TerrainNav::motionUpdate() - currEstimate = (4061709, 594669.57, 49.76) 2024-11-06T03:07:17Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:07:17Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:07:17Z- TNavPF::Projecting Measurements 2024-11-06T03:07:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:07:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:07:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:07:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:07:17Z- TNavFilter::4 good beams. 2024-11-06T03:07:17Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:07:17Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:07:17Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:07:17Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:07:17Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:07:17Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:07:17Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:07:17Z- TerrainMapOctree: (vehN, vehE ) = (4061709.16, 594669.27). 2024-11-06T03:07:17Z- TerrainMapOctree: Min Distance = 90.47. 2024-11-06T03:07:17Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:07:17Z- Forcing Subcloud Comparison 2024-11-06T03:07:17Z- Weighting particles with subcloud comparison 2024-11-06T03:07:17Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:17Z- meanExpectedMeasDiff: 1.184917 delta_rms_squared: 1.285508 alpha: 0.984680 2024-11-06T03:07:17Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:17Z- meanExpectedMeasDiff: -0.711164 delta_rms_squared: 0.893727 alpha: 0.978112 2024-11-06T03:07:17Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:17Z- meanExpectedMeasDiff: -0.373359 delta_rms_squared: 1.004217 alpha: 0.980473 2024-11-06T03:07:17Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:17Z- meanExpectedMeasDiff: 0.781952 delta_rms_squared: 1.127711 alpha: 0.982574 2024-11-06T03:07:17Z- Subcloud NIS: 0.199932 2024-11-06T03:07:17Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:07:17Z- TNavPF:: sumWeights = 0.481555 2024-11-06T03:07:17Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:07:17Z- TNavPF::First Term of Map Covariance Matrix is 0.44 2024-11-06T03:07:17Z- TNavPF::Calculated NIS Value : 0.26 numBeams normalized NIS: 0.06 2024-11-06T03:07:17Z- TerrainNav::Resampling particle filter... 2024-11-06T03:07:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862436.66 sec. 2024-11-06T03:07:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:07:17Z- variances([0], [2], and psi_berg) = 0.667232 0.208677 0.000000 2024-11-06T03:07:17Z- publishEstimatesDecoder() - at 1730862437.00 sec 2024-11-06T03:07:17Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:07:17Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061741.35 594653.11 60.69 2024-11-06T03:07:17Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061741.80 594653.11 60.30 lat/lon: 36.70 -121.94 2024-11-06T03:07:17Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.67 0.21 0.62 0.00 2024-11-06T03:07:17Z- AHRS_M2 msg: 1730862437.38 epoch sec; seqNo:2507 2024-11-06T03:07:17Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.48 psi 2024-11-06T03:07:17Z- handleDepth()- Depth_Keller msg: 1730862437.39 epoch sec; seqNo:2668; depth 49.78 2024-11-06T03:07:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862437.40 epoch sec; seqNo:2668; 4061709.21 north; 594670.79 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:07:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862437.00 2024-11-06T03:07:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:17Z- waiting for 1730862441.00, time is 1730862437.40 2024-11-06T03:07:17Z- AHRS_M2 msg: 1730862437.79 epoch sec; seqNo:2508 2024-11-06T03:07:17Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.48 psi 2024-11-06T03:07:17Z- handleDepth()- Depth_Keller msg: 1730862437.80 epoch sec; seqNo:2669; depth 49.78 2024-11-06T03:07:17Z- handleDVL() - DVL msg epochMillisec: 1730862437.83 2024-11-06T03:07:17Z- handleDvl() - RDI_Pathfinder msg: 1730862437.83 sec; seqNo:866; #beams:4 2024-11-06T03:07:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.14 , 29.82 , 31.80 , 35.01 2024-11-06T03:07:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.01 2024-11-06T03:07:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862437.84 epoch sec; seqNo:2669; 4061709.21 north; 594670.79 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:07:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862437.00 2024-11-06T03:07:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:17Z- waiting for 1730862441.00, time is 1730862437.84 2024-11-06T03:07:18Z- AHRS_M2 msg: 1730862438.19 epoch sec; seqNo:2509 2024-11-06T03:07:18Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.48 psi 2024-11-06T03:07:18Z- handleDepth()- Depth_Keller msg: 1730862438.19 epoch sec; seqNo:2670; depth 49.80 2024-11-06T03:07:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862438.21 epoch sec; seqNo:2670; 4061709.21 north; 594670.79 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:07:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862437.00 2024-11-06T03:07:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:18Z- waiting for 1730862441.00, time is 1730862438.21 2024-11-06T03:07:18Z- AHRS_M2 msg: 1730862438.60 epoch sec; seqNo:2510 2024-11-06T03:07:18Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.48 psi 2024-11-06T03:07:18Z- handleDepth()- Depth_Keller msg: 1730862438.60 epoch sec; seqNo:2671; depth 49.81 2024-11-06T03:07:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862438.61 epoch sec; seqNo:2671; 4061709.59 north; 594670.79 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:07:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862437.00 2024-11-06T03:07:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:18Z- waiting for 1730862441.00, time is 1730862438.61 2024-11-06T03:07:19Z- AHRS_M2 msg: 1730862439.00 epoch sec; seqNo:2511 2024-11-06T03:07:19Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.48 psi 2024-11-06T03:07:19Z- handleDepth()- Depth_Keller msg: 1730862439.01 epoch sec; seqNo:2672; depth 49.82 2024-11-06T03:07:19Z- handleDVL() - DVL msg epochMillisec: 1730862439.03 2024-11-06T03:07:19Z- handleDvl() - RDI_Pathfinder msg: 1730862439.03 sec; seqNo:867; #beams:4 2024-11-06T03:07:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.44 , 30.12 , 31.34 , 34.71 2024-11-06T03:07:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , -0.00 2024-11-06T03:07:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862439.05 epoch sec; seqNo:2672; 4061709.60 north; 594672.01 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:07:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862437.00 2024-11-06T03:07:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:19Z- waiting for 1730862441.00, time is 1730862439.05 2024-11-06T03:07:19Z- AHRS_M2 msg: 1730862439.40 epoch sec; seqNo:2512 2024-11-06T03:07:19Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.48 psi 2024-11-06T03:07:19Z- handleDepth()- Depth_Keller msg: 1730862439.41 epoch sec; seqNo:2673; depth 49.85 2024-11-06T03:07:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862439.43 epoch sec; seqNo:2673; 4061709.60 north; 594672.01 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:07:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862437.00 2024-11-06T03:07:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:19Z- waiting for 1730862441.00, time is 1730862439.43 2024-11-06T03:07:19Z- AHRS_M2 msg: 1730862439.82 epoch sec; seqNo:2513 2024-11-06T03:07:19Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.48 psi 2024-11-06T03:07:19Z- handleDepth()- Depth_Keller msg: 1730862439.83 epoch sec; seqNo:2674; depth 49.87 2024-11-06T03:07:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862439.84 epoch sec; seqNo:2674; 4061709.60 north; 594672.01 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:07:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862437.00 2024-11-06T03:07:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:19Z- waiting for 1730862441.00, time is 1730862439.84 2024-11-06T03:07:20Z- AHRS_M2 msg: 1730862440.21 epoch sec; seqNo:2514 2024-11-06T03:07:20Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.48 psi 2024-11-06T03:07:20Z- handleDepth()- Depth_Keller msg: 1730862440.21 epoch sec; seqNo:2675; depth 49.87 2024-11-06T03:07:20Z- handleDVL() - DVL msg epochMillisec: 1730862440.22 2024-11-06T03:07:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:20Z- handleDvl() - RDI_Pathfinder msg: 1730862440.22 sec; seqNo:868; #beams:0 2024-11-06T03:07:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , -0.00 2024-11-06T03:07:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862440.24 epoch sec; seqNo:2675; 4061709.62 north; 594673.23 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:07:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862437.00 2024-11-06T03:07:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:20Z- waiting for 1730862441.00, time is 1730862440.24 2024-11-06T03:07:20Z- AHRS_M2 msg: 1730862440.62 epoch sec; seqNo:2515 2024-11-06T03:07:20Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.48 psi 2024-11-06T03:07:20Z- handleDepth()- Depth_Keller msg: 1730862440.63 epoch sec; seqNo:2676; depth 49.89 2024-11-06T03:07:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862440.64 epoch sec; seqNo:2676; 4061709.62 north; 594673.23 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:07:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862437.00 2024-11-06T03:07:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:20Z- waiting for 1730862441.00, time is 1730862440.64 2024-11-06T03:07:21Z- AHRS_M2 msg: 1730862441.02 epoch sec; seqNo:2516 2024-11-06T03:07:21Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.48 psi 2024-11-06T03:07:21Z- handleDepth()- Depth_Keller msg: 1730862441.03 epoch sec; seqNo:2677; depth 49.92 2024-11-06T03:07:21Z- handleDVL() - DVL msg epochMillisec: 1730862441.05 2024-11-06T03:07:21Z- handleDvl() - RDI_Pathfinder msg: 1730862441.05 sec; seqNo:869; #beams:4 2024-11-06T03:07:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.75 , 30.12 , 31.34 , 34.71 2024-11-06T03:07:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , -0.01 2024-11-06T03:07:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862441.06 epoch sec; seqNo:2677; 4061709.62 north; 594673.23 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:07:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862437.00 2024-11-06T03:07:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:21Z- waiting for 1730862441.00, time is 1730862441.06 2024-11-06T03:07:21Z- LcmTrn::updateTrn() >>>> pose time 1730862441.06, meas time 1730862441.05 <<<< 2024-11-06T03:07:21Z- LcmTrn::updateTrn() equating pose 1730862441.05 and meas times 1730862441.05 2024-11-06T03:07:21Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862441.05 2024-11-06T03:07:21Z- TerrainNav::motionUpdate() - currEstimate = (4061710, 594673.23, 49.92) 2024-11-06T03:07:21Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:07:21Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:07:21Z- TNavPF::Projecting Measurements 2024-11-06T03:07:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:07:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:07:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:07:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:07:21Z- TNavFilter::4 good beams. 2024-11-06T03:07:21Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:07:21Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:07:21Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:07:21Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:07:21Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:07:21Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:07:21Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:07:21Z- TerrainMapOctree: (vehN, vehE ) = (4061709.62, 594673.23). 2024-11-06T03:07:21Z- TerrainMapOctree: Min Distance = 86.74. 2024-11-06T03:07:21Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:07:21Z- Forcing Subcloud Comparison 2024-11-06T03:07:21Z- Weighting particles with subcloud comparison 2024-11-06T03:07:21Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:21Z- meanExpectedMeasDiff: 2.515504 delta_rms_squared: 2.497136 alpha: 0.992054 2024-11-06T03:07:21Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:21Z- meanExpectedMeasDiff: -0.162472 delta_rms_squared: 0.992383 alpha: 0.980245 2024-11-06T03:07:21Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:21Z- meanExpectedMeasDiff: -0.729913 delta_rms_squared: 1.114414 alpha: 0.982370 2024-11-06T03:07:21Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:21Z- meanExpectedMeasDiff: 0.638240 delta_rms_squared: 1.255696 alpha: 0.984322 2024-11-06T03:07:21Z- Subcloud NIS: 0.209200 2024-11-06T03:07:21Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:07:21Z- TNavPF:: sumWeights = 0.311019 2024-11-06T03:07:21Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:07:21Z- TNavPF::First Term of Map Covariance Matrix is 0.80 2024-11-06T03:07:21Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T03:07:21Z- TerrainNav::Resampling particle filter... 2024-11-06T03:07:21Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862441.05 sec, ping # 0. 2024-11-06T03:07:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:07:21Z- variances([0], [2], and psi_berg) = 0.544399 0.207063 0.000000 2024-11-06T03:07:21Z- publishEstimatesDecoder() - at 1730862441.05 sec 2024-11-06T03:07:21Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:07:21Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061741.70 594656.74 60.99 2024-11-06T03:07:21Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061742.35 594656.74 60.18 lat/lon: 36.70 -121.94 2024-11-06T03:07:21Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.54 0.21 0.35 0.00 2024-11-06T03:07:21Z- AHRS_M2 msg: 1730862441.42 epoch sec; seqNo:2517 2024-11-06T03:07:21Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.48 psi 2024-11-06T03:07:21Z- handleDepth()- Depth_Keller msg: 1730862441.43 epoch sec; seqNo:2678; depth 49.91 2024-11-06T03:07:21Z- handleDVL() - DVL msg epochMillisec: 1730862441.44 2024-11-06T03:07:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:21Z- handleDvl() - RDI_Pathfinder msg: 1730862441.44 sec; seqNo:870; #beams:0 2024-11-06T03:07:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , -0.01 2024-11-06T03:07:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862441.46 epoch sec; seqNo:2678; 4061709.62 north; 594673.23 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:07:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862441.05 2024-11-06T03:07:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:21Z- waiting for 1730862445.05, time is 1730862441.46 2024-11-06T03:07:21Z- AHRS_M2 msg: 1730862441.83 epoch sec; seqNo:2518 2024-11-06T03:07:21Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.49 psi 2024-11-06T03:07:21Z- handleDepth()- Depth_Keller msg: 1730862441.83 epoch sec; seqNo:2679; depth 49.95 2024-11-06T03:07:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862441.84 epoch sec; seqNo:2679; 4061709.63 north; 594674.45 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:07:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862441.05 2024-11-06T03:07:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:21Z- waiting for 1730862445.05, time is 1730862441.84 2024-11-06T03:07:22Z- AHRS_M2 msg: 1730862442.23 epoch sec; seqNo:2519 2024-11-06T03:07:22Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.49 psi 2024-11-06T03:07:22Z- handleDepth()- Depth_Keller msg: 1730862442.24 epoch sec; seqNo:2680; depth 49.96 2024-11-06T03:07:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862442.24 epoch sec; seqNo:2680; 4061709.63 north; 594674.45 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:07:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862441.05 2024-11-06T03:07:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:22Z- waiting for 1730862445.05, time is 1730862442.24 2024-11-06T03:07:22Z- handleDepth()- Depth_Keller msg: 1730862442.73 epoch sec; seqNo:2681; depth 49.98 2024-11-06T03:07:22Z- handleDVL() - DVL msg epochMillisec: 1730862442.73 2024-11-06T03:07:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:22Z- handleDvl() - RDI_Pathfinder msg: 1730862442.73 sec; seqNo:871; #beams:0 2024-11-06T03:07:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , -0.01 2024-11-06T03:07:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862442.77 epoch sec; seqNo:2681; 4061710.01 north; 594674.45 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:07:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862441.05 2024-11-06T03:07:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:22Z- waiting for 1730862445.05, time is 1730862442.77 2024-11-06T03:07:23Z- handleDepth()- Depth_Keller msg: 1730862443.06 epoch sec; seqNo:2682; depth 50.00 2024-11-06T03:07:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862443.08 epoch sec; seqNo:2682; 4061710.01 north; 594674.45 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:07:23Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:07:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862441.05 2024-11-06T03:07:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:23Z- waiting for 1730862445.05, time is 1730862443.08 2024-11-06T03:07:23Z- AHRS_M2 msg: 1730862443.44 epoch sec; seqNo:2520 2024-11-06T03:07:23Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.49 psi 2024-11-06T03:07:23Z- handleDepth()- Depth_Keller msg: 1730862443.45 epoch sec; seqNo:2683; depth 50.02 2024-11-06T03:07:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862443.46 epoch sec; seqNo:2683; 4061710.02 north; 594675.67 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:07:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862441.05 2024-11-06T03:07:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:23Z- waiting for 1730862445.05, time is 1730862443.46 2024-11-06T03:07:23Z- AHRS_M2 msg: 1730862443.85 epoch sec; seqNo:2521 2024-11-06T03:07:23Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.49 psi 2024-11-06T03:07:23Z- handleDepth()- Depth_Keller msg: 1730862443.86 epoch sec; seqNo:2684; depth 50.04 2024-11-06T03:07:23Z- handleDVL() - DVL msg epochMillisec: 1730862443.88 2024-11-06T03:07:23Z- handleDvl() - RDI_Pathfinder msg: 1730862443.88 sec; seqNo:872; #beams:4 2024-11-06T03:07:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.06 , 29.97 , 31.34 , 34.71 2024-11-06T03:07:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.02 2024-11-06T03:07:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862443.89 epoch sec; seqNo:2684; 4061710.02 north; 594675.67 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:07:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862441.05 2024-11-06T03:07:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:24Z- waiting for 1730862445.05, time is 1730862443.89 2024-11-06T03:07:24Z- AHRS_M2 msg: 1730862444.25 epoch sec; seqNo:2522 2024-11-06T03:07:24Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.49 psi 2024-11-06T03:07:24Z- handleDepth()- Depth_Keller msg: 1730862444.26 epoch sec; seqNo:2685; depth 50.05 2024-11-06T03:07:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862444.28 epoch sec; seqNo:2685; 4061710.02 north; 594675.67 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:07:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862441.05 2024-11-06T03:07:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:24Z- waiting for 1730862445.05, time is 1730862444.28 2024-11-06T03:07:24Z- AHRS_M2 msg: 1730862444.67 epoch sec; seqNo:2523 2024-11-06T03:07:24Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.49 psi 2024-11-06T03:07:24Z- handleDepth()- Depth_Keller msg: 1730862444.68 epoch sec; seqNo:2686; depth 50.08 2024-11-06T03:07:24Z- handleDVL() - DVL msg epochMillisec: 1730862444.74 2024-11-06T03:07:24Z- handleDvl() - RDI_Pathfinder msg: 1730862444.74 sec; seqNo:873; #beams:4 2024-11-06T03:07:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.37 , 29.97 , 31.19 , 34.56 2024-11-06T03:07:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.03 2024-11-06T03:07:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862444.76 epoch sec; seqNo:2686; 4061710.04 north; 594676.89 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:07:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862441.05 2024-11-06T03:07:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:24Z- waiting for 1730862445.05, time is 1730862444.76 2024-11-06T03:07:25Z- AHRS_M2 msg: 1730862445.06 epoch sec; seqNo:2524 2024-11-06T03:07:25Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.48 psi 2024-11-06T03:07:25Z- handleDepth()- Depth_Keller msg: 1730862445.07 epoch sec; seqNo:2687; depth 50.09 2024-11-06T03:07:25Z- handleDVL() - DVL msg epochMillisec: 1730862445.07 2024-11-06T03:07:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:25Z- handleDvl() - RDI_Pathfinder msg: 1730862445.07 sec; seqNo:874; #beams:0 2024-11-06T03:07:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.03 2024-11-06T03:07:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862445.09 epoch sec; seqNo:2687; 4061710.04 north; 594676.89 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:07:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862441.05 2024-11-06T03:07:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:25Z- waiting for 1730862445.05, time is 1730862445.09 2024-11-06T03:07:25Z- LcmTrn::updateTrn() >>>> pose time 1730862445.09, meas time 1730862445.07 <<<< 2024-11-06T03:07:25Z- LcmTrn::updateTrn() equating pose 1730862445.07 and meas times 1730862445.07 2024-11-06T03:07:25Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862445.07 2024-11-06T03:07:25Z- TerrainNav::motionUpdate() - currEstimate = (4061710, 594676.89, 50.09) 2024-11-06T03:07:25Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:07:25Z- TerrainNav:: SubcloudNIS is 0.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:07:25Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:07:25Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:07:25Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:07:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:07:25Z- variances([0], [2], and psi_berg) = 0.547232 0.209601 0.000000 2024-11-06T03:07:25Z- publishEstimatesDecoder() - at 1730862445.07 sec 2024-11-06T03:07:25Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:07:25Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061742.07 594660.43 61.11 2024-11-06T03:07:25Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061742.76 594660.40 60.35 lat/lon: 36.70 -121.94 2024-11-06T03:07:25Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.55 0.21 0.59 0.00 2024-11-06T03:07:25Z- AHRS_M2 msg: 1730862445.47 epoch sec; seqNo:2525 2024-11-06T03:07:25Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.49 psi 2024-11-06T03:07:25Z- handleDepth()- Depth_Keller msg: 1730862445.48 epoch sec; seqNo:2688; depth 50.10 2024-11-06T03:07:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862445.49 epoch sec; seqNo:2688; 4061710.04 north; 594676.89 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:07:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862445.07 2024-11-06T03:07:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:25Z- waiting for 1730862449.07, time is 1730862445.49 2024-11-06T03:07:25Z- AHRS_M2 msg: 1730862445.87 epoch sec; seqNo:2526 2024-11-06T03:07:25Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.49 psi 2024-11-06T03:07:25Z- handleDepth()- Depth_Keller msg: 1730862445.87 epoch sec; seqNo:2689; depth 50.13 2024-11-06T03:07:25Z- handleDVL() - DVL msg epochMillisec: 1730862445.89 2024-11-06T03:07:25Z- handleDvl() - RDI_Pathfinder msg: 1730862445.89 sec; seqNo:875; #beams:4 2024-11-06T03:07:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.75 , 29.82 , 31.19 , 34.71 2024-11-06T03:07:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , 0.04 2024-11-06T03:07:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862445.93 epoch sec; seqNo:2689; 4061710.04 north; 594676.89 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:07:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862445.07 2024-11-06T03:07:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:26Z- waiting for 1730862449.07, time is 1730862445.93 2024-11-06T03:07:26Z- AHRS_M2 msg: 1730862446.27 epoch sec; seqNo:2527 2024-11-06T03:07:26Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.49 psi 2024-11-06T03:07:26Z- handleDepth()- Depth_Keller msg: 1730862446.28 epoch sec; seqNo:2690; depth 50.15 2024-11-06T03:07:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862446.29 epoch sec; seqNo:2690; 4061710.43 north; 594678.10 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:07:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862445.07 2024-11-06T03:07:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:26Z- waiting for 1730862449.07, time is 1730862446.29 2024-11-06T03:07:26Z- AHRS_M2 msg: 1730862446.68 epoch sec; seqNo:2528 2024-11-06T03:07:26Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.49 psi 2024-11-06T03:07:26Z- handleDepth()- Depth_Keller msg: 1730862446.69 epoch sec; seqNo:2691; depth 50.16 2024-11-06T03:07:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862446.76 epoch sec; seqNo:2691; 4061710.43 north; 594678.10 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:07:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862445.07 2024-11-06T03:07:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:26Z- waiting for 1730862449.07, time is 1730862446.76 2024-11-06T03:07:27Z- AHRS_M2 msg: 1730862447.08 epoch sec; seqNo:2529 2024-11-06T03:07:27Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.49 psi 2024-11-06T03:07:27Z- handleDepth()- Depth_Keller msg: 1730862447.09 epoch sec; seqNo:2692; depth 50.18 2024-11-06T03:07:27Z- handleDVL() - DVL msg epochMillisec: 1730862447.12 2024-11-06T03:07:27Z- handleDvl() - RDI_Pathfinder msg: 1730862447.12 sec; seqNo:876; #beams:4 2024-11-06T03:07:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.52 , 29.82 , 31.50 , 34.40 2024-11-06T03:07:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.03 2024-11-06T03:07:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862447.13 epoch sec; seqNo:2692; 4061710.43 north; 594678.10 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:07:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862445.07 2024-11-06T03:07:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:27Z- waiting for 1730862449.07, time is 1730862447.13 2024-11-06T03:07:27Z- AHRS_M2 msg: 1730862447.48 epoch sec; seqNo:2530 2024-11-06T03:07:27Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.49 psi 2024-11-06T03:07:27Z- handleDepth()- Depth_Keller msg: 1730862447.49 epoch sec; seqNo:2693; depth 50.20 2024-11-06T03:07:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862447.50 epoch sec; seqNo:2693; 4061710.44 north; 594679.32 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:07:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862445.07 2024-11-06T03:07:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:27Z- waiting for 1730862449.07, time is 1730862447.50 2024-11-06T03:07:27Z- AHRS_M2 msg: 1730862447.89 epoch sec; seqNo:2531 2024-11-06T03:07:27Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.49 psi 2024-11-06T03:07:27Z- handleDepth()- Depth_Keller msg: 1730862447.90 epoch sec; seqNo:2694; depth 50.20 2024-11-06T03:07:27Z- handleDVL() - DVL msg epochMillisec: 1730862447.92 2024-11-06T03:07:27Z- handleDvl() - RDI_Pathfinder msg: 1730862447.92 sec; seqNo:877; #beams:4 2024-11-06T03:07:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.06 , 29.82 , 31.50 , 34.56 2024-11-06T03:07:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , 0.02 2024-11-06T03:07:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862447.95 epoch sec; seqNo:2694; 4061710.44 north; 594679.32 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:07:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862445.07 2024-11-06T03:07:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:28Z- waiting for 1730862449.07, time is 1730862447.95 2024-11-06T03:07:28Z- AHRS_M2 msg: 1730862448.29 epoch sec; seqNo:2532 2024-11-06T03:07:28Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.49 psi 2024-11-06T03:07:28Z- handleDepth()- Depth_Keller msg: 1730862448.30 epoch sec; seqNo:2695; depth 50.22 2024-11-06T03:07:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862448.31 epoch sec; seqNo:2695; 4061710.44 north; 594679.32 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:07:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862445.07 2024-11-06T03:07:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:28Z- waiting for 1730862449.07, time is 1730862448.31 2024-11-06T03:07:28Z- AHRS_M2 msg: 1730862448.69 epoch sec; seqNo:2533 2024-11-06T03:07:28Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.49 psi 2024-11-06T03:07:28Z- handleDepth()- Depth_Keller msg: 1730862448.70 epoch sec; seqNo:2696; depth 50.22 2024-11-06T03:07:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862448.72 epoch sec; seqNo:2696; 4061710.46 north; 594680.54 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:07:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862445.07 2024-11-06T03:07:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:28Z- waiting for 1730862449.07, time is 1730862448.72 2024-11-06T03:07:29Z- AHRS_M2 msg: 1730862449.10 epoch sec; seqNo:2534 2024-11-06T03:07:29Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.50 psi 2024-11-06T03:07:29Z- handleDepth()- Depth_Keller msg: 1730862449.11 epoch sec; seqNo:2697; depth 50.24 2024-11-06T03:07:29Z- handleDVL() - DVL msg epochMillisec: 1730862449.14 2024-11-06T03:07:29Z- handleDvl() - RDI_Pathfinder msg: 1730862449.14 sec; seqNo:878; #beams:4 2024-11-06T03:07:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.75 , 29.82 , 31.50 , 34.40 2024-11-06T03:07:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , -0.00 2024-11-06T03:07:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862449.15 epoch sec; seqNo:2697; 4061710.46 north; 594680.54 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:07:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862445.07 2024-11-06T03:07:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:29Z- waiting for 1730862449.07, time is 1730862449.15 2024-11-06T03:07:29Z- LcmTrn::updateTrn() >>>> pose time 1730862449.15, meas time 1730862449.14 <<<< 2024-11-06T03:07:29Z- LcmTrn::updateTrn() equating pose 1730862449.14 and meas times 1730862449.14 2024-11-06T03:07:29Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862449.14 2024-11-06T03:07:29Z- TerrainNav::motionUpdate() - currEstimate = (4061710, 594680.54, 50.24) 2024-11-06T03:07:29Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:07:29Z- TerrainNav:: SubcloudNIS is 0.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:07:29Z- TNavPF::Projecting Measurements 2024-11-06T03:07:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:07:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:07:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:07:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:07:29Z- TNavFilter::4 good beams. 2024-11-06T03:07:29Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:07:29Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:07:29Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:07:29Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:07:29Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:07:29Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:07:29Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:07:29Z- TerrainMapOctree: (vehN, vehE ) = (4061710.46, 594680.54). 2024-11-06T03:07:29Z- TerrainMapOctree: Min Distance = 79.92. 2024-11-06T03:07:29Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:07:29Z- Forcing Subcloud Comparison 2024-11-06T03:07:29Z- Weighting particles with subcloud comparison 2024-11-06T03:07:29Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:29Z- meanExpectedMeasDiff: 3.641091 delta_rms_squared: 1.364613 alpha: 0.985556 2024-11-06T03:07:29Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:29Z- meanExpectedMeasDiff: -0.485312 delta_rms_squared: 0.957720 alpha: 0.979544 2024-11-06T03:07:29Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:29Z- meanExpectedMeasDiff: -0.515795 delta_rms_squared: 1.070196 alpha: 0.981655 2024-11-06T03:07:29Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:29Z- meanExpectedMeasDiff: 0.453495 delta_rms_squared: 1.204778 alpha: 0.983671 2024-11-06T03:07:29Z- Subcloud NIS: 0.219003 2024-11-06T03:07:29Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:07:29Z- TNavPF:: sumWeights = 0.148838 2024-11-06T03:07:29Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:07:29Z- TNavPF::First Term of Map Covariance Matrix is 1.21 2024-11-06T03:07:29Z- TNavPF::Calculated NIS Value : 0.07 numBeams normalized NIS: 0.02 2024-11-06T03:07:29Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862449.14 sec, ping # 0. 2024-11-06T03:07:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:07:29Z- variances([0], [2], and psi_berg) = 0.551693 0.214099 0.000000 2024-11-06T03:07:29Z- publishEstimatesDecoder() - at 1730862449.14 sec 2024-11-06T03:07:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:07:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061743.52 594663.33 59.91 2024-11-06T03:07:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061743.17 594664.06 60.17 lat/lon: 36.70 -121.94 2024-11-06T03:07:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.55 0.21 0.38 0.00 2024-11-06T03:07:29Z- AHRS_M2 msg: 1730862449.50 epoch sec; seqNo:2535 2024-11-06T03:07:29Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.50 psi 2024-11-06T03:07:29Z- handleDepth()- Depth_Keller msg: 1730862449.51 epoch sec; seqNo:2698; depth 50.25 2024-11-06T03:07:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862449.53 epoch sec; seqNo:2698; 4061710.46 north; 594680.54 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:07:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862449.14 2024-11-06T03:07:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:29Z- waiting for 1730862453.14, time is 1730862449.53 2024-11-06T03:07:29Z- handleDepth()- Depth_Keller msg: 1730862449.93 epoch sec; seqNo:2699; depth 50.27 2024-11-06T03:07:29Z- handleDVL() - DVL msg epochMillisec: 1730862449.98 2024-11-06T03:07:29Z- handleDvl() - RDI_Pathfinder msg: 1730862449.98 sec; seqNo:879; #beams:4 2024-11-06T03:07:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.67 , 29.82 , 31.50 , 34.10 2024-11-06T03:07:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , -0.03 2024-11-06T03:07:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862449.99 epoch sec; seqNo:2699; 4061710.46 north; 594680.54 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:07:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862449.14 2024-11-06T03:07:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:30Z- waiting for 1730862453.14, time is 1730862449.99 2024-11-06T03:07:30Z- AHRS_M2 msg: 1730862450.31 epoch sec; seqNo:2536 2024-11-06T03:07:30Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.50 psi 2024-11-06T03:07:30Z- handleDepth()- Depth_Keller msg: 1730862450.32 epoch sec; seqNo:2700; depth 50.26 2024-11-06T03:07:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862450.34 epoch sec; seqNo:2700; 4061710.47 north; 594681.76 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:07:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862449.14 2024-11-06T03:07:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:30Z- waiting for 1730862453.14, time is 1730862450.34 2024-11-06T03:07:30Z- AHRS_M2 msg: 1730862450.71 epoch sec; seqNo:2537 2024-11-06T03:07:30Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.50 psi 2024-11-06T03:07:30Z- handleDepth()- Depth_Keller msg: 1730862450.72 epoch sec; seqNo:2701; depth 50.29 2024-11-06T03:07:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862450.73 epoch sec; seqNo:2701; 4061710.47 north; 594681.76 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:07:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862449.14 2024-11-06T03:07:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:30Z- waiting for 1730862453.14, time is 1730862450.73 2024-11-06T03:07:31Z- AHRS_M2 msg: 1730862451.12 epoch sec; seqNo:2538 2024-11-06T03:07:31Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.50 psi 2024-11-06T03:07:31Z- handleDepth()- Depth_Keller msg: 1730862451.13 epoch sec; seqNo:2702; depth 50.29 2024-11-06T03:07:31Z- handleDVL() - DVL msg epochMillisec: 1730862451.15 2024-11-06T03:07:31Z- handleDvl() - RDI_Pathfinder msg: 1730862451.15 sec; seqNo:880; #beams:4 2024-11-06T03:07:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.37 , 29.51 , 31.34 , 34.10 2024-11-06T03:07:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , -0.03 2024-11-06T03:07:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862451.18 epoch sec; seqNo:2702; 4061710.47 north; 594681.76 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:07:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862449.14 2024-11-06T03:07:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:31Z- waiting for 1730862453.14, time is 1730862451.18 2024-11-06T03:07:31Z- AHRS_M2 msg: 1730862451.53 epoch sec; seqNo:2539 2024-11-06T03:07:31Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.50 psi 2024-11-06T03:07:31Z- handleDepth()- Depth_Keller msg: 1730862451.53 epoch sec; seqNo:2703; depth 50.29 2024-11-06T03:07:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862451.55 epoch sec; seqNo:2703; 4061710.48 north; 594682.99 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:07:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862449.14 2024-11-06T03:07:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:31Z- waiting for 1730862453.14, time is 1730862451.55 2024-11-06T03:07:31Z- AHRS_M2 msg: 1730862451.93 epoch sec; seqNo:2540 2024-11-06T03:07:31Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.50 psi 2024-11-06T03:07:31Z- handleDepth()- Depth_Keller msg: 1730862451.93 epoch sec; seqNo:2704; depth 50.32 2024-11-06T03:07:31Z- handleDVL() - DVL msg epochMillisec: 1730862451.97 2024-11-06T03:07:31Z- handleDvl() - RDI_Pathfinder msg: 1730862451.97 sec; seqNo:881; #beams:4 2024-11-06T03:07:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.67 , 29.35 , 31.19 , 33.94 2024-11-06T03:07:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.00 , -0.03 2024-11-06T03:07:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862451.98 epoch sec; seqNo:2704; 4061710.48 north; 594682.99 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:07:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862449.14 2024-11-06T03:07:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:32Z- waiting for 1730862453.14, time is 1730862451.98 2024-11-06T03:07:32Z- AHRS_M2 msg: 1730862452.33 epoch sec; seqNo:2541 2024-11-06T03:07:32Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.50 psi 2024-11-06T03:07:32Z- handleDepth()- Depth_Keller msg: 1730862452.34 epoch sec; seqNo:2705; depth 50.33 2024-11-06T03:07:32Z- handleDVL() - DVL msg epochMillisec: 1730862452.34 2024-11-06T03:07:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:32Z- handleDvl() - RDI_Pathfinder msg: 1730862452.34 sec; seqNo:882; #beams:0 2024-11-06T03:07:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.00 , -0.03 2024-11-06T03:07:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862452.35 epoch sec; seqNo:2705; 4061710.86 north; 594682.98 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:07:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862449.14 2024-11-06T03:07:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:32Z- waiting for 1730862453.14, time is 1730862452.35 2024-11-06T03:07:32Z- AHRS_M2 msg: 1730862452.74 epoch sec; seqNo:2542 2024-11-06T03:07:32Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.50 psi 2024-11-06T03:07:32Z- handleDepth()- Depth_Keller msg: 1730862452.75 epoch sec; seqNo:2706; depth 50.33 2024-11-06T03:07:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862452.76 epoch sec; seqNo:2706; 4061710.86 north; 594682.98 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:07:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862449.14 2024-11-06T03:07:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:32Z- waiting for 1730862453.14, time is 1730862452.76 2024-11-06T03:07:33Z- AHRS_M2 msg: 1730862453.14 epoch sec; seqNo:2543 2024-11-06T03:07:33Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.50 psi 2024-11-06T03:07:33Z- handleDepth()- Depth_Keller msg: 1730862453.14 epoch sec; seqNo:2707; depth 50.34 2024-11-06T03:07:33Z- handleDVL() - DVL msg epochMillisec: 1730862453.17 2024-11-06T03:07:33Z- handleDvl() - RDI_Pathfinder msg: 1730862453.17 sec; seqNo:883; #beams:4 2024-11-06T03:07:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.29 , 29.51 , 31.19 , 33.79 2024-11-06T03:07:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.03 2024-11-06T03:07:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862453.19 epoch sec; seqNo:2707; 4061710.88 north; 594684.20 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:07:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862449.14 2024-11-06T03:07:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:33Z- waiting for 1730862453.14, time is 1730862453.19 2024-11-06T03:07:33Z- LcmTrn::updateTrn() >>>> pose time 1730862453.19, meas time 1730862453.17 <<<< 2024-11-06T03:07:33Z- LcmTrn::updateTrn() equating pose 1730862453.17 and meas times 1730862453.17 2024-11-06T03:07:33Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862453.17 2024-11-06T03:07:33Z- TerrainNav::motionUpdate() - currEstimate = (4061711, 594684.20, 50.34) 2024-11-06T03:07:33Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:07:33Z- TerrainNav:: SubcloudNIS is 0.22, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:07:33Z- TNavPF::Projecting Measurements 2024-11-06T03:07:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:07:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:07:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:07:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:07:33Z- TNavFilter::4 good beams. 2024-11-06T03:07:33Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:07:33Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:07:33Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:07:33Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:07:33Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:07:33Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:07:33Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:07:33Z- TerrainMapOctree: (vehN, vehE ) = (4061710.88, 594684.20). 2024-11-06T03:07:33Z- TerrainMapOctree: Min Distance = 76.55. 2024-11-06T03:07:33Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:07:33Z- Forcing Subcloud Comparison 2024-11-06T03:07:33Z- Weighting particles with subcloud comparison 2024-11-06T03:07:33Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:33Z- meanExpectedMeasDiff: 2.994129 delta_rms_squared: 1.017479 alpha: 0.980722 2024-11-06T03:07:33Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:33Z- meanExpectedMeasDiff: -1.081540 delta_rms_squared: 0.719638 alpha: 0.972960 2024-11-06T03:07:33Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:33Z- meanExpectedMeasDiff: -1.157117 delta_rms_squared: 0.805844 alpha: 0.975782 2024-11-06T03:07:33Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:33Z- meanExpectedMeasDiff: -0.312427 delta_rms_squared: 0.896742 alpha: 0.978184 2024-11-06T03:07:33Z- Subcloud NIS: 0.233559 2024-11-06T03:07:33Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:07:33Z- TNavPF:: sumWeights = 0.169041 2024-11-06T03:07:33Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:07:33Z- TNavPF::First Term of Map Covariance Matrix is 1.12 2024-11-06T03:07:33Z- TNavPF::Calculated NIS Value : 0.09 numBeams normalized NIS: 0.02 2024-11-06T03:07:33Z- TerrainNav::Resampling particle filter... 2024-11-06T03:07:33Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862453.17 sec, ping # 0. 2024-11-06T03:07:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:07:33Z- variances([0], [2], and psi_berg) = 0.559459 0.215660 0.000000 2024-11-06T03:07:33Z- publishEstimatesDecoder() - at 1730862453.17 sec 2024-11-06T03:07:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:07:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061742.82 594667.70 60.72 2024-11-06T03:07:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061743.58 594667.72 60.26 lat/lon: 36.70 -121.94 2024-11-06T03:07:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.56 0.22 0.33 0.00 2024-11-06T03:07:33Z- AHRS_M2 msg: 1730862453.54 epoch sec; seqNo:2544 2024-11-06T03:07:33Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T03:07:33Z- handleDepth()- Depth_Keller msg: 1730862453.55 epoch sec; seqNo:2708; depth 50.35 2024-11-06T03:07:33Z- handleDVL() - DVL msg epochMillisec: 1730862453.55 2024-11-06T03:07:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:33Z- handleDvl() - RDI_Pathfinder msg: 1730862453.55 sec; seqNo:884; #beams:0 2024-11-06T03:07:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , -0.03 2024-11-06T03:07:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862453.57 epoch sec; seqNo:2708; 4061710.88 north; 594684.20 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:07:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862453.17 2024-11-06T03:07:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:33Z- waiting for 1730862457.17, time is 1730862453.57 2024-11-06T03:07:33Z- AHRS_M2 msg: 1730862453.95 epoch sec; seqNo:2545 2024-11-06T03:07:33Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T03:07:33Z- handleDepth()- Depth_Keller msg: 1730862453.96 epoch sec; seqNo:2709; depth 50.36 2024-11-06T03:07:34Z- handleDVL() - DVL msg epochMillisec: 1730862453.99 2024-11-06T03:07:34Z- handleDvl() - RDI_Pathfinder msg: 1730862453.99 sec; seqNo:885; #beams:4 2024-11-06T03:07:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.36 , 29.05 , 31.19 , 33.79 2024-11-06T03:07:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.04 , -0.01 2024-11-06T03:07:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862454.00 epoch sec; seqNo:2709; 4061710.88 north; 594684.20 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:07:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862453.17 2024-11-06T03:07:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:34Z- waiting for 1730862457.17, time is 1730862454.00 2024-11-06T03:07:34Z- AHRS_M2 msg: 1730862454.35 epoch sec; seqNo:2546 2024-11-06T03:07:34Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T03:07:34Z- handleDepth()- Depth_Keller msg: 1730862454.36 epoch sec; seqNo:2710; depth 50.38 2024-11-06T03:07:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862454.37 epoch sec; seqNo:2710; 4061710.89 north; 594685.42 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:07:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862453.17 2024-11-06T03:07:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:34Z- waiting for 1730862457.17, time is 1730862454.37 2024-11-06T03:07:34Z- AHRS_M2 msg: 1730862454.75 epoch sec; seqNo:2547 2024-11-06T03:07:34Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T03:07:34Z- handleDepth()- Depth_Keller msg: 1730862454.77 epoch sec; seqNo:2711; depth 50.39 2024-11-06T03:07:34Z- handleDVL() - DVL msg epochMillisec: 1730862454.77 2024-11-06T03:07:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:34Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:34Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:34Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:34Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:34Z- handleDvl() - RDI_Pathfinder msg: 1730862454.77 sec; seqNo:886; #beams:0 2024-11-06T03:07:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.04 , -0.01 2024-11-06T03:07:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862454.79 epoch sec; seqNo:2711; 4061710.89 north; 594685.42 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:07:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862453.17 2024-11-06T03:07:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:34Z- waiting for 1730862457.17, time is 1730862454.79 2024-11-06T03:07:35Z- AHRS_M2 msg: 1730862455.16 epoch sec; seqNo:2548 2024-11-06T03:07:35Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T03:07:35Z- handleDepth()- Depth_Keller msg: 1730862455.16 epoch sec; seqNo:2712; depth 50.40 2024-11-06T03:07:35Z- handleDVL() - DVL msg epochMillisec: 1730862455.19 2024-11-06T03:07:35Z- handleDvl() - RDI_Pathfinder msg: 1730862455.19 sec; seqNo:887; #beams:4 2024-11-06T03:07:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.67 , 29.05 , 31.19 , 33.63 2024-11-06T03:07:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , 0.01 2024-11-06T03:07:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862455.20 epoch sec; seqNo:2712; 4061710.89 north; 594685.42 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:07:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862453.17 2024-11-06T03:07:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:35Z- waiting for 1730862457.17, time is 1730862455.20 2024-11-06T03:07:35Z- AHRS_M2 msg: 1730862455.57 epoch sec; seqNo:2549 2024-11-06T03:07:35Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T03:07:35Z- handleDepth()- Depth_Keller msg: 1730862455.57 epoch sec; seqNo:2713; depth 50.41 2024-11-06T03:07:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862455.58 epoch sec; seqNo:2713; 4061710.89 north; 594685.42 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:07:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862453.17 2024-11-06T03:07:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:35Z- waiting for 1730862457.17, time is 1730862455.58 2024-11-06T03:07:35Z- AHRS_M2 msg: 1730862455.98 epoch sec; seqNo:2550 2024-11-06T03:07:35Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T03:07:35Z- handleDepth()- Depth_Keller msg: 1730862455.99 epoch sec; seqNo:2714; depth 50.42 2024-11-06T03:07:36Z- handleDVL() - DVL msg epochMillisec: 1730862456.01 2024-11-06T03:07:36Z- handleDvl() - RDI_Pathfinder msg: 1730862456.01 sec; seqNo:888; #beams:4 2024-11-06T03:07:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.51 , 29.05 , 31.19 , 33.63 2024-11-06T03:07:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , 0.01 2024-11-06T03:07:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862456.03 epoch sec; seqNo:2714; 4061710.90 north; 594686.64 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:07:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862453.17 2024-11-06T03:07:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:36Z- waiting for 1730862457.17, time is 1730862456.03 2024-11-06T03:07:36Z- AHRS_M2 msg: 1730862456.37 epoch sec; seqNo:2551 2024-11-06T03:07:36Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T03:07:36Z- handleDepth()- Depth_Keller msg: 1730862456.38 epoch sec; seqNo:2715; depth 50.43 2024-11-06T03:07:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862456.40 epoch sec; seqNo:2715; 4061711.28 north; 594686.64 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:07:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862453.17 2024-11-06T03:07:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:36Z- waiting for 1730862457.17, time is 1730862456.40 2024-11-06T03:07:36Z- AHRS_M2 msg: 1730862456.77 epoch sec; seqNo:2552 2024-11-06T03:07:36Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T03:07:36Z- handleDepth()- Depth_Keller msg: 1730862456.78 epoch sec; seqNo:2716; depth 50.45 2024-11-06T03:07:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862456.80 epoch sec; seqNo:2716; 4061711.28 north; 594686.64 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:07:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862453.17 2024-11-06T03:07:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:36Z- waiting for 1730862457.17, time is 1730862456.80 2024-11-06T03:07:37Z- handleDepth()- Depth_Keller msg: 1730862457.20 epoch sec; seqNo:2717; depth 50.47 2024-11-06T03:07:37Z- handleDVL() - DVL msg epochMillisec: 1730862457.22 2024-11-06T03:07:37Z- handleDvl() - RDI_Pathfinder msg: 1730862457.22 sec; seqNo:889; #beams:4 2024-11-06T03:07:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.36 , 28.74 , 31.34 , 33.33 2024-11-06T03:07:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.02 2024-11-06T03:07:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862457.25 epoch sec; seqNo:2717; 4061711.28 north; 594686.64 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:07:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862453.17 2024-11-06T03:07:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:37Z- waiting for 1730862457.17, time is 1730862457.25 2024-11-06T03:07:37Z- LcmTrn::updateTrn() >>>> pose time 1730862457.25, meas time 1730862457.22 <<<< 2024-11-06T03:07:37Z- LcmTrn::updateTrn() equating pose 1730862457.22 and meas times 1730862457.22 2024-11-06T03:07:37Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862457.22 2024-11-06T03:07:37Z- TerrainNav::motionUpdate() - currEstimate = (4061711, 594686.64, 50.47) 2024-11-06T03:07:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:07:37Z- TerrainNav:: SubcloudNIS is 0.23, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:07:37Z- TNavPF::Projecting Measurements 2024-11-06T03:07:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:07:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:07:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:07:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:07:37Z- TNavFilter::4 good beams. 2024-11-06T03:07:37Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:07:37Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:07:37Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:07:37Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:07:37Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:07:37Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:07:37Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:07:37Z- TerrainMapOctree: (vehN, vehE ) = (4061711.28, 594686.64). 2024-11-06T03:07:37Z- TerrainMapOctree: Min Distance = 74.26. 2024-11-06T03:07:37Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:07:37Z- Forcing Subcloud Comparison 2024-11-06T03:07:37Z- Weighting particles with subcloud comparison 2024-11-06T03:07:37Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:37Z- meanExpectedMeasDiff: 3.569325 delta_rms_squared: 1.505176 alpha: 0.986887 2024-11-06T03:07:37Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:37Z- meanExpectedMeasDiff: -1.660557 delta_rms_squared: 0.657120 alpha: 0.970463 2024-11-06T03:07:37Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:37Z- meanExpectedMeasDiff: -1.156183 delta_rms_squared: 0.752211 alpha: 0.974100 2024-11-06T03:07:37Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:37Z- meanExpectedMeasDiff: -0.281672 delta_rms_squared: 0.805338 alpha: 0.975768 2024-11-06T03:07:37Z- Subcloud NIS: 0.257967 2024-11-06T03:07:37Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:07:37Z- TNavPF:: sumWeights = 0.094627 2024-11-06T03:07:37Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:07:37Z- TNavPF::First Term of Map Covariance Matrix is 0.93 2024-11-06T03:07:37Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T03:07:37Z- TerrainNav::Resampling particle filter... 2024-11-06T03:07:37Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862457.22 sec, ping # 0. 2024-11-06T03:07:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:07:37Z- variances([0], [2], and psi_berg) = 0.535821 0.201345 0.000000 2024-11-06T03:07:37Z- publishEstimatesDecoder() - at 1730862457.22 sec 2024-11-06T03:07:37Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:07:37Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061743.23 594670.12 59.86 2024-11-06T03:07:37Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061744.09 594670.32 60.40 lat/lon: 36.70 -121.94 2024-11-06T03:07:37Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.54 0.20 0.32 0.00 2024-11-06T03:07:37Z- AHRS_M2 msg: 1730862457.59 epoch sec; seqNo:2553 2024-11-06T03:07:37Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T03:07:37Z- handleDepth()- Depth_Keller msg: 1730862457.60 epoch sec; seqNo:2718; depth 50.47 2024-11-06T03:07:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862457.62 epoch sec; seqNo:2718; 4061711.30 north; 594687.86 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:07:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862457.22 2024-11-06T03:07:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:37Z- waiting for 1730862461.22, time is 1730862457.62 2024-11-06T03:07:38Z- AHRS_M2 msg: 1730862458.00 epoch sec; seqNo:2554 2024-11-06T03:07:38Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T03:07:38Z- handleDepth()- Depth_Keller msg: 1730862458.00 epoch sec; seqNo:2719; depth 50.49 2024-11-06T03:07:38Z- handleDVL() - DVL msg epochMillisec: 1730862458.03 2024-11-06T03:07:38Z- handleDvl() - RDI_Pathfinder msg: 1730862458.03 sec; seqNo:890; #beams:4 2024-11-06T03:07:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.51 , 28.28 , 32.11 , 33.33 2024-11-06T03:07:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , 0.01 2024-11-06T03:07:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862458.05 epoch sec; seqNo:2719; 4061711.30 north; 594687.86 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:07:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862457.22 2024-11-06T03:07:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:38Z- waiting for 1730862461.22, time is 1730862458.05 2024-11-06T03:07:38Z- AHRS_M2 msg: 1730862458.39 epoch sec; seqNo:2555 2024-11-06T03:07:38Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.51 psi 2024-11-06T03:07:38Z- handleDepth()- Depth_Keller msg: 1730862458.40 epoch sec; seqNo:2720; depth 50.48 2024-11-06T03:07:38Z- handleDVL() - DVL msg epochMillisec: 1730862458.40 2024-11-06T03:07:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:38Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:38Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:38Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:38Z- handleDvl() - RDI_Pathfinder msg: 1730862458.40 sec; seqNo:891; #beams:0 2024-11-06T03:07:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , 0.01 2024-11-06T03:07:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862458.42 epoch sec; seqNo:2720; 4061711.30 north; 594687.86 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:07:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862457.22 2024-11-06T03:07:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:38Z- waiting for 1730862461.22, time is 1730862458.42 2024-11-06T03:07:38Z- AHRS_M2 msg: 1730862458.80 epoch sec; seqNo:2556 2024-11-06T03:07:38Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.51 psi 2024-11-06T03:07:38Z- handleDepth()- Depth_Keller msg: 1730862458.80 epoch sec; seqNo:2721; depth 50.50 2024-11-06T03:07:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862458.81 epoch sec; seqNo:2721; 4061711.31 north; 594689.08 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:07:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862457.22 2024-11-06T03:07:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:38Z- waiting for 1730862461.22, time is 1730862458.81 2024-11-06T03:07:39Z- handleDepth()- Depth_Keller msg: 1730862459.22 epoch sec; seqNo:2722; depth 50.52 2024-11-06T03:07:39Z- handleDVL() - DVL msg epochMillisec: 1730862459.24 2024-11-06T03:07:39Z- handleDvl() - RDI_Pathfinder msg: 1730862459.24 sec; seqNo:892; #beams:4 2024-11-06T03:07:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.51 , 28.13 , 32.41 , 33.18 2024-11-06T03:07:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , 0.00 2024-11-06T03:07:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862459.27 epoch sec; seqNo:2722; 4061711.31 north; 594689.08 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:07:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862457.22 2024-11-06T03:07:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:39Z- waiting for 1730862461.22, time is 1730862459.27 2024-11-06T03:07:39Z- AHRS_M2 msg: 1730862459.60 epoch sec; seqNo:2557 2024-11-06T03:07:39Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.51 psi 2024-11-06T03:07:39Z- handleDepth()- Depth_Keller msg: 1730862459.61 epoch sec; seqNo:2723; depth 50.52 2024-11-06T03:07:39Z- handleDVL() - DVL msg epochMillisec: 1730862459.61 2024-11-06T03:07:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:39Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:39Z- handleDvl() - RDI_Pathfinder msg: 1730862459.61 sec; seqNo:893; #beams:0 2024-11-06T03:07:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , 0.00 2024-11-06T03:07:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862459.65 epoch sec; seqNo:2723; 4061711.31 north; 594689.08 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:07:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862457.22 2024-11-06T03:07:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:39Z- waiting for 1730862461.22, time is 1730862459.65 2024-11-06T03:07:40Z- handleDepth()- Depth_Keller msg: 1730862460.03 epoch sec; seqNo:2724; depth 50.52 2024-11-06T03:07:40Z- handleDVL() - DVL msg epochMillisec: 1730862460.05 2024-11-06T03:07:40Z- handleDvl() - RDI_Pathfinder msg: 1730862460.05 sec; seqNo:894; #beams:4 2024-11-06T03:07:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.51 , 28.74 , 32.11 , 32.87 2024-11-06T03:07:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , -0.01 2024-11-06T03:07:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862460.07 epoch sec; seqNo:2724; 4061711.69 north; 594689.07 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:07:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862457.22 2024-11-06T03:07:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:40Z- waiting for 1730862461.22, time is 1730862460.07 2024-11-06T03:07:40Z- AHRS_M2 msg: 1730862460.42 epoch sec; seqNo:2558 2024-11-06T03:07:40Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.51 psi 2024-11-06T03:07:40Z- handleDepth()- Depth_Keller msg: 1730862460.43 epoch sec; seqNo:2725; depth 50.53 2024-11-06T03:07:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862460.44 epoch sec; seqNo:2725; 4061711.70 north; 594690.29 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:07:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862457.22 2024-11-06T03:07:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:40Z- waiting for 1730862461.22, time is 1730862460.44 2024-11-06T03:07:40Z- AHRS_M2 msg: 1730862460.82 epoch sec; seqNo:2559 2024-11-06T03:07:40Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.51 psi 2024-11-06T03:07:40Z- handleDepth()- Depth_Keller msg: 1730862460.83 epoch sec; seqNo:2726; depth 50.55 2024-11-06T03:07:40Z- handleDVL() - DVL msg epochMillisec: 1730862460.83 2024-11-06T03:07:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:40Z- handleDvl() - RDI_Pathfinder msg: 1730862460.83 sec; seqNo:895; #beams:0 2024-11-06T03:07:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , -0.01 2024-11-06T03:07:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862460.84 epoch sec; seqNo:2726; 4061711.70 north; 594690.29 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:07:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862457.22 2024-11-06T03:07:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:40Z- waiting for 1730862461.22, time is 1730862460.84 2024-11-06T03:07:41Z- AHRS_M2 msg: 1730862461.23 epoch sec; seqNo:2560 2024-11-06T03:07:41Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.52 psi 2024-11-06T03:07:41Z- handleDepth()- Depth_Keller msg: 1730862461.24 epoch sec; seqNo:2727; depth 50.55 2024-11-06T03:07:41Z- handleDVL() - DVL msg epochMillisec: 1730862461.27 2024-11-06T03:07:41Z- handleDvl() - RDI_Pathfinder msg: 1730862461.27 sec; seqNo:896; #beams:4 2024-11-06T03:07:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.82 , 28.90 , 32.26 , 33.02 2024-11-06T03:07:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.01 2024-11-06T03:07:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862461.29 epoch sec; seqNo:2727; 4061711.70 north; 594690.29 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:07:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862457.22 2024-11-06T03:07:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:41Z- waiting for 1730862461.22, time is 1730862461.29 2024-11-06T03:07:41Z- LcmTrn::updateTrn() >>>> pose time 1730862461.29, meas time 1730862461.27 <<<< 2024-11-06T03:07:41Z- LcmTrn::updateTrn() equating pose 1730862461.27 and meas times 1730862461.27 2024-11-06T03:07:41Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862461.27 2024-11-06T03:07:41Z- TerrainNav::motionUpdate() - currEstimate = (4061712, 594690.29, 50.55) 2024-11-06T03:07:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:07:41Z- TerrainNav:: SubcloudNIS is 0.26, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:07:41Z- TNavPF::Projecting Measurements 2024-11-06T03:07:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:07:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:07:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:07:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:07:41Z- TNavFilter::4 good beams. 2024-11-06T03:07:41Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:07:41Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:07:41Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:07:41Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:07:41Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:07:41Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:07:41Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:07:41Z- TerrainMapOctree: (vehN, vehE ) = (4061711.70, 594690.29). 2024-11-06T03:07:41Z- TerrainMapOctree: Min Distance = 70.93. 2024-11-06T03:07:41Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:07:41Z- Forcing Subcloud Comparison 2024-11-06T03:07:41Z- Weighting particles with subcloud comparison 2024-11-06T03:07:41Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:41Z- meanExpectedMeasDiff: 4.434322 delta_rms_squared: 1.411436 alpha: 0.986028 2024-11-06T03:07:41Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:41Z- meanExpectedMeasDiff: -1.335545 delta_rms_squared: 0.643160 alpha: 0.969841 2024-11-06T03:07:41Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:41Z- meanExpectedMeasDiff: -0.337166 delta_rms_squared: 0.749383 alpha: 0.974005 2024-11-06T03:07:41Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:41Z- meanExpectedMeasDiff: -0.328559 delta_rms_squared: 0.784987 alpha: 0.975155 2024-11-06T03:07:41Z- Subcloud NIS: 0.277586 2024-11-06T03:07:41Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:07:41Z- TNavPF:: sumWeights = 0.063097 2024-11-06T03:07:41Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:07:41Z- TNavPF::First Term of Map Covariance Matrix is 0.94 2024-11-06T03:07:41Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.01 2024-11-06T03:07:41Z- TerrainNav::Resampling particle filter... 2024-11-06T03:07:41Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862461.27 sec, ping # 0. 2024-11-06T03:07:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:07:41Z- variances([0], [2], and psi_berg) = 0.556045 0.209964 0.000000 2024-11-06T03:07:41Z- publishEstimatesDecoder() - at 1730862461.27 sec 2024-11-06T03:07:41Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:07:41Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061743.68 594673.81 59.94 2024-11-06T03:07:41Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061744.61 594673.96 60.22 lat/lon: 36.70 -121.94 2024-11-06T03:07:41Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.56 0.21 0.32 0.00 2024-11-06T03:07:41Z- AHRS_M2 msg: 1730862461.63 epoch sec; seqNo:2561 2024-11-06T03:07:41Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.52 psi 2024-11-06T03:07:41Z- handleDepth()- Depth_Keller msg: 1730862461.64 epoch sec; seqNo:2728; depth 50.57 2024-11-06T03:07:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862461.66 epoch sec; seqNo:2728; 4061711.71 north; 594691.51 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T03:07:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862461.27 2024-11-06T03:07:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:41Z- waiting for 1730862465.27, time is 1730862461.66 2024-11-06T03:07:42Z- AHRS_M2 msg: 1730862462.03 epoch sec; seqNo:2562 2024-11-06T03:07:42Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.52 psi 2024-11-06T03:07:42Z- handleDepth()- Depth_Keller msg: 1730862462.03 epoch sec; seqNo:2729; depth 50.58 2024-11-06T03:07:42Z- handleDVL() - DVL msg epochMillisec: 1730862462.06 2024-11-06T03:07:42Z- handleDvl() - RDI_Pathfinder msg: 1730862462.06 sec; seqNo:897; #beams:4 2024-11-06T03:07:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.67 , 28.74 , 32.41 , 32.72 2024-11-06T03:07:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , -0.01 2024-11-06T03:07:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862462.07 epoch sec; seqNo:2729; 4061711.71 north; 594691.51 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T03:07:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862461.27 2024-11-06T03:07:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:42Z- waiting for 1730862465.27, time is 1730862462.07 2024-11-06T03:07:42Z- AHRS_M2 msg: 1730862462.43 epoch sec; seqNo:2563 2024-11-06T03:07:42Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.52 psi 2024-11-06T03:07:42Z- handleDepth()- Depth_Keller msg: 1730862462.44 epoch sec; seqNo:2730; depth 50.59 2024-11-06T03:07:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862462.46 epoch sec; seqNo:2730; 4061711.71 north; 594691.51 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:07:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862461.27 2024-11-06T03:07:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:42Z- waiting for 1730862465.27, time is 1730862462.46 2024-11-06T03:07:42Z- AHRS_M2 msg: 1730862462.84 epoch sec; seqNo:2564 2024-11-06T03:07:42Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.52 psi 2024-11-06T03:07:42Z- handleDepth()- Depth_Keller msg: 1730862462.84 epoch sec; seqNo:2731; depth 50.59 2024-11-06T03:07:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862462.85 epoch sec; seqNo:2731; 4061711.71 north; 594691.51 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:07:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862461.27 2024-11-06T03:07:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:42Z- waiting for 1730862465.27, time is 1730862462.85 2024-11-06T03:07:43Z- AHRS_M2 msg: 1730862463.24 epoch sec; seqNo:2565 2024-11-06T03:07:43Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.52 psi 2024-11-06T03:07:43Z- handleDepth()- Depth_Keller msg: 1730862463.25 epoch sec; seqNo:2732; depth 50.59 2024-11-06T03:07:43Z- handleDVL() - DVL msg epochMillisec: 1730862463.28 2024-11-06T03:07:43Z- handleDvl() - RDI_Pathfinder msg: 1730862463.28 sec; seqNo:898; #beams:4 2024-11-06T03:07:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.67 , 28.90 , 32.41 , 32.56 2024-11-06T03:07:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , -0.00 2024-11-06T03:07:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862463.29 epoch sec; seqNo:2732; 4061711.73 north; 594692.74 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:07:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862461.27 2024-11-06T03:07:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:43Z- waiting for 1730862465.27, time is 1730862463.29 2024-11-06T03:07:43Z- AHRS_M2 msg: 1730862463.65 epoch sec; seqNo:2566 2024-11-06T03:07:43Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.52 psi 2024-11-06T03:07:43Z- handleDepth()- Depth_Keller msg: 1730862463.65 epoch sec; seqNo:2733; depth 50.60 2024-11-06T03:07:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862463.67 epoch sec; seqNo:2733; 4061711.73 north; 594692.74 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:07:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862461.27 2024-11-06T03:07:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:43Z- waiting for 1730862465.27, time is 1730862463.67 2024-11-06T03:07:44Z- AHRS_M2 msg: 1730862464.05 epoch sec; seqNo:2567 2024-11-06T03:07:44Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.52 psi 2024-11-06T03:07:44Z- handleDepth()- Depth_Keller msg: 1730862464.06 epoch sec; seqNo:2734; depth 50.61 2024-11-06T03:07:44Z- handleDVL() - DVL msg epochMillisec: 1730862464.08 2024-11-06T03:07:44Z- handleDvl() - RDI_Pathfinder msg: 1730862464.08 sec; seqNo:899; #beams:4 2024-11-06T03:07:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.67 , 28.44 , 32.56 , 32.41 2024-11-06T03:07:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , 0.00 2024-11-06T03:07:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862464.09 epoch sec; seqNo:2734; 4061711.73 north; 594692.74 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:07:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862461.27 2024-11-06T03:07:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:44Z- waiting for 1730862465.27, time is 1730862464.09 2024-11-06T03:07:44Z- AHRS_M2 msg: 1730862464.45 epoch sec; seqNo:2568 2024-11-06T03:07:44Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.52 psi 2024-11-06T03:07:44Z- handleDepth()- Depth_Keller msg: 1730862464.46 epoch sec; seqNo:2735; depth 50.60 2024-11-06T03:07:44Z- handleDVL() - DVL msg epochMillisec: 1730862464.46 2024-11-06T03:07:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:44Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:44Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:44Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:44Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:44Z- handleDvl() - RDI_Pathfinder msg: 1730862464.46 sec; seqNo:900; #beams:0 2024-11-06T03:07:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , 0.00 2024-11-06T03:07:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862464.50 epoch sec; seqNo:2735; 4061711.74 north; 594693.96 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:07:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862461.27 2024-11-06T03:07:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:44Z- waiting for 1730862465.27, time is 1730862464.50 2024-11-06T03:07:44Z- AHRS_M2 msg: 1730862464.85 epoch sec; seqNo:2569 2024-11-06T03:07:44Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.52 psi 2024-11-06T03:07:44Z- handleDepth()- Depth_Keller msg: 1730862464.86 epoch sec; seqNo:2736; depth 50.63 2024-11-06T03:07:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862464.87 epoch sec; seqNo:2736; 4061711.74 north; 594693.96 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:07:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862461.27 2024-11-06T03:07:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:45Z- waiting for 1730862465.27, time is 1730862464.87 2024-11-06T03:07:45Z- AHRS_M2 msg: 1730862465.26 epoch sec; seqNo:2570 2024-11-06T03:07:45Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.53 psi 2024-11-06T03:07:45Z- handleDepth()- Depth_Keller msg: 1730862465.26 epoch sec; seqNo:2737; depth 50.62 2024-11-06T03:07:45Z- handleDVL() - DVL msg epochMillisec: 1730862465.29 2024-11-06T03:07:45Z- handleDvl() - RDI_Pathfinder msg: 1730862465.29 sec; seqNo:901; #beams:4 2024-11-06T03:07:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.67 , 28.44 , 32.72 , 32.26 2024-11-06T03:07:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , 0.01 2024-11-06T03:07:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862465.32 epoch sec; seqNo:2737; 4061711.74 north; 594693.96 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:07:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862461.27 2024-11-06T03:07:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:45Z- waiting for 1730862465.27, time is 1730862465.32 2024-11-06T03:07:45Z- LcmTrn::updateTrn() >>>> pose time 1730862465.32, meas time 1730862465.29 <<<< 2024-11-06T03:07:45Z- LcmTrn::updateTrn() equating pose 1730862465.29 and meas times 1730862465.29 2024-11-06T03:07:45Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862465.29 2024-11-06T03:07:45Z- TerrainNav::motionUpdate() - currEstimate = (4061712, 594693.96, 50.62) 2024-11-06T03:07:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:07:45Z- TerrainNav:: SubcloudNIS is 0.28, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:07:45Z- TNavPF::Projecting Measurements 2024-11-06T03:07:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:07:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:07:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:07:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:07:45Z- TNavFilter::4 good beams. 2024-11-06T03:07:45Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:07:45Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:07:45Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:07:45Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:07:45Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:07:45Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:07:45Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:07:45Z- TerrainMapOctree: (vehN, vehE ) = (4061711.74, 594693.96). 2024-11-06T03:07:45Z- TerrainMapOctree: Min Distance = 67.86. 2024-11-06T03:07:45Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:07:45Z- Forcing Subcloud Comparison 2024-11-06T03:07:45Z- Weighting particles with subcloud comparison 2024-11-06T03:07:45Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:45Z- meanExpectedMeasDiff: 4.155102 delta_rms_squared: 0.928153 alpha: 0.978906 2024-11-06T03:07:45Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:45Z- meanExpectedMeasDiff: -1.985662 delta_rms_squared: 0.641285 alpha: 0.969756 2024-11-06T03:07:45Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:45Z- meanExpectedMeasDiff: -0.176802 delta_rms_squared: 0.751435 alpha: 0.974074 2024-11-06T03:07:45Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:45Z- meanExpectedMeasDiff: -1.231988 delta_rms_squared: 0.779334 alpha: 0.974979 2024-11-06T03:07:45Z- Subcloud NIS: 0.307150 2024-11-06T03:07:45Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:07:45Z- TNavPF:: sumWeights = 0.045254 2024-11-06T03:07:45Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:07:45Z- TNavPF::First Term of Map Covariance Matrix is 0.73 2024-11-06T03:07:45Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T03:07:45Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862465.29 sec, ping # 0. 2024-11-06T03:07:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:07:45Z- variances([0], [2], and psi_berg) = 0.550324 0.213197 0.000000 2024-11-06T03:07:45Z- publishEstimatesDecoder() - at 1730862465.29 sec 2024-11-06T03:07:45Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:07:45Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061745.18 594677.18 60.27 2024-11-06T03:07:45Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061744.65 594677.62 60.28 lat/lon: 36.70 -121.94 2024-11-06T03:07:45Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.55 0.21 0.31 0.00 2024-11-06T03:07:45Z- AHRS_M2 msg: 1730862465.66 epoch sec; seqNo:2571 2024-11-06T03:07:45Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.53 psi 2024-11-06T03:07:45Z- handleDepth()- Depth_Keller msg: 1730862465.68 epoch sec; seqNo:2738; depth 50.63 2024-11-06T03:07:45Z- handleDVL() - DVL msg epochMillisec: 1730862465.68 2024-11-06T03:07:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:45Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:45Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:45Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:45Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:45Z- handleDvl() - RDI_Pathfinder msg: 1730862465.68 sec; seqNo:902; #beams:0 2024-11-06T03:07:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , 0.01 2024-11-06T03:07:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862465.69 epoch sec; seqNo:2738; 4061711.74 north; 594693.96 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:07:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862465.29 2024-11-06T03:07:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:45Z- waiting for 1730862469.29, time is 1730862465.69 2024-11-06T03:07:46Z- AHRS_M2 msg: 1730862466.07 epoch sec; seqNo:2572 2024-11-06T03:07:46Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 1.53 psi 2024-11-06T03:07:46Z- handleDepth()- Depth_Keller msg: 1730862466.08 epoch sec; seqNo:2739; depth 50.63 2024-11-06T03:07:46Z- handleDVL() - DVL msg epochMillisec: 1730862466.11 2024-11-06T03:07:46Z- handleDvl() - RDI_Pathfinder msg: 1730862466.11 sec; seqNo:903; #beams:4 2024-11-06T03:07:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.82 , 28.44 , 32.56 , 32.26 2024-11-06T03:07:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , 0.00 2024-11-06T03:07:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862466.13 epoch sec; seqNo:2739; 4061712.13 north; 594695.17 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:07:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862465.29 2024-11-06T03:07:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:46Z- waiting for 1730862469.29, time is 1730862466.13 2024-11-06T03:07:46Z- AHRS_M2 msg: 1730862466.47 epoch sec; seqNo:2573 2024-11-06T03:07:46Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 1.53 psi 2024-11-06T03:07:46Z- handleDepth()- Depth_Keller msg: 1730862466.49 epoch sec; seqNo:2740; depth 50.64 2024-11-06T03:07:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862466.50 epoch sec; seqNo:2740; 4061712.13 north; 594695.17 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:07:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862465.29 2024-11-06T03:07:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:46Z- waiting for 1730862469.29, time is 1730862466.50 2024-11-06T03:07:46Z- handleDepth()- Depth_Keller msg: 1730862466.91 epoch sec; seqNo:2741; depth 50.64 2024-11-06T03:07:46Z- handleDVL() - DVL msg epochMillisec: 1730862466.91 2024-11-06T03:07:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:46Z- handleDvl() - RDI_Pathfinder msg: 1730862466.91 sec; seqNo:904; #beams:0 2024-11-06T03:07:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , 0.00 2024-11-06T03:07:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862466.92 epoch sec; seqNo:2741; 4061712.13 north; 594695.17 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:07:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862465.29 2024-11-06T03:07:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:47Z- waiting for 1730862469.29, time is 1730862466.92 2024-11-06T03:07:47Z- AHRS_M2 msg: 1730862467.28 epoch sec; seqNo:2574 2024-11-06T03:07:47Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.53 psi 2024-11-06T03:07:47Z- handleDepth()- Depth_Keller msg: 1730862467.28 epoch sec; seqNo:2742; depth 50.64 2024-11-06T03:07:47Z- handleDVL() - DVL msg epochMillisec: 1730862467.30 2024-11-06T03:07:47Z- handleDvl() - RDI_Pathfinder msg: 1730862467.30 sec; seqNo:905; #beams:4 2024-11-06T03:07:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.97 , 28.59 , 33.33 , 31.80 2024-11-06T03:07:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , 0.00 2024-11-06T03:07:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862467.33 epoch sec; seqNo:2742; 4061712.15 north; 594696.39 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:07:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862465.29 2024-11-06T03:07:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:47Z- waiting for 1730862469.29, time is 1730862467.33 2024-11-06T03:07:47Z- AHRS_M2 msg: 1730862467.71 epoch sec; seqNo:2575 2024-11-06T03:07:47Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.53 psi 2024-11-06T03:07:47Z- handleDepth()- Depth_Keller msg: 1730862467.71 epoch sec; seqNo:2743; depth 50.63 2024-11-06T03:07:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862467.73 epoch sec; seqNo:2743; 4061712.15 north; 594696.39 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:07:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862465.29 2024-11-06T03:07:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:47Z- waiting for 1730862469.29, time is 1730862467.73 2024-11-06T03:07:48Z- AHRS_M2 msg: 1730862468.12 epoch sec; seqNo:2576 2024-11-06T03:07:48Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.53 psi 2024-11-06T03:07:48Z- handleDepth()- Depth_Keller msg: 1730862468.13 epoch sec; seqNo:2744; depth 50.65 2024-11-06T03:07:48Z- handleDVL() - DVL msg epochMillisec: 1730862468.16 2024-11-06T03:07:48Z- handleDvl() - RDI_Pathfinder msg: 1730862468.16 sec; seqNo:906; #beams:4 2024-11-06T03:07:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.51 , 28.44 , 33.18 , 31.95 2024-11-06T03:07:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , -0.01 2024-11-06T03:07:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862468.17 epoch sec; seqNo:2744; 4061712.15 north; 594696.39 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:07:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862465.29 2024-11-06T03:07:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:48Z- waiting for 1730862469.29, time is 1730862468.17 2024-11-06T03:07:48Z- AHRS_M2 msg: 1730862468.52 epoch sec; seqNo:2577 2024-11-06T03:07:48Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.53 psi 2024-11-06T03:07:48Z- handleDepth()- Depth_Keller msg: 1730862468.53 epoch sec; seqNo:2745; depth 50.64 2024-11-06T03:07:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862468.55 epoch sec; seqNo:2745; 4061712.16 north; 594697.61 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:07:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862465.29 2024-11-06T03:07:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:48Z- waiting for 1730862469.29, time is 1730862468.55 2024-11-06T03:07:48Z- AHRS_M2 msg: 1730862468.93 epoch sec; seqNo:2578 2024-11-06T03:07:48Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.53 psi 2024-11-06T03:07:48Z- handleDepth()- Depth_Keller msg: 1730862468.93 epoch sec; seqNo:2746; depth 50.63 2024-11-06T03:07:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862468.94 epoch sec; seqNo:2746; 4061712.16 north; 594697.61 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:07:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862465.29 2024-11-06T03:07:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:49Z- waiting for 1730862469.29, time is 1730862468.94 2024-11-06T03:07:49Z- AHRS_M2 msg: 1730862469.33 epoch sec; seqNo:2579 2024-11-06T03:07:49Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.53 psi 2024-11-06T03:07:49Z- handleDepth()- Depth_Keller msg: 1730862469.34 epoch sec; seqNo:2747; depth 50.64 2024-11-06T03:07:49Z- handleDVL() - DVL msg epochMillisec: 1730862469.36 2024-11-06T03:07:49Z- handleDvl() - RDI_Pathfinder msg: 1730862469.36 sec; seqNo:907; #beams:4 2024-11-06T03:07:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.06 , 28.59 , 33.79 , 31.65 2024-11-06T03:07:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , -0.03 2024-11-06T03:07:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862469.38 epoch sec; seqNo:2747; 4061712.16 north; 594697.61 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:07:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862465.29 2024-11-06T03:07:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:49Z- waiting for 1730862469.29, time is 1730862469.38 2024-11-06T03:07:49Z- LcmTrn::updateTrn() >>>> pose time 1730862469.38, meas time 1730862469.36 <<<< 2024-11-06T03:07:49Z- LcmTrn::updateTrn() equating pose 1730862469.36 and meas times 1730862469.36 2024-11-06T03:07:49Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862469.36 2024-11-06T03:07:49Z- TerrainNav::motionUpdate() - currEstimate = (4061712, 594697.61, 50.64) 2024-11-06T03:07:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:07:49Z- TerrainNav:: SubcloudNIS is 0.31, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:07:49Z- TNavPF::Projecting Measurements 2024-11-06T03:07:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:07:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:07:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:07:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:07:49Z- TNavFilter::4 good beams. 2024-11-06T03:07:49Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:07:49Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:07:49Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:07:49Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:07:49Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:07:49Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:07:49Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:07:49Z- TerrainMapOctree: (vehN, vehE ) = (4061712.16, 594697.61). 2024-11-06T03:07:49Z- TerrainMapOctree: Min Distance = 64.62. 2024-11-06T03:07:49Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:07:49Z- Forcing Subcloud Comparison 2024-11-06T03:07:49Z- Weighting particles with subcloud comparison 2024-11-06T03:07:49Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:49Z- meanExpectedMeasDiff: 2.955999 delta_rms_squared: 0.925316 alpha: 0.978843 2024-11-06T03:07:49Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:49Z- meanExpectedMeasDiff: -2.094682 delta_rms_squared: 0.641682 alpha: 0.969774 2024-11-06T03:07:49Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:49Z- meanExpectedMeasDiff: 0.819979 delta_rms_squared: 0.742281 alpha: 0.973763 2024-11-06T03:07:49Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:49Z- meanExpectedMeasDiff: -1.518770 delta_rms_squared: 0.751379 alpha: 0.974072 2024-11-06T03:07:49Z- Subcloud NIS: 0.330586 2024-11-06T03:07:49Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:07:49Z- TNavPF:: sumWeights = 0.088578 2024-11-06T03:07:49Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:07:49Z- TNavPF::First Term of Map Covariance Matrix is 0.70 2024-11-06T03:07:49Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T03:07:49Z- TerrainNav::Resampling particle filter... 2024-11-06T03:07:49Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862469.36 sec, ping # 0. 2024-11-06T03:07:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:07:49Z- variances([0], [2], and psi_berg) = 0.528214 0.215291 0.000000 2024-11-06T03:07:49Z- publishEstimatesDecoder() - at 1730862469.36 sec 2024-11-06T03:07:49Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:07:49Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061744.13 594681.19 60.43 2024-11-06T03:07:49Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061745.08 594681.28 60.33 lat/lon: 36.70 -121.94 2024-11-06T03:07:49Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.53 0.22 0.31 0.00 2024-11-06T03:07:49Z- AHRS_M2 msg: 1730862469.73 epoch sec; seqNo:2580 2024-11-06T03:07:49Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.53 psi 2024-11-06T03:07:49Z- handleDepth()- Depth_Keller msg: 1730862469.73 epoch sec; seqNo:2748; depth 50.65 2024-11-06T03:07:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862469.75 epoch sec; seqNo:2748; 4061712.16 north; 594697.61 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:07:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862469.36 2024-11-06T03:07:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:49Z- waiting for 1730862473.36, time is 1730862469.75 2024-11-06T03:07:50Z- AHRS_M2 msg: 1730862470.13 epoch sec; seqNo:2581 2024-11-06T03:07:50Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.53 psi 2024-11-06T03:07:50Z- handleDepth()- Depth_Keller msg: 1730862470.15 epoch sec; seqNo:2749; depth 50.64 2024-11-06T03:07:50Z- handleDVL() - DVL msg epochMillisec: 1730862470.17 2024-11-06T03:07:50Z- handleDvl() - RDI_Pathfinder msg: 1730862470.17 sec; seqNo:908; #beams:4 2024-11-06T03:07:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.06 , 28.28 , 33.79 , 31.34 2024-11-06T03:07:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.04 2024-11-06T03:07:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862470.19 epoch sec; seqNo:2749; 4061712.17 north; 594698.83 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:07:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862469.36 2024-11-06T03:07:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:50Z- waiting for 1730862473.36, time is 1730862470.19 2024-11-06T03:07:50Z- AHRS_M2 msg: 1730862470.54 epoch sec; seqNo:2582 2024-11-06T03:07:50Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.53 psi 2024-11-06T03:07:50Z- handleDepth()- Depth_Keller msg: 1730862470.55 epoch sec; seqNo:2750; depth 50.64 2024-11-06T03:07:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862470.56 epoch sec; seqNo:2750; 4061712.17 north; 594698.83 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:07:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862469.36 2024-11-06T03:07:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:50Z- waiting for 1730862473.36, time is 1730862470.56 2024-11-06T03:07:50Z- handleDepth()- Depth_Keller msg: 1730862470.97 epoch sec; seqNo:2751; depth 50.64 2024-11-06T03:07:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862470.97 epoch sec; seqNo:2751; 4061712.17 north; 594698.83 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:07:51Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:07:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862469.36 2024-11-06T03:07:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:51Z- waiting for 1730862473.36, time is 1730862470.97 2024-11-06T03:07:51Z- AHRS_M2 msg: 1730862471.35 epoch sec; seqNo:2583 2024-11-06T03:07:51Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.53 psi 2024-11-06T03:07:51Z- handleDepth()- Depth_Keller msg: 1730862471.35 epoch sec; seqNo:2752; depth 50.63 2024-11-06T03:07:51Z- handleDVL() - DVL msg epochMillisec: 1730862471.39 2024-11-06T03:07:51Z- handleDvl() - RDI_Pathfinder msg: 1730862471.39 sec; seqNo:909; #beams:4 2024-11-06T03:07:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.06 , 27.83 , 34.09 , 31.19 2024-11-06T03:07:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , -0.05 2024-11-06T03:07:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862471.40 epoch sec; seqNo:2752; 4061712.19 north; 594700.05 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:07:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862469.36 2024-11-06T03:07:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:51Z- waiting for 1730862473.36, time is 1730862471.40 2024-11-06T03:07:51Z- AHRS_M2 msg: 1730862471.75 epoch sec; seqNo:2584 2024-11-06T03:07:51Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.53 psi 2024-11-06T03:07:51Z- handleDepth()- Depth_Keller msg: 1730862471.76 epoch sec; seqNo:2753; depth 50.64 2024-11-06T03:07:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862471.78 epoch sec; seqNo:2753; 4061712.19 north; 594700.05 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:07:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862469.36 2024-11-06T03:07:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:51Z- waiting for 1730862473.36, time is 1730862471.78 2024-11-06T03:07:52Z- AHRS_M2 msg: 1730862472.15 epoch sec; seqNo:2585 2024-11-06T03:07:52Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.53 psi 2024-11-06T03:07:52Z- handleDepth()- Depth_Keller msg: 1730862472.16 epoch sec; seqNo:2754; depth 50.62 2024-11-06T03:07:52Z- handleDVL() - DVL msg epochMillisec: 1730862472.19 2024-11-06T03:07:52Z- handleDvl() - RDI_Pathfinder msg: 1730862472.19 sec; seqNo:910; #beams:4 2024-11-06T03:07:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.21 , 27.52 , 34.55 , 30.88 2024-11-06T03:07:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , -0.05 2024-11-06T03:07:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862472.20 epoch sec; seqNo:2754; 4061712.19 north; 594700.05 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:07:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862469.36 2024-11-06T03:07:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:52Z- waiting for 1730862473.36, time is 1730862472.20 2024-11-06T03:07:52Z- AHRS_M2 msg: 1730862472.56 epoch sec; seqNo:2586 2024-11-06T03:07:52Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.53 psi 2024-11-06T03:07:52Z- handleDepth()- Depth_Keller msg: 1730862472.57 epoch sec; seqNo:2755; depth 50.65 2024-11-06T03:07:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862472.58 epoch sec; seqNo:2755; 4061712.19 north; 594700.05 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:07:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862469.36 2024-11-06T03:07:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:52Z- waiting for 1730862473.36, time is 1730862472.58 2024-11-06T03:07:52Z- AHRS_M2 msg: 1730862472.96 epoch sec; seqNo:2587 2024-11-06T03:07:52Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.53 psi 2024-11-06T03:07:52Z- handleDepth()- Depth_Keller msg: 1730862472.97 epoch sec; seqNo:2756; depth 50.64 2024-11-06T03:07:52Z- handleDVL() - DVL msg epochMillisec: 1730862472.97 2024-11-06T03:07:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:52Z- handleDvl() - RDI_Pathfinder msg: 1730862472.97 sec; seqNo:911; #beams:0 2024-11-06T03:07:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , -0.05 2024-11-06T03:07:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862472.98 epoch sec; seqNo:2756; 4061712.20 north; 594701.27 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:07:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862469.36 2024-11-06T03:07:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:53Z- waiting for 1730862473.36, time is 1730862472.98 2024-11-06T03:07:53Z- AHRS_M2 msg: 1730862473.37 epoch sec; seqNo:2588 2024-11-06T03:07:53Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.53 psi 2024-11-06T03:07:53Z- handleDepth()- Depth_Keller msg: 1730862473.38 epoch sec; seqNo:2757; depth 50.64 2024-11-06T03:07:53Z- handleDVL() - DVL msg epochMillisec: 1730862473.40 2024-11-06T03:07:53Z- handleDvl() - RDI_Pathfinder msg: 1730862473.40 sec; seqNo:912; #beams:4 2024-11-06T03:07:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.36 , 27.21 , 34.40 , 30.73 2024-11-06T03:07:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.04 , -0.04 2024-11-06T03:07:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862473.42 epoch sec; seqNo:2757; 4061712.58 north; 594701.27 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:07:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862469.36 2024-11-06T03:07:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:53Z- waiting for 1730862473.36, time is 1730862473.42 2024-11-06T03:07:53Z- LcmTrn::updateTrn() >>>> pose time 1730862473.42, meas time 1730862473.40 <<<< 2024-11-06T03:07:53Z- LcmTrn::updateTrn() equating pose 1730862473.40 and meas times 1730862473.40 2024-11-06T03:07:53Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862473.40 2024-11-06T03:07:53Z- TerrainNav::motionUpdate() - currEstimate = (4061713, 594701.27, 50.64) 2024-11-06T03:07:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:07:53Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:07:53Z- TNavPF::Projecting Measurements 2024-11-06T03:07:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:07:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:07:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:07:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:07:53Z- TNavFilter::4 good beams. 2024-11-06T03:07:53Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:07:53Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:07:53Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:07:53Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:07:53Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:07:53Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:07:53Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:07:53Z- TerrainMapOctree: (vehN, vehE ) = (4061712.58, 594701.27). 2024-11-06T03:07:53Z- TerrainMapOctree: Min Distance = 61.44. 2024-11-06T03:07:53Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:07:53Z- Forcing Subcloud Comparison 2024-11-06T03:07:53Z- Weighting particles with subcloud comparison 2024-11-06T03:07:53Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:53Z- meanExpectedMeasDiff: 2.949035 delta_rms_squared: 1.292981 alpha: 0.984767 2024-11-06T03:07:53Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:53Z- meanExpectedMeasDiff: -3.271513 delta_rms_squared: 0.614731 alpha: 0.968491 2024-11-06T03:07:53Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:53Z- meanExpectedMeasDiff: 0.762966 delta_rms_squared: 0.750579 alpha: 0.974045 2024-11-06T03:07:53Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:53Z- meanExpectedMeasDiff: -1.912769 delta_rms_squared: 0.706323 alpha: 0.972464 2024-11-06T03:07:53Z- Subcloud NIS: 0.362600 2024-11-06T03:07:53Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:07:53Z- TNavPF:: sumWeights = 0.027206 2024-11-06T03:07:53Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:07:53Z- TNavPF::First Term of Map Covariance Matrix is 0.26 2024-11-06T03:07:53Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:07:53Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862473.40 sec, ping # 0. 2024-11-06T03:07:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:07:53Z- variances([0], [2], and psi_berg) = 0.425478 0.218958 0.000000 2024-11-06T03:07:53Z- publishEstimatesDecoder() - at 1730862473.40 sec 2024-11-06T03:07:53Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:07:53Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061745.99 594684.99 60.82 2024-11-06T03:07:53Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061745.60 594684.92 60.50 lat/lon: 36.70 -121.94 2024-11-06T03:07:53Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.43 0.22 0.28 0.00 2024-11-06T03:07:53Z- AHRS_M2 msg: 1730862473.77 epoch sec; seqNo:2589 2024-11-06T03:07:53Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.53 psi 2024-11-06T03:07:53Z- handleDepth()- Depth_Keller msg: 1730862473.78 epoch sec; seqNo:2758; depth 50.64 2024-11-06T03:07:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862473.79 epoch sec; seqNo:2758; 4061712.58 north; 594701.27 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:07:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862473.40 2024-11-06T03:07:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:53Z- waiting for 1730862477.40, time is 1730862473.79 2024-11-06T03:07:54Z- AHRS_M2 msg: 1730862474.18 epoch sec; seqNo:2590 2024-11-06T03:07:54Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.53 psi 2024-11-06T03:07:54Z- handleDepth()- Depth_Keller msg: 1730862474.19 epoch sec; seqNo:2759; depth 50.65 2024-11-06T03:07:54Z- handleDVL() - DVL msg epochMillisec: 1730862474.21 2024-11-06T03:07:54Z- handleDvl() - RDI_Pathfinder msg: 1730862474.21 sec; seqNo:913; #beams:4 2024-11-06T03:07:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.36 , 26.91 , 35.32 , 30.73 2024-11-06T03:07:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , -0.02 2024-11-06T03:07:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862474.24 epoch sec; seqNo:2759; 4061712.58 north; 594701.27 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:07:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862473.40 2024-11-06T03:07:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:54Z- waiting for 1730862477.40, time is 1730862474.24 2024-11-06T03:07:54Z- AHRS_M2 msg: 1730862474.58 epoch sec; seqNo:2591 2024-11-06T03:07:54Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.53 psi 2024-11-06T03:07:54Z- handleDepth()- Depth_Keller msg: 1730862474.59 epoch sec; seqNo:2760; depth 50.65 2024-11-06T03:07:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862474.61 epoch sec; seqNo:2760; 4061712.59 north; 594702.49 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:07:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862473.40 2024-11-06T03:07:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:54Z- waiting for 1730862477.40, time is 1730862474.61 2024-11-06T03:07:54Z- AHRS_M2 msg: 1730862474.98 epoch sec; seqNo:2592 2024-11-06T03:07:54Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T03:07:54Z- handleDepth()- Depth_Keller msg: 1730862474.99 epoch sec; seqNo:2761; depth 50.64 2024-11-06T03:07:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862475.00 epoch sec; seqNo:2761; 4061712.59 north; 594702.49 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:07:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862473.40 2024-11-06T03:07:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:55Z- waiting for 1730862477.40, time is 1730862475.00 2024-11-06T03:07:55Z- AHRS_M2 msg: 1730862475.39 epoch sec; seqNo:2593 2024-11-06T03:07:55Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.53 psi 2024-11-06T03:07:55Z- handleDepth()- Depth_Keller msg: 1730862475.39 epoch sec; seqNo:2762; depth 50.65 2024-11-06T03:07:55Z- handleDVL() - DVL msg epochMillisec: 1730862475.41 2024-11-06T03:07:55Z- handleDvl() - RDI_Pathfinder msg: 1730862475.41 sec; seqNo:914; #beams:4 2024-11-06T03:07:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.67 , 26.91 , 35.78 , 30.73 2024-11-06T03:07:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , 0.01 2024-11-06T03:07:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862475.45 epoch sec; seqNo:2762; 4061712.59 north; 594702.49 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:07:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862473.40 2024-11-06T03:07:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:55Z- waiting for 1730862477.40, time is 1730862475.45 2024-11-06T03:07:55Z- AHRS_M2 msg: 1730862475.79 epoch sec; seqNo:2594 2024-11-06T03:07:55Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.53 psi 2024-11-06T03:07:55Z- handleDepth()- Depth_Keller msg: 1730862475.80 epoch sec; seqNo:2763; depth 50.63 2024-11-06T03:07:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862475.81 epoch sec; seqNo:2763; 4061712.61 north; 594703.71 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:07:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862473.40 2024-11-06T03:07:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:55Z- waiting for 1730862477.40, time is 1730862475.81 2024-11-06T03:07:56Z- AHRS_M2 msg: 1730862476.22 epoch sec; seqNo:2595 2024-11-06T03:07:56Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 1.52 psi 2024-11-06T03:07:56Z- handleDepth()- Depth_Keller msg: 1730862476.22 epoch sec; seqNo:2764; depth 50.65 2024-11-06T03:07:56Z- handleDVL() - DVL msg epochMillisec: 1730862476.24 2024-11-06T03:07:56Z- handleDvl() - RDI_Pathfinder msg: 1730862476.24 sec; seqNo:915; #beams:4 2024-11-06T03:07:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.51 , 27.37 , 36.08 , 30.58 2024-11-06T03:07:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.03 2024-11-06T03:07:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862476.27 epoch sec; seqNo:2764; 4061712.61 north; 594703.71 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:07:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862473.40 2024-11-06T03:07:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:56Z- waiting for 1730862477.40, time is 1730862476.27 2024-11-06T03:07:56Z- handleDepth()- Depth_Keller msg: 1730862476.62 epoch sec; seqNo:2765; depth 50.63 2024-11-06T03:07:56Z- handleDVL() - DVL msg epochMillisec: 1730862476.62 2024-11-06T03:07:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:56Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:56Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:56Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:56Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:56Z- handleDvl() - RDI_Pathfinder msg: 1730862476.62 sec; seqNo:916; #beams:0 2024-11-06T03:07:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.03 2024-11-06T03:07:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862476.65 epoch sec; seqNo:2765; 4061712.61 north; 594703.71 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:07:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862473.40 2024-11-06T03:07:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:56Z- waiting for 1730862477.40, time is 1730862476.65 2024-11-06T03:07:57Z- AHRS_M2 msg: 1730862477.02 epoch sec; seqNo:2596 2024-11-06T03:07:57Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 1.52 psi 2024-11-06T03:07:57Z- handleDepth()- Depth_Keller msg: 1730862477.03 epoch sec; seqNo:2766; depth 50.63 2024-11-06T03:07:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862477.05 epoch sec; seqNo:2766; 4061712.61 north; 594703.71 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:07:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862473.40 2024-11-06T03:07:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:57Z- waiting for 1730862477.40, time is 1730862477.05 2024-11-06T03:07:57Z- AHRS_M2 msg: 1730862477.41 epoch sec; seqNo:2597 2024-11-06T03:07:57Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 1.52 psi 2024-11-06T03:07:57Z- handleDepth()- Depth_Keller msg: 1730862477.42 epoch sec; seqNo:2767; depth 50.63 2024-11-06T03:07:57Z- handleDVL() - DVL msg epochMillisec: 1730862477.44 2024-11-06T03:07:57Z- handleDvl() - RDI_Pathfinder msg: 1730862477.44 sec; seqNo:917; #beams:4 2024-11-06T03:07:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.90 , 27.37 , 35.78 , 30.42 2024-11-06T03:07:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.05 2024-11-06T03:07:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862477.46 epoch sec; seqNo:2767; 4061712.62 north; 594704.93 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:07:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862473.40 2024-11-06T03:07:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:57Z- waiting for 1730862477.40, time is 1730862477.46 2024-11-06T03:07:57Z- LcmTrn::updateTrn() >>>> pose time 1730862477.46, meas time 1730862477.44 <<<< 2024-11-06T03:07:57Z- LcmTrn::updateTrn() equating pose 1730862477.44 and meas times 1730862477.44 2024-11-06T03:07:57Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862477.44 2024-11-06T03:07:57Z- TerrainNav::motionUpdate() - currEstimate = (4061713, 594704.93, 50.63) 2024-11-06T03:07:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:07:57Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:07:57Z- TNavPF::Projecting Measurements 2024-11-06T03:07:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:07:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:07:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:07:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:07:57Z- TNavFilter::4 good beams. 2024-11-06T03:07:57Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:07:57Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:07:57Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:07:57Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:07:57Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:07:57Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:07:57Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:07:57Z- TerrainMapOctree: (vehN, vehE ) = (4061712.62, 594704.93). 2024-11-06T03:07:57Z- TerrainMapOctree: Min Distance = 58.55. 2024-11-06T03:07:57Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:07:57Z- Forcing Subcloud Comparison 2024-11-06T03:07:57Z- Weighting particles with subcloud comparison 2024-11-06T03:07:57Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:57Z- meanExpectedMeasDiff: 4.495333 delta_rms_squared: 0.826240 alpha: 0.976366 2024-11-06T03:07:57Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:57Z- meanExpectedMeasDiff: -3.279135 delta_rms_squared: 0.613959 alpha: 0.968452 2024-11-06T03:07:57Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:57Z- meanExpectedMeasDiff: 1.839079 delta_rms_squared: 0.755160 alpha: 0.974199 2024-11-06T03:07:57Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:07:57Z- meanExpectedMeasDiff: -1.415811 delta_rms_squared: 0.663557 alpha: 0.970741 2024-11-06T03:07:57Z- Subcloud NIS: 0.400257 2024-11-06T03:07:57Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:07:57Z- TNavPF:: sumWeights = 0.006052 2024-11-06T03:07:57Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:07:57Z- TNavPF::First Term of Map Covariance Matrix is 0.12 2024-11-06T03:07:57Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:07:57Z- TerrainNav::Resampling particle filter... 2024-11-06T03:07:57Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862477.44 sec, ping # 0. 2024-11-06T03:07:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:07:57Z- variances([0], [2], and psi_berg) = 0.426639 0.221335 0.000000 2024-11-06T03:07:57Z- publishEstimatesDecoder() - at 1730862477.44 sec 2024-11-06T03:07:57Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:07:57Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061744.78 594688.34 59.03 2024-11-06T03:07:57Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061745.64 594688.58 60.43 lat/lon: 36.70 -121.94 2024-11-06T03:07:57Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.43 0.22 0.29 0.00 2024-11-06T03:07:57Z- AHRS_M2 msg: 1730862477.81 epoch sec; seqNo:2598 2024-11-06T03:07:57Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 1.53 psi 2024-11-06T03:07:57Z- handleDepth()- Depth_Keller msg: 1730862477.82 epoch sec; seqNo:2768; depth 50.62 2024-11-06T03:07:57Z- handleDVL() - DVL msg epochMillisec: 1730862477.82 2024-11-06T03:07:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:07:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:07:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:07:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:07:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:07:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:07:57Z- handleDvl() - RDI_Pathfinder msg: 1730862477.82 sec; seqNo:918; #beams:0 2024-11-06T03:07:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:07:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.05 2024-11-06T03:07:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862477.84 epoch sec; seqNo:2768; 4061712.62 north; 594704.93 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:07:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862477.44 2024-11-06T03:07:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:57Z- waiting for 1730862481.44, time is 1730862477.84 2024-11-06T03:07:58Z- AHRS_M2 msg: 1730862478.24 epoch sec; seqNo:2599 2024-11-06T03:07:58Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 1.53 psi 2024-11-06T03:07:58Z- handleDepth()- Depth_Keller msg: 1730862478.24 epoch sec; seqNo:2769; depth 50.62 2024-11-06T03:07:58Z- handleDVL() - DVL msg epochMillisec: 1730862478.27 2024-11-06T03:07:58Z- handleDvl() - RDI_Pathfinder msg: 1730862478.27 sec; seqNo:919; #beams:4 2024-11-06T03:07:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.06 , 27.06 , 36.54 , 30.58 2024-11-06T03:07:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , 0.05 2024-11-06T03:07:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862478.29 epoch sec; seqNo:2769; 4061713.00 north; 594704.93 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:07:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862477.44 2024-11-06T03:07:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:58Z- waiting for 1730862481.44, time is 1730862478.29 2024-11-06T03:07:58Z- AHRS_M2 msg: 1730862478.63 epoch sec; seqNo:2600 2024-11-06T03:07:58Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 1.53 psi 2024-11-06T03:07:58Z- handleDepth()- Depth_Keller msg: 1730862478.64 epoch sec; seqNo:2770; depth 50.59 2024-11-06T03:07:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862478.66 epoch sec; seqNo:2770; 4061713.01 north; 594706.15 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:07:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862477.44 2024-11-06T03:07:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:58Z- waiting for 1730862481.44, time is 1730862478.66 2024-11-06T03:07:59Z- AHRS_M2 msg: 1730862479.02 epoch sec; seqNo:2601 2024-11-06T03:07:59Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 1.53 psi 2024-11-06T03:07:59Z- handleDepth()- Depth_Keller msg: 1730862479.03 epoch sec; seqNo:2771; depth 50.58 2024-11-06T03:07:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862479.04 epoch sec; seqNo:2771; 4061713.01 north; 594706.15 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T03:07:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862477.44 2024-11-06T03:07:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:59Z- waiting for 1730862481.44, time is 1730862479.04 2024-11-06T03:07:59Z- AHRS_M2 msg: 1730862479.43 epoch sec; seqNo:2602 2024-11-06T03:07:59Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 1.53 psi 2024-11-06T03:07:59Z- handleDepth()- Depth_Keller msg: 1730862479.43 epoch sec; seqNo:2772; depth 50.56 2024-11-06T03:07:59Z- handleDVL() - DVL msg epochMillisec: 1730862479.45 2024-11-06T03:07:59Z- handleDvl() - RDI_Pathfinder msg: 1730862479.45 sec; seqNo:920; #beams:4 2024-11-06T03:07:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.36 , 27.06 , 37.00 , 30.73 2024-11-06T03:07:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , 0.03 2024-11-06T03:07:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862479.48 epoch sec; seqNo:2772; 4061713.01 north; 594706.15 east; 36.70 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T03:07:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:07:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862477.44 2024-11-06T03:07:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:59Z- waiting for 1730862481.44, time is 1730862479.48 2024-11-06T03:07:59Z- AHRS_M2 msg: 1730862479.83 epoch sec; seqNo:2603 2024-11-06T03:07:59Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 1.53 psi 2024-11-06T03:07:59Z- handleDepth()- Depth_Keller msg: 1730862479.85 epoch sec; seqNo:2773; depth 50.53 2024-11-06T03:07:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862479.86 epoch sec; seqNo:2773; 4061713.01 north; 594706.15 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:07:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:07:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862477.44 2024-11-06T03:07:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:07:59Z- waiting for 1730862481.44, time is 1730862479.86 2024-11-06T03:08:00Z- AHRS_M2 msg: 1730862480.27 epoch sec; seqNo:2604 2024-11-06T03:08:00Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 1.53 psi 2024-11-06T03:08:00Z- handleDepth()- Depth_Keller msg: 1730862480.28 epoch sec; seqNo:2774; depth 50.53 2024-11-06T03:08:00Z- handleDVL() - DVL msg epochMillisec: 1730862480.30 2024-11-06T03:08:00Z- handleDvl() - RDI_Pathfinder msg: 1730862480.30 sec; seqNo:921; #beams:4 2024-11-06T03:08:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.82 , 27.21 , 37.61 , 30.73 2024-11-06T03:08:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.03 , 0.00 2024-11-06T03:08:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862480.32 epoch sec; seqNo:2774; 4061713.03 north; 594707.37 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:08:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862477.44 2024-11-06T03:08:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:00Z- waiting for 1730862481.44, time is 1730862480.32 2024-11-06T03:08:00Z- AHRS_M2 msg: 1730862480.65 epoch sec; seqNo:2605 2024-11-06T03:08:00Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 1.53 psi 2024-11-06T03:08:00Z- handleDepth()- Depth_Keller msg: 1730862480.66 epoch sec; seqNo:2775; depth 50.50 2024-11-06T03:08:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862480.68 epoch sec; seqNo:2775; 4061713.03 north; 594707.37 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:08:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862477.44 2024-11-06T03:08:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:00Z- waiting for 1730862481.44, time is 1730862480.68 2024-11-06T03:08:01Z- AHRS_M2 msg: 1730862481.05 epoch sec; seqNo:2606 2024-11-06T03:08:01Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 1.53 psi 2024-11-06T03:08:01Z- handleDepth()- Depth_Keller msg: 1730862481.06 epoch sec; seqNo:2776; depth 50.49 2024-11-06T03:08:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862481.07 epoch sec; seqNo:2776; 4061713.03 north; 594707.37 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:08:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862477.44 2024-11-06T03:08:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:01Z- waiting for 1730862481.44, time is 1730862481.07 2024-11-06T03:08:01Z- AHRS_M2 msg: 1730862481.45 epoch sec; seqNo:2607 2024-11-06T03:08:01Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 1.53 psi 2024-11-06T03:08:01Z- handleDepth()- Depth_Keller msg: 1730862481.46 epoch sec; seqNo:2777; depth 50.47 2024-11-06T03:08:01Z- handleDVL() - DVL msg epochMillisec: 1730862481.48 2024-11-06T03:08:01Z- handleDvl() - RDI_Pathfinder msg: 1730862481.48 sec; seqNo:922; #beams:4 2024-11-06T03:08:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.67 , 27.37 , 37.92 , 30.42 2024-11-06T03:08:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.04 , -0.04 2024-11-06T03:08:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862481.50 epoch sec; seqNo:2777; 4061713.04 north; 594708.59 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:08:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862477.44 2024-11-06T03:08:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:01Z- waiting for 1730862481.44, time is 1730862481.50 2024-11-06T03:08:01Z- LcmTrn::updateTrn() >>>> pose time 1730862481.50, meas time 1730862481.48 <<<< 2024-11-06T03:08:01Z- LcmTrn::updateTrn() equating pose 1730862481.48 and meas times 1730862481.48 2024-11-06T03:08:01Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862481.48 2024-11-06T03:08:01Z- TerrainNav::motionUpdate() - currEstimate = (4061713, 594708.59, 50.47) 2024-11-06T03:08:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:08:01Z- TerrainNav:: SubcloudNIS is 0.40, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:08:01Z- TNavPF::Projecting Measurements 2024-11-06T03:08:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:08:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:08:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:08:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:08:01Z- TNavFilter::4 good beams. 2024-11-06T03:08:01Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:08:01Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:08:01Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:08:01Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:08:01Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:08:01Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:08:01Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:08:01Z- TerrainMapOctree: (vehN, vehE ) = (4061713.04, 594708.59). 2024-11-06T03:08:01Z- TerrainMapOctree: Min Distance = 55.51. 2024-11-06T03:08:01Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:08:01Z- Forcing Subcloud Comparison 2024-11-06T03:08:01Z- Weighting particles with subcloud comparison 2024-11-06T03:08:01Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:01Z- meanExpectedMeasDiff: 4.553823 delta_rms_squared: 0.990405 alpha: 0.980206 2024-11-06T03:08:01Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:01Z- meanExpectedMeasDiff: -3.363143 delta_rms_squared: 0.631232 alpha: 0.969289 2024-11-06T03:08:01Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:01Z- meanExpectedMeasDiff: 3.483948 delta_rms_squared: 0.795035 alpha: 0.975461 2024-11-06T03:08:01Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:01Z- meanExpectedMeasDiff: -1.615095 delta_rms_squared: 0.687067 alpha: 0.971714 2024-11-06T03:08:01Z- Subcloud NIS: 0.456541 2024-11-06T03:08:01Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:08:01Z- TNavPF:: sumWeights = 0.001818 2024-11-06T03:08:01Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:08:01Z- TNavPF::First Term of Map Covariance Matrix is 0.03 2024-11-06T03:08:01Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:08:01Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862481.48 sec, ping # 0. 2024-11-06T03:08:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:08:01Z- variances([0], [2], and psi_berg) = 0.439600 0.228728 0.000000 2024-11-06T03:08:01Z- publishEstimatesDecoder() - at 1730862481.48 sec 2024-11-06T03:08:01Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:08:01Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061747.05 594691.90 60.13 2024-11-06T03:08:01Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061746.08 594692.23 60.08 lat/lon: 36.70 -121.94 2024-11-06T03:08:01Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.44 0.23 0.31 0.00 2024-11-06T03:08:01Z- AHRS_M2 msg: 1730862481.85 epoch sec; seqNo:2608 2024-11-06T03:08:01Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 1.54 psi 2024-11-06T03:08:01Z- handleDepth()- Depth_Keller msg: 1730862481.86 epoch sec; seqNo:2778; depth 50.44 2024-11-06T03:08:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862481.87 epoch sec; seqNo:2778; 4061713.04 north; 594708.59 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:08:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862481.48 2024-11-06T03:08:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:02Z- waiting for 1730862485.48, time is 1730862481.87 2024-11-06T03:08:02Z- AHRS_M2 msg: 1730862482.30 epoch sec; seqNo:2609 2024-11-06T03:08:02Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 1.54 psi 2024-11-06T03:08:02Z- handleDepth()- Depth_Keller msg: 1730862482.30 epoch sec; seqNo:2779; depth 50.41 2024-11-06T03:08:02Z- handleDVL() - DVL msg epochMillisec: 1730862482.33 2024-11-06T03:08:02Z- handleDvl() - RDI_Pathfinder msg: 1730862482.33 sec; seqNo:923; #beams:4 2024-11-06T03:08:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.36 , 27.37 , 37.76 , 30.58 2024-11-06T03:08:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.07 2024-11-06T03:08:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862482.34 epoch sec; seqNo:2779; 4061713.04 north; 594708.59 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:08:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862481.48 2024-11-06T03:08:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:02Z- waiting for 1730862485.48, time is 1730862482.34 2024-11-06T03:08:02Z- AHRS_M2 msg: 1730862482.67 epoch sec; seqNo:2610 2024-11-06T03:08:02Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 1.54 psi 2024-11-06T03:08:02Z- handleDepth()- Depth_Keller msg: 1730862482.69 epoch sec; seqNo:2780; depth 50.38 2024-11-06T03:08:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862482.75 epoch sec; seqNo:2780; 4061713.05 north; 594709.81 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:08:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862481.48 2024-11-06T03:08:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:02Z- waiting for 1730862485.48, time is 1730862482.75 2024-11-06T03:08:03Z- AHRS_M2 msg: 1730862483.06 epoch sec; seqNo:2611 2024-11-06T03:08:03Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 1.54 psi 2024-11-06T03:08:03Z- handleDepth()- Depth_Keller msg: 1730862483.07 epoch sec; seqNo:2781; depth 50.34 2024-11-06T03:08:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862483.08 epoch sec; seqNo:2781; 4061713.05 north; 594709.81 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:08:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862481.48 2024-11-06T03:08:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:03Z- waiting for 1730862485.48, time is 1730862483.08 2024-11-06T03:08:03Z- AHRS_M2 msg: 1730862483.47 epoch sec; seqNo:2612 2024-11-06T03:08:03Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 1.54 psi 2024-11-06T03:08:03Z- handleDepth()- Depth_Keller msg: 1730862483.48 epoch sec; seqNo:2782; depth 50.33 2024-11-06T03:08:03Z- handleDVL() - DVL msg epochMillisec: 1730862483.50 2024-11-06T03:08:03Z- handleDvl() - RDI_Pathfinder msg: 1730862483.50 sec; seqNo:924; #beams:4 2024-11-06T03:08:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.36 , 27.52 , 37.76 , 30.58 2024-11-06T03:08:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , -0.09 2024-11-06T03:08:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862483.51 epoch sec; seqNo:2782; 4061713.05 north; 594709.81 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:08:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862481.48 2024-11-06T03:08:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:03Z- waiting for 1730862485.48, time is 1730862483.51 2024-11-06T03:08:03Z- AHRS_M2 msg: 1730862483.87 epoch sec; seqNo:2613 2024-11-06T03:08:03Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 1.53 psi 2024-11-06T03:08:03Z- handleDepth()- Depth_Keller msg: 1730862483.89 epoch sec; seqNo:2783; depth 50.28 2024-11-06T03:08:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862483.90 epoch sec; seqNo:2783; 4061713.07 north; 594711.03 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:08:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862481.48 2024-11-06T03:08:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:04Z- waiting for 1730862485.48, time is 1730862483.90 2024-11-06T03:08:04Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:08:04Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:08:04Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:08:04Z- AHRS_M2 msg: 1730862484.28 epoch sec; seqNo:2614 2024-11-06T03:08:04Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 1.53 psi 2024-11-06T03:08:04Z- handleDepth()- Depth_Keller msg: 1730862484.35 epoch sec; seqNo:2784; depth 50.28 2024-11-06T03:08:04Z- handleDVL() - DVL msg epochMillisec: 1730862484.37 2024-11-06T03:08:04Z- handleDvl() - RDI_Pathfinder msg: 1730862484.37 sec; seqNo:925; #beams:4 2024-11-06T03:08:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.21 , 27.67 , 37.76 , 30.42 2024-11-06T03:08:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , -0.08 2024-11-06T03:08:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862484.38 epoch sec; seqNo:2784; 4061713.07 north; 594711.03 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:08:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862481.48 2024-11-06T03:08:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:04Z- waiting for 1730862485.48, time is 1730862484.38 2024-11-06T03:08:04Z- handleDepth()- Depth_Keller msg: 1730862484.70 epoch sec; seqNo:2785; depth 50.22 2024-11-06T03:08:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862484.72 epoch sec; seqNo:2785; 4061713.07 north; 594711.03 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:08:04Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:08:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862481.48 2024-11-06T03:08:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:04Z- waiting for 1730862485.48, time is 1730862484.72 2024-11-06T03:08:05Z- AHRS_M2 msg: 1730862485.08 epoch sec; seqNo:2615 2024-11-06T03:08:05Z- AHRS_M2 msg: 0.12 phi; 0.07 theta; 1.53 psi 2024-11-06T03:08:05Z- handleDepth()- Depth_Keller msg: 1730862485.09 epoch sec; seqNo:2786; depth 50.20 2024-11-06T03:08:05Z- handleDVL() - DVL msg epochMillisec: 1730862485.11 2024-11-06T03:08:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:05Z- handleDvl() - RDI_Pathfinder msg: 1730862485.11 sec; seqNo:926; #beams:0 2024-11-06T03:08:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , -0.08 2024-11-06T03:08:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862485.12 epoch sec; seqNo:2786; 4061713.07 north; 594711.03 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:08:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862481.48 2024-11-06T03:08:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:05Z- waiting for 1730862485.48, time is 1730862485.12 2024-11-06T03:08:05Z- handleDepth()- Depth_Keller msg: 1730862485.51 epoch sec; seqNo:2787; depth 50.18 2024-11-06T03:08:05Z- handleDVL() - DVL msg epochMillisec: 1730862485.53 2024-11-06T03:08:05Z- handleDvl() - RDI_Pathfinder msg: 1730862485.53 sec; seqNo:927; #beams:4 2024-11-06T03:08:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.75 , 27.67 , 37.61 , 30.42 2024-11-06T03:08:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.05 , -0.06 2024-11-06T03:08:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862485.55 epoch sec; seqNo:2787; 4061713.08 north; 594712.25 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:08:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862481.48 2024-11-06T03:08:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:05Z- waiting for 1730862485.48, time is 1730862485.55 2024-11-06T03:08:05Z- LcmTrn::updateTrn() >>>> pose time 1730862485.55, meas time 1730862485.53 <<<< 2024-11-06T03:08:05Z- LcmTrn::updateTrn() equating pose 1730862485.53 and meas times 1730862485.53 2024-11-06T03:08:05Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862485.53 2024-11-06T03:08:05Z- TerrainNav::motionUpdate() - currEstimate = (4061713, 594712.25, 50.18) 2024-11-06T03:08:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:08:05Z- TerrainNav:: SubcloudNIS is 0.46, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:08:05Z- TNavPF::Projecting Measurements 2024-11-06T03:08:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:08:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:08:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:08:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:08:05Z- TNavFilter::4 good beams. 2024-11-06T03:08:05Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:08:05Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:08:05Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:08:05Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:08:05Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:08:05Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:08:05Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:08:05Z- TerrainMapOctree: (vehN, vehE ) = (4061713.08, 594712.25). 2024-11-06T03:08:05Z- TerrainMapOctree: Min Distance = 52.80. 2024-11-06T03:08:05Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:08:05Z- Forcing Subcloud Comparison 2024-11-06T03:08:05Z- Weighting particles with subcloud comparison 2024-11-06T03:08:05Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:05Z- meanExpectedMeasDiff: 2.762304 delta_rms_squared: 1.686795 alpha: 0.988282 2024-11-06T03:08:05Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:05Z- meanExpectedMeasDiff: -3.857351 delta_rms_squared: 0.660429 alpha: 0.970607 2024-11-06T03:08:05Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:05Z- meanExpectedMeasDiff: 2.697521 delta_rms_squared: 0.696565 alpha: 0.972089 2024-11-06T03:08:05Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:05Z- meanExpectedMeasDiff: -2.007474 delta_rms_squared: 0.699342 alpha: 0.972197 2024-11-06T03:08:05Z- Subcloud NIS: 0.507679 2024-11-06T03:08:05Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:08:05Z- TNavPF:: sumWeights = 0.006323 2024-11-06T03:08:05Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:08:05Z- TNavPF::First Term of Map Covariance Matrix is 0.05 2024-11-06T03:08:05Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:08:05Z- TerrainNav::Resampling particle filter... 2024-11-06T03:08:05Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862485.53 sec, ping # 0. 2024-11-06T03:08:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:08:05Z- variances([0], [2], and psi_berg) = 0.359963 0.221156 0.000000 2024-11-06T03:08:05Z- publishEstimatesDecoder() - at 1730862485.53 sec 2024-11-06T03:08:05Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:08:05Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061744.55 594695.89 59.64 2024-11-06T03:08:05Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061746.26 594695.90 59.88 lat/lon: 36.70 -121.94 2024-11-06T03:08:05Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.36 0.22 0.28 0.00 2024-11-06T03:08:05Z- AHRS_M2 msg: 1730862485.91 epoch sec; seqNo:2616 2024-11-06T03:08:05Z- AHRS_M2 msg: 0.11 phi; 0.07 theta; 1.52 psi 2024-11-06T03:08:05Z- handleDepth()- Depth_Keller msg: 1730862485.92 epoch sec; seqNo:2788; depth 50.14 2024-11-06T03:08:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862485.95 epoch sec; seqNo:2788; 4061713.08 north; 594712.25 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:08:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862485.53 2024-11-06T03:08:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:06Z- waiting for 1730862489.53, time is 1730862485.95 2024-11-06T03:08:06Z- AHRS_M2 msg: 1730862486.36 epoch sec; seqNo:2617 2024-11-06T03:08:06Z- AHRS_M2 msg: 0.11 phi; 0.07 theta; 1.52 psi 2024-11-06T03:08:06Z- handleDepth()- Depth_Keller msg: 1730862486.36 epoch sec; seqNo:2789; depth 50.10 2024-11-06T03:08:06Z- handleDVL() - DVL msg epochMillisec: 1730862486.38 2024-11-06T03:08:06Z- handleDvl() - RDI_Pathfinder msg: 1730862486.38 sec; seqNo:928; #beams:4 2024-11-06T03:08:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.75 , 27.98 , 37.76 , 30.42 2024-11-06T03:08:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.07 , -0.03 2024-11-06T03:08:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862486.40 epoch sec; seqNo:2789; 4061713.08 north; 594712.25 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:08:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862485.53 2024-11-06T03:08:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:06Z- waiting for 1730862489.53, time is 1730862486.40 2024-11-06T03:08:06Z- AHRS_M2 msg: 1730862486.70 epoch sec; seqNo:2618 2024-11-06T03:08:06Z- AHRS_M2 msg: 0.11 phi; 0.07 theta; 1.51 psi 2024-11-06T03:08:06Z- handleDepth()- Depth_Keller msg: 1730862486.71 epoch sec; seqNo:2790; depth 50.09 2024-11-06T03:08:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862486.73 epoch sec; seqNo:2790; 4061713.08 north; 594712.25 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:08:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862485.53 2024-11-06T03:08:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:06Z- waiting for 1730862489.53, time is 1730862486.73 2024-11-06T03:08:07Z- AHRS_M2 msg: 1730862487.11 epoch sec; seqNo:2619 2024-11-06T03:08:07Z- AHRS_M2 msg: 0.11 phi; 0.07 theta; 1.51 psi 2024-11-06T03:08:07Z- handleDepth()- Depth_Keller msg: 1730862487.12 epoch sec; seqNo:2791; depth 50.06 2024-11-06T03:08:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862487.13 epoch sec; seqNo:2791; 4061713.09 north; 594713.47 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:08:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862485.53 2024-11-06T03:08:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:07Z- waiting for 1730862489.53, time is 1730862487.13 2024-11-06T03:08:07Z- AHRS_M2 msg: 1730862487.51 epoch sec; seqNo:2620 2024-11-06T03:08:07Z- AHRS_M2 msg: 0.11 phi; 0.07 theta; 1.50 psi 2024-11-06T03:08:07Z- handleDepth()- Depth_Keller msg: 1730862487.52 epoch sec; seqNo:2792; depth 50.05 2024-11-06T03:08:07Z- handleDVL() - DVL msg epochMillisec: 1730862487.54 2024-11-06T03:08:07Z- handleDvl() - RDI_Pathfinder msg: 1730862487.54 sec; seqNo:929; #beams:4 2024-11-06T03:08:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.44 , 28.13 , 37.61 , 30.42 2024-11-06T03:08:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.07 , 0.00 2024-11-06T03:08:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862487.57 epoch sec; seqNo:2792; 4061713.47 north; 594713.46 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:08:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862485.53 2024-11-06T03:08:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:07Z- waiting for 1730862489.53, time is 1730862487.57 2024-11-06T03:08:07Z- AHRS_M2 msg: 1730862487.91 epoch sec; seqNo:2621 2024-11-06T03:08:07Z- AHRS_M2 msg: 0.11 phi; 0.06 theta; 1.50 psi 2024-11-06T03:08:07Z- handleDepth()- Depth_Keller msg: 1730862487.93 epoch sec; seqNo:2793; depth 50.04 2024-11-06T03:08:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862487.94 epoch sec; seqNo:2793; 4061713.47 north; 594713.46 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:08:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862485.53 2024-11-06T03:08:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:08Z- waiting for 1730862489.53, time is 1730862487.94 2024-11-06T03:08:08Z- AHRS_M2 msg: 1730862488.35 epoch sec; seqNo:2622 2024-11-06T03:08:08Z- AHRS_M2 msg: 0.11 phi; 0.06 theta; 1.49 psi 2024-11-06T03:08:08Z- handleDepth()- Depth_Keller msg: 1730862488.37 epoch sec; seqNo:2794; depth 50.01 2024-11-06T03:08:08Z- handleDVL() - DVL msg epochMillisec: 1730862488.40 2024-11-06T03:08:08Z- handleDvl() - RDI_Pathfinder msg: 1730862488.40 sec; seqNo:930; #beams:4 2024-11-06T03:08:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.75 , 28.44 , 37.61 , 30.73 2024-11-06T03:08:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.05 , 0.04 2024-11-06T03:08:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862488.42 epoch sec; seqNo:2794; 4061713.47 north; 594713.46 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:08:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862485.53 2024-11-06T03:08:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:08Z- waiting for 1730862489.53, time is 1730862488.42 2024-11-06T03:08:08Z- AHRS_M2 msg: 1730862488.73 epoch sec; seqNo:2623 2024-11-06T03:08:08Z- AHRS_M2 msg: 0.11 phi; 0.06 theta; 1.49 psi 2024-11-06T03:08:08Z- handleDepth()- Depth_Keller msg: 1730862488.73 epoch sec; seqNo:2795; depth 49.99 2024-11-06T03:08:08Z- handleDVL() - DVL msg epochMillisec: 1730862488.73 2024-11-06T03:08:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:08Z- handleDvl() - RDI_Pathfinder msg: 1730862488.73 sec; seqNo:931; #beams:0 2024-11-06T03:08:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.05 , 0.04 2024-11-06T03:08:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862488.75 epoch sec; seqNo:2795; 4061713.49 north; 594714.68 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:08:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862485.53 2024-11-06T03:08:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:08Z- waiting for 1730862489.53, time is 1730862488.75 2024-11-06T03:08:09Z- handleDepth()- Depth_Keller msg: 1730862489.16 epoch sec; seqNo:2796; depth 49.98 2024-11-06T03:08:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862489.17 epoch sec; seqNo:2796; 4061713.49 north; 594714.68 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:08:09Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:08:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862485.53 2024-11-06T03:08:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:09Z- waiting for 1730862489.53, time is 1730862489.17 2024-11-06T03:08:09Z- AHRS_M2 msg: 1730862489.53 epoch sec; seqNo:2624 2024-11-06T03:08:09Z- AHRS_M2 msg: 0.11 phi; 0.06 theta; 1.48 psi 2024-11-06T03:08:09Z- handleDepth()- Depth_Keller msg: 1730862489.54 epoch sec; seqNo:2797; depth 49.98 2024-11-06T03:08:09Z- handleDVL() - DVL msg epochMillisec: 1730862489.56 2024-11-06T03:08:09Z- handleDvl() - RDI_Pathfinder msg: 1730862489.56 sec; seqNo:932; #beams:4 2024-11-06T03:08:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.90 , 28.59 , 37.61 , 30.73 2024-11-06T03:08:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , 0.06 2024-11-06T03:08:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862489.58 epoch sec; seqNo:2797; 4061713.49 north; 594714.68 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:08:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862485.53 2024-11-06T03:08:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:09Z- waiting for 1730862489.53, time is 1730862489.58 2024-11-06T03:08:09Z- LcmTrn::updateTrn() >>>> pose time 1730862489.58, meas time 1730862489.56 <<<< 2024-11-06T03:08:09Z- LcmTrn::updateTrn() equating pose 1730862489.56 and meas times 1730862489.56 2024-11-06T03:08:09Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862489.56 2024-11-06T03:08:09Z- TerrainNav::motionUpdate() - currEstimate = (4061713, 594714.68, 49.98) 2024-11-06T03:08:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:08:09Z- TerrainNav:: SubcloudNIS is 0.51, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:08:09Z- TNavPF::Projecting Measurements 2024-11-06T03:08:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:08:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:08:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:08:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:08:09Z- TNavFilter::4 good beams. 2024-11-06T03:08:09Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:08:09Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:08:09Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:08:09Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:08:09Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:08:09Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:08:09Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:08:09Z- TerrainMapOctree: (vehN, vehE ) = (4061713.49, 594714.68). 2024-11-06T03:08:09Z- TerrainMapOctree: Min Distance = 50.80. 2024-11-06T03:08:09Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:08:09Z- Forcing Subcloud Comparison 2024-11-06T03:08:09Z- Weighting particles with subcloud comparison 2024-11-06T03:08:09Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:09Z- meanExpectedMeasDiff: 2.778600 delta_rms_squared: 0.902012 alpha: 0.978308 2024-11-06T03:08:09Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:09Z- meanExpectedMeasDiff: -3.200531 delta_rms_squared: 0.605937 alpha: 0.968048 2024-11-06T03:08:09Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:09Z- meanExpectedMeasDiff: 3.751377 delta_rms_squared: 0.691983 alpha: 0.971909 2024-11-06T03:08:09Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:09Z- meanExpectedMeasDiff: -1.840094 delta_rms_squared: 0.636555 alpha: 0.969538 2024-11-06T03:08:09Z- Subcloud NIS: 0.552811 2024-11-06T03:08:09Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:08:09Z- TNavPF:: sumWeights = 0.006685 2024-11-06T03:08:09Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:08:09Z- TNavPF::First Term of Map Covariance Matrix is 0.05 2024-11-06T03:08:09Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:08:09Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862489.56 sec, ping # 0. 2024-11-06T03:08:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:08:09Z- variances([0], [2], and psi_berg) = 0.412045 0.230512 0.000000 2024-11-06T03:08:09Z- publishEstimatesDecoder() - at 1730862489.56 sec 2024-11-06T03:08:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:08:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061746.41 594696.69 59.85 2024-11-06T03:08:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061746.62 594698.33 59.66 lat/lon: 36.70 -121.94 2024-11-06T03:08:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.41 0.23 0.31 0.00 2024-11-06T03:08:09Z- AHRS_M2 msg: 1730862489.93 epoch sec; seqNo:2625 2024-11-06T03:08:09Z- AHRS_M2 msg: 0.11 phi; 0.06 theta; 1.48 psi 2024-11-06T03:08:09Z- handleDepth()- Depth_Keller msg: 1730862489.95 epoch sec; seqNo:2798; depth 49.96 2024-11-06T03:08:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862489.96 epoch sec; seqNo:2798; 4061713.49 north; 594714.68 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:08:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862489.56 2024-11-06T03:08:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:10Z- waiting for 1730862493.56, time is 1730862489.96 2024-11-06T03:08:10Z- AHRS_M2 msg: 1730862490.35 epoch sec; seqNo:2626 2024-11-06T03:08:10Z- AHRS_M2 msg: 0.12 phi; 0.06 theta; 1.48 psi 2024-11-06T03:08:10Z- handleDepth()- Depth_Keller msg: 1730862490.37 epoch sec; seqNo:2799; depth 49.95 2024-11-06T03:08:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862490.42 epoch sec; seqNo:2799; 4061713.50 north; 594715.90 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:08:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862489.56 2024-11-06T03:08:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:10Z- waiting for 1730862493.56, time is 1730862490.42 2024-11-06T03:08:10Z- AHRS_M2 msg: 1730862490.75 epoch sec; seqNo:2627 2024-11-06T03:08:10Z- AHRS_M2 msg: 0.12 phi; 0.06 theta; 1.48 psi 2024-11-06T03:08:10Z- handleDepth()- Depth_Keller msg: 1730862490.76 epoch sec; seqNo:2800; depth 49.92 2024-11-06T03:08:10Z- handleDVL() - DVL msg epochMillisec: 1730862490.78 2024-11-06T03:08:10Z- handleDvl() - RDI_Pathfinder msg: 1730862490.78 sec; seqNo:933; #beams:4 2024-11-06T03:08:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.21 , 28.90 , 37.76 , 30.88 2024-11-06T03:08:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , 0.06 2024-11-06T03:08:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862490.81 epoch sec; seqNo:2800; 4061713.88 north; 594715.90 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:08:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862489.56 2024-11-06T03:08:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:10Z- waiting for 1730862493.56, time is 1730862490.81 2024-11-06T03:08:11Z- AHRS_M2 msg: 1730862491.15 epoch sec; seqNo:2628 2024-11-06T03:08:11Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 1.48 psi 2024-11-06T03:08:11Z- handleDepth()- Depth_Keller msg: 1730862491.15 epoch sec; seqNo:2801; depth 49.93 2024-11-06T03:08:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862491.18 epoch sec; seqNo:2801; 4061713.88 north; 594715.90 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:08:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862489.56 2024-11-06T03:08:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:11Z- waiting for 1730862493.56, time is 1730862491.18 2024-11-06T03:08:11Z- AHRS_M2 msg: 1730862491.55 epoch sec; seqNo:2629 2024-11-06T03:08:11Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 1.48 psi 2024-11-06T03:08:11Z- handleDepth()- Depth_Keller msg: 1730862491.56 epoch sec; seqNo:2802; depth 49.91 2024-11-06T03:08:11Z- handleDVL() - DVL msg epochMillisec: 1730862491.58 2024-11-06T03:08:11Z- handleDvl() - RDI_Pathfinder msg: 1730862491.58 sec; seqNo:934; #beams:4 2024-11-06T03:08:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.77 , 29.05 , 37.71 , 31.09 2024-11-06T03:08:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , 0.05 2024-11-06T03:08:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862491.60 epoch sec; seqNo:2802; 4061713.89 north; 594717.12 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:08:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862489.56 2024-11-06T03:08:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:11Z- waiting for 1730862493.56, time is 1730862491.60 2024-11-06T03:08:11Z- AHRS_M2 msg: 1730862491.95 epoch sec; seqNo:2630 2024-11-06T03:08:11Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 1.48 psi 2024-11-06T03:08:11Z- handleDepth()- Depth_Keller msg: 1730862491.96 epoch sec; seqNo:2803; depth 49.89 2024-11-06T03:08:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862491.97 epoch sec; seqNo:2803; 4061713.89 north; 594717.12 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:08:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862489.56 2024-11-06T03:08:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:12Z- waiting for 1730862493.56, time is 1730862491.97 2024-11-06T03:08:12Z- AHRS_M2 msg: 1730862492.37 epoch sec; seqNo:2631 2024-11-06T03:08:12Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 1.48 psi 2024-11-06T03:08:12Z- handleDepth()- Depth_Keller msg: 1730862492.37 epoch sec; seqNo:2804; depth 49.88 2024-11-06T03:08:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862492.40 epoch sec; seqNo:2804; 4061713.89 north; 594717.12 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:08:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862489.56 2024-11-06T03:08:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:12Z- waiting for 1730862493.56, time is 1730862492.40 2024-11-06T03:08:12Z- AHRS_M2 msg: 1730862492.77 epoch sec; seqNo:2632 2024-11-06T03:08:12Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 1.48 psi 2024-11-06T03:08:12Z- handleDepth()- Depth_Keller msg: 1730862492.78 epoch sec; seqNo:2805; depth 49.86 2024-11-06T03:08:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862492.81 epoch sec; seqNo:2805; 4061713.89 north; 594717.12 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:08:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862489.56 2024-11-06T03:08:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:12Z- waiting for 1730862493.56, time is 1730862492.81 2024-11-06T03:08:13Z- AHRS_M2 msg: 1730862493.17 epoch sec; seqNo:2633 2024-11-06T03:08:13Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 1.48 psi 2024-11-06T03:08:13Z- handleDepth()- Depth_Keller msg: 1730862493.17 epoch sec; seqNo:2806; depth 49.85 2024-11-06T03:08:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862493.20 epoch sec; seqNo:2806; 4061713.91 north; 594718.34 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:08:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862489.56 2024-11-06T03:08:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:13Z- waiting for 1730862493.56, time is 1730862493.20 2024-11-06T03:08:13Z- AHRS_M2 msg: 1730862493.57 epoch sec; seqNo:2634 2024-11-06T03:08:13Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 1.48 psi 2024-11-06T03:08:13Z- handleDepth()- Depth_Keller msg: 1730862493.58 epoch sec; seqNo:2807; depth 49.83 2024-11-06T03:08:13Z- handleDVL() - DVL msg epochMillisec: 1730862493.61 2024-11-06T03:08:13Z- handleDvl() - RDI_Pathfinder msg: 1730862493.61 sec; seqNo:935; #beams:4 2024-11-06T03:08:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.77 , 29.56 , 37.88 , 31.60 2024-11-06T03:08:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , -0.00 2024-11-06T03:08:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862493.63 epoch sec; seqNo:2807; 4061713.91 north; 594718.34 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:08:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862489.56 2024-11-06T03:08:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:13Z- waiting for 1730862493.56, time is 1730862493.63 2024-11-06T03:08:13Z- LcmTrn::updateTrn() >>>> pose time 1730862493.63, meas time 1730862493.61 <<<< 2024-11-06T03:08:13Z- LcmTrn::updateTrn() equating pose 1730862493.61 and meas times 1730862493.61 2024-11-06T03:08:13Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862493.61 2024-11-06T03:08:13Z- TerrainNav::motionUpdate() - currEstimate = (4061714, 594718.34, 49.83) 2024-11-06T03:08:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:08:13Z- TerrainNav:: SubcloudNIS is 0.55, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:08:13Z- TNavPF::Projecting Measurements 2024-11-06T03:08:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:08:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:08:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:08:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:08:13Z- TNavFilter::4 good beams. 2024-11-06T03:08:13Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:08:13Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:08:13Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:08:13Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:08:13Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:08:13Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:08:13Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:08:13Z- TerrainMapOctree: (vehN, vehE ) = (4061713.91, 594718.34). 2024-11-06T03:08:13Z- TerrainMapOctree: Min Distance = 48.01. 2024-11-06T03:08:13Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:08:13Z- Forcing Subcloud Comparison 2024-11-06T03:08:13Z- Weighting particles with subcloud comparison 2024-11-06T03:08:13Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:13Z- meanExpectedMeasDiff: 3.832346 delta_rms_squared: 1.052289 alpha: 0.981348 2024-11-06T03:08:13Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:13Z- meanExpectedMeasDiff: -2.009085 delta_rms_squared: 0.644945 alpha: 0.969922 2024-11-06T03:08:13Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:13Z- meanExpectedMeasDiff: 4.818625 delta_rms_squared: 0.578516 alpha: 0.966584 2024-11-06T03:08:13Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:13Z- meanExpectedMeasDiff: -1.471587 delta_rms_squared: 0.708689 alpha: 0.972553 2024-11-06T03:08:13Z- Subcloud NIS: 0.605384 2024-11-06T03:08:13Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:08:13Z- TNavPF:: sumWeights = 0.004508 2024-11-06T03:08:13Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:08:13Z- TNavPF::First Term of Map Covariance Matrix is 0.05 2024-11-06T03:08:13Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:08:13Z- TerrainNav::Resampling particle filter... 2024-11-06T03:08:13Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862493.61 sec, ping # 0. 2024-11-06T03:08:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:08:13Z- variances([0], [2], and psi_berg) = 0.527187 0.311547 0.000000 2024-11-06T03:08:13Z- publishEstimatesDecoder() - at 1730862493.61 sec 2024-11-06T03:08:13Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:08:13Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061745.37 594701.98 58.47 2024-11-06T03:08:13Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061746.87 594701.80 59.24 lat/lon: 36.70 -121.94 2024-11-06T03:08:13Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.53 0.31 0.34 0.00 2024-11-06T03:08:13Z- AHRS_M2 msg: 1730862493.98 epoch sec; seqNo:2635 2024-11-06T03:08:13Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 1.49 psi 2024-11-06T03:08:13Z- handleDepth()- Depth_Keller msg: 1730862493.99 epoch sec; seqNo:2808; depth 49.82 2024-11-06T03:08:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862494.01 epoch sec; seqNo:2808; 4061713.91 north; 594718.34 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:08:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862493.61 2024-11-06T03:08:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:14Z- waiting for 1730862497.61, time is 1730862494.01 2024-11-06T03:08:14Z- AHRS_M2 msg: 1730862494.39 epoch sec; seqNo:2636 2024-11-06T03:08:14Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.49 psi 2024-11-06T03:08:14Z- handleDepth()- Depth_Keller msg: 1730862494.39 epoch sec; seqNo:2809; depth 49.80 2024-11-06T03:08:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862494.43 epoch sec; seqNo:2809; 4061713.92 north; 594719.56 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:08:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862493.61 2024-11-06T03:08:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:14Z- waiting for 1730862497.61, time is 1730862494.43 2024-11-06T03:08:14Z- AHRS_M2 msg: 1730862494.78 epoch sec; seqNo:2637 2024-11-06T03:08:14Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.49 psi 2024-11-06T03:08:14Z- handleDepth()- Depth_Keller msg: 1730862494.79 epoch sec; seqNo:2810; depth 49.77 2024-11-06T03:08:14Z- handleDVL() - DVL msg epochMillisec: 1730862494.81 2024-11-06T03:08:14Z- handleDvl() - RDI_Pathfinder msg: 1730862494.81 sec; seqNo:936; #beams:4 2024-11-06T03:08:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.11 , 29.73 , 37.71 , 31.43 2024-11-06T03:08:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , -0.03 2024-11-06T03:08:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862494.84 epoch sec; seqNo:2810; 4061713.92 north; 594719.56 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:08:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862493.61 2024-11-06T03:08:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:14Z- waiting for 1730862497.61, time is 1730862494.84 2024-11-06T03:08:15Z- AHRS_M2 msg: 1730862495.19 epoch sec; seqNo:2638 2024-11-06T03:08:15Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.49 psi 2024-11-06T03:08:15Z- handleDepth()- Depth_Keller msg: 1730862495.20 epoch sec; seqNo:2811; depth 49.75 2024-11-06T03:08:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862495.22 epoch sec; seqNo:2811; 4061713.92 north; 594719.56 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:08:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862493.61 2024-11-06T03:08:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:15Z- waiting for 1730862497.61, time is 1730862495.22 2024-11-06T03:08:15Z- AHRS_M2 msg: 1730862495.59 epoch sec; seqNo:2639 2024-11-06T03:08:15Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.49 psi 2024-11-06T03:08:15Z- handleDepth()- Depth_Keller msg: 1730862495.60 epoch sec; seqNo:2812; depth 49.72 2024-11-06T03:08:15Z- handleDVL() - DVL msg epochMillisec: 1730862495.63 2024-11-06T03:08:15Z- handleDvl() - RDI_Pathfinder msg: 1730862495.63 sec; seqNo:937; #beams:4 2024-11-06T03:08:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.11 , 29.73 , 37.88 , 31.77 2024-11-06T03:08:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , -0.05 2024-11-06T03:08:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862495.65 epoch sec; seqNo:2812; 4061713.93 north; 594720.78 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:08:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862493.61 2024-11-06T03:08:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:15Z- waiting for 1730862497.61, time is 1730862495.65 2024-11-06T03:08:16Z- AHRS_M2 msg: 1730862496.00 epoch sec; seqNo:2640 2024-11-06T03:08:16Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.49 psi 2024-11-06T03:08:16Z- handleDepth()- Depth_Keller msg: 1730862496.01 epoch sec; seqNo:2813; depth 49.70 2024-11-06T03:08:16Z- handleDVL() - DVL msg epochMillisec: 1730862496.01 2024-11-06T03:08:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:16Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:16Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:16Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:16Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:16Z- handleDvl() - RDI_Pathfinder msg: 1730862496.01 sec; seqNo:938; #beams:0 2024-11-06T03:08:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , -0.05 2024-11-06T03:08:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862496.03 epoch sec; seqNo:2813; 4061714.31 north; 594720.78 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:08:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862493.61 2024-11-06T03:08:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:16Z- waiting for 1730862497.61, time is 1730862496.03 2024-11-06T03:08:16Z- AHRS_M2 msg: 1730862496.40 epoch sec; seqNo:2641 2024-11-06T03:08:16Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.49 psi 2024-11-06T03:08:16Z- handleDepth()- Depth_Keller msg: 1730862496.40 epoch sec; seqNo:2814; depth 49.67 2024-11-06T03:08:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862496.42 epoch sec; seqNo:2814; 4061714.31 north; 594720.78 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:08:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862493.61 2024-11-06T03:08:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:16Z- waiting for 1730862497.61, time is 1730862496.42 2024-11-06T03:08:16Z- AHRS_M2 msg: 1730862496.80 epoch sec; seqNo:2642 2024-11-06T03:08:16Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.49 psi 2024-11-06T03:08:16Z- handleDepth()- Depth_Keller msg: 1730862496.81 epoch sec; seqNo:2815; depth 49.66 2024-11-06T03:08:16Z- handleDVL() - DVL msg epochMillisec: 1730862496.84 2024-11-06T03:08:16Z- handleDvl() - RDI_Pathfinder msg: 1730862496.84 sec; seqNo:939; #beams:4 2024-11-06T03:08:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.45 , 29.56 , 37.71 , 31.77 2024-11-06T03:08:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , -0.06 2024-11-06T03:08:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862496.85 epoch sec; seqNo:2815; 4061714.31 north; 594720.78 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:08:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862493.61 2024-11-06T03:08:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:16Z- waiting for 1730862497.61, time is 1730862496.85 2024-11-06T03:08:17Z- AHRS_M2 msg: 1730862497.21 epoch sec; seqNo:2643 2024-11-06T03:08:17Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T03:08:17Z- handleDepth()- Depth_Keller msg: 1730862497.21 epoch sec; seqNo:2816; depth 49.64 2024-11-06T03:08:17Z- handleDVL() - DVL msg epochMillisec: 1730862497.21 2024-11-06T03:08:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:17Z- handleDvl() - RDI_Pathfinder msg: 1730862497.21 sec; seqNo:940; #beams:0 2024-11-06T03:08:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , -0.06 2024-11-06T03:08:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862497.23 epoch sec; seqNo:2816; 4061714.33 north; 594722.00 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:08:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862493.61 2024-11-06T03:08:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:17Z- waiting for 1730862497.61, time is 1730862497.23 2024-11-06T03:08:17Z- AHRS_M2 msg: 1730862497.61 epoch sec; seqNo:2644 2024-11-06T03:08:17Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.48 psi 2024-11-06T03:08:17Z- handleDepth()- Depth_Keller msg: 1730862497.62 epoch sec; seqNo:2817; depth 49.60 2024-11-06T03:08:17Z- handleDVL() - DVL msg epochMillisec: 1730862497.64 2024-11-06T03:08:17Z- handleDvl() - RDI_Pathfinder msg: 1730862497.64 sec; seqNo:941; #beams:4 2024-11-06T03:08:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.28 , 29.39 , 37.54 , 31.94 2024-11-06T03:08:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.05 2024-11-06T03:08:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862497.68 epoch sec; seqNo:2817; 4061714.33 north; 594722.00 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:08:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862493.61 2024-11-06T03:08:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:17Z- waiting for 1730862497.61, time is 1730862497.68 2024-11-06T03:08:17Z- LcmTrn::updateTrn() >>>> pose time 1730862497.68, meas time 1730862497.64 <<<< 2024-11-06T03:08:17Z- LcmTrn::updateTrn() equating pose 1730862497.64 and meas times 1730862497.64 2024-11-06T03:08:17Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862497.64 2024-11-06T03:08:17Z- TerrainNav::motionUpdate() - currEstimate = (4061714, 594722.00, 49.60) 2024-11-06T03:08:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:08:17Z- TerrainNav:: SubcloudNIS is 0.61, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:08:17Z- TNavPF::Projecting Measurements 2024-11-06T03:08:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:08:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:08:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:08:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:08:17Z- TNavFilter::4 good beams. 2024-11-06T03:08:17Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:08:17Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:08:17Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:08:17Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:08:17Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:08:17Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:08:17Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:08:17Z- TerrainMapOctree: (vehN, vehE ) = (4061714.33, 594722.00). 2024-11-06T03:08:17Z- TerrainMapOctree: Min Distance = 45.35. 2024-11-06T03:08:17Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:08:17Z- Forcing Subcloud Comparison 2024-11-06T03:08:17Z- Weighting particles with subcloud comparison 2024-11-06T03:08:17Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:17Z- meanExpectedMeasDiff: 3.812492 delta_rms_squared: 1.145137 alpha: 0.982835 2024-11-06T03:08:17Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:17Z- meanExpectedMeasDiff: -2.670282 delta_rms_squared: 0.672900 alpha: 0.971136 2024-11-06T03:08:17Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:17Z- meanExpectedMeasDiff: 5.225135 delta_rms_squared: 0.594857 alpha: 0.967472 2024-11-06T03:08:17Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:17Z- meanExpectedMeasDiff: -2.144022 delta_rms_squared: 0.761749 alpha: 0.974416 2024-11-06T03:08:17Z- Subcloud NIS: 0.675831 2024-11-06T03:08:17Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:08:17Z- TNavPF:: sumWeights = 0.001130 2024-11-06T03:08:17Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:08:17Z- TNavPF::First Term of Map Covariance Matrix is 0.01 2024-11-06T03:08:17Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:08:17Z- TerrainNav::Resampling particle filter... 2024-11-06T03:08:17Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862497.64 sec, ping # 0. 2024-11-06T03:08:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:08:17Z- variances([0], [2], and psi_berg) = 0.593855 0.308683 0.000000 2024-11-06T03:08:17Z- publishEstimatesDecoder() - at 1730862497.64 sec 2024-11-06T03:08:17Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:08:17Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061745.78 594705.63 58.84 2024-11-06T03:08:17Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061747.18 594705.27 58.85 lat/lon: 36.70 -121.94 2024-11-06T03:08:17Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.59 0.31 0.32 0.00 2024-11-06T03:08:18Z- AHRS_M2 msg: 1730862498.01 epoch sec; seqNo:2645 2024-11-06T03:08:18Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.48 psi 2024-11-06T03:08:18Z- handleDepth()- Depth_Keller msg: 1730862498.02 epoch sec; seqNo:2818; depth 49.60 2024-11-06T03:08:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862498.05 epoch sec; seqNo:2818; 4061714.33 north; 594722.00 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:08:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862497.64 2024-11-06T03:08:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:18Z- waiting for 1730862501.64, time is 1730862498.05 2024-11-06T03:08:18Z- AHRS_M2 msg: 1730862498.42 epoch sec; seqNo:2646 2024-11-06T03:08:18Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.48 psi 2024-11-06T03:08:18Z- handleDepth()- Depth_Keller msg: 1730862498.42 epoch sec; seqNo:2819; depth 49.56 2024-11-06T03:08:18Z- handleDVL() - DVL msg epochMillisec: 1730862498.43 2024-11-06T03:08:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:18Z- handleDvl() - RDI_Pathfinder msg: 1730862498.43 sec; seqNo:942; #beams:0 2024-11-06T03:08:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.05 2024-11-06T03:08:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862498.44 epoch sec; seqNo:2819; 4061714.34 north; 594723.22 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:08:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862497.64 2024-11-06T03:08:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:18Z- waiting for 1730862501.64, time is 1730862498.44 2024-11-06T03:08:18Z- AHRS_M2 msg: 1730862498.83 epoch sec; seqNo:2647 2024-11-06T03:08:18Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.48 psi 2024-11-06T03:08:18Z- handleDepth()- Depth_Keller msg: 1730862498.83 epoch sec; seqNo:2820; depth 49.56 2024-11-06T03:08:18Z- handleDVL() - DVL msg epochMillisec: 1730862498.85 2024-11-06T03:08:18Z- handleDvl() - RDI_Pathfinder msg: 1730862498.85 sec; seqNo:943; #beams:4 2024-11-06T03:08:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.94 , 29.05 , 37.54 , 32.28 2024-11-06T03:08:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , -0.03 2024-11-06T03:08:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862498.88 epoch sec; seqNo:2820; 4061714.34 north; 594723.22 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:08:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862497.64 2024-11-06T03:08:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:18Z- waiting for 1730862501.64, time is 1730862498.88 2024-11-06T03:08:19Z- AHRS_M2 msg: 1730862499.23 epoch sec; seqNo:2648 2024-11-06T03:08:19Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.47 psi 2024-11-06T03:08:19Z- handleDepth()- Depth_Keller msg: 1730862499.23 epoch sec; seqNo:2821; depth 49.53 2024-11-06T03:08:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862499.25 epoch sec; seqNo:2821; 4061714.34 north; 594723.22 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T03:08:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862497.64 2024-11-06T03:08:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:19Z- waiting for 1730862501.64, time is 1730862499.25 2024-11-06T03:08:19Z- AHRS_M2 msg: 1730862499.63 epoch sec; seqNo:2649 2024-11-06T03:08:19Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.47 psi 2024-11-06T03:08:19Z- handleDepth()- Depth_Keller msg: 1730862499.65 epoch sec; seqNo:2822; depth 49.53 2024-11-06T03:08:19Z- handleDVL() - DVL msg epochMillisec: 1730862499.68 2024-11-06T03:08:19Z- handleDvl() - RDI_Pathfinder msg: 1730862499.68 sec; seqNo:944; #beams:4 2024-11-06T03:08:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.94 , 29.05 , 37.03 , 32.28 2024-11-06T03:08:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , -0.02 2024-11-06T03:08:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862499.69 epoch sec; seqNo:2822; 4061714.34 north; 594723.22 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T03:08:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862497.64 2024-11-06T03:08:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:19Z- waiting for 1730862501.64, time is 1730862499.69 2024-11-06T03:08:20Z- AHRS_M2 msg: 1730862500.04 epoch sec; seqNo:2650 2024-11-06T03:08:20Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.47 psi 2024-11-06T03:08:20Z- handleDepth()- Depth_Keller msg: 1730862500.04 epoch sec; seqNo:2823; depth 49.52 2024-11-06T03:08:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862500.06 epoch sec; seqNo:2823; 4061714.73 north; 594724.43 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T03:08:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862497.64 2024-11-06T03:08:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:20Z- waiting for 1730862501.64, time is 1730862500.06 2024-11-06T03:08:20Z- AHRS_M2 msg: 1730862500.44 epoch sec; seqNo:2651 2024-11-06T03:08:20Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.47 psi 2024-11-06T03:08:20Z- handleDepth()- Depth_Keller msg: 1730862500.45 epoch sec; seqNo:2824; depth 49.50 2024-11-06T03:08:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862500.46 epoch sec; seqNo:2824; 4061714.73 north; 594724.43 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:08:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862497.64 2024-11-06T03:08:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:20Z- waiting for 1730862501.64, time is 1730862500.46 2024-11-06T03:08:20Z- AHRS_M2 msg: 1730862500.84 epoch sec; seqNo:2652 2024-11-06T03:08:20Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.47 psi 2024-11-06T03:08:20Z- handleDepth()- Depth_Keller msg: 1730862500.85 epoch sec; seqNo:2825; depth 49.51 2024-11-06T03:08:20Z- handleDVL() - DVL msg epochMillisec: 1730862500.87 2024-11-06T03:08:20Z- handleDvl() - RDI_Pathfinder msg: 1730862500.87 sec; seqNo:945; #beams:4 2024-11-06T03:08:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.94 , 29.39 , 36.86 , 32.44 2024-11-06T03:08:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , -0.01 2024-11-06T03:08:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862500.91 epoch sec; seqNo:2825; 4061714.73 north; 594724.43 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T03:08:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862497.64 2024-11-06T03:08:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:21Z- waiting for 1730862501.64, time is 1730862500.91 2024-11-06T03:08:21Z- AHRS_M2 msg: 1730862501.25 epoch sec; seqNo:2653 2024-11-06T03:08:21Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.47 psi 2024-11-06T03:08:21Z- handleDepth()- Depth_Keller msg: 1730862501.25 epoch sec; seqNo:2826; depth 49.50 2024-11-06T03:08:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862501.27 epoch sec; seqNo:2826; 4061714.73 north; 594724.43 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:08:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862497.64 2024-11-06T03:08:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:21Z- waiting for 1730862501.64, time is 1730862501.27 2024-11-06T03:08:21Z- handleDepth()- Depth_Keller msg: 1730862501.67 epoch sec; seqNo:2827; depth 49.50 2024-11-06T03:08:21Z- handleDVL() - DVL msg epochMillisec: 1730862501.69 2024-11-06T03:08:21Z- handleDvl() - RDI_Pathfinder msg: 1730862501.69 sec; seqNo:946; #beams:4 2024-11-06T03:08:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.77 , 29.56 , 36.86 , 32.78 2024-11-06T03:08:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , -0.01 2024-11-06T03:08:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862501.71 epoch sec; seqNo:2827; 4061714.74 north; 594725.65 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:08:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862497.64 2024-11-06T03:08:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:21Z- waiting for 1730862501.64, time is 1730862501.71 2024-11-06T03:08:21Z- LcmTrn::updateTrn() >>>> pose time 1730862501.71, meas time 1730862501.69 <<<< 2024-11-06T03:08:21Z- LcmTrn::updateTrn() equating pose 1730862501.69 and meas times 1730862501.69 2024-11-06T03:08:21Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862501.69 2024-11-06T03:08:21Z- TerrainNav::motionUpdate() - currEstimate = (4061715, 594725.65, 49.50) 2024-11-06T03:08:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:08:21Z- TerrainNav:: SubcloudNIS is 0.68, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:08:21Z- TNavPF::Projecting Measurements 2024-11-06T03:08:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:08:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:08:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:08:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:08:21Z- TNavFilter::4 good beams. 2024-11-06T03:08:21Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:08:21Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:08:21Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:08:21Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:08:21Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:08:21Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:08:21Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:08:21Z- TerrainMapOctree: (vehN, vehE ) = (4061714.74, 594725.65). 2024-11-06T03:08:21Z- TerrainMapOctree: Min Distance = 42.84. 2024-11-06T03:08:21Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:08:21Z- Forcing Subcloud Comparison 2024-11-06T03:08:21Z- Weighting particles with subcloud comparison 2024-11-06T03:08:21Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:21Z- meanExpectedMeasDiff: 3.855735 delta_rms_squared: 1.831503 alpha: 0.989198 2024-11-06T03:08:21Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:21Z- meanExpectedMeasDiff: -2.827134 delta_rms_squared: 0.677754 alpha: 0.971337 2024-11-06T03:08:21Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:21Z- meanExpectedMeasDiff: 5.154673 delta_rms_squared: 0.830442 alpha: 0.976483 2024-11-06T03:08:21Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:21Z- meanExpectedMeasDiff: -1.810164 delta_rms_squared: 0.774824 alpha: 0.974837 2024-11-06T03:08:21Z- Subcloud NIS: 0.744151 2024-11-06T03:08:21Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:08:21Z- TNavPF:: sumWeights = 0.001200 2024-11-06T03:08:21Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:08:21Z- TNavPF::First Term of Map Covariance Matrix is 0.01 2024-11-06T03:08:21Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:08:21Z- TerrainNav::Resampling particle filter... 2024-11-06T03:08:21Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862501.69 sec, ping # 0. 2024-11-06T03:08:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:08:21Z- variances([0], [2], and psi_berg) = 0.801268 0.353944 0.000000 2024-11-06T03:08:21Z- publishEstimatesDecoder() - at 1730862501.69 sec 2024-11-06T03:08:21Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:08:21Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061746.25 594709.30 58.93 2024-11-06T03:08:21Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061747.68 594708.87 58.35 lat/lon: 36.70 -121.94 2024-11-06T03:08:21Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.80 0.35 0.30 0.00 2024-11-06T03:08:22Z- AHRS_M2 msg: 1730862502.06 epoch sec; seqNo:2654 2024-11-06T03:08:22Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.47 psi 2024-11-06T03:08:22Z- handleDepth()- Depth_Keller msg: 1730862502.07 epoch sec; seqNo:2828; depth 49.48 2024-11-06T03:08:22Z- handleDVL() - DVL msg epochMillisec: 1730862502.08 2024-11-06T03:08:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:22Z- handleDvl() - RDI_Pathfinder msg: 1730862502.08 sec; seqNo:947; #beams:0 2024-11-06T03:08:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , -0.01 2024-11-06T03:08:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862502.12 epoch sec; seqNo:2828; 4061714.74 north; 594725.65 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:08:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862501.69 2024-11-06T03:08:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:22Z- waiting for 1730862505.69, time is 1730862502.12 2024-11-06T03:08:22Z- AHRS_M2 msg: 1730862502.46 epoch sec; seqNo:2655 2024-11-06T03:08:22Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.47 psi 2024-11-06T03:08:22Z- handleDepth()- Depth_Keller msg: 1730862502.47 epoch sec; seqNo:2829; depth 49.48 2024-11-06T03:08:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862502.48 epoch sec; seqNo:2829; 4061714.74 north; 594725.65 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:08:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862501.69 2024-11-06T03:08:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:22Z- waiting for 1730862505.69, time is 1730862502.48 2024-11-06T03:08:22Z- AHRS_M2 msg: 1730862502.86 epoch sec; seqNo:2656 2024-11-06T03:08:22Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.47 psi 2024-11-06T03:08:22Z- handleDepth()- Depth_Keller msg: 1730862502.87 epoch sec; seqNo:2830; depth 49.47 2024-11-06T03:08:22Z- handleDVL() - DVL msg epochMillisec: 1730862502.89 2024-11-06T03:08:22Z- handleDvl() - RDI_Pathfinder msg: 1730862502.89 sec; seqNo:948; #beams:4 2024-11-06T03:08:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.11 , 29.39 , 36.69 , 32.78 2024-11-06T03:08:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , -0.01 2024-11-06T03:08:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862502.90 epoch sec; seqNo:2830; 4061714.76 north; 594726.87 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:08:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862501.69 2024-11-06T03:08:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:23Z- waiting for 1730862505.69, time is 1730862502.90 2024-11-06T03:08:23Z- AHRS_M2 msg: 1730862503.27 epoch sec; seqNo:2657 2024-11-06T03:08:23Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.47 psi 2024-11-06T03:08:23Z- handleDepth()- Depth_Keller msg: 1730862503.28 epoch sec; seqNo:2831; depth 49.48 2024-11-06T03:08:23Z- handleDVL() - DVL msg epochMillisec: 1730862503.28 2024-11-06T03:08:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:23Z- handleDvl() - RDI_Pathfinder msg: 1730862503.28 sec; seqNo:949; #beams:0 2024-11-06T03:08:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , -0.01 2024-11-06T03:08:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862503.30 epoch sec; seqNo:2831; 4061714.76 north; 594726.87 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:08:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862501.69 2024-11-06T03:08:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:23Z- waiting for 1730862505.69, time is 1730862503.30 2024-11-06T03:08:23Z- AHRS_M2 msg: 1730862503.67 epoch sec; seqNo:2658 2024-11-06T03:08:23Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.47 psi 2024-11-06T03:08:23Z- handleDepth()- Depth_Keller msg: 1730862503.68 epoch sec; seqNo:2832; depth 49.50 2024-11-06T03:08:23Z- handleDVL() - DVL msg epochMillisec: 1730862503.70 2024-11-06T03:08:23Z- handleDvl() - RDI_Pathfinder msg: 1730862503.70 sec; seqNo:950; #beams:4 2024-11-06T03:08:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.77 , 29.73 , 36.52 , 33.12 2024-11-06T03:08:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , -0.01 2024-11-06T03:08:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862503.71 epoch sec; seqNo:2832; 4061715.14 north; 594726.87 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:08:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862501.69 2024-11-06T03:08:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:23Z- waiting for 1730862505.69, time is 1730862503.71 2024-11-06T03:08:24Z- AHRS_M2 msg: 1730862504.08 epoch sec; seqNo:2659 2024-11-06T03:08:24Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.48 psi 2024-11-06T03:08:24Z- handleDepth()- Depth_Keller msg: 1730862504.10 epoch sec; seqNo:2833; depth 49.48 2024-11-06T03:08:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862504.11 epoch sec; seqNo:2833; 4061715.14 north; 594726.87 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:08:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862501.69 2024-11-06T03:08:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:24Z- waiting for 1730862505.69, time is 1730862504.11 2024-11-06T03:08:24Z- AHRS_M2 msg: 1730862504.48 epoch sec; seqNo:2660 2024-11-06T03:08:24Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.48 psi 2024-11-06T03:08:24Z- handleDepth()- Depth_Keller msg: 1730862504.49 epoch sec; seqNo:2834; depth 49.49 2024-11-06T03:08:24Z- handleDVL() - DVL msg epochMillisec: 1730862504.49 2024-11-06T03:08:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:24Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:24Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:24Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:24Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:24Z- handleDvl() - RDI_Pathfinder msg: 1730862504.49 sec; seqNo:951; #beams:0 2024-11-06T03:08:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , -0.01 2024-11-06T03:08:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862504.50 epoch sec; seqNo:2834; 4061715.15 north; 594728.09 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:08:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862501.69 2024-11-06T03:08:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:24Z- waiting for 1730862505.69, time is 1730862504.50 2024-11-06T03:08:24Z- AHRS_M2 msg: 1730862504.88 epoch sec; seqNo:2661 2024-11-06T03:08:24Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.48 psi 2024-11-06T03:08:24Z- handleDepth()- Depth_Keller msg: 1730862504.90 epoch sec; seqNo:2835; depth 49.50 2024-11-06T03:08:24Z- handleDVL() - DVL msg epochMillisec: 1730862504.92 2024-11-06T03:08:24Z- handleDvl() - RDI_Pathfinder msg: 1730862504.92 sec; seqNo:952; #beams:4 2024-11-06T03:08:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.11 , 30.07 , 36.52 , 33.12 2024-11-06T03:08:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , -0.01 2024-11-06T03:08:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862504.93 epoch sec; seqNo:2835; 4061715.15 north; 594728.09 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:08:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862501.69 2024-11-06T03:08:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:25Z- waiting for 1730862505.69, time is 1730862504.93 2024-11-06T03:08:25Z- AHRS_M2 msg: 1730862505.29 epoch sec; seqNo:2662 2024-11-06T03:08:25Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.48 psi 2024-11-06T03:08:25Z- handleDepth()- Depth_Keller msg: 1730862505.29 epoch sec; seqNo:2836; depth 49.50 2024-11-06T03:08:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862505.31 epoch sec; seqNo:2836; 4061715.15 north; 594728.09 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:08:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862501.69 2024-11-06T03:08:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:25Z- waiting for 1730862505.69, time is 1730862505.31 2024-11-06T03:08:25Z- AHRS_M2 msg: 1730862505.69 epoch sec; seqNo:2663 2024-11-06T03:08:25Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.48 psi 2024-11-06T03:08:25Z- handleDepth()- Depth_Keller msg: 1730862505.70 epoch sec; seqNo:2837; depth 49.50 2024-11-06T03:08:25Z- handleDVL() - DVL msg epochMillisec: 1730862505.70 2024-11-06T03:08:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:25Z- handleDvl() - RDI_Pathfinder msg: 1730862505.70 sec; seqNo:953; #beams:0 2024-11-06T03:08:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , -0.01 2024-11-06T03:08:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862505.71 epoch sec; seqNo:2837; 4061715.15 north; 594728.09 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:08:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862501.69 2024-11-06T03:08:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:25Z- waiting for 1730862505.69, time is 1730862505.71 2024-11-06T03:08:25Z- LcmTrn::updateTrn() >>>> pose time 1730862505.71, meas time 1730862505.70 <<<< 2024-11-06T03:08:25Z- LcmTrn::updateTrn() equating pose 1730862505.70 and meas times 1730862505.70 2024-11-06T03:08:25Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862505.70 2024-11-06T03:08:25Z- TerrainNav::motionUpdate() - currEstimate = (4061715, 594728.09, 49.50) 2024-11-06T03:08:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:08:25Z- TerrainNav:: SubcloudNIS is 0.74, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:08:25Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:08:25Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:08:25Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:08:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:08:25Z- variances([0], [2], and psi_berg) = 0.802346 0.356085 0.000000 2024-11-06T03:08:25Z- publishEstimatesDecoder() - at 1730862505.70 sec 2024-11-06T03:08:25Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:08:25Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061746.65 594711.70 59.02 2024-11-06T03:08:25Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061748.09 594711.31 58.35 lat/lon: 36.70 -121.94 2024-11-06T03:08:25Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.80 0.36 0.55 0.00 2024-11-06T03:08:26Z- AHRS_M2 msg: 1730862506.10 epoch sec; seqNo:2664 2024-11-06T03:08:26Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.47 psi 2024-11-06T03:08:26Z- handleDepth()- Depth_Keller msg: 1730862506.11 epoch sec; seqNo:2838; depth 49.49 2024-11-06T03:08:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862506.12 epoch sec; seqNo:2838; 4061715.16 north; 594729.31 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:08:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862505.70 2024-11-06T03:08:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:26Z- waiting for 1730862509.70, time is 1730862506.12 2024-11-06T03:08:26Z- AHRS_M2 msg: 1730862506.50 epoch sec; seqNo:2665 2024-11-06T03:08:26Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.47 psi 2024-11-06T03:08:26Z- handleDepth()- Depth_Keller msg: 1730862506.51 epoch sec; seqNo:2839; depth 49.52 2024-11-06T03:08:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862506.52 epoch sec; seqNo:2839; 4061715.16 north; 594729.31 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T03:08:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862505.70 2024-11-06T03:08:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:26Z- waiting for 1730862509.70, time is 1730862506.52 2024-11-06T03:08:26Z- AHRS_M2 msg: 1730862506.90 epoch sec; seqNo:2666 2024-11-06T03:08:26Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.47 psi 2024-11-06T03:08:26Z- handleDepth()- Depth_Keller msg: 1730862506.91 epoch sec; seqNo:2840; depth 49.52 2024-11-06T03:08:26Z- handleDVL() - DVL msg epochMillisec: 1730862506.94 2024-11-06T03:08:26Z- handleDvl() - RDI_Pathfinder msg: 1730862506.94 sec; seqNo:954; #beams:4 2024-11-06T03:08:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.45 , 30.24 , 36.18 , 33.47 2024-11-06T03:08:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , 0.01 2024-11-06T03:08:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862506.95 epoch sec; seqNo:2840; 4061715.54 north; 594729.31 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T03:08:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862505.70 2024-11-06T03:08:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:27Z- waiting for 1730862509.70, time is 1730862506.95 2024-11-06T03:08:27Z- AHRS_M2 msg: 1730862507.31 epoch sec; seqNo:2667 2024-11-06T03:08:27Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.47 psi 2024-11-06T03:08:27Z- handleDepth()- Depth_Keller msg: 1730862507.31 epoch sec; seqNo:2841; depth 49.51 2024-11-06T03:08:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862507.32 epoch sec; seqNo:2841; 4061715.56 north; 594730.53 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T03:08:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862505.70 2024-11-06T03:08:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:27Z- waiting for 1730862509.70, time is 1730862507.32 2024-11-06T03:08:27Z- AHRS_M2 msg: 1730862507.71 epoch sec; seqNo:2668 2024-11-06T03:08:27Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.47 psi 2024-11-06T03:08:27Z- handleDepth()- Depth_Keller msg: 1730862507.74 epoch sec; seqNo:2842; depth 49.52 2024-11-06T03:08:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862507.76 epoch sec; seqNo:2842; 4061715.56 north; 594730.53 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T03:08:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862505.70 2024-11-06T03:08:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:27Z- waiting for 1730862509.70, time is 1730862507.76 2024-11-06T03:08:28Z- handleDepth()- Depth_Keller msg: 1730862508.14 epoch sec; seqNo:2843; depth 49.53 2024-11-06T03:08:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862508.15 epoch sec; seqNo:2843; 4061715.56 north; 594730.53 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T03:08:28Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:08:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862505.70 2024-11-06T03:08:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:28Z- waiting for 1730862509.70, time is 1730862508.15 2024-11-06T03:08:28Z- AHRS_M2 msg: 1730862508.52 epoch sec; seqNo:2669 2024-11-06T03:08:28Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.47 psi 2024-11-06T03:08:28Z- handleDepth()- Depth_Keller msg: 1730862508.53 epoch sec; seqNo:2844; depth 49.54 2024-11-06T03:08:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862508.54 epoch sec; seqNo:2844; 4061715.56 north; 594730.53 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T03:08:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862505.70 2024-11-06T03:08:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:28Z- waiting for 1730862509.70, time is 1730862508.54 2024-11-06T03:08:28Z- AHRS_M2 msg: 1730862508.92 epoch sec; seqNo:2670 2024-11-06T03:08:28Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.47 psi 2024-11-06T03:08:28Z- handleDepth()- Depth_Keller msg: 1730862508.93 epoch sec; seqNo:2845; depth 49.55 2024-11-06T03:08:28Z- handleDVL() - DVL msg epochMillisec: 1730862508.96 2024-11-06T03:08:28Z- handleDvl() - RDI_Pathfinder msg: 1730862508.96 sec; seqNo:955; #beams:4 2024-11-06T03:08:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.28 , 30.75 , 35.84 , 33.97 2024-11-06T03:08:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , 0.01 2024-11-06T03:08:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862508.98 epoch sec; seqNo:2845; 4061715.57 north; 594731.75 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:08:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862505.70 2024-11-06T03:08:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:29Z- waiting for 1730862509.70, time is 1730862508.98 2024-11-06T03:08:29Z- AHRS_M2 msg: 1730862509.33 epoch sec; seqNo:2671 2024-11-06T03:08:29Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.47 psi 2024-11-06T03:08:29Z- handleDepth()- Depth_Keller msg: 1730862509.33 epoch sec; seqNo:2846; depth 49.55 2024-11-06T03:08:29Z- handleDVL() - DVL msg epochMillisec: 1730862509.33 2024-11-06T03:08:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:29Z- handleDvl() - RDI_Pathfinder msg: 1730862509.33 sec; seqNo:956; #beams:0 2024-11-06T03:08:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , 0.01 2024-11-06T03:08:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862509.35 epoch sec; seqNo:2846; 4061715.57 north; 594731.75 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:08:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862505.70 2024-11-06T03:08:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:29Z- waiting for 1730862509.70, time is 1730862509.35 2024-11-06T03:08:29Z- AHRS_M2 msg: 1730862509.74 epoch sec; seqNo:2672 2024-11-06T03:08:29Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.47 psi 2024-11-06T03:08:29Z- handleDepth()- Depth_Keller msg: 1730862509.74 epoch sec; seqNo:2847; depth 49.55 2024-11-06T03:08:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862509.75 epoch sec; seqNo:2847; 4061715.57 north; 594731.75 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:08:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862505.70 2024-11-06T03:08:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:29Z- waiting for 1730862509.70, time is 1730862509.75 2024-11-06T03:08:29Z- LcmTrn::updateTrn() >>>> pose time 1730862509.75, meas time 1730862509.33 <<<< 2024-11-06T03:08:29Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862509.75 2024-11-06T03:08:29Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:08:29Z- TerrainNav::motionUpdate() - currEstimate = (4061716, 594731.75, 49.55) 2024-11-06T03:08:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:08:29Z- TerrainNav:: SubcloudNIS is 0.74, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:08:29Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:08:29Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:08:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:08:29Z- variances([0], [2], and psi_berg) = 0.804959 0.359257 0.000000 2024-11-06T03:08:29Z- publishEstimatesDecoder() - at 1730862509.75 sec 2024-11-06T03:08:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:08:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061747.04 594715.36 58.44 2024-11-06T03:08:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061748.51 594714.96 58.40 lat/lon: 36.70 -121.94 2024-11-06T03:08:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.80 0.36 0.79 0.00 2024-11-06T03:08:30Z- AHRS_M2 msg: 1730862510.14 epoch sec; seqNo:2673 2024-11-06T03:08:30Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.47 psi 2024-11-06T03:08:30Z- handleDepth()- Depth_Keller msg: 1730862510.15 epoch sec; seqNo:2848; depth 49.56 2024-11-06T03:08:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862510.16 epoch sec; seqNo:2848; 4061715.58 north; 594732.97 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:08:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862509.75 2024-11-06T03:08:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:30Z- waiting for 1730862513.75, time is 1730862510.16 2024-11-06T03:08:30Z- AHRS_M2 msg: 1730862510.54 epoch sec; seqNo:2674 2024-11-06T03:08:30Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.47 psi 2024-11-06T03:08:30Z- handleDepth()- Depth_Keller msg: 1730862510.55 epoch sec; seqNo:2849; depth 49.57 2024-11-06T03:08:30Z- handleDVL() - DVL msg epochMillisec: 1730862510.55 2024-11-06T03:08:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:30Z- handleDvl() - RDI_Pathfinder msg: 1730862510.55 sec; seqNo:957; #beams:0 2024-11-06T03:08:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , 0.01 2024-11-06T03:08:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862510.58 epoch sec; seqNo:2849; 4061715.58 north; 594732.97 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:08:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862509.75 2024-11-06T03:08:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:30Z- waiting for 1730862513.75, time is 1730862510.58 2024-11-06T03:08:30Z- handleDepth()- Depth_Keller msg: 1730862510.96 epoch sec; seqNo:2850; depth 49.57 2024-11-06T03:08:30Z- handleDVL() - DVL msg epochMillisec: 1730862510.98 2024-11-06T03:08:30Z- handleDvl() - RDI_Pathfinder msg: 1730862510.98 sec; seqNo:958; #beams:4 2024-11-06T03:08:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.11 , 31.26 , 34.99 , 34.14 2024-11-06T03:08:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , -0.02 2024-11-06T03:08:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862511.00 epoch sec; seqNo:2850; 4061715.96 north; 594732.96 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:08:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862509.75 2024-11-06T03:08:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:31Z- waiting for 1730862513.75, time is 1730862511.00 2024-11-06T03:08:31Z- AHRS_M2 msg: 1730862511.35 epoch sec; seqNo:2675 2024-11-06T03:08:31Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.47 psi 2024-11-06T03:08:31Z- handleDepth()- Depth_Keller msg: 1730862511.36 epoch sec; seqNo:2851; depth 49.58 2024-11-06T03:08:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862511.37 epoch sec; seqNo:2851; 4061715.96 north; 594732.96 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:08:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862509.75 2024-11-06T03:08:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:31Z- waiting for 1730862513.75, time is 1730862511.37 2024-11-06T03:08:31Z- AHRS_M2 msg: 1730862511.75 epoch sec; seqNo:2676 2024-11-06T03:08:31Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.46 psi 2024-11-06T03:08:31Z- handleDepth()- Depth_Keller msg: 1730862511.76 epoch sec; seqNo:2852; depth 49.60 2024-11-06T03:08:31Z- handleDVL() - DVL msg epochMillisec: 1730862511.76 2024-11-06T03:08:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:31Z- handleDvl() - RDI_Pathfinder msg: 1730862511.76 sec; seqNo:959; #beams:0 2024-11-06T03:08:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , -0.02 2024-11-06T03:08:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862511.78 epoch sec; seqNo:2852; 4061715.98 north; 594734.18 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:08:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862509.75 2024-11-06T03:08:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:31Z- waiting for 1730862513.75, time is 1730862511.78 2024-11-06T03:08:32Z- AHRS_M2 msg: 1730862512.16 epoch sec; seqNo:2677 2024-11-06T03:08:32Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.46 psi 2024-11-06T03:08:32Z- handleDepth()- Depth_Keller msg: 1730862512.17 epoch sec; seqNo:2853; depth 49.61 2024-11-06T03:08:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862512.17 epoch sec; seqNo:2853; 4061715.98 north; 594734.18 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:08:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862509.75 2024-11-06T03:08:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:32Z- waiting for 1730862513.75, time is 1730862512.17 2024-11-06T03:08:32Z- AHRS_M2 msg: 1730862512.56 epoch sec; seqNo:2678 2024-11-06T03:08:32Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.46 psi 2024-11-06T03:08:32Z- handleDepth()- Depth_Keller msg: 1730862512.57 epoch sec; seqNo:2854; depth 49.60 2024-11-06T03:08:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862512.58 epoch sec; seqNo:2854; 4061715.98 north; 594734.18 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:08:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862509.75 2024-11-06T03:08:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:32Z- waiting for 1730862513.75, time is 1730862512.58 2024-11-06T03:08:32Z- AHRS_M2 msg: 1730862512.96 epoch sec; seqNo:2679 2024-11-06T03:08:32Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.46 psi 2024-11-06T03:08:32Z- handleDepth()- Depth_Keller msg: 1730862512.98 epoch sec; seqNo:2855; depth 49.61 2024-11-06T03:08:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862513.00 epoch sec; seqNo:2855; 4061715.99 north; 594735.40 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:08:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862509.75 2024-11-06T03:08:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:33Z- waiting for 1730862513.75, time is 1730862513.00 2024-11-06T03:08:33Z- AHRS_M2 msg: 1730862513.37 epoch sec; seqNo:2680 2024-11-06T03:08:33Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.46 psi 2024-11-06T03:08:33Z- handleDepth()- Depth_Keller msg: 1730862513.38 epoch sec; seqNo:2856; depth 49.61 2024-11-06T03:08:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862513.39 epoch sec; seqNo:2856; 4061715.99 north; 594735.40 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:08:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862509.75 2024-11-06T03:08:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:33Z- waiting for 1730862513.75, time is 1730862513.39 2024-11-06T03:08:33Z- AHRS_M2 msg: 1730862513.77 epoch sec; seqNo:2681 2024-11-06T03:08:33Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T03:08:33Z- handleDepth()- Depth_Keller msg: 1730862513.78 epoch sec; seqNo:2857; depth 49.63 2024-11-06T03:08:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862513.79 epoch sec; seqNo:2857; 4061715.99 north; 594735.40 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:08:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862509.75 2024-11-06T03:08:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:33Z- waiting for 1730862513.75, time is 1730862513.79 2024-11-06T03:08:33Z- LcmTrn::updateTrn() >>>> pose time 1730862513.79, meas time 1730862511.76 <<<< 2024-11-06T03:08:33Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862513.79 2024-11-06T03:08:33Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:08:33Z- TerrainNav::motionUpdate() - currEstimate = (4061716, 594735.40, 49.63) 2024-11-06T03:08:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:08:33Z- TerrainNav:: SubcloudNIS is 0.74, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:08:33Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:08:33Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:08:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:08:33Z- variances([0], [2], and psi_berg) = 0.807479 0.361001 0.000000 2024-11-06T03:08:33Z- publishEstimatesDecoder() - at 1730862513.79 sec 2024-11-06T03:08:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:08:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061747.47 594718.98 59.40 2024-11-06T03:08:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061748.93 594718.62 58.48 lat/lon: 36.70 -121.94 2024-11-06T03:08:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.81 0.36 1.04 0.00 2024-11-06T03:08:34Z- AHRS_M2 msg: 1730862514.17 epoch sec; seqNo:2682 2024-11-06T03:08:34Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T03:08:34Z- handleDepth()- Depth_Keller msg: 1730862514.19 epoch sec; seqNo:2858; depth 49.63 2024-11-06T03:08:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862514.20 epoch sec; seqNo:2858; 4061715.99 north; 594735.40 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:08:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862513.79 2024-11-06T03:08:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:34Z- waiting for 1730862517.79, time is 1730862514.20 2024-11-06T03:08:34Z- AHRS_M2 msg: 1730862514.58 epoch sec; seqNo:2683 2024-11-06T03:08:34Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T03:08:34Z- handleDepth()- Depth_Keller msg: 1730862514.58 epoch sec; seqNo:2859; depth 49.64 2024-11-06T03:08:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862514.59 epoch sec; seqNo:2859; 4061716.00 north; 594736.62 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:08:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862513.79 2024-11-06T03:08:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:34Z- waiting for 1730862517.79, time is 1730862514.59 2024-11-06T03:08:34Z- AHRS_M2 msg: 1730862514.98 epoch sec; seqNo:2684 2024-11-06T03:08:34Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.47 psi 2024-11-06T03:08:35Z- handleDepth()- Depth_Keller msg: 1730862515.00 epoch sec; seqNo:2860; depth 49.65 2024-11-06T03:08:35Z- handleDVL() - DVL msg epochMillisec: 1730862515.02 2024-11-06T03:08:35Z- handleDvl() - RDI_Pathfinder msg: 1730862515.02 sec; seqNo:960; #beams:4 2024-11-06T03:08:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.94 , 31.77 , 34.14 , 35.16 2024-11-06T03:08:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , -0.02 2024-11-06T03:08:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862515.03 epoch sec; seqNo:2860; 4061716.00 north; 594736.62 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:08:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862513.79 2024-11-06T03:08:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:35Z- waiting for 1730862517.79, time is 1730862515.03 2024-11-06T03:08:35Z- AHRS_M2 msg: 1730862515.39 epoch sec; seqNo:2685 2024-11-06T03:08:35Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.47 psi 2024-11-06T03:08:35Z- handleDepth()- Depth_Keller msg: 1730862515.39 epoch sec; seqNo:2861; depth 49.66 2024-11-06T03:08:35Z- handleDVL() - DVL msg epochMillisec: 1730862515.39 2024-11-06T03:08:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:35Z- handleDvl() - RDI_Pathfinder msg: 1730862515.39 sec; seqNo:961; #beams:0 2024-11-06T03:08:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , -0.02 2024-11-06T03:08:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862515.41 epoch sec; seqNo:2861; 4061716.38 north; 594736.62 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:08:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862513.79 2024-11-06T03:08:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:35Z- waiting for 1730862517.79, time is 1730862515.41 2024-11-06T03:08:35Z- AHRS_M2 msg: 1730862515.79 epoch sec; seqNo:2686 2024-11-06T03:08:35Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.47 psi 2024-11-06T03:08:35Z- handleDepth()- Depth_Keller msg: 1730862515.80 epoch sec; seqNo:2862; depth 49.68 2024-11-06T03:08:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862515.81 epoch sec; seqNo:2862; 4061716.39 north; 594737.84 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:08:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862513.79 2024-11-06T03:08:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:35Z- waiting for 1730862517.79, time is 1730862515.81 2024-11-06T03:08:36Z- AHRS_M2 msg: 1730862516.19 epoch sec; seqNo:2687 2024-11-06T03:08:36Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.47 psi 2024-11-06T03:08:36Z- handleDepth()- Depth_Keller msg: 1730862516.20 epoch sec; seqNo:2863; depth 49.68 2024-11-06T03:08:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862516.23 epoch sec; seqNo:2863; 4061716.39 north; 594737.84 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:08:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862513.79 2024-11-06T03:08:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:36Z- waiting for 1730862517.79, time is 1730862516.23 2024-11-06T03:08:36Z- AHRS_M2 msg: 1730862516.60 epoch sec; seqNo:2688 2024-11-06T03:08:36Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.47 psi 2024-11-06T03:08:36Z- handleDepth()- Depth_Keller msg: 1730862516.60 epoch sec; seqNo:2864; depth 49.69 2024-11-06T03:08:36Z- handleDVL() - DVL msg epochMillisec: 1730862516.61 2024-11-06T03:08:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:36Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:36Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:36Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:36Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:36Z- handleDvl() - RDI_Pathfinder msg: 1730862516.61 sec; seqNo:962; #beams:0 2024-11-06T03:08:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , -0.02 2024-11-06T03:08:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862516.62 epoch sec; seqNo:2864; 4061716.39 north; 594737.84 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:08:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862513.79 2024-11-06T03:08:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:36Z- waiting for 1730862517.79, time is 1730862516.62 2024-11-06T03:08:37Z- AHRS_M2 msg: 1730862517.00 epoch sec; seqNo:2689 2024-11-06T03:08:37Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.47 psi 2024-11-06T03:08:37Z- handleDepth()- Depth_Keller msg: 1730862517.02 epoch sec; seqNo:2865; depth 49.71 2024-11-06T03:08:37Z- handleDVL() - DVL msg epochMillisec: 1730862517.04 2024-11-06T03:08:37Z- handleDvl() - RDI_Pathfinder msg: 1730862517.04 sec; seqNo:963; #beams:4 2024-11-06T03:08:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.62 , 31.77 , 34.14 , 36.01 2024-11-06T03:08:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , -0.02 2024-11-06T03:08:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862517.06 epoch sec; seqNo:2865; 4061716.39 north; 594737.84 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:08:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862513.79 2024-11-06T03:08:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:37Z- waiting for 1730862517.79, time is 1730862517.06 2024-11-06T03:08:37Z- AHRS_M2 msg: 1730862517.40 epoch sec; seqNo:2690 2024-11-06T03:08:37Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.47 psi 2024-11-06T03:08:37Z- handleDepth()- Depth_Keller msg: 1730862517.42 epoch sec; seqNo:2866; depth 49.72 2024-11-06T03:08:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862517.43 epoch sec; seqNo:2866; 4061716.41 north; 594739.06 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:08:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862513.79 2024-11-06T03:08:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:37Z- waiting for 1730862517.79, time is 1730862517.43 2024-11-06T03:08:37Z- AHRS_M2 msg: 1730862517.81 epoch sec; seqNo:2691 2024-11-06T03:08:37Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.47 psi 2024-11-06T03:08:37Z- handleDepth()- Depth_Keller msg: 1730862517.82 epoch sec; seqNo:2867; depth 49.75 2024-11-06T03:08:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862517.83 epoch sec; seqNo:2867; 4061716.41 north; 594739.06 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:08:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862513.79 2024-11-06T03:08:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:37Z- waiting for 1730862517.79, time is 1730862517.83 2024-11-06T03:08:37Z- LcmTrn::updateTrn() >>>> pose time 1730862517.83, meas time 1730862517.04 <<<< 2024-11-06T03:08:37Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862517.83 2024-11-06T03:08:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862517.04 sec, ping # 0; waiting for INS data more recent than 1730862513.79... 2024-11-06T03:08:37Z- TerrainNav::motionUpdate() - currEstimate = (4061716, 594739.06, 49.75) 2024-11-06T03:08:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:08:37Z- TerrainNav:: SubcloudNIS is 0.74, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:08:37Z- TNavPF::Projecting Measurements 2024-11-06T03:08:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:08:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:08:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:08:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:08:37Z- TNavFilter::4 good beams. 2024-11-06T03:08:37Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:08:37Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:08:37Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:08:37Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:08:37Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:08:37Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:08:37Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:08:37Z- TerrainMapOctree: (vehN, vehE ) = (4061716.33, 594738.35). 2024-11-06T03:08:37Z- TerrainMapOctree: Min Distance = 35.63. 2024-11-06T03:08:37Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:08:38Z- Forcing Subcloud Comparison 2024-11-06T03:08:38Z- Weighting particles with subcloud comparison 2024-11-06T03:08:38Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:38Z- meanExpectedMeasDiff: 2.858343 delta_rms_squared: 2.206761 alpha: 0.991018 2024-11-06T03:08:38Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:38Z- meanExpectedMeasDiff: -0.519697 delta_rms_squared: 1.462845 alpha: 0.986512 2024-11-06T03:08:38Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:38Z- meanExpectedMeasDiff: 2.256518 delta_rms_squared: 2.163179 alpha: 0.990839 2024-11-06T03:08:38Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:38Z- meanExpectedMeasDiff: -0.197625 delta_rms_squared: 1.842009 alpha: 0.989259 2024-11-06T03:08:38Z- Subcloud NIS: 0.773327 2024-11-06T03:08:38Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:08:38Z- TNavPF:: sumWeights = 0.171638 2024-11-06T03:08:38Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:08:38Z- TNavPF::First Term of Map Covariance Matrix is 0.65 2024-11-06T03:08:38Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T03:08:38Z- TerrainNav::Resampling particle filter... 2024-11-06T03:08:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862517.04 sec. 2024-11-06T03:08:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:08:38Z- variances([0], [2], and psi_berg) = 0.750590 0.355474 0.000000 2024-11-06T03:08:38Z- publishEstimatesDecoder() - at 1730862517.83 sec 2024-11-06T03:08:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:08:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061747.76 594722.65 57.66 2024-11-06T03:08:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061749.38 594722.31 58.01 lat/lon: 36.70 -121.94 2024-11-06T03:08:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.75 0.36 0.70 0.00 2024-11-06T03:08:38Z- AHRS_M2 msg: 1730862518.21 epoch sec; seqNo:2692 2024-11-06T03:08:38Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.47 psi 2024-11-06T03:08:38Z- handleDepth()- Depth_Keller msg: 1730862518.23 epoch sec; seqNo:2868; depth 49.76 2024-11-06T03:08:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862518.25 epoch sec; seqNo:2868; 4061716.41 north; 594739.06 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:08:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862517.83 2024-11-06T03:08:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:38Z- waiting for 1730862521.83, time is 1730862518.25 2024-11-06T03:08:38Z- AHRS_M2 msg: 1730862518.62 epoch sec; seqNo:2693 2024-11-06T03:08:38Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.47 psi 2024-11-06T03:08:38Z- handleDepth()- Depth_Keller msg: 1730862518.63 epoch sec; seqNo:2869; depth 49.78 2024-11-06T03:08:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862518.64 epoch sec; seqNo:2869; 4061716.42 north; 594740.28 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:08:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862517.83 2024-11-06T03:08:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:38Z- waiting for 1730862521.83, time is 1730862518.64 2024-11-06T03:08:39Z- AHRS_M2 msg: 1730862519.03 epoch sec; seqNo:2694 2024-11-06T03:08:39Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.47 psi 2024-11-06T03:08:39Z- handleDepth()- Depth_Keller msg: 1730862519.03 epoch sec; seqNo:2870; depth 49.81 2024-11-06T03:08:39Z- handleDVL() - DVL msg epochMillisec: 1730862519.05 2024-11-06T03:08:39Z- handleDvl() - RDI_Pathfinder msg: 1730862519.05 sec; seqNo:964; #beams:4 2024-11-06T03:08:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.11 , 31.94 , 33.47 , 36.01 2024-11-06T03:08:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , -0.05 2024-11-06T03:08:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862519.08 epoch sec; seqNo:2870; 4061716.42 north; 594740.28 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:08:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862517.83 2024-11-06T03:08:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:39Z- waiting for 1730862521.83, time is 1730862519.08 2024-11-06T03:08:39Z- AHRS_M2 msg: 1730862519.43 epoch sec; seqNo:2695 2024-11-06T03:08:39Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.47 psi 2024-11-06T03:08:39Z- handleDepth()- Depth_Keller msg: 1730862519.43 epoch sec; seqNo:2871; depth 49.82 2024-11-06T03:08:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862519.45 epoch sec; seqNo:2871; 4061716.42 north; 594740.28 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:08:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862517.83 2024-11-06T03:08:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:39Z- waiting for 1730862521.83, time is 1730862519.45 2024-11-06T03:08:39Z- AHRS_M2 msg: 1730862519.83 epoch sec; seqNo:2696 2024-11-06T03:08:39Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.47 psi 2024-11-06T03:08:39Z- handleDepth()- Depth_Keller msg: 1730862519.84 epoch sec; seqNo:2872; depth 49.85 2024-11-06T03:08:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862519.86 epoch sec; seqNo:2872; 4061716.80 north; 594740.28 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:08:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862517.83 2024-11-06T03:08:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:39Z- waiting for 1730862521.83, time is 1730862519.86 2024-11-06T03:08:40Z- AHRS_M2 msg: 1730862520.23 epoch sec; seqNo:2697 2024-11-06T03:08:40Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.47 psi 2024-11-06T03:08:40Z- handleDepth()- Depth_Keller msg: 1730862520.24 epoch sec; seqNo:2873; depth 49.88 2024-11-06T03:08:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862520.25 epoch sec; seqNo:2873; 4061716.81 north; 594741.50 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:08:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862517.83 2024-11-06T03:08:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:40Z- waiting for 1730862521.83, time is 1730862520.25 2024-11-06T03:08:40Z- AHRS_M2 msg: 1730862520.65 epoch sec; seqNo:2698 2024-11-06T03:08:40Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.47 psi 2024-11-06T03:08:40Z- handleDepth()- Depth_Keller msg: 1730862520.65 epoch sec; seqNo:2874; depth 49.90 2024-11-06T03:08:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862520.66 epoch sec; seqNo:2874; 4061716.81 north; 594741.50 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:08:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862517.83 2024-11-06T03:08:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:40Z- waiting for 1730862521.83, time is 1730862520.66 2024-11-06T03:08:41Z- AHRS_M2 msg: 1730862521.05 epoch sec; seqNo:2699 2024-11-06T03:08:41Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.47 psi 2024-11-06T03:08:41Z- handleDepth()- Depth_Keller msg: 1730862521.05 epoch sec; seqNo:2875; depth 49.92 2024-11-06T03:08:41Z- handleDVL() - DVL msg epochMillisec: 1730862521.08 2024-11-06T03:08:41Z- handleDvl() - RDI_Pathfinder msg: 1730862521.08 sec; seqNo:965; #beams:4 2024-11-06T03:08:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.77 , 31.94 , 32.78 , 36.18 2024-11-06T03:08:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , -0.04 2024-11-06T03:08:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862521.09 epoch sec; seqNo:2875; 4061716.81 north; 594741.50 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:08:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862517.83 2024-11-06T03:08:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:41Z- waiting for 1730862521.83, time is 1730862521.09 2024-11-06T03:08:41Z- AHRS_M2 msg: 1730862521.45 epoch sec; seqNo:2700 2024-11-06T03:08:41Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.47 psi 2024-11-06T03:08:41Z- handleDepth()- Depth_Keller msg: 1730862521.45 epoch sec; seqNo:2876; depth 49.94 2024-11-06T03:08:41Z- handleDVL() - DVL msg epochMillisec: 1730862521.46 2024-11-06T03:08:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:41Z- handleDvl() - RDI_Pathfinder msg: 1730862521.46 sec; seqNo:966; #beams:0 2024-11-06T03:08:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , -0.04 2024-11-06T03:08:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862521.47 epoch sec; seqNo:2876; 4061716.81 north; 594741.50 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:08:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862517.83 2024-11-06T03:08:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:41Z- waiting for 1730862521.83, time is 1730862521.47 2024-11-06T03:08:41Z- AHRS_M2 msg: 1730862521.85 epoch sec; seqNo:2701 2024-11-06T03:08:41Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.47 psi 2024-11-06T03:08:41Z- handleDepth()- Depth_Keller msg: 1730862521.86 epoch sec; seqNo:2877; depth 49.98 2024-11-06T03:08:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862521.88 epoch sec; seqNo:2877; 4061716.83 north; 594742.72 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:08:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862517.83 2024-11-06T03:08:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:42Z- waiting for 1730862521.83, time is 1730862521.88 2024-11-06T03:08:42Z- LcmTrn::updateTrn() >>>> pose time 1730862521.88, meas time 1730862521.46 <<<< 2024-11-06T03:08:42Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862521.88 2024-11-06T03:08:42Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:08:42Z- TerrainNav::motionUpdate() - currEstimate = (4061717, 594742.72, 49.98) 2024-11-06T03:08:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:08:42Z- TerrainNav:: SubcloudNIS is 0.77, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:08:42Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:08:42Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:08:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:08:42Z- variances([0], [2], and psi_berg) = 0.752997 0.356610 0.000000 2024-11-06T03:08:42Z- publishEstimatesDecoder() - at 1730862521.88 sec 2024-11-06T03:08:42Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:08:42Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061748.27 594726.23 57.76 2024-11-06T03:08:42Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061749.80 594725.97 58.24 lat/lon: 36.70 -121.94 2024-11-06T03:08:42Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.75 0.36 0.96 0.00 2024-11-06T03:08:42Z- AHRS_M2 msg: 1730862522.26 epoch sec; seqNo:2702 2024-11-06T03:08:42Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.47 psi 2024-11-06T03:08:42Z- handleDepth()- Depth_Keller msg: 1730862522.27 epoch sec; seqNo:2878; depth 50.00 2024-11-06T03:08:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862522.28 epoch sec; seqNo:2878; 4061716.83 north; 594742.72 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:08:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862521.88 2024-11-06T03:08:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:42Z- waiting for 1730862525.88, time is 1730862522.28 2024-11-06T03:08:42Z- AHRS_M2 msg: 1730862522.66 epoch sec; seqNo:2703 2024-11-06T03:08:42Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.46 psi 2024-11-06T03:08:42Z- handleDepth()- Depth_Keller msg: 1730862522.68 epoch sec; seqNo:2879; depth 50.02 2024-11-06T03:08:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862522.69 epoch sec; seqNo:2879; 4061716.83 north; 594742.72 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:08:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862521.88 2024-11-06T03:08:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:42Z- waiting for 1730862525.88, time is 1730862522.69 2024-11-06T03:08:43Z- AHRS_M2 msg: 1730862523.06 epoch sec; seqNo:2704 2024-11-06T03:08:43Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.46 psi 2024-11-06T03:08:43Z- handleDepth()- Depth_Keller msg: 1730862523.07 epoch sec; seqNo:2880; depth 50.05 2024-11-06T03:08:43Z- handleDVL() - DVL msg epochMillisec: 1730862523.10 2024-11-06T03:08:43Z- handleDvl() - RDI_Pathfinder msg: 1730862523.10 sec; seqNo:967; #beams:4 2024-11-06T03:08:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.77 , 32.28 , 32.62 , 36.35 2024-11-06T03:08:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.02 2024-11-06T03:08:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862523.11 epoch sec; seqNo:2880; 4061717.22 north; 594743.93 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:08:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862521.88 2024-11-06T03:08:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:43Z- waiting for 1730862525.88, time is 1730862523.11 2024-11-06T03:08:43Z- AHRS_M2 msg: 1730862523.47 epoch sec; seqNo:2705 2024-11-06T03:08:43Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.46 psi 2024-11-06T03:08:43Z- handleDepth()- Depth_Keller msg: 1730862523.47 epoch sec; seqNo:2881; depth 50.09 2024-11-06T03:08:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862523.48 epoch sec; seqNo:2881; 4061717.22 north; 594743.93 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:08:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862521.88 2024-11-06T03:08:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:43Z- waiting for 1730862525.88, time is 1730862523.48 2024-11-06T03:08:43Z- AHRS_M2 msg: 1730862523.87 epoch sec; seqNo:2706 2024-11-06T03:08:43Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.46 psi 2024-11-06T03:08:43Z- handleDepth()- Depth_Keller msg: 1730862523.88 epoch sec; seqNo:2882; depth 50.10 2024-11-06T03:08:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862523.89 epoch sec; seqNo:2882; 4061717.22 north; 594743.93 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:08:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862521.88 2024-11-06T03:08:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:44Z- waiting for 1730862525.88, time is 1730862523.89 2024-11-06T03:08:44Z- AHRS_M2 msg: 1730862524.28 epoch sec; seqNo:2707 2024-11-06T03:08:44Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.46 psi 2024-11-06T03:08:44Z- handleDepth()- Depth_Keller msg: 1730862524.28 epoch sec; seqNo:2883; depth 50.12 2024-11-06T03:08:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862524.30 epoch sec; seqNo:2883; 4061717.22 north; 594743.93 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:08:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862521.88 2024-11-06T03:08:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:44Z- waiting for 1730862525.88, time is 1730862524.30 2024-11-06T03:08:44Z- AHRS_M2 msg: 1730862524.68 epoch sec; seqNo:2708 2024-11-06T03:08:44Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.46 psi 2024-11-06T03:08:44Z- handleDepth()- Depth_Keller msg: 1730862524.70 epoch sec; seqNo:2884; depth 50.16 2024-11-06T03:08:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862524.76 epoch sec; seqNo:2884; 4061717.23 north; 594745.15 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:08:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862521.88 2024-11-06T03:08:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:44Z- waiting for 1730862525.88, time is 1730862524.76 2024-11-06T03:08:45Z- AHRS_M2 msg: 1730862525.08 epoch sec; seqNo:2709 2024-11-06T03:08:45Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.47 psi 2024-11-06T03:08:45Z- handleDepth()- Depth_Keller msg: 1730862525.09 epoch sec; seqNo:2885; depth 50.18 2024-11-06T03:08:45Z- handleDVL() - DVL msg epochMillisec: 1730862525.11 2024-11-06T03:08:45Z- handleDvl() - RDI_Pathfinder msg: 1730862525.11 sec; seqNo:968; #beams:4 2024-11-06T03:08:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.94 , 32.28 , 32.28 , 36.18 2024-11-06T03:08:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , 0.04 2024-11-06T03:08:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862525.13 epoch sec; seqNo:2885; 4061717.23 north; 594745.15 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:08:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862521.88 2024-11-06T03:08:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:45Z- waiting for 1730862525.88, time is 1730862525.13 2024-11-06T03:08:45Z- AHRS_M2 msg: 1730862525.49 epoch sec; seqNo:2710 2024-11-06T03:08:45Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.47 psi 2024-11-06T03:08:45Z- handleDepth()- Depth_Keller msg: 1730862525.50 epoch sec; seqNo:2886; depth 50.22 2024-11-06T03:08:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862525.54 epoch sec; seqNo:2886; 4061717.23 north; 594745.15 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:08:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862521.88 2024-11-06T03:08:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:45Z- waiting for 1730862525.88, time is 1730862525.54 2024-11-06T03:08:45Z- AHRS_M2 msg: 1730862525.89 epoch sec; seqNo:2711 2024-11-06T03:08:45Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.47 psi 2024-11-06T03:08:45Z- handleDepth()- Depth_Keller msg: 1730862525.90 epoch sec; seqNo:2887; depth 50.24 2024-11-06T03:08:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862525.91 epoch sec; seqNo:2887; 4061717.25 north; 594746.37 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:08:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862521.88 2024-11-06T03:08:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:46Z- waiting for 1730862525.88, time is 1730862525.91 2024-11-06T03:08:46Z- LcmTrn::updateTrn() >>>> pose time 1730862525.91, meas time 1730862525.11 <<<< 2024-11-06T03:08:46Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862525.91 2024-11-06T03:08:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862525.11 sec, ping # 0; waiting for INS data more recent than 1730862521.88... 2024-11-06T03:08:46Z- TerrainNav::motionUpdate() - currEstimate = (4061717, 594746.37, 50.24) 2024-11-06T03:08:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:08:46Z- TerrainNav:: SubcloudNIS is 0.77, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:08:46Z- TNavPF::Projecting Measurements 2024-11-06T03:08:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:08:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:08:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:08:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:08:46Z- TNavFilter::4 good beams. 2024-11-06T03:08:46Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:08:46Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:08:46Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:08:46Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:08:46Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:08:46Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:08:46Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:08:46Z- TerrainMapOctree: (vehN, vehE ) = (4061717.16, 594745.65). 2024-11-06T03:08:46Z- TerrainMapOctree: Min Distance = 33.12. 2024-11-06T03:08:46Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:08:46Z- Forcing Subcloud Comparison 2024-11-06T03:08:46Z- Weighting particles with subcloud comparison 2024-11-06T03:08:46Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:46Z- meanExpectedMeasDiff: 1.875475 delta_rms_squared: 2.022497 alpha: 0.990208 2024-11-06T03:08:46Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:46Z- meanExpectedMeasDiff: 3.751213 delta_rms_squared: 1.163071 alpha: 0.983095 2024-11-06T03:08:46Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:46Z- meanExpectedMeasDiff: 1.188222 delta_rms_squared: 1.559064 alpha: 0.987334 2024-11-06T03:08:46Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:08:46Z- meanExpectedMeasDiff: -0.149997 delta_rms_squared: 1.748286 alpha: 0.988690 2024-11-06T03:08:46Z- Subcloud NIS: 0.788907 2024-11-06T03:08:46Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:08:46Z- TNavPF:: sumWeights = 0.100352 2024-11-06T03:08:46Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:08:46Z- TNavPF::First Term of Map Covariance Matrix is 0.13 2024-11-06T03:08:46Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T03:08:46Z- TerrainNav::Resampling particle filter... 2024-11-06T03:08:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862525.11 sec. 2024-11-06T03:08:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:08:46Z- variances([0], [2], and psi_berg) = 0.731789 0.408918 0.000000 2024-11-06T03:08:46Z- publishEstimatesDecoder() - at 1730862525.91 sec 2024-11-06T03:08:46Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:08:46Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061748.60 594729.89 58.32 2024-11-06T03:08:46Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061750.39 594729.52 57.67 lat/lon: 36.70 -121.94 2024-11-06T03:08:46Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.73 0.41 0.72 0.00 2024-11-06T03:08:46Z- AHRS_M2 msg: 1730862526.30 epoch sec; seqNo:2712 2024-11-06T03:08:46Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.47 psi 2024-11-06T03:08:46Z- handleDepth()- Depth_Keller msg: 1730862526.30 epoch sec; seqNo:2888; depth 50.26 2024-11-06T03:08:46Z- handleDVL() - DVL msg epochMillisec: 1730862526.30 2024-11-06T03:08:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:46Z- handleDvl() - RDI_Pathfinder msg: 1730862526.30 sec; seqNo:969; #beams:0 2024-11-06T03:08:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , 0.04 2024-11-06T03:08:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862526.33 epoch sec; seqNo:2888; 4061717.63 north; 594746.37 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:08:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862525.91 2024-11-06T03:08:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:46Z- waiting for 1730862529.91, time is 1730862526.33 2024-11-06T03:08:46Z- AHRS_M2 msg: 1730862526.75 epoch sec; seqNo:2713 2024-11-06T03:08:46Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.47 psi 2024-11-06T03:08:46Z- handleDepth()- Depth_Keller msg: 1730862526.77 epoch sec; seqNo:2889; depth 50.28 2024-11-06T03:08:46Z- handleDVL() - DVL msg epochMillisec: 1730862526.79 2024-11-06T03:08:46Z- handleDvl() - RDI_Pathfinder msg: 1730862526.79 sec; seqNo:970; #beams:4 2024-11-06T03:08:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.94 , 32.28 , 32.28 , 36.01 2024-11-06T03:08:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.05 , 0.00 2024-11-06T03:08:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862526.80 epoch sec; seqNo:2889; 4061717.63 north; 594746.37 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:08:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862525.91 2024-11-06T03:08:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:46Z- waiting for 1730862529.91, time is 1730862526.80 2024-11-06T03:08:47Z- handleDepth()- Depth_Keller msg: 1730862527.12 epoch sec; seqNo:2890; depth 50.31 2024-11-06T03:08:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862527.14 epoch sec; seqNo:2890; 4061717.63 north; 594746.37 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:08:47Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:08:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862525.91 2024-11-06T03:08:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:47Z- waiting for 1730862529.91, time is 1730862527.14 2024-11-06T03:08:47Z- AHRS_M2 msg: 1730862527.51 epoch sec; seqNo:2714 2024-11-06T03:08:47Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.48 psi 2024-11-06T03:08:47Z- handleDepth()- Depth_Keller msg: 1730862527.52 epoch sec; seqNo:2891; depth 50.30 2024-11-06T03:08:47Z- handleDVL() - DVL msg epochMillisec: 1730862527.53 2024-11-06T03:08:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:47Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:47Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:47Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:47Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:47Z- handleDvl() - RDI_Pathfinder msg: 1730862527.53 sec; seqNo:971; #beams:0 2024-11-06T03:08:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.05 , 0.00 2024-11-06T03:08:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862527.54 epoch sec; seqNo:2891; 4061717.64 north; 594747.59 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:08:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862525.91 2024-11-06T03:08:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:47Z- waiting for 1730862529.91, time is 1730862527.54 2024-11-06T03:08:47Z- AHRS_M2 msg: 1730862527.91 epoch sec; seqNo:2715 2024-11-06T03:08:47Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 1.48 psi 2024-11-06T03:08:47Z- handleDepth()- Depth_Keller msg: 1730862527.92 epoch sec; seqNo:2892; depth 50.33 2024-11-06T03:08:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862527.93 epoch sec; seqNo:2892; 4061717.64 north; 594747.59 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:08:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862525.91 2024-11-06T03:08:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:48Z- waiting for 1730862529.91, time is 1730862527.93 2024-11-06T03:08:48Z- AHRS_M2 msg: 1730862528.32 epoch sec; seqNo:2716 2024-11-06T03:08:48Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 1.49 psi 2024-11-06T03:08:48Z- handleDepth()- Depth_Keller msg: 1730862528.33 epoch sec; seqNo:2893; depth 50.32 2024-11-06T03:08:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862528.33 epoch sec; seqNo:2893; 4061717.64 north; 594747.59 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:08:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862525.91 2024-11-06T03:08:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:48Z- waiting for 1730862529.91, time is 1730862528.33 2024-11-06T03:08:48Z- AHRS_M2 msg: 1730862528.76 epoch sec; seqNo:2717 2024-11-06T03:08:48Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 1.49 psi 2024-11-06T03:08:48Z- handleDepth()- Depth_Keller msg: 1730862528.77 epoch sec; seqNo:2894; depth 50.35 2024-11-06T03:08:48Z- handleDVL() - DVL msg epochMillisec: 1730862528.80 2024-11-06T03:08:48Z- handleDvl() - RDI_Pathfinder msg: 1730862528.80 sec; seqNo:972; #beams:4 2024-11-06T03:08:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.77 , 32.28 , 31.60 , 36.01 2024-11-06T03:08:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , -0.06 2024-11-06T03:08:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862528.81 epoch sec; seqNo:2894; 4061717.65 north; 594748.81 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:08:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862525.91 2024-11-06T03:08:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:48Z- waiting for 1730862529.91, time is 1730862528.81 2024-11-06T03:08:49Z- AHRS_M2 msg: 1730862529.12 epoch sec; seqNo:2718 2024-11-06T03:08:49Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 1.49 psi 2024-11-06T03:08:49Z- handleDepth()- Depth_Keller msg: 1730862529.13 epoch sec; seqNo:2895; depth 50.35 2024-11-06T03:08:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862529.16 epoch sec; seqNo:2895; 4061717.65 north; 594748.81 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:08:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862525.91 2024-11-06T03:08:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:49Z- waiting for 1730862529.91, time is 1730862529.16 2024-11-06T03:08:49Z- AHRS_M2 msg: 1730862529.53 epoch sec; seqNo:2719 2024-11-06T03:08:49Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.49 psi 2024-11-06T03:08:49Z- handleDepth()- Depth_Keller msg: 1730862529.53 epoch sec; seqNo:2896; depth 50.35 2024-11-06T03:08:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862529.55 epoch sec; seqNo:2896; 4061717.65 north; 594748.81 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:08:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862525.91 2024-11-06T03:08:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:49Z- waiting for 1730862529.91, time is 1730862529.55 2024-11-06T03:08:49Z- AHRS_M2 msg: 1730862529.93 epoch sec; seqNo:2720 2024-11-06T03:08:49Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.49 psi 2024-11-06T03:08:49Z- handleDepth()- Depth_Keller msg: 1730862529.94 epoch sec; seqNo:2897; depth 50.37 2024-11-06T03:08:49Z- handleDVL() - DVL msg epochMillisec: 1730862529.94 2024-11-06T03:08:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:49Z- handleDvl() - RDI_Pathfinder msg: 1730862529.94 sec; seqNo:973; #beams:0 2024-11-06T03:08:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , -0.06 2024-11-06T03:08:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862529.95 epoch sec; seqNo:2897; 4061717.67 north; 594750.03 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:08:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862525.91 2024-11-06T03:08:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:50Z- waiting for 1730862529.91, time is 1730862529.95 2024-11-06T03:08:50Z- LcmTrn::updateTrn() >>>> pose time 1730862529.95, meas time 1730862529.94 <<<< 2024-11-06T03:08:50Z- LcmTrn::updateTrn() equating pose 1730862529.94 and meas times 1730862529.94 2024-11-06T03:08:50Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862529.94 2024-11-06T03:08:50Z- TerrainNav::motionUpdate() - currEstimate = (4061718, 594750.03, 50.37) 2024-11-06T03:08:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:08:50Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:08:50Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:08:50Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:08:50Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:08:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:08:50Z- variances([0], [2], and psi_berg) = 0.734674 0.411274 0.000000 2024-11-06T03:08:50Z- publishEstimatesDecoder() - at 1730862529.94 sec 2024-11-06T03:08:50Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:08:50Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061749.04 594733.51 58.57 2024-11-06T03:08:50Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061750.81 594733.18 57.81 lat/lon: 36.70 -121.94 2024-11-06T03:08:50Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.73 0.41 0.96 0.00 2024-11-06T03:08:50Z- AHRS_M2 msg: 1730862530.33 epoch sec; seqNo:2721 2024-11-06T03:08:50Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.49 psi 2024-11-06T03:08:50Z- handleDepth()- Depth_Keller msg: 1730862530.34 epoch sec; seqNo:2898; depth 50.38 2024-11-06T03:08:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862530.36 epoch sec; seqNo:2898; 4061717.67 north; 594750.03 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:08:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862529.94 2024-11-06T03:08:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:50Z- waiting for 1730862533.94, time is 1730862530.36 2024-11-06T03:08:50Z- AHRS_M2 msg: 1730862530.76 epoch sec; seqNo:2722 2024-11-06T03:08:50Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.49 psi 2024-11-06T03:08:50Z- handleDepth()- Depth_Keller msg: 1730862530.77 epoch sec; seqNo:2899; depth 50.37 2024-11-06T03:08:50Z- handleDVL() - DVL msg epochMillisec: 1730862530.80 2024-11-06T03:08:50Z- handleDvl() - RDI_Pathfinder msg: 1730862530.80 sec; seqNo:974; #beams:4 2024-11-06T03:08:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.43 , 31.77 , 32.28 , 35.84 2024-11-06T03:08:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , -0.06 2024-11-06T03:08:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862530.81 epoch sec; seqNo:2899; 4061717.67 north; 594750.03 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:08:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862529.94 2024-11-06T03:08:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:50Z- waiting for 1730862533.94, time is 1730862530.81 2024-11-06T03:08:51Z- AHRS_M2 msg: 1730862531.15 epoch sec; seqNo:2723 2024-11-06T03:08:51Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 1.49 psi 2024-11-06T03:08:51Z- handleDepth()- Depth_Keller msg: 1730862531.16 epoch sec; seqNo:2900; depth 50.38 2024-11-06T03:08:51Z- handleDVL() - DVL msg epochMillisec: 1730862531.16 2024-11-06T03:08:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:51Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:51Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:51Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:51Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:51Z- handleDvl() - RDI_Pathfinder msg: 1730862531.16 sec; seqNo:975; #beams:0 2024-11-06T03:08:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , -0.06 2024-11-06T03:08:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862531.18 epoch sec; seqNo:2900; 4061717.67 north; 594750.03 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:08:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862529.94 2024-11-06T03:08:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:51Z- waiting for 1730862533.94, time is 1730862531.18 2024-11-06T03:08:51Z- AHRS_M2 msg: 1730862531.55 epoch sec; seqNo:2724 2024-11-06T03:08:51Z- AHRS_M2 msg: 0.17 phi; -0.01 theta; 1.49 psi 2024-11-06T03:08:51Z- handleDepth()- Depth_Keller msg: 1730862531.55 epoch sec; seqNo:2901; depth 50.38 2024-11-06T03:08:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862531.58 epoch sec; seqNo:2901; 4061717.68 north; 594751.25 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:08:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862529.94 2024-11-06T03:08:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:51Z- waiting for 1730862533.94, time is 1730862531.58 2024-11-06T03:08:51Z- AHRS_M2 msg: 1730862531.95 epoch sec; seqNo:2725 2024-11-06T03:08:51Z- AHRS_M2 msg: 0.17 phi; -0.01 theta; 1.50 psi 2024-11-06T03:08:51Z- handleDepth()- Depth_Keller msg: 1730862531.96 epoch sec; seqNo:2902; depth 50.39 2024-11-06T03:08:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862531.98 epoch sec; seqNo:2902; 4061717.68 north; 594751.25 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:08:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862529.94 2024-11-06T03:08:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:52Z- waiting for 1730862533.94, time is 1730862531.98 2024-11-06T03:08:52Z- handleDepth()- Depth_Keller msg: 1730862532.38 epoch sec; seqNo:2903; depth 50.38 2024-11-06T03:08:52Z- handleDVL() - DVL msg epochMillisec: 1730862532.38 2024-11-06T03:08:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:52Z- handleDvl() - RDI_Pathfinder msg: 1730862532.38 sec; seqNo:976; #beams:0 2024-11-06T03:08:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , -0.06 2024-11-06T03:08:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862532.39 epoch sec; seqNo:2903; 4061717.68 north; 594751.25 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:08:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862529.94 2024-11-06T03:08:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:52Z- waiting for 1730862533.94, time is 1730862532.39 2024-11-06T03:08:52Z- AHRS_M2 msg: 1730862532.79 epoch sec; seqNo:2726 2024-11-06T03:08:52Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 1.49 psi 2024-11-06T03:08:52Z- handleDepth()- Depth_Keller msg: 1730862532.80 epoch sec; seqNo:2904; depth 50.38 2024-11-06T03:08:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862532.83 epoch sec; seqNo:2904; 4061717.69 north; 594752.47 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:08:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862529.94 2024-11-06T03:08:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:52Z- waiting for 1730862533.94, time is 1730862532.83 2024-11-06T03:08:53Z- AHRS_M2 msg: 1730862533.16 epoch sec; seqNo:2727 2024-11-06T03:08:53Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 1.49 psi 2024-11-06T03:08:53Z- handleDepth()- Depth_Keller msg: 1730862533.18 epoch sec; seqNo:2905; depth 50.39 2024-11-06T03:08:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862533.19 epoch sec; seqNo:2905; 4061717.69 north; 594752.47 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:08:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862529.94 2024-11-06T03:08:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:53Z- waiting for 1730862533.94, time is 1730862533.19 2024-11-06T03:08:53Z- AHRS_M2 msg: 1730862533.57 epoch sec; seqNo:2728 2024-11-06T03:08:53Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.49 psi 2024-11-06T03:08:53Z- handleDepth()- Depth_Keller msg: 1730862533.57 epoch sec; seqNo:2906; depth 50.38 2024-11-06T03:08:53Z- handleDVL() - DVL msg epochMillisec: 1730862533.57 2024-11-06T03:08:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:53Z- handleDvl() - RDI_Pathfinder msg: 1730862533.57 sec; seqNo:977; #beams:0 2024-11-06T03:08:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , -0.06 2024-11-06T03:08:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862533.61 epoch sec; seqNo:2906; 4061718.07 north; 594752.47 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:08:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862529.94 2024-11-06T03:08:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:53Z- waiting for 1730862533.94, time is 1730862533.61 2024-11-06T03:08:53Z- AHRS_M2 msg: 1730862533.97 epoch sec; seqNo:2729 2024-11-06T03:08:53Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.49 psi 2024-11-06T03:08:53Z- handleDepth()- Depth_Keller msg: 1730862533.98 epoch sec; seqNo:2907; depth 50.38 2024-11-06T03:08:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862533.99 epoch sec; seqNo:2907; 4061718.07 north; 594752.47 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:08:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862529.94 2024-11-06T03:08:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:54Z- waiting for 1730862533.94, time is 1730862533.99 2024-11-06T03:08:54Z- LcmTrn::updateTrn() >>>> pose time 1730862533.99, meas time 1730862533.57 <<<< 2024-11-06T03:08:54Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862533.99 2024-11-06T03:08:54Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:08:54Z- TerrainNav::motionUpdate() - currEstimate = (4061718, 594752.47, 50.38) 2024-11-06T03:08:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:08:54Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:08:54Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:08:54Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:08:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:08:54Z- variances([0], [2], and psi_berg) = 0.735897 0.411776 0.000000 2024-11-06T03:08:54Z- publishEstimatesDecoder() - at 1730862533.99 sec 2024-11-06T03:08:54Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:08:54Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061749.46 594735.98 58.65 2024-11-06T03:08:54Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061751.22 594735.61 57.81 lat/lon: 36.70 -121.94 2024-11-06T03:08:54Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.74 0.41 1.21 0.00 2024-11-06T03:08:54Z- AHRS_M2 msg: 1730862534.37 epoch sec; seqNo:2730 2024-11-06T03:08:54Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.49 psi 2024-11-06T03:08:54Z- handleDepth()- Depth_Keller msg: 1730862534.39 epoch sec; seqNo:2908; depth 50.39 2024-11-06T03:08:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862534.39 epoch sec; seqNo:2908; 4061718.09 north; 594753.69 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:08:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862533.99 2024-11-06T03:08:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:54Z- waiting for 1730862537.99, time is 1730862534.39 2024-11-06T03:08:54Z- AHRS_M2 msg: 1730862534.78 epoch sec; seqNo:2731 2024-11-06T03:08:54Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.49 psi 2024-11-06T03:08:54Z- handleDepth()- Depth_Keller msg: 1730862534.79 epoch sec; seqNo:2909; depth 50.39 2024-11-06T03:08:54Z- handleDVL() - DVL msg epochMillisec: 1730862534.81 2024-11-06T03:08:54Z- handleDvl() - RDI_Pathfinder msg: 1730862534.81 sec; seqNo:978; #beams:4 2024-11-06T03:08:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.75 , 31.94 , 33.29 , 36.01 2024-11-06T03:08:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , 0.02 2024-11-06T03:08:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862534.83 epoch sec; seqNo:2909; 4061718.09 north; 594753.69 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:08:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862533.99 2024-11-06T03:08:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:54Z- waiting for 1730862537.99, time is 1730862534.83 2024-11-06T03:08:55Z- AHRS_M2 msg: 1730862535.18 epoch sec; seqNo:2732 2024-11-06T03:08:55Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.49 psi 2024-11-06T03:08:55Z- handleDepth()- Depth_Keller msg: 1730862535.19 epoch sec; seqNo:2910; depth 50.39 2024-11-06T03:08:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862535.20 epoch sec; seqNo:2910; 4061718.09 north; 594753.69 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:08:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862533.99 2024-11-06T03:08:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:55Z- waiting for 1730862537.99, time is 1730862535.20 2024-11-06T03:08:55Z- AHRS_M2 msg: 1730862535.59 epoch sec; seqNo:2733 2024-11-06T03:08:55Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.49 psi 2024-11-06T03:08:55Z- handleDepth()- Depth_Keller msg: 1730862535.60 epoch sec; seqNo:2911; depth 50.40 2024-11-06T03:08:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862535.61 epoch sec; seqNo:2911; 4061718.10 north; 594754.91 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:08:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862533.99 2024-11-06T03:08:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:55Z- waiting for 1730862537.99, time is 1730862535.61 2024-11-06T03:08:56Z- AHRS_M2 msg: 1730862536.00 epoch sec; seqNo:2734 2024-11-06T03:08:56Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.49 psi 2024-11-06T03:08:56Z- handleDepth()- Depth_Keller msg: 1730862536.01 epoch sec; seqNo:2912; depth 50.39 2024-11-06T03:08:56Z- handleDVL() - DVL msg epochMillisec: 1730862536.01 2024-11-06T03:08:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:56Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:56Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:56Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:56Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:56Z- handleDvl() - RDI_Pathfinder msg: 1730862536.01 sec; seqNo:979; #beams:0 2024-11-06T03:08:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , 0.02 2024-11-06T03:08:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862536.02 epoch sec; seqNo:2912; 4061718.10 north; 594754.91 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:08:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862533.99 2024-11-06T03:08:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:56Z- waiting for 1730862537.99, time is 1730862536.02 2024-11-06T03:08:56Z- AHRS_M2 msg: 1730862536.39 epoch sec; seqNo:2735 2024-11-06T03:08:56Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.49 psi 2024-11-06T03:08:56Z- handleDepth()- Depth_Keller msg: 1730862536.40 epoch sec; seqNo:2913; depth 50.40 2024-11-06T03:08:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862536.42 epoch sec; seqNo:2913; 4061718.10 north; 594754.91 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:08:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862533.99 2024-11-06T03:08:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:56Z- waiting for 1730862537.99, time is 1730862536.42 2024-11-06T03:08:56Z- AHRS_M2 msg: 1730862536.80 epoch sec; seqNo:2736 2024-11-06T03:08:56Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.49 psi 2024-11-06T03:08:56Z- handleDepth()- Depth_Keller msg: 1730862536.81 epoch sec; seqNo:2914; depth 50.42 2024-11-06T03:08:56Z- handleDVL() - DVL msg epochMillisec: 1730862536.85 2024-11-06T03:08:56Z- handleDvl() - RDI_Pathfinder msg: 1730862536.85 sec; seqNo:980; #beams:4 2024-11-06T03:08:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.41 , 32.11 , 33.29 , 35.67 2024-11-06T03:08:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , -0.01 2024-11-06T03:08:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862536.87 epoch sec; seqNo:2914; 4061718.10 north; 594754.91 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:08:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862533.99 2024-11-06T03:08:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:56Z- waiting for 1730862537.99, time is 1730862536.87 2024-11-06T03:08:57Z- AHRS_M2 msg: 1730862537.20 epoch sec; seqNo:2737 2024-11-06T03:08:57Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.49 psi 2024-11-06T03:08:57Z- handleDepth()- Depth_Keller msg: 1730862537.21 epoch sec; seqNo:2915; depth 50.41 2024-11-06T03:08:57Z- handleDVL() - DVL msg epochMillisec: 1730862537.22 2024-11-06T03:08:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:57Z- handleDvl() - RDI_Pathfinder msg: 1730862537.22 sec; seqNo:981; #beams:0 2024-11-06T03:08:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , -0.01 2024-11-06T03:08:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862537.23 epoch sec; seqNo:2915; 4061718.11 north; 594756.13 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:08:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862533.99 2024-11-06T03:08:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:57Z- waiting for 1730862537.99, time is 1730862537.23 2024-11-06T03:08:57Z- AHRS_M2 msg: 1730862537.61 epoch sec; seqNo:2738 2024-11-06T03:08:57Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.49 psi 2024-11-06T03:08:57Z- handleDepth()- Depth_Keller msg: 1730862537.62 epoch sec; seqNo:2916; depth 50.43 2024-11-06T03:08:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862537.64 epoch sec; seqNo:2916; 4061718.11 north; 594756.13 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:08:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862533.99 2024-11-06T03:08:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:57Z- waiting for 1730862537.99, time is 1730862537.64 2024-11-06T03:08:58Z- AHRS_M2 msg: 1730862538.01 epoch sec; seqNo:2739 2024-11-06T03:08:58Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T03:08:58Z- handleDepth()- Depth_Keller msg: 1730862538.02 epoch sec; seqNo:2917; depth 50.42 2024-11-06T03:08:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862538.03 epoch sec; seqNo:2917; 4061718.11 north; 594756.13 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:08:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862533.99 2024-11-06T03:08:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:58Z- waiting for 1730862537.99, time is 1730862538.03 2024-11-06T03:08:58Z- LcmTrn::updateTrn() >>>> pose time 1730862538.03, meas time 1730862537.22 <<<< 2024-11-06T03:08:58Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862538.03 2024-11-06T03:08:58Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:08:58Z- TerrainNav::motionUpdate() - currEstimate = (4061718, 594756.13, 50.42) 2024-11-06T03:08:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:08:58Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:08:58Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:08:58Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:08:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:08:58Z- variances([0], [2], and psi_berg) = 0.737464 0.413782 0.000000 2024-11-06T03:08:58Z- publishEstimatesDecoder() - at 1730862538.03 sec 2024-11-06T03:08:58Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:08:58Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061749.51 594739.69 58.85 2024-11-06T03:08:58Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061751.26 594739.27 57.85 lat/lon: 36.70 -121.94 2024-11-06T03:08:58Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.74 0.41 1.45 0.00 2024-11-06T03:08:58Z- AHRS_M2 msg: 1730862538.41 epoch sec; seqNo:2740 2024-11-06T03:08:58Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T03:08:58Z- handleDepth()- Depth_Keller msg: 1730862538.43 epoch sec; seqNo:2918; depth 50.43 2024-11-06T03:08:58Z- handleDVL() - DVL msg epochMillisec: 1730862538.43 2024-11-06T03:08:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:58Z- handleDvl() - RDI_Pathfinder msg: 1730862538.43 sec; seqNo:982; #beams:0 2024-11-06T03:08:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , -0.01 2024-11-06T03:08:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862538.44 epoch sec; seqNo:2918; 4061718.13 north; 594757.35 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:08:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862538.03 2024-11-06T03:08:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:58Z- waiting for 1730862542.03, time is 1730862538.44 2024-11-06T03:08:58Z- AHRS_M2 msg: 1730862538.82 epoch sec; seqNo:2741 2024-11-06T03:08:58Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.49 psi 2024-11-06T03:08:58Z- handleDepth()- Depth_Keller msg: 1730862538.83 epoch sec; seqNo:2919; depth 50.44 2024-11-06T03:08:58Z- handleDVL() - DVL msg epochMillisec: 1730862538.86 2024-11-06T03:08:58Z- handleDvl() - RDI_Pathfinder msg: 1730862538.86 sec; seqNo:983; #beams:4 2024-11-06T03:08:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.58 , 32.11 , 33.12 , 35.67 2024-11-06T03:08:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , -0.02 2024-11-06T03:08:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862538.87 epoch sec; seqNo:2919; 4061718.13 north; 594757.35 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:08:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862538.03 2024-11-06T03:08:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:58Z- waiting for 1730862542.03, time is 1730862538.87 2024-11-06T03:08:59Z- AHRS_M2 msg: 1730862539.22 epoch sec; seqNo:2742 2024-11-06T03:08:59Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.49 psi 2024-11-06T03:08:59Z- handleDepth()- Depth_Keller msg: 1730862539.23 epoch sec; seqNo:2920; depth 50.44 2024-11-06T03:08:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862539.24 epoch sec; seqNo:2920; 4061718.13 north; 594757.35 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:08:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:08:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862538.03 2024-11-06T03:08:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:59Z- waiting for 1730862542.03, time is 1730862539.24 2024-11-06T03:08:59Z- AHRS_M2 msg: 1730862539.63 epoch sec; seqNo:2743 2024-11-06T03:08:59Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.49 psi 2024-11-06T03:08:59Z- handleDepth()- Depth_Keller msg: 1730862539.64 epoch sec; seqNo:2921; depth 50.44 2024-11-06T03:08:59Z- handleDVL() - DVL msg epochMillisec: 1730862539.65 2024-11-06T03:08:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:08:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:08:59Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:08:59Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:08:59Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:08:59Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:08:59Z- handleDvl() - RDI_Pathfinder msg: 1730862539.65 sec; seqNo:984; #beams:0 2024-11-06T03:08:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:08:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , -0.02 2024-11-06T03:08:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862539.66 epoch sec; seqNo:2921; 4061718.50 north; 594757.35 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:08:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:08:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862538.03 2024-11-06T03:08:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:08:59Z- waiting for 1730862542.03, time is 1730862539.66 2024-11-06T03:09:00Z- AHRS_M2 msg: 1730862540.03 epoch sec; seqNo:2744 2024-11-06T03:09:00Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.49 psi 2024-11-06T03:09:00Z- handleDepth()- Depth_Keller msg: 1730862540.05 epoch sec; seqNo:2922; depth 50.44 2024-11-06T03:09:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862540.06 epoch sec; seqNo:2922; 4061718.52 north; 594758.57 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:09:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862538.03 2024-11-06T03:09:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:00Z- waiting for 1730862542.03, time is 1730862540.06 2024-11-06T03:09:00Z- AHRS_M2 msg: 1730862540.43 epoch sec; seqNo:2745 2024-11-06T03:09:00Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.49 psi 2024-11-06T03:09:00Z- handleDepth()- Depth_Keller msg: 1730862540.44 epoch sec; seqNo:2923; depth 50.45 2024-11-06T03:09:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862540.45 epoch sec; seqNo:2923; 4061718.52 north; 594758.57 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:09:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862538.03 2024-11-06T03:09:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:00Z- waiting for 1730862542.03, time is 1730862540.45 2024-11-06T03:09:00Z- AHRS_M2 msg: 1730862540.84 epoch sec; seqNo:2746 2024-11-06T03:09:00Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.49 psi 2024-11-06T03:09:00Z- handleDepth()- Depth_Keller msg: 1730862540.85 epoch sec; seqNo:2924; depth 50.46 2024-11-06T03:09:00Z- handleDVL() - DVL msg epochMillisec: 1730862540.87 2024-11-06T03:09:00Z- handleDvl() - RDI_Pathfinder msg: 1730862540.87 sec; seqNo:985; #beams:4 2024-11-06T03:09:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.24 , 32.11 , 33.29 , 35.67 2024-11-06T03:09:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , 0.01 2024-11-06T03:09:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862540.89 epoch sec; seqNo:2924; 4061718.52 north; 594758.57 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:09:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862538.03 2024-11-06T03:09:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:01Z- waiting for 1730862542.03, time is 1730862540.89 2024-11-06T03:09:01Z- AHRS_M2 msg: 1730862541.25 epoch sec; seqNo:2747 2024-11-06T03:09:01Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.49 psi 2024-11-06T03:09:01Z- handleDepth()- Depth_Keller msg: 1730862541.25 epoch sec; seqNo:2925; depth 50.44 2024-11-06T03:09:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862541.27 epoch sec; seqNo:2925; 4061718.53 north; 594759.79 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:09:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862538.03 2024-11-06T03:09:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:01Z- waiting for 1730862542.03, time is 1730862541.27 2024-11-06T03:09:01Z- AHRS_M2 msg: 1730862541.65 epoch sec; seqNo:2748 2024-11-06T03:09:01Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.48 psi 2024-11-06T03:09:01Z- handleDepth()- Depth_Keller msg: 1730862541.68 epoch sec; seqNo:2926; depth 50.45 2024-11-06T03:09:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862541.69 epoch sec; seqNo:2926; 4061718.53 north; 594759.79 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:09:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862538.03 2024-11-06T03:09:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:01Z- waiting for 1730862542.03, time is 1730862541.69 2024-11-06T03:09:02Z- AHRS_M2 msg: 1730862542.05 epoch sec; seqNo:2749 2024-11-06T03:09:02Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.48 psi 2024-11-06T03:09:02Z- handleDepth()- Depth_Keller msg: 1730862542.06 epoch sec; seqNo:2927; depth 50.46 2024-11-06T03:09:02Z- handleDVL() - DVL msg epochMillisec: 1730862542.06 2024-11-06T03:09:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:09:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:09:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:09:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:09:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:09:02Z- handleDvl() - RDI_Pathfinder msg: 1730862542.06 sec; seqNo:986; #beams:0 2024-11-06T03:09:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:09:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , 0.01 2024-11-06T03:09:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862542.09 epoch sec; seqNo:2927; 4061718.53 north; 594759.79 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:09:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862538.03 2024-11-06T03:09:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:02Z- waiting for 1730862542.03, time is 1730862542.09 2024-11-06T03:09:02Z- LcmTrn::updateTrn() >>>> pose time 1730862542.09, meas time 1730862542.06 <<<< 2024-11-06T03:09:02Z- LcmTrn::updateTrn() equating pose 1730862542.06 and meas times 1730862542.06 2024-11-06T03:09:02Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862542.06 2024-11-06T03:09:02Z- TerrainNav::motionUpdate() - currEstimate = (4061719, 594759.79, 50.46) 2024-11-06T03:09:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:09:02Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:09:02Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:09:02Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:09:02Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:09:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:09:02Z- variances([0], [2], and psi_berg) = 0.739746 0.415818 0.000000 2024-11-06T03:09:02Z- publishEstimatesDecoder() - at 1730862542.06 sec 2024-11-06T03:09:02Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:09:02Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061749.88 594743.36 59.75 2024-11-06T03:09:02Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061751.68 594742.93 57.89 lat/lon: 36.70 -121.94 2024-11-06T03:09:02Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.74 0.42 1.72 0.00 2024-11-06T03:09:02Z- AHRS_M2 msg: 1730862542.45 epoch sec; seqNo:2750 2024-11-06T03:09:02Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.48 psi 2024-11-06T03:09:02Z- handleDepth()- Depth_Keller msg: 1730862542.46 epoch sec; seqNo:2928; depth 50.45 2024-11-06T03:09:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862542.47 epoch sec; seqNo:2928; 4061718.53 north; 594759.79 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:09:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862542.06 2024-11-06T03:09:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:02Z- waiting for 1730862546.06, time is 1730862542.47 2024-11-06T03:09:02Z- handleDepth()- Depth_Keller msg: 1730862542.88 epoch sec; seqNo:2929; depth 50.45 2024-11-06T03:09:02Z- handleDVL() - DVL msg epochMillisec: 1730862542.90 2024-11-06T03:09:02Z- handleDvl() - RDI_Pathfinder msg: 1730862542.90 sec; seqNo:987; #beams:4 2024-11-06T03:09:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.58 , 32.28 , 35.33 , 35.84 2024-11-06T03:09:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.04 2024-11-06T03:09:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862542.91 epoch sec; seqNo:2929; 4061718.55 north; 594761.01 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:09:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862542.06 2024-11-06T03:09:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:03Z- waiting for 1730862546.06, time is 1730862542.91 2024-11-06T03:09:03Z- AHRS_M2 msg: 1730862543.26 epoch sec; seqNo:2751 2024-11-06T03:09:03Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:03Z- handleDepth()- Depth_Keller msg: 1730862543.27 epoch sec; seqNo:2930; depth 50.44 2024-11-06T03:09:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862543.29 epoch sec; seqNo:2930; 4061718.55 north; 594761.01 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:09:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862542.06 2024-11-06T03:09:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:03Z- waiting for 1730862546.06, time is 1730862543.29 2024-11-06T03:09:03Z- AHRS_M2 msg: 1730862543.67 epoch sec; seqNo:2752 2024-11-06T03:09:03Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:03Z- handleDepth()- Depth_Keller msg: 1730862543.68 epoch sec; seqNo:2931; depth 50.45 2024-11-06T03:09:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862543.69 epoch sec; seqNo:2931; 4061718.55 north; 594761.01 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:09:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862542.06 2024-11-06T03:09:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:03Z- waiting for 1730862546.06, time is 1730862543.69 2024-11-06T03:09:04Z- AHRS_M2 msg: 1730862544.08 epoch sec; seqNo:2753 2024-11-06T03:09:04Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:04Z- handleDepth()- Depth_Keller msg: 1730862544.08 epoch sec; seqNo:2932; depth 50.44 2024-11-06T03:09:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862544.09 epoch sec; seqNo:2932; 4061718.94 north; 594762.22 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:09:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862542.06 2024-11-06T03:09:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:04Z- waiting for 1730862546.06, time is 1730862544.09 2024-11-06T03:09:04Z- AHRS_M2 msg: 1730862544.47 epoch sec; seqNo:2754 2024-11-06T03:09:04Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:04Z- handleDepth()- Depth_Keller msg: 1730862544.49 epoch sec; seqNo:2933; depth 50.43 2024-11-06T03:09:04Z- handleDVL() - DVL msg epochMillisec: 1730862544.49 2024-11-06T03:09:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:09:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:09:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:09:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:09:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:09:04Z- handleDvl() - RDI_Pathfinder msg: 1730862544.49 sec; seqNo:988; #beams:0 2024-11-06T03:09:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:09:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.04 2024-11-06T03:09:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862544.50 epoch sec; seqNo:2933; 4061718.94 north; 594762.22 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:09:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862542.06 2024-11-06T03:09:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:04Z- waiting for 1730862546.06, time is 1730862544.50 2024-11-06T03:09:04Z- AHRS_M2 msg: 1730862544.88 epoch sec; seqNo:2755 2024-11-06T03:09:04Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.49 psi 2024-11-06T03:09:04Z- handleDepth()- Depth_Keller msg: 1730862544.89 epoch sec; seqNo:2934; depth 50.43 2024-11-06T03:09:04Z- handleDVL() - DVL msg epochMillisec: 1730862544.92 2024-11-06T03:09:04Z- handleDvl() - RDI_Pathfinder msg: 1730862544.92 sec; seqNo:989; #beams:4 2024-11-06T03:09:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.92 , 32.44 , 35.84 , 35.50 2024-11-06T03:09:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.02 , 0.01 2024-11-06T03:09:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862544.93 epoch sec; seqNo:2934; 4061718.94 north; 594762.22 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:09:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862542.06 2024-11-06T03:09:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:05Z- waiting for 1730862546.06, time is 1730862544.93 2024-11-06T03:09:05Z- AHRS_M2 msg: 1730862545.29 epoch sec; seqNo:2756 2024-11-06T03:09:05Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.49 psi 2024-11-06T03:09:05Z- handleDepth()- Depth_Keller msg: 1730862545.30 epoch sec; seqNo:2935; depth 50.42 2024-11-06T03:09:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862545.31 epoch sec; seqNo:2935; 4061718.94 north; 594762.22 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:09:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862542.06 2024-11-06T03:09:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:05Z- waiting for 1730862546.06, time is 1730862545.31 2024-11-06T03:09:05Z- AHRS_M2 msg: 1730862545.69 epoch sec; seqNo:2757 2024-11-06T03:09:05Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.49 psi 2024-11-06T03:09:05Z- handleDepth()- Depth_Keller msg: 1730862545.70 epoch sec; seqNo:2936; depth 50.41 2024-11-06T03:09:05Z- handleDVL() - DVL msg epochMillisec: 1730862545.70 2024-11-06T03:09:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:09:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:09:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:09:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:09:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:09:05Z- handleDvl() - RDI_Pathfinder msg: 1730862545.70 sec; seqNo:990; #beams:0 2024-11-06T03:09:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:09:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.02 , 0.01 2024-11-06T03:09:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862545.71 epoch sec; seqNo:2936; 4061718.95 north; 594763.44 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:09:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862542.06 2024-11-06T03:09:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:05Z- waiting for 1730862546.06, time is 1730862545.71 2024-11-06T03:09:06Z- AHRS_M2 msg: 1730862546.09 epoch sec; seqNo:2758 2024-11-06T03:09:06Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.49 psi 2024-11-06T03:09:06Z- handleDepth()- Depth_Keller msg: 1730862546.10 epoch sec; seqNo:2937; depth 50.39 2024-11-06T03:09:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862546.11 epoch sec; seqNo:2937; 4061718.95 north; 594763.44 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:09:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862542.06 2024-11-06T03:09:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:06Z- waiting for 1730862546.06, time is 1730862546.11 2024-11-06T03:09:06Z- LcmTrn::updateTrn() >>>> pose time 1730862546.11, meas time 1730862545.70 <<<< 2024-11-06T03:09:06Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862546.11 2024-11-06T03:09:06Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:09:06Z- TerrainNav::motionUpdate() - currEstimate = (4061719, 594763.44, 50.39) 2024-11-06T03:09:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:09:06Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:09:06Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:09:06Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:09:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:09:06Z- variances([0], [2], and psi_berg) = 0.741484 0.417771 0.000000 2024-11-06T03:09:06Z- publishEstimatesDecoder() - at 1730862546.11 sec 2024-11-06T03:09:06Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:09:06Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061750.27 594746.98 59.75 2024-11-06T03:09:06Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061752.10 594746.59 57.83 lat/lon: 36.70 -121.94 2024-11-06T03:09:06Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.74 0.42 1.97 0.00 2024-11-06T03:09:06Z- AHRS_M2 msg: 1730862546.49 epoch sec; seqNo:2759 2024-11-06T03:09:06Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.50 psi 2024-11-06T03:09:06Z- handleDepth()- Depth_Keller msg: 1730862546.51 epoch sec; seqNo:2938; depth 50.38 2024-11-06T03:09:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862546.52 epoch sec; seqNo:2938; 4061718.95 north; 594763.44 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:09:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862546.11 2024-11-06T03:09:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:06Z- waiting for 1730862550.11, time is 1730862546.52 2024-11-06T03:09:06Z- AHRS_M2 msg: 1730862546.90 epoch sec; seqNo:2760 2024-11-06T03:09:06Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.50 psi 2024-11-06T03:09:06Z- handleDepth()- Depth_Keller msg: 1730862546.91 epoch sec; seqNo:2939; depth 50.36 2024-11-06T03:09:06Z- handleDVL() - DVL msg epochMillisec: 1730862546.94 2024-11-06T03:09:06Z- handleDvl() - RDI_Pathfinder msg: 1730862546.94 sec; seqNo:991; #beams:4 2024-11-06T03:09:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.43 , 31.94 , 36.18 , 35.16 2024-11-06T03:09:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , -0.03 2024-11-06T03:09:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862546.97 epoch sec; seqNo:2939; 4061718.96 north; 594764.66 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:09:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862546.11 2024-11-06T03:09:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:07Z- waiting for 1730862550.11, time is 1730862546.97 2024-11-06T03:09:07Z- AHRS_M2 msg: 1730862547.30 epoch sec; seqNo:2761 2024-11-06T03:09:07Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.50 psi 2024-11-06T03:09:07Z- handleDepth()- Depth_Keller msg: 1730862547.31 epoch sec; seqNo:2940; depth 50.34 2024-11-06T03:09:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862547.33 epoch sec; seqNo:2940; 4061718.96 north; 594764.66 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:09:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862546.11 2024-11-06T03:09:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:07Z- waiting for 1730862550.11, time is 1730862547.33 2024-11-06T03:09:07Z- AHRS_M2 msg: 1730862547.71 epoch sec; seqNo:2762 2024-11-06T03:09:07Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.50 psi 2024-11-06T03:09:07Z- handleDepth()- Depth_Keller msg: 1730862547.72 epoch sec; seqNo:2941; depth 50.32 2024-11-06T03:09:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862547.73 epoch sec; seqNo:2941; 4061718.96 north; 594764.66 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:09:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862546.11 2024-11-06T03:09:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:07Z- waiting for 1730862550.11, time is 1730862547.73 2024-11-06T03:09:08Z- AHRS_M2 msg: 1730862548.11 epoch sec; seqNo:2763 2024-11-06T03:09:08Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.50 psi 2024-11-06T03:09:08Z- handleDepth()- Depth_Keller msg: 1730862548.12 epoch sec; seqNo:2942; depth 50.29 2024-11-06T03:09:08Z- handleDVL() - DVL msg epochMillisec: 1730862548.12 2024-11-06T03:09:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:09:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:09:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:09:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:09:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:09:08Z- handleDvl() - RDI_Pathfinder msg: 1730862548.12 sec; seqNo:992; #beams:0 2024-11-06T03:09:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:09:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , -0.03 2024-11-06T03:09:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862548.13 epoch sec; seqNo:2942; 4061718.98 north; 594765.88 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:09:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862546.11 2024-11-06T03:09:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:08Z- waiting for 1730862550.11, time is 1730862548.13 2024-11-06T03:09:08Z- AHRS_M2 msg: 1730862548.52 epoch sec; seqNo:2764 2024-11-06T03:09:08Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.50 psi 2024-11-06T03:09:08Z- handleDepth()- Depth_Keller msg: 1730862548.53 epoch sec; seqNo:2943; depth 50.28 2024-11-06T03:09:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862548.54 epoch sec; seqNo:2943; 4061718.98 north; 594765.88 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:09:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862546.11 2024-11-06T03:09:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:08Z- waiting for 1730862550.11, time is 1730862548.54 2024-11-06T03:09:08Z- AHRS_M2 msg: 1730862548.92 epoch sec; seqNo:2765 2024-11-06T03:09:08Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.50 psi 2024-11-06T03:09:08Z- handleDepth()- Depth_Keller msg: 1730862548.93 epoch sec; seqNo:2944; depth 50.25 2024-11-06T03:09:08Z- handleDVL() - DVL msg epochMillisec: 1730862548.96 2024-11-06T03:09:08Z- handleDvl() - RDI_Pathfinder msg: 1730862548.96 sec; seqNo:993; #beams:4 2024-11-06T03:09:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.92 , 31.09 , 36.18 , 34.99 2024-11-06T03:09:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , -0.05 2024-11-06T03:09:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862548.97 epoch sec; seqNo:2944; 4061718.98 north; 594765.88 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:09:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862546.11 2024-11-06T03:09:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:09Z- waiting for 1730862550.11, time is 1730862548.97 2024-11-06T03:09:09Z- AHRS_M2 msg: 1730862549.32 epoch sec; seqNo:2766 2024-11-06T03:09:09Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.50 psi 2024-11-06T03:09:09Z- handleDepth()- Depth_Keller msg: 1730862549.33 epoch sec; seqNo:2945; depth 50.24 2024-11-06T03:09:09Z- handleDVL() - DVL msg epochMillisec: 1730862549.33 2024-11-06T03:09:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:09:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:09:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:09:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:09:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:09:09Z- handleDvl() - RDI_Pathfinder msg: 1730862549.33 sec; seqNo:994; #beams:0 2024-11-06T03:09:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:09:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , -0.05 2024-11-06T03:09:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862549.35 epoch sec; seqNo:2945; 4061718.98 north; 594765.88 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:09:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862546.11 2024-11-06T03:09:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:09Z- waiting for 1730862550.11, time is 1730862549.35 2024-11-06T03:09:09Z- handleDepth()- Depth_Keller msg: 1730862549.75 epoch sec; seqNo:2946; depth 50.22 2024-11-06T03:09:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862549.76 epoch sec; seqNo:2946; 4061719.37 north; 594767.10 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:09:09Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:09:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862546.11 2024-11-06T03:09:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:09Z- waiting for 1730862550.11, time is 1730862549.76 2024-11-06T03:09:10Z- AHRS_M2 msg: 1730862550.13 epoch sec; seqNo:2767 2024-11-06T03:09:10Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.50 psi 2024-11-06T03:09:10Z- handleDepth()- Depth_Keller msg: 1730862550.14 epoch sec; seqNo:2947; depth 50.20 2024-11-06T03:09:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862550.15 epoch sec; seqNo:2947; 4061719.37 north; 594767.10 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:09:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862546.11 2024-11-06T03:09:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:10Z- waiting for 1730862550.11, time is 1730862550.15 2024-11-06T03:09:10Z- LcmTrn::updateTrn() >>>> pose time 1730862550.15, meas time 1730862549.33 <<<< 2024-11-06T03:09:10Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862550.15 2024-11-06T03:09:10Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:09:10Z- TerrainNav::motionUpdate() - currEstimate = (4061719, 594767.10, 50.20) 2024-11-06T03:09:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:09:10Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:09:10Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:09:10Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:09:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:09:10Z- variances([0], [2], and psi_berg) = 0.745055 0.419393 0.000000 2024-11-06T03:09:10Z- publishEstimatesDecoder() - at 1730862550.15 sec 2024-11-06T03:09:10Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:09:10Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061750.70 594750.69 59.50 2024-11-06T03:09:10Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061752.52 594750.24 57.64 lat/lon: 36.70 -121.94 2024-11-06T03:09:10Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.75 0.42 2.20 0.00 2024-11-06T03:09:10Z- AHRS_M2 msg: 1730862550.53 epoch sec; seqNo:2768 2024-11-06T03:09:10Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.50 psi 2024-11-06T03:09:10Z- handleDepth()- Depth_Keller msg: 1730862550.55 epoch sec; seqNo:2948; depth 50.20 2024-11-06T03:09:10Z- handleDVL() - DVL msg epochMillisec: 1730862550.55 2024-11-06T03:09:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:09:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:09:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:09:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:09:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:09:10Z- handleDvl() - RDI_Pathfinder msg: 1730862550.55 sec; seqNo:995; #beams:0 2024-11-06T03:09:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:09:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , -0.05 2024-11-06T03:09:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862550.56 epoch sec; seqNo:2948; 4061719.37 north; 594767.10 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:09:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862550.15 2024-11-06T03:09:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:10Z- waiting for 1730862554.15, time is 1730862550.56 2024-11-06T03:09:10Z- AHRS_M2 msg: 1730862550.94 epoch sec; seqNo:2769 2024-11-06T03:09:10Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.50 psi 2024-11-06T03:09:10Z- handleDepth()- Depth_Keller msg: 1730862550.95 epoch sec; seqNo:2949; depth 50.17 2024-11-06T03:09:10Z- handleDVL() - DVL msg epochMillisec: 1730862550.97 2024-11-06T03:09:10Z- handleDvl() - RDI_Pathfinder msg: 1730862550.97 sec; seqNo:996; #beams:4 2024-11-06T03:09:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.41 , 30.41 , 36.01 , 34.82 2024-11-06T03:09:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.01 , -0.03 2024-11-06T03:09:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862551.00 epoch sec; seqNo:2949; 4061719.38 north; 594768.32 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:09:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862550.15 2024-11-06T03:09:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:11Z- waiting for 1730862554.15, time is 1730862551.00 2024-11-06T03:09:11Z- AHRS_M2 msg: 1730862551.34 epoch sec; seqNo:2770 2024-11-06T03:09:11Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.50 psi 2024-11-06T03:09:11Z- handleDepth()- Depth_Keller msg: 1730862551.35 epoch sec; seqNo:2950; depth 50.17 2024-11-06T03:09:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862551.37 epoch sec; seqNo:2950; 4061719.38 north; 594768.32 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:09:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862550.15 2024-11-06T03:09:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:11Z- waiting for 1730862554.15, time is 1730862551.37 2024-11-06T03:09:11Z- AHRS_M2 msg: 1730862551.75 epoch sec; seqNo:2771 2024-11-06T03:09:11Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.50 psi 2024-11-06T03:09:11Z- handleDepth()- Depth_Keller msg: 1730862551.76 epoch sec; seqNo:2951; depth 50.15 2024-11-06T03:09:11Z- handleDVL() - DVL msg epochMillisec: 1730862551.76 2024-11-06T03:09:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:09:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:09:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:09:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:09:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:09:11Z- handleDvl() - RDI_Pathfinder msg: 1730862551.76 sec; seqNo:997; #beams:0 2024-11-06T03:09:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:09:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.01 , -0.03 2024-11-06T03:09:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862551.78 epoch sec; seqNo:2951; 4061719.38 north; 594768.32 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:09:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862550.15 2024-11-06T03:09:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:11Z- waiting for 1730862554.15, time is 1730862551.78 2024-11-06T03:09:12Z- AHRS_M2 msg: 1730862552.15 epoch sec; seqNo:2772 2024-11-06T03:09:12Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:09:12Z- handleDepth()- Depth_Keller msg: 1730862552.16 epoch sec; seqNo:2952; depth 50.16 2024-11-06T03:09:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862552.17 epoch sec; seqNo:2952; 4061719.38 north; 594768.32 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:09:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862550.15 2024-11-06T03:09:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:12Z- waiting for 1730862554.15, time is 1730862552.17 2024-11-06T03:09:12Z- AHRS_M2 msg: 1730862552.57 epoch sec; seqNo:2773 2024-11-06T03:09:12Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:09:12Z- handleDepth()- Depth_Keller msg: 1730862552.58 epoch sec; seqNo:2953; depth 50.16 2024-11-06T03:09:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862552.58 epoch sec; seqNo:2953; 4061719.40 north; 594769.54 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:09:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862550.15 2024-11-06T03:09:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:12Z- waiting for 1730862554.15, time is 1730862552.58 2024-11-06T03:09:12Z- AHRS_M2 msg: 1730862552.97 epoch sec; seqNo:2774 2024-11-06T03:09:12Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.49 psi 2024-11-06T03:09:12Z- handleDepth()- Depth_Keller msg: 1730862552.97 epoch sec; seqNo:2954; depth 50.16 2024-11-06T03:09:13Z- handleDVL() - DVL msg epochMillisec: 1730862552.98 2024-11-06T03:09:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:09:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:09:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:09:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:09:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:09:13Z- handleDvl() - RDI_Pathfinder msg: 1730862552.98 sec; seqNo:998; #beams:0 2024-11-06T03:09:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:09:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.01 , -0.03 2024-11-06T03:09:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862553.02 epoch sec; seqNo:2954; 4061719.40 north; 594769.54 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:09:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862550.15 2024-11-06T03:09:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:13Z- waiting for 1730862554.15, time is 1730862553.02 2024-11-06T03:09:13Z- AHRS_M2 msg: 1730862553.36 epoch sec; seqNo:2775 2024-11-06T03:09:13Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.49 psi 2024-11-06T03:09:13Z- handleDepth()- Depth_Keller msg: 1730862553.37 epoch sec; seqNo:2955; depth 50.17 2024-11-06T03:09:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862553.38 epoch sec; seqNo:2955; 4061719.40 north; 594769.54 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:09:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862550.15 2024-11-06T03:09:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:13Z- waiting for 1730862554.15, time is 1730862553.38 2024-11-06T03:09:13Z- AHRS_M2 msg: 1730862553.78 epoch sec; seqNo:2776 2024-11-06T03:09:13Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.49 psi 2024-11-06T03:09:13Z- handleDepth()- Depth_Keller msg: 1730862553.78 epoch sec; seqNo:2956; depth 50.14 2024-11-06T03:09:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862553.80 epoch sec; seqNo:2956; 4061719.40 north; 594769.54 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:09:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862550.15 2024-11-06T03:09:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:13Z- waiting for 1730862554.15, time is 1730862553.80 2024-11-06T03:09:14Z- AHRS_M2 msg: 1730862554.17 epoch sec; seqNo:2777 2024-11-06T03:09:14Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.49 psi 2024-11-06T03:09:14Z- handleDepth()- Depth_Keller msg: 1730862554.18 epoch sec; seqNo:2957; depth 50.15 2024-11-06T03:09:14Z- handleDVL() - DVL msg epochMillisec: 1730862554.18 2024-11-06T03:09:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:09:14Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:09:14Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:09:14Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:09:14Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:09:14Z- handleDvl() - RDI_Pathfinder msg: 1730862554.18 sec; seqNo:999; #beams:0 2024-11-06T03:09:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:09:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.01 , -0.03 2024-11-06T03:09:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862554.20 epoch sec; seqNo:2957; 4061719.41 north; 594770.76 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:09:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862550.15 2024-11-06T03:09:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:14Z- waiting for 1730862554.15, time is 1730862554.20 2024-11-06T03:09:14Z- LcmTrn::updateTrn() >>>> pose time 1730862554.20, meas time 1730862554.18 <<<< 2024-11-06T03:09:14Z- LcmTrn::updateTrn() equating pose 1730862554.18 and meas times 1730862554.18 2024-11-06T03:09:14Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862554.18 2024-11-06T03:09:14Z- TerrainNav::motionUpdate() - currEstimate = (4061719, 594770.76, 50.15) 2024-11-06T03:09:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:09:14Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:09:14Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:09:14Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:09:14Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:09:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:09:14Z- variances([0], [2], and psi_berg) = 0.747209 0.421747 0.000000 2024-11-06T03:09:14Z- publishEstimatesDecoder() - at 1730862554.18 sec 2024-11-06T03:09:14Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:09:14Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061750.79 594754.33 59.75 2024-11-06T03:09:14Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061752.56 594753.90 57.59 lat/lon: 36.70 -121.94 2024-11-06T03:09:14Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.75 0.42 2.45 0.00 2024-11-06T03:09:14Z- AHRS_M2 msg: 1730862554.58 epoch sec; seqNo:2778 2024-11-06T03:09:14Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.49 psi 2024-11-06T03:09:14Z- handleDepth()- Depth_Keller msg: 1730862554.59 epoch sec; seqNo:2958; depth 50.16 2024-11-06T03:09:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862554.60 epoch sec; seqNo:2958; 4061719.41 north; 594770.76 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:09:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862554.18 2024-11-06T03:09:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:14Z- waiting for 1730862558.18, time is 1730862554.60 2024-11-06T03:09:14Z- AHRS_M2 msg: 1730862554.99 epoch sec; seqNo:2779 2024-11-06T03:09:14Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:15Z- handleDepth()- Depth_Keller msg: 1730862554.99 epoch sec; seqNo:2959; depth 50.17 2024-11-06T03:09:15Z- handleDVL() - DVL msg epochMillisec: 1730862555.02 2024-11-06T03:09:15Z- handleDvl() - RDI_Pathfinder msg: 1730862555.02 sec; seqNo:1000; #beams:4 2024-11-06T03:09:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.90 , 30.75 , 35.67 , 34.65 2024-11-06T03:09:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , 0.05 2024-11-06T03:09:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862555.03 epoch sec; seqNo:2959; 4061719.41 north; 594770.76 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:09:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862554.18 2024-11-06T03:09:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:15Z- waiting for 1730862558.18, time is 1730862555.03 2024-11-06T03:09:15Z- AHRS_M2 msg: 1730862555.39 epoch sec; seqNo:2780 2024-11-06T03:09:15Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:15Z- handleDepth()- Depth_Keller msg: 1730862555.40 epoch sec; seqNo:2960; depth 50.17 2024-11-06T03:09:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862555.42 epoch sec; seqNo:2960; 4061719.79 north; 594770.76 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:09:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862554.18 2024-11-06T03:09:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:15Z- waiting for 1730862558.18, time is 1730862555.42 2024-11-06T03:09:15Z- AHRS_M2 msg: 1730862555.79 epoch sec; seqNo:2781 2024-11-06T03:09:15Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:15Z- handleDepth()- Depth_Keller msg: 1730862555.80 epoch sec; seqNo:2961; depth 50.18 2024-11-06T03:09:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862555.82 epoch sec; seqNo:2961; 4061719.80 north; 594771.98 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:09:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862554.18 2024-11-06T03:09:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:15Z- waiting for 1730862558.18, time is 1730862555.82 2024-11-06T03:09:16Z- AHRS_M2 msg: 1730862556.19 epoch sec; seqNo:2782 2024-11-06T03:09:16Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:16Z- handleDepth()- Depth_Keller msg: 1730862556.21 epoch sec; seqNo:2962; depth 50.18 2024-11-06T03:09:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862556.21 epoch sec; seqNo:2962; 4061719.80 north; 594771.98 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:09:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862554.18 2024-11-06T03:09:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:16Z- waiting for 1730862558.18, time is 1730862556.21 2024-11-06T03:09:16Z- AHRS_M2 msg: 1730862556.60 epoch sec; seqNo:2783 2024-11-06T03:09:16Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:16Z- handleDepth()- Depth_Keller msg: 1730862556.60 epoch sec; seqNo:2963; depth 50.20 2024-11-06T03:09:16Z- handleDVL() - DVL msg epochMillisec: 1730862556.61 2024-11-06T03:09:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:09:16Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:09:16Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:09:16Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:09:16Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:09:16Z- handleDvl() - RDI_Pathfinder msg: 1730862556.61 sec; seqNo:1001; #beams:0 2024-11-06T03:09:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:09:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , 0.05 2024-11-06T03:09:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862556.62 epoch sec; seqNo:2963; 4061719.80 north; 594771.98 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:09:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862554.18 2024-11-06T03:09:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:16Z- waiting for 1730862558.18, time is 1730862556.62 2024-11-06T03:09:17Z- AHRS_M2 msg: 1730862557.00 epoch sec; seqNo:2784 2024-11-06T03:09:17Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:17Z- handleDepth()- Depth_Keller msg: 1730862557.01 epoch sec; seqNo:2964; depth 50.20 2024-11-06T03:09:17Z- handleDVL() - DVL msg epochMillisec: 1730862557.05 2024-11-06T03:09:17Z- handleDvl() - RDI_Pathfinder msg: 1730862557.05 sec; seqNo:1002; #beams:4 2024-11-06T03:09:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.73 , 30.92 , 35.33 , 34.14 2024-11-06T03:09:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , 0.06 2024-11-06T03:09:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862557.07 epoch sec; seqNo:2964; 4061719.82 north; 594773.20 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:09:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862554.18 2024-11-06T03:09:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:17Z- waiting for 1730862558.18, time is 1730862557.07 2024-11-06T03:09:17Z- AHRS_M2 msg: 1730862557.42 epoch sec; seqNo:2785 2024-11-06T03:09:17Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:17Z- handleDepth()- Depth_Keller msg: 1730862557.42 epoch sec; seqNo:2965; depth 50.18 2024-11-06T03:09:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862557.44 epoch sec; seqNo:2965; 4061719.82 north; 594773.20 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:09:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862554.18 2024-11-06T03:09:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:17Z- waiting for 1730862558.18, time is 1730862557.44 2024-11-06T03:09:17Z- AHRS_M2 msg: 1730862557.81 epoch sec; seqNo:2786 2024-11-06T03:09:17Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:17Z- handleDepth()- Depth_Keller msg: 1730862557.82 epoch sec; seqNo:2966; depth 50.20 2024-11-06T03:09:17Z- handleDVL() - DVL msg epochMillisec: 1730862557.82 2024-11-06T03:09:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:09:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:09:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:09:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:09:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:09:17Z- handleDvl() - RDI_Pathfinder msg: 1730862557.82 sec; seqNo:1003; #beams:0 2024-11-06T03:09:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:09:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , 0.06 2024-11-06T03:09:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862557.83 epoch sec; seqNo:2966; 4061719.82 north; 594773.20 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:09:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862554.18 2024-11-06T03:09:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:17Z- waiting for 1730862558.18, time is 1730862557.83 2024-11-06T03:09:18Z- AHRS_M2 msg: 1730862558.22 epoch sec; seqNo:2787 2024-11-06T03:09:18Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:18Z- handleDepth()- Depth_Keller msg: 1730862558.22 epoch sec; seqNo:2967; depth 50.19 2024-11-06T03:09:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862558.23 epoch sec; seqNo:2967; 4061719.82 north; 594773.20 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:09:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862554.18 2024-11-06T03:09:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:18Z- waiting for 1730862558.18, time is 1730862558.23 2024-11-06T03:09:18Z- LcmTrn::updateTrn() >>>> pose time 1730862558.23, meas time 1730862557.82 <<<< 2024-11-06T03:09:18Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862558.23 2024-11-06T03:09:18Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:09:18Z- TerrainNav::motionUpdate() - currEstimate = (4061720, 594773.20, 50.19) 2024-11-06T03:09:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:09:18Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:09:18Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:09:18Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:09:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:09:18Z- variances([0], [2], and psi_berg) = 0.748317 0.423151 0.000000 2024-11-06T03:09:18Z- publishEstimatesDecoder() - at 1730862558.23 sec 2024-11-06T03:09:18Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:09:18Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061751.11 594756.78 59.54 2024-11-06T03:09:18Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061752.96 594756.34 57.62 lat/lon: 36.70 -121.94 2024-11-06T03:09:18Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.75 0.42 2.68 0.00 2024-11-06T03:09:18Z- AHRS_M2 msg: 1730862558.62 epoch sec; seqNo:2788 2024-11-06T03:09:18Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:18Z- handleDepth()- Depth_Keller msg: 1730862558.63 epoch sec; seqNo:2968; depth 50.18 2024-11-06T03:09:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862558.65 epoch sec; seqNo:2968; 4061719.83 north; 594774.42 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:09:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862558.23 2024-11-06T03:09:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:18Z- waiting for 1730862562.23, time is 1730862558.65 2024-11-06T03:09:19Z- handleDepth()- Depth_Keller msg: 1730862559.05 epoch sec; seqNo:2969; depth 50.18 2024-11-06T03:09:19Z- handleDVL() - DVL msg epochMillisec: 1730862559.08 2024-11-06T03:09:19Z- handleDvl() - RDI_Pathfinder msg: 1730862559.08 sec; seqNo:1004; #beams:4 2024-11-06T03:09:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.90 , 30.75 , 35.50 , 33.97 2024-11-06T03:09:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , 0.03 2024-11-06T03:09:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862559.10 epoch sec; seqNo:2969; 4061720.21 north; 594774.41 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:09:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862558.23 2024-11-06T03:09:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:19Z- waiting for 1730862562.23, time is 1730862559.10 2024-11-06T03:09:19Z- AHRS_M2 msg: 1730862559.43 epoch sec; seqNo:2789 2024-11-06T03:09:19Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:19Z- handleDepth()- Depth_Keller msg: 1730862559.43 epoch sec; seqNo:2970; depth 50.17 2024-11-06T03:09:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862559.46 epoch sec; seqNo:2970; 4061720.21 north; 594774.41 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:09:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862558.23 2024-11-06T03:09:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:19Z- waiting for 1730862562.23, time is 1730862559.46 2024-11-06T03:09:19Z- AHRS_M2 msg: 1730862559.83 epoch sec; seqNo:2790 2024-11-06T03:09:19Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:19Z- handleDepth()- Depth_Keller msg: 1730862559.84 epoch sec; seqNo:2971; depth 50.17 2024-11-06T03:09:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862559.84 epoch sec; seqNo:2971; 4061720.22 north; 594775.63 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:09:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862558.23 2024-11-06T03:09:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:19Z- waiting for 1730862562.23, time is 1730862559.84 2024-11-06T03:09:20Z- AHRS_M2 msg: 1730862560.24 epoch sec; seqNo:2791 2024-11-06T03:09:20Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:20Z- handleDepth()- Depth_Keller msg: 1730862560.24 epoch sec; seqNo:2972; depth 50.15 2024-11-06T03:09:20Z- handleDVL() - DVL msg epochMillisec: 1730862560.24 2024-11-06T03:09:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:09:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:09:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:09:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:09:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:09:20Z- handleDvl() - RDI_Pathfinder msg: 1730862560.24 sec; seqNo:1005; #beams:0 2024-11-06T03:09:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:09:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , 0.03 2024-11-06T03:09:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862560.25 epoch sec; seqNo:2972; 4061720.22 north; 594775.63 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:09:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862558.23 2024-11-06T03:09:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:20Z- waiting for 1730862562.23, time is 1730862560.25 2024-11-06T03:09:20Z- AHRS_M2 msg: 1730862560.64 epoch sec; seqNo:2792 2024-11-06T03:09:20Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:20Z- handleDepth()- Depth_Keller msg: 1730862560.66 epoch sec; seqNo:2973; depth 50.15 2024-11-06T03:09:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862560.66 epoch sec; seqNo:2973; 4061720.22 north; 594775.63 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:09:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862558.23 2024-11-06T03:09:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:20Z- waiting for 1730862562.23, time is 1730862560.66 2024-11-06T03:09:21Z- AHRS_M2 msg: 1730862561.04 epoch sec; seqNo:2793 2024-11-06T03:09:21Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:21Z- handleDepth()- Depth_Keller msg: 1730862561.05 epoch sec; seqNo:2974; depth 50.12 2024-11-06T03:09:21Z- handleDVL() - DVL msg epochMillisec: 1730862561.08 2024-11-06T03:09:21Z- handleDvl() - RDI_Pathfinder msg: 1730862561.08 sec; seqNo:1006; #beams:4 2024-11-06T03:09:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.24 , 30.58 , 35.84 , 33.12 2024-11-06T03:09:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.06 , -0.03 2024-11-06T03:09:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862561.10 epoch sec; seqNo:2974; 4061720.24 north; 594776.85 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:09:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862558.23 2024-11-06T03:09:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:21Z- waiting for 1730862562.23, time is 1730862561.10 2024-11-06T03:09:21Z- AHRS_M2 msg: 1730862561.45 epoch sec; seqNo:2794 2024-11-06T03:09:21Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:21Z- handleDepth()- Depth_Keller msg: 1730862561.46 epoch sec; seqNo:2975; depth 50.10 2024-11-06T03:09:21Z- handleDVL() - DVL msg epochMillisec: 1730862561.46 2024-11-06T03:09:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:09:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:09:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:09:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:09:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:09:21Z- handleDvl() - RDI_Pathfinder msg: 1730862561.46 sec; seqNo:1007; #beams:0 2024-11-06T03:09:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:09:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.06 , -0.03 2024-11-06T03:09:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862561.48 epoch sec; seqNo:2975; 4061720.24 north; 594776.85 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:09:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862558.23 2024-11-06T03:09:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:21Z- waiting for 1730862562.23, time is 1730862561.48 2024-11-06T03:09:21Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:09:21Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:09:21Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:09:21Z- AHRS_M2 msg: 1730862561.84 epoch sec; seqNo:2795 2024-11-06T03:09:21Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:21Z- handleDepth()- Depth_Keller msg: 1730862561.87 epoch sec; seqNo:2976; depth 50.10 2024-11-06T03:09:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862561.88 epoch sec; seqNo:2976; 4061720.24 north; 594776.85 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:09:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862558.23 2024-11-06T03:09:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:22Z- waiting for 1730862562.23, time is 1730862561.88 2024-11-06T03:09:22Z- AHRS_M2 msg: 1730862562.25 epoch sec; seqNo:2796 2024-11-06T03:09:22Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.49 psi 2024-11-06T03:09:22Z- handleDepth()- Depth_Keller msg: 1730862562.27 epoch sec; seqNo:2977; depth 50.06 2024-11-06T03:09:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862562.28 epoch sec; seqNo:2977; 4061720.25 north; 594778.07 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:09:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862558.23 2024-11-06T03:09:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:22Z- waiting for 1730862562.23, time is 1730862562.28 2024-11-06T03:09:22Z- LcmTrn::updateTrn() >>>> pose time 1730862562.28, meas time 1730862561.46 <<<< 2024-11-06T03:09:22Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862562.28 2024-11-06T03:09:22Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:09:22Z- TerrainNav::motionUpdate() - currEstimate = (4061720, 594778.07, 50.06) 2024-11-06T03:09:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:09:22Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:09:22Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:09:22Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:09:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:09:22Z- variances([0], [2], and psi_berg) = 0.753833 0.426811 0.000000 2024-11-06T03:09:22Z- publishEstimatesDecoder() - at 1730862562.28 sec 2024-11-06T03:09:22Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:09:22Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061751.54 594761.67 59.51 2024-11-06T03:09:22Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061753.40 594761.22 57.49 lat/lon: 36.70 -121.94 2024-11-06T03:09:22Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.75 0.43 2.92 0.00 2024-11-06T03:09:22Z- AHRS_M2 msg: 1730862562.66 epoch sec; seqNo:2797 2024-11-06T03:09:22Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.49 psi 2024-11-06T03:09:22Z- handleDepth()- Depth_Keller msg: 1730862562.68 epoch sec; seqNo:2978; depth 50.06 2024-11-06T03:09:22Z- handleDVL() - DVL msg epochMillisec: 1730862562.68 2024-11-06T03:09:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:09:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:09:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:09:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:09:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:09:22Z- handleDvl() - RDI_Pathfinder msg: 1730862562.68 sec; seqNo:1008; #beams:0 2024-11-06T03:09:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:09:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.06 , -0.03 2024-11-06T03:09:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862562.73 epoch sec; seqNo:2978; 4061720.25 north; 594778.07 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:09:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862562.28 2024-11-06T03:09:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:22Z- waiting for 1730862566.28, time is 1730862562.73 2024-11-06T03:09:23Z- AHRS_M2 msg: 1730862563.07 epoch sec; seqNo:2798 2024-11-06T03:09:23Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.48 psi 2024-11-06T03:09:23Z- handleDepth()- Depth_Keller msg: 1730862563.07 epoch sec; seqNo:2979; depth 50.04 2024-11-06T03:09:23Z- handleDVL() - DVL msg epochMillisec: 1730862563.10 2024-11-06T03:09:23Z- handleDvl() - RDI_Pathfinder msg: 1730862563.10 sec; seqNo:1009; #beams:4 2024-11-06T03:09:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.24 , 30.58 , 36.01 , 33.12 2024-11-06T03:09:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , -0.07 2024-11-06T03:09:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862563.11 epoch sec; seqNo:2979; 4061720.25 north; 594778.07 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:09:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862562.28 2024-11-06T03:09:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:23Z- waiting for 1730862566.28, time is 1730862563.11 2024-11-06T03:09:23Z- AHRS_M2 msg: 1730862563.47 epoch sec; seqNo:2799 2024-11-06T03:09:23Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.48 psi 2024-11-06T03:09:23Z- handleDepth()- Depth_Keller msg: 1730862563.49 epoch sec; seqNo:2980; depth 50.02 2024-11-06T03:09:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862563.50 epoch sec; seqNo:2980; 4061720.25 north; 594778.07 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:09:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862562.28 2024-11-06T03:09:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:23Z- waiting for 1730862566.28, time is 1730862563.50 2024-11-06T03:09:23Z- handleDepth()- Depth_Keller msg: 1730862563.90 epoch sec; seqNo:2981; depth 50.00 2024-11-06T03:09:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862563.91 epoch sec; seqNo:2981; 4061720.26 north; 594779.29 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:09:24Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:09:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862562.28 2024-11-06T03:09:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:24Z- waiting for 1730862566.28, time is 1730862563.91 2024-11-06T03:09:24Z- AHRS_M2 msg: 1730862564.28 epoch sec; seqNo:2800 2024-11-06T03:09:24Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.48 psi 2024-11-06T03:09:24Z- handleDepth()- Depth_Keller msg: 1730862564.28 epoch sec; seqNo:2982; depth 49.97 2024-11-06T03:09:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862564.29 epoch sec; seqNo:2982; 4061720.26 north; 594779.29 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:09:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862562.28 2024-11-06T03:09:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:24Z- waiting for 1730862566.28, time is 1730862564.29 2024-11-06T03:09:24Z- AHRS_M2 msg: 1730862564.68 epoch sec; seqNo:2801 2024-11-06T03:09:24Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.48 psi 2024-11-06T03:09:24Z- handleDepth()- Depth_Keller msg: 1730862564.69 epoch sec; seqNo:2983; depth 49.95 2024-11-06T03:09:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862564.72 epoch sec; seqNo:2983; 4061720.26 north; 594779.29 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:09:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862562.28 2024-11-06T03:09:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:24Z- waiting for 1730862566.28, time is 1730862564.72 2024-11-06T03:09:25Z- AHRS_M2 msg: 1730862565.08 epoch sec; seqNo:2802 2024-11-06T03:09:25Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.47 psi 2024-11-06T03:09:25Z- handleDepth()- Depth_Keller msg: 1730862565.09 epoch sec; seqNo:2984; depth 49.93 2024-11-06T03:09:25Z- handleDVL() - DVL msg epochMillisec: 1730862565.11 2024-11-06T03:09:25Z- handleDvl() - RDI_Pathfinder msg: 1730862565.11 sec; seqNo:1010; #beams:4 2024-11-06T03:09:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.90 , 30.41 , 36.01 , 33.29 2024-11-06T03:09:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.01 , -0.06 2024-11-06T03:09:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862565.13 epoch sec; seqNo:2984; 4061720.28 north; 594780.51 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:09:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862562.28 2024-11-06T03:09:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:25Z- waiting for 1730862566.28, time is 1730862565.13 2024-11-06T03:09:25Z- AHRS_M2 msg: 1730862565.49 epoch sec; seqNo:2803 2024-11-06T03:09:25Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.47 psi 2024-11-06T03:09:25Z- handleDepth()- Depth_Keller msg: 1730862565.49 epoch sec; seqNo:2985; depth 49.91 2024-11-06T03:09:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862565.52 epoch sec; seqNo:2985; 4061720.28 north; 594780.51 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:09:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862562.28 2024-11-06T03:09:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:25Z- waiting for 1730862566.28, time is 1730862565.52 2024-11-06T03:09:25Z- AHRS_M2 msg: 1730862565.89 epoch sec; seqNo:2804 2024-11-06T03:09:25Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.47 psi 2024-11-06T03:09:25Z- handleDepth()- Depth_Keller msg: 1730862565.89 epoch sec; seqNo:2986; depth 49.91 2024-11-06T03:09:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862565.90 epoch sec; seqNo:2986; 4061720.28 north; 594780.51 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:09:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862562.28 2024-11-06T03:09:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:26Z- waiting for 1730862566.28, time is 1730862565.90 2024-11-06T03:09:26Z- AHRS_M2 msg: 1730862566.30 epoch sec; seqNo:2805 2024-11-06T03:09:26Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.47 psi 2024-11-06T03:09:26Z- handleDepth()- Depth_Keller msg: 1730862566.30 epoch sec; seqNo:2987; depth 49.88 2024-11-06T03:09:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862566.31 epoch sec; seqNo:2987; 4061720.28 north; 594780.51 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:09:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862562.28 2024-11-06T03:09:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:26Z- waiting for 1730862566.28, time is 1730862566.31 2024-11-06T03:09:26Z- LcmTrn::updateTrn() >>>> pose time 1730862566.31, meas time 1730862565.11 <<<< 2024-11-06T03:09:26Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862566.31 2024-11-06T03:09:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862565.11 sec, ping # 0; waiting for INS data more recent than 1730862562.28... 2024-11-06T03:09:26Z- TerrainNav::motionUpdate() - currEstimate = (4061720, 594780.51, 49.88) 2024-11-06T03:09:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:09:26Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:09:26Z- TNavPF::Projecting Measurements 2024-11-06T03:09:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:09:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:09:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:09:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:09:26Z- TNavFilter::4 good beams. 2024-11-06T03:09:26Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:09:26Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:09:26Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:09:26Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:09:26Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:09:26Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:09:26Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:09:26Z- TerrainMapOctree: (vehN, vehE ) = (4061720.27, 594779.79). 2024-11-06T03:09:26Z- TerrainMapOctree: Min Distance = 42.09. 2024-11-06T03:09:26Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:09:26Z- Forcing Subcloud Comparison 2024-11-06T03:09:26Z- Weighting particles with subcloud comparison 2024-11-06T03:09:26Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:26Z- meanExpectedMeasDiff: 5.576886 delta_rms_squared: 5.063501 alpha: 0.996066 2024-11-06T03:09:26Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:26Z- meanExpectedMeasDiff: 3.057455 delta_rms_squared: 3.670586 alpha: 0.994581 2024-11-06T03:09:26Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:26Z- meanExpectedMeasDiff: 5.018559 delta_rms_squared: 4.466732 alpha: 0.995542 2024-11-06T03:09:26Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:26Z- meanExpectedMeasDiff: 2.875683 delta_rms_squared: 5.502289 alpha: 0.996378 2024-11-06T03:09:26Z- Subcloud NIS: 0.835951 2024-11-06T03:09:26Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:09:26Z- TNavPF:: sumWeights = 0.041700 2024-11-06T03:09:26Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:09:26Z- TNavPF::First Term of Map Covariance Matrix is 0.20 2024-11-06T03:09:26Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:09:26Z- TerrainNav::Resampling particle filter... 2024-11-06T03:09:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862565.11 sec. 2024-11-06T03:09:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:09:26Z- variances([0], [2], and psi_berg) = 0.830956 0.373983 0.000000 2024-11-06T03:09:26Z- publishEstimatesDecoder() - at 1730862566.31 sec 2024-11-06T03:09:26Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:09:26Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061752.57 594764.25 53.62 2024-11-06T03:09:26Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061753.38 594763.79 54.54 lat/lon: 36.70 -121.94 2024-11-06T03:09:26Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.83 0.37 0.75 0.00 2024-11-06T03:09:26Z- AHRS_M2 msg: 1730862566.73 epoch sec; seqNo:2806 2024-11-06T03:09:26Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.47 psi 2024-11-06T03:09:26Z- handleDepth()- Depth_Keller msg: 1730862566.76 epoch sec; seqNo:2988; depth 49.87 2024-11-06T03:09:26Z- handleDVL() - DVL msg epochMillisec: 1730862566.79 2024-11-06T03:09:26Z- handleDvl() - RDI_Pathfinder msg: 1730862566.79 sec; seqNo:1011; #beams:4 2024-11-06T03:09:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.05 , 30.41 , 35.67 , 31.77 2024-11-06T03:09:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.05 , -0.00 2024-11-06T03:09:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862566.80 epoch sec; seqNo:2988; 4061720.67 north; 594781.73 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:09:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862566.31 2024-11-06T03:09:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:26Z- waiting for 1730862570.31, time is 1730862566.80 2024-11-06T03:09:27Z- AHRS_M2 msg: 1730862567.11 epoch sec; seqNo:2807 2024-11-06T03:09:27Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:27Z- handleDepth()- Depth_Keller msg: 1730862567.12 epoch sec; seqNo:2989; depth 49.87 2024-11-06T03:09:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862567.13 epoch sec; seqNo:2989; 4061720.67 north; 594781.73 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:09:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862566.31 2024-11-06T03:09:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:27Z- waiting for 1730862570.31, time is 1730862567.13 2024-11-06T03:09:27Z- AHRS_M2 msg: 1730862567.51 epoch sec; seqNo:2808 2024-11-06T03:09:27Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:27Z- handleDepth()- Depth_Keller msg: 1730862567.51 epoch sec; seqNo:2990; depth 49.87 2024-11-06T03:09:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862567.53 epoch sec; seqNo:2990; 4061720.67 north; 594781.73 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:09:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862566.31 2024-11-06T03:09:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:27Z- waiting for 1730862570.31, time is 1730862567.53 2024-11-06T03:09:27Z- AHRS_M2 msg: 1730862567.91 epoch sec; seqNo:2809 2024-11-06T03:09:27Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:27Z- handleDepth()- Depth_Keller msg: 1730862567.92 epoch sec; seqNo:2991; depth 49.87 2024-11-06T03:09:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862567.94 epoch sec; seqNo:2991; 4061720.67 north; 594781.73 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:09:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862566.31 2024-11-06T03:09:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:28Z- waiting for 1730862570.31, time is 1730862567.94 2024-11-06T03:09:28Z- AHRS_M2 msg: 1730862568.31 epoch sec; seqNo:2810 2024-11-06T03:09:28Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:28Z- handleDepth()- Depth_Keller msg: 1730862568.32 epoch sec; seqNo:2992; depth 49.87 2024-11-06T03:09:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862568.33 epoch sec; seqNo:2992; 4061720.68 north; 594782.95 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:09:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862566.31 2024-11-06T03:09:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:28Z- waiting for 1730862570.31, time is 1730862568.33 2024-11-06T03:09:28Z- AHRS_M2 msg: 1730862568.75 epoch sec; seqNo:2811 2024-11-06T03:09:28Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:28Z- handleDepth()- Depth_Keller msg: 1730862568.77 epoch sec; seqNo:2993; depth 49.88 2024-11-06T03:09:28Z- handleDVL() - DVL msg epochMillisec: 1730862568.79 2024-11-06T03:09:28Z- handleDvl() - RDI_Pathfinder msg: 1730862568.79 sec; seqNo:1012; #beams:4 2024-11-06T03:09:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.61 , 30.58 , 35.93 , 32.26 2024-11-06T03:09:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.04 , 0.06 2024-11-06T03:09:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862568.80 epoch sec; seqNo:2993; 4061720.68 north; 594782.95 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:09:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862566.31 2024-11-06T03:09:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:28Z- waiting for 1730862570.31, time is 1730862568.80 2024-11-06T03:09:29Z- AHRS_M2 msg: 1730862569.13 epoch sec; seqNo:2812 2024-11-06T03:09:29Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:29Z- handleDepth()- Depth_Keller msg: 1730862569.14 epoch sec; seqNo:2994; depth 49.87 2024-11-06T03:09:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862569.15 epoch sec; seqNo:2994; 4061720.68 north; 594782.95 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:09:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862566.31 2024-11-06T03:09:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:29Z- waiting for 1730862570.31, time is 1730862569.15 2024-11-06T03:09:29Z- AHRS_M2 msg: 1730862569.53 epoch sec; seqNo:2813 2024-11-06T03:09:29Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.45 psi 2024-11-06T03:09:29Z- handleDepth()- Depth_Keller msg: 1730862569.53 epoch sec; seqNo:2995; depth 49.87 2024-11-06T03:09:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862569.54 epoch sec; seqNo:2995; 4061720.68 north; 594782.95 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:09:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862566.31 2024-11-06T03:09:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:29Z- waiting for 1730862570.31, time is 1730862569.54 2024-11-06T03:09:29Z- AHRS_M2 msg: 1730862569.94 epoch sec; seqNo:2814 2024-11-06T03:09:29Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.45 psi 2024-11-06T03:09:29Z- handleDepth()- Depth_Keller msg: 1730862569.94 epoch sec; seqNo:2996; depth 49.88 2024-11-06T03:09:29Z- handleDVL() - DVL msg epochMillisec: 1730862569.94 2024-11-06T03:09:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:09:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:09:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:09:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:09:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:09:29Z- handleDvl() - RDI_Pathfinder msg: 1730862569.94 sec; seqNo:1013; #beams:0 2024-11-06T03:09:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:09:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.04 , 0.06 2024-11-06T03:09:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862569.95 epoch sec; seqNo:2996; 4061721.08 north; 594784.17 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:09:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862566.31 2024-11-06T03:09:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:30Z- waiting for 1730862570.31, time is 1730862569.95 2024-11-06T03:09:30Z- AHRS_M2 msg: 1730862570.34 epoch sec; seqNo:2815 2024-11-06T03:09:30Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.45 psi 2024-11-06T03:09:30Z- handleDepth()- Depth_Keller msg: 1730862570.34 epoch sec; seqNo:2997; depth 49.89 2024-11-06T03:09:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862570.35 epoch sec; seqNo:2997; 4061721.08 north; 594784.17 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:09:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862566.31 2024-11-06T03:09:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:30Z- waiting for 1730862570.31, time is 1730862570.35 2024-11-06T03:09:30Z- LcmTrn::updateTrn() >>>> pose time 1730862570.35, meas time 1730862569.94 <<<< 2024-11-06T03:09:30Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862570.35 2024-11-06T03:09:30Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:09:30Z- TerrainNav::motionUpdate() - currEstimate = (4061721, 594784.17, 49.89) 2024-11-06T03:09:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:09:30Z- TerrainNav:: SubcloudNIS is 0.84, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:09:30Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:09:30Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:09:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:09:30Z- variances([0], [2], and psi_berg) = 0.832507 0.376199 0.000000 2024-11-06T03:09:30Z- publishEstimatesDecoder() - at 1730862570.35 sec 2024-11-06T03:09:30Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:09:30Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061753.31 594767.87 54.04 2024-11-06T03:09:30Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061754.18 594767.44 54.54 lat/lon: 36.70 -121.94 2024-11-06T03:09:30Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.83 0.38 1.00 0.00 2024-11-06T03:09:30Z- AHRS_M2 msg: 1730862570.77 epoch sec; seqNo:2816 2024-11-06T03:09:30Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.45 psi 2024-11-06T03:09:30Z- handleDepth()- Depth_Keller msg: 1730862570.77 epoch sec; seqNo:2998; depth 49.87 2024-11-06T03:09:30Z- handleDVL() - DVL msg epochMillisec: 1730862570.79 2024-11-06T03:09:30Z- handleDvl() - RDI_Pathfinder msg: 1730862570.79 sec; seqNo:1014; #beams:4 2024-11-06T03:09:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.30 , 30.88 , 35.62 , 32.41 2024-11-06T03:09:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , 0.08 2024-11-06T03:09:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862570.81 epoch sec; seqNo:2998; 4061721.08 north; 594784.17 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:09:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862570.35 2024-11-06T03:09:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:30Z- waiting for 1730862574.35, time is 1730862570.81 2024-11-06T03:09:31Z- AHRS_M2 msg: 1730862571.14 epoch sec; seqNo:2817 2024-11-06T03:09:31Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.45 psi 2024-11-06T03:09:31Z- handleDepth()- Depth_Keller msg: 1730862571.15 epoch sec; seqNo:2999; depth 49.88 2024-11-06T03:09:31Z- handleDVL() - DVL msg epochMillisec: 1730862571.15 2024-11-06T03:09:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:09:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:09:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:09:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:09:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:09:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:09:31Z- handleDvl() - RDI_Pathfinder msg: 1730862571.15 sec; seqNo:1015; #beams:0 2024-11-06T03:09:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:09:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , 0.08 2024-11-06T03:09:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862571.18 epoch sec; seqNo:2999; 4061721.08 north; 594784.17 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:09:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862570.35 2024-11-06T03:09:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:31Z- waiting for 1730862574.35, time is 1730862571.18 2024-11-06T03:09:31Z- AHRS_M2 msg: 1730862571.55 epoch sec; seqNo:2818 2024-11-06T03:09:31Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.45 psi 2024-11-06T03:09:31Z- handleDepth()- Depth_Keller msg: 1730862571.55 epoch sec; seqNo:3000; depth 49.89 2024-11-06T03:09:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862571.56 epoch sec; seqNo:3000; 4061721.09 north; 594785.39 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:09:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862570.35 2024-11-06T03:09:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:31Z- waiting for 1730862574.35, time is 1730862571.56 2024-11-06T03:09:31Z- AHRS_M2 msg: 1730862571.96 epoch sec; seqNo:2819 2024-11-06T03:09:31Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.45 psi 2024-11-06T03:09:31Z- handleDepth()- Depth_Keller msg: 1730862571.96 epoch sec; seqNo:3001; depth 49.88 2024-11-06T03:09:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862571.97 epoch sec; seqNo:3001; 4061721.09 north; 594785.39 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:09:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862570.35 2024-11-06T03:09:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:32Z- waiting for 1730862574.35, time is 1730862571.97 2024-11-06T03:09:32Z- AHRS_M2 msg: 1730862572.35 epoch sec; seqNo:2820 2024-11-06T03:09:32Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.45 psi 2024-11-06T03:09:32Z- handleDepth()- Depth_Keller msg: 1730862572.37 epoch sec; seqNo:3002; depth 49.87 2024-11-06T03:09:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862572.38 epoch sec; seqNo:3002; 4061721.09 north; 594785.39 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:09:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862570.35 2024-11-06T03:09:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:32Z- waiting for 1730862574.35, time is 1730862572.38 2024-11-06T03:09:32Z- AHRS_M2 msg: 1730862572.80 epoch sec; seqNo:2821 2024-11-06T03:09:32Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.45 psi 2024-11-06T03:09:32Z- handleDepth()- Depth_Keller msg: 1730862572.81 epoch sec; seqNo:3003; depth 49.87 2024-11-06T03:09:32Z- handleDVL() - DVL msg epochMillisec: 1730862572.83 2024-11-06T03:09:32Z- handleDvl() - RDI_Pathfinder msg: 1730862572.83 sec; seqNo:1016; #beams:4 2024-11-06T03:09:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.44 , 31.03 , 35.32 , 32.72 2024-11-06T03:09:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , 0.05 2024-11-06T03:09:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862572.87 epoch sec; seqNo:3003; 4061721.48 north; 594786.60 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:09:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862570.35 2024-11-06T03:09:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:32Z- waiting for 1730862574.35, time is 1730862572.87 2024-11-06T03:09:33Z- handleDepth()- Depth_Keller msg: 1730862573.19 epoch sec; seqNo:3004; depth 49.86 2024-11-06T03:09:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862573.21 epoch sec; seqNo:3004; 4061721.48 north; 594786.60 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:09:33Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:09:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862570.35 2024-11-06T03:09:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:33Z- waiting for 1730862574.35, time is 1730862573.21 2024-11-06T03:09:33Z- AHRS_M2 msg: 1730862573.57 epoch sec; seqNo:2822 2024-11-06T03:09:33Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.45 psi 2024-11-06T03:09:33Z- handleDepth()- Depth_Keller msg: 1730862573.57 epoch sec; seqNo:3005; depth 49.86 2024-11-06T03:09:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862573.59 epoch sec; seqNo:3005; 4061721.48 north; 594786.60 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:09:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862570.35 2024-11-06T03:09:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:33Z- waiting for 1730862574.35, time is 1730862573.59 2024-11-06T03:09:33Z- AHRS_M2 msg: 1730862573.97 epoch sec; seqNo:2823 2024-11-06T03:09:33Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.45 psi 2024-11-06T03:09:33Z- handleDepth()- Depth_Keller msg: 1730862573.98 epoch sec; seqNo:3006; depth 49.85 2024-11-06T03:09:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862574.01 epoch sec; seqNo:3006; 4061721.48 north; 594786.60 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:09:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862570.35 2024-11-06T03:09:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:34Z- waiting for 1730862574.35, time is 1730862574.01 2024-11-06T03:09:34Z- AHRS_M2 msg: 1730862574.37 epoch sec; seqNo:2824 2024-11-06T03:09:34Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:34Z- handleDepth()- Depth_Keller msg: 1730862574.38 epoch sec; seqNo:3007; depth 49.84 2024-11-06T03:09:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862574.39 epoch sec; seqNo:3007; 4061721.49 north; 594787.82 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:09:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862570.35 2024-11-06T03:09:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:34Z- waiting for 1730862574.35, time is 1730862574.39 2024-11-06T03:09:34Z- LcmTrn::updateTrn() >>>> pose time 1730862574.39, meas time 1730862572.83 <<<< 2024-11-06T03:09:34Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862574.39 2024-11-06T03:09:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862572.83 sec, ping # 0; waiting for INS data more recent than 1730862570.35... 2024-11-06T03:09:34Z- TerrainNav::motionUpdate() - currEstimate = (4061721, 594787.82, 49.84) 2024-11-06T03:09:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:09:34Z- TerrainNav:: SubcloudNIS is 0.84, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:09:34Z- TNavPF::Projecting Measurements 2024-11-06T03:09:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:09:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:09:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:09:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:09:34Z- TNavFilter::4 good beams. 2024-11-06T03:09:34Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:09:34Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:09:34Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:09:34Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:09:34Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:09:34Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:09:34Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:09:34Z- TerrainMapOctree: (vehN, vehE ) = (4061721.33, 594786.41). 2024-11-06T03:09:34Z- TerrainMapOctree: Min Distance = 46.34. 2024-11-06T03:09:34Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:09:34Z- Forcing Subcloud Comparison 2024-11-06T03:09:34Z- Weighting particles with subcloud comparison 2024-11-06T03:09:34Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:34Z- meanExpectedMeasDiff: 3.595055 delta_rms_squared: 2.924298 alpha: 0.993207 2024-11-06T03:09:34Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:34Z- meanExpectedMeasDiff: -0.143094 delta_rms_squared: 1.345594 alpha: 0.985354 2024-11-06T03:09:34Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:34Z- meanExpectedMeasDiff: 0.918035 delta_rms_squared: 1.726717 alpha: 0.988550 2024-11-06T03:09:34Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:34Z- meanExpectedMeasDiff: -0.726205 delta_rms_squared: 1.787771 alpha: 0.988937 2024-11-06T03:09:34Z- Subcloud NIS: 0.788684 2024-11-06T03:09:34Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:09:34Z- TNavPF:: sumWeights = 0.137646 2024-11-06T03:09:34Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:09:34Z- TNavPF::First Term of Map Covariance Matrix is 0.94 2024-11-06T03:09:34Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.01 2024-11-06T03:09:34Z- TerrainNav::Resampling particle filter... 2024-11-06T03:09:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862572.83 sec. 2024-11-06T03:09:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:09:34Z- variances([0], [2], and psi_berg) = 0.934819 0.389568 0.000000 2024-11-06T03:09:34Z- publishEstimatesDecoder() - at 1730862574.39 sec 2024-11-06T03:09:34Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:09:34Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061753.82 594771.49 53.75 2024-11-06T03:09:34Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061754.50 594771.08 53.88 lat/lon: 36.70 -121.94 2024-11-06T03:09:34Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.93 0.39 0.72 0.00 2024-11-06T03:09:34Z- AHRS_M2 msg: 1730862574.78 epoch sec; seqNo:2825 2024-11-06T03:09:34Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:34Z- handleDepth()- Depth_Keller msg: 1730862574.79 epoch sec; seqNo:3008; depth 49.83 2024-11-06T03:09:34Z- handleDVL() - DVL msg epochMillisec: 1730862574.84 2024-11-06T03:09:34Z- handleDvl() - RDI_Pathfinder msg: 1730862574.84 sec; seqNo:1017; #beams:4 2024-11-06T03:09:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.41 , 31.26 , 34.65 , 33.29 2024-11-06T03:09:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.05 , 0.00 2024-11-06T03:09:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862574.86 epoch sec; seqNo:3008; 4061721.49 north; 594787.82 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:09:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862574.39 2024-11-06T03:09:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:34Z- waiting for 1730862578.39, time is 1730862574.86 2024-11-06T03:09:35Z- AHRS_M2 msg: 1730862575.18 epoch sec; seqNo:2826 2024-11-06T03:09:35Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:35Z- handleDepth()- Depth_Keller msg: 1730862575.19 epoch sec; seqNo:3009; depth 49.79 2024-11-06T03:09:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862575.20 epoch sec; seqNo:3009; 4061721.49 north; 594787.82 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:09:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862574.39 2024-11-06T03:09:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:35Z- waiting for 1730862578.39, time is 1730862575.20 2024-11-06T03:09:35Z- AHRS_M2 msg: 1730862575.59 epoch sec; seqNo:2827 2024-11-06T03:09:35Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:35Z- handleDepth()- Depth_Keller msg: 1730862575.59 epoch sec; seqNo:3010; depth 49.77 2024-11-06T03:09:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862575.60 epoch sec; seqNo:3010; 4061721.51 north; 594789.04 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:09:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862574.39 2024-11-06T03:09:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:35Z- waiting for 1730862578.39, time is 1730862575.60 2024-11-06T03:09:36Z- AHRS_M2 msg: 1730862575.99 epoch sec; seqNo:2828 2024-11-06T03:09:36Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:36Z- handleDepth()- Depth_Keller msg: 1730862576.00 epoch sec; seqNo:3011; depth 49.77 2024-11-06T03:09:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862576.02 epoch sec; seqNo:3011; 4061721.51 north; 594789.04 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:09:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862574.39 2024-11-06T03:09:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:36Z- waiting for 1730862578.39, time is 1730862576.02 2024-11-06T03:09:36Z- AHRS_M2 msg: 1730862576.39 epoch sec; seqNo:2829 2024-11-06T03:09:36Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:36Z- handleDepth()- Depth_Keller msg: 1730862576.41 epoch sec; seqNo:3012; depth 49.74 2024-11-06T03:09:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862576.41 epoch sec; seqNo:3012; 4061721.51 north; 594789.04 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:09:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862574.39 2024-11-06T03:09:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:36Z- waiting for 1730862578.39, time is 1730862576.41 2024-11-06T03:09:36Z- AHRS_M2 msg: 1730862576.80 epoch sec; seqNo:2830 2024-11-06T03:09:36Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:36Z- handleDepth()- Depth_Keller msg: 1730862576.81 epoch sec; seqNo:3013; depth 49.73 2024-11-06T03:09:36Z- handleDVL() - DVL msg epochMillisec: 1730862576.83 2024-11-06T03:09:36Z- handleDvl() - RDI_Pathfinder msg: 1730862576.83 sec; seqNo:1018; #beams:4 2024-11-06T03:09:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.22 , 31.60 , 34.31 , 33.12 2024-11-06T03:09:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , -0.05 2024-11-06T03:09:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862576.85 epoch sec; seqNo:3013; 4061721.52 north; 594790.26 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:09:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862574.39 2024-11-06T03:09:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:36Z- waiting for 1730862578.39, time is 1730862576.85 2024-11-06T03:09:37Z- AHRS_M2 msg: 1730862577.20 epoch sec; seqNo:2831 2024-11-06T03:09:37Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:37Z- handleDepth()- Depth_Keller msg: 1730862577.21 epoch sec; seqNo:3014; depth 49.69 2024-11-06T03:09:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862577.23 epoch sec; seqNo:3014; 4061721.52 north; 594790.26 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:09:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862574.39 2024-11-06T03:09:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:37Z- waiting for 1730862578.39, time is 1730862577.23 2024-11-06T03:09:37Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:09:37Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:09:37Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:09:37Z- AHRS_M2 msg: 1730862577.60 epoch sec; seqNo:2832 2024-11-06T03:09:37Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:37Z- handleDepth()- Depth_Keller msg: 1730862577.64 epoch sec; seqNo:3015; depth 49.66 2024-11-06T03:09:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862577.65 epoch sec; seqNo:3015; 4061721.52 north; 594790.26 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:09:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862574.39 2024-11-06T03:09:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:37Z- waiting for 1730862578.39, time is 1730862577.65 2024-11-06T03:09:38Z- AHRS_M2 msg: 1730862578.01 epoch sec; seqNo:2833 2024-11-06T03:09:38Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:38Z- handleDepth()- Depth_Keller msg: 1730862578.02 epoch sec; seqNo:3016; depth 49.64 2024-11-06T03:09:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862578.03 epoch sec; seqNo:3016; 4061721.54 north; 594791.48 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:09:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862574.39 2024-11-06T03:09:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:38Z- waiting for 1730862578.39, time is 1730862578.03 2024-11-06T03:09:38Z- AHRS_M2 msg: 1730862578.41 epoch sec; seqNo:2834 2024-11-06T03:09:38Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:38Z- handleDepth()- Depth_Keller msg: 1730862578.42 epoch sec; seqNo:3017; depth 49.62 2024-11-06T03:09:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862578.43 epoch sec; seqNo:3017; 4061721.54 north; 594791.48 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:09:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862574.39 2024-11-06T03:09:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:38Z- waiting for 1730862578.39, time is 1730862578.43 2024-11-06T03:09:38Z- LcmTrn::updateTrn() >>>> pose time 1730862578.43, meas time 1730862576.83 <<<< 2024-11-06T03:09:38Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862578.43 2024-11-06T03:09:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862576.83 sec, ping # 0; waiting for INS data more recent than 1730862574.39... 2024-11-06T03:09:38Z- TerrainNav::motionUpdate() - currEstimate = (4061722, 594791.48, 49.62) 2024-11-06T03:09:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:09:38Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:09:38Z- TNavPF::Projecting Measurements 2024-11-06T03:09:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:09:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:09:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:09:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:09:38Z- TNavFilter::4 good beams. 2024-11-06T03:09:38Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:09:38Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:09:38Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:09:38Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:09:38Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:09:38Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:09:38Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:09:38Z- TerrainMapOctree: (vehN, vehE ) = (4061721.52, 594790.03). 2024-11-06T03:09:38Z- TerrainMapOctree: Min Distance = 49.13. 2024-11-06T03:09:38Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:09:38Z- Forcing Subcloud Comparison 2024-11-06T03:09:38Z- Weighting particles with subcloud comparison 2024-11-06T03:09:38Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:38Z- meanExpectedMeasDiff: 0.292543 delta_rms_squared: 1.777296 alpha: 0.988872 2024-11-06T03:09:38Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:38Z- meanExpectedMeasDiff: -0.641814 delta_rms_squared: 1.086977 alpha: 0.981933 2024-11-06T03:09:38Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:38Z- meanExpectedMeasDiff: -1.657966 delta_rms_squared: 2.315557 alpha: 0.991437 2024-11-06T03:09:38Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:38Z- meanExpectedMeasDiff: -0.434540 delta_rms_squared: 1.871572 alpha: 0.989427 2024-11-06T03:09:38Z- Subcloud NIS: 0.798745 2024-11-06T03:09:38Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:09:38Z- TNavPF:: sumWeights = 0.416200 2024-11-06T03:09:38Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:09:38Z- TNavPF::First Term of Map Covariance Matrix is 0.34 2024-11-06T03:09:38Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T03:09:38Z- TerrainNav::Resampling particle filter... 2024-11-06T03:09:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862576.83 sec. 2024-11-06T03:09:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:09:38Z- variances([0], [2], and psi_berg) = 0.857859 0.366565 0.000000 2024-11-06T03:09:38Z- publishEstimatesDecoder() - at 1730862578.43 sec 2024-11-06T03:09:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:09:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061753.91 594775.17 54.80 2024-11-06T03:09:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061754.67 594774.71 54.00 lat/lon: 36.70 -121.94 2024-11-06T03:09:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.86 0.37 0.59 0.00 2024-11-06T03:09:38Z- AHRS_M2 msg: 1730862578.82 epoch sec; seqNo:2835 2024-11-06T03:09:38Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:38Z- handleDepth()- Depth_Keller msg: 1730862578.83 epoch sec; seqNo:3018; depth 49.60 2024-11-06T03:09:38Z- handleDVL() - DVL msg epochMillisec: 1730862578.85 2024-11-06T03:09:38Z- handleDvl() - RDI_Pathfinder msg: 1730862578.85 sec; seqNo:1019; #beams:4 2024-11-06T03:09:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.08 , 31.34 , 34.09 , 33.33 2024-11-06T03:09:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , -0.06 2024-11-06T03:09:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862578.87 epoch sec; seqNo:3018; 4061721.91 north; 594791.48 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:09:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862578.43 2024-11-06T03:09:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:38Z- waiting for 1730862582.43, time is 1730862578.87 2024-11-06T03:09:39Z- AHRS_M2 msg: 1730862579.22 epoch sec; seqNo:2836 2024-11-06T03:09:39Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:39Z- handleDepth()- Depth_Keller msg: 1730862579.24 epoch sec; seqNo:3019; depth 49.57 2024-11-06T03:09:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862579.26 epoch sec; seqNo:3019; 4061721.91 north; 594791.48 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:09:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862578.43 2024-11-06T03:09:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:39Z- waiting for 1730862582.43, time is 1730862579.26 2024-11-06T03:09:39Z- AHRS_M2 msg: 1730862579.64 epoch sec; seqNo:2837 2024-11-06T03:09:39Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:39Z- handleDepth()- Depth_Keller msg: 1730862579.65 epoch sec; seqNo:3020; depth 49.55 2024-11-06T03:09:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862579.66 epoch sec; seqNo:3020; 4061721.93 north; 594792.70 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:09:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862578.43 2024-11-06T03:09:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:39Z- waiting for 1730862582.43, time is 1730862579.66 2024-11-06T03:09:40Z- AHRS_M2 msg: 1730862580.04 epoch sec; seqNo:2838 2024-11-06T03:09:40Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.46 psi 2024-11-06T03:09:40Z- handleDepth()- Depth_Keller msg: 1730862580.05 epoch sec; seqNo:3021; depth 49.52 2024-11-06T03:09:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862580.05 epoch sec; seqNo:3021; 4061721.93 north; 594792.70 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T03:09:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862578.43 2024-11-06T03:09:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:40Z- waiting for 1730862582.43, time is 1730862580.05 2024-11-06T03:09:40Z- AHRS_M2 msg: 1730862580.43 epoch sec; seqNo:2839 2024-11-06T03:09:40Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.46 psi 2024-11-06T03:09:40Z- handleDepth()- Depth_Keller msg: 1730862580.45 epoch sec; seqNo:3022; depth 49.51 2024-11-06T03:09:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862580.46 epoch sec; seqNo:3022; 4061721.93 north; 594792.70 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T03:09:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862578.43 2024-11-06T03:09:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:40Z- waiting for 1730862582.43, time is 1730862580.46 2024-11-06T03:09:40Z- AHRS_M2 msg: 1730862580.84 epoch sec; seqNo:2840 2024-11-06T03:09:40Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.46 psi 2024-11-06T03:09:40Z- handleDepth()- Depth_Keller msg: 1730862580.84 epoch sec; seqNo:3023; depth 49.49 2024-11-06T03:09:40Z- handleDVL() - DVL msg epochMillisec: 1730862580.87 2024-11-06T03:09:40Z- handleDvl() - RDI_Pathfinder msg: 1730862580.87 sec; seqNo:1020; #beams:4 2024-11-06T03:09:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.08 , 31.65 , 34.09 , 33.48 2024-11-06T03:09:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.01 , -0.03 2024-11-06T03:09:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862580.88 epoch sec; seqNo:3023; 4061721.94 north; 594793.92 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:09:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862578.43 2024-11-06T03:09:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:40Z- waiting for 1730862582.43, time is 1730862580.88 2024-11-06T03:09:41Z- AHRS_M2 msg: 1730862581.24 epoch sec; seqNo:2841 2024-11-06T03:09:41Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.45 psi 2024-11-06T03:09:41Z- handleDepth()- Depth_Keller msg: 1730862581.25 epoch sec; seqNo:3024; depth 49.47 2024-11-06T03:09:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862581.26 epoch sec; seqNo:3024; 4061721.94 north; 594793.92 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:09:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862578.43 2024-11-06T03:09:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:41Z- waiting for 1730862582.43, time is 1730862581.26 2024-11-06T03:09:41Z- AHRS_M2 msg: 1730862581.65 epoch sec; seqNo:2842 2024-11-06T03:09:41Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.45 psi 2024-11-06T03:09:41Z- handleDepth()- Depth_Keller msg: 1730862581.66 epoch sec; seqNo:3025; depth 49.48 2024-11-06T03:09:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862581.67 epoch sec; seqNo:3025; 4061721.94 north; 594793.92 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:09:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862578.43 2024-11-06T03:09:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:41Z- waiting for 1730862582.43, time is 1730862581.67 2024-11-06T03:09:42Z- AHRS_M2 msg: 1730862582.05 epoch sec; seqNo:2843 2024-11-06T03:09:42Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.45 psi 2024-11-06T03:09:42Z- handleDepth()- Depth_Keller msg: 1730862582.07 epoch sec; seqNo:3026; depth 49.48 2024-11-06T03:09:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862582.08 epoch sec; seqNo:3026; 4061721.94 north; 594793.92 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:09:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862578.43 2024-11-06T03:09:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:42Z- waiting for 1730862582.43, time is 1730862582.08 2024-11-06T03:09:42Z- AHRS_M2 msg: 1730862582.46 epoch sec; seqNo:2844 2024-11-06T03:09:42Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.45 psi 2024-11-06T03:09:42Z- handleDepth()- Depth_Keller msg: 1730862582.47 epoch sec; seqNo:3027; depth 49.44 2024-11-06T03:09:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862582.47 epoch sec; seqNo:3027; 4061721.94 north; 594793.92 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:09:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862578.43 2024-11-06T03:09:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:42Z- waiting for 1730862582.43, time is 1730862582.47 2024-11-06T03:09:42Z- LcmTrn::updateTrn() >>>> pose time 1730862582.47, meas time 1730862580.87 <<<< 2024-11-06T03:09:42Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862582.47 2024-11-06T03:09:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862580.87 sec, ping # 0; waiting for INS data more recent than 1730862578.43... 2024-11-06T03:09:42Z- TerrainNav::motionUpdate() - currEstimate = (4061722, 594793.92, 49.44) 2024-11-06T03:09:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:09:42Z- TerrainNav:: SubcloudNIS is 0.80, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:09:42Z- TNavPF::Projecting Measurements 2024-11-06T03:09:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:09:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:09:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:09:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:09:42Z- TNavFilter::4 good beams. 2024-11-06T03:09:42Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:09:42Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:09:42Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:09:42Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:09:42Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:09:42Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:09:42Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:09:42Z- TerrainMapOctree: (vehN, vehE ) = (4061721.78, 594792.95). 2024-11-06T03:09:42Z- TerrainMapOctree: Min Distance = 51.40. 2024-11-06T03:09:42Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:09:42Z- Forcing Subcloud Comparison 2024-11-06T03:09:42Z- Weighting particles with subcloud comparison 2024-11-06T03:09:42Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:42Z- meanExpectedMeasDiff: -1.679048 delta_rms_squared: 1.345394 alpha: 0.985352 2024-11-06T03:09:42Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:42Z- meanExpectedMeasDiff: -0.495642 delta_rms_squared: 0.972118 alpha: 0.979841 2024-11-06T03:09:42Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:42Z- meanExpectedMeasDiff: -2.058174 delta_rms_squared: 1.153709 alpha: 0.982960 2024-11-06T03:09:42Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:42Z- meanExpectedMeasDiff: 0.437657 delta_rms_squared: 1.642305 alpha: 0.987969 2024-11-06T03:09:42Z- Subcloud NIS: 0.797583 2024-11-06T03:09:42Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:09:42Z- TNavPF:: sumWeights = 0.302043 2024-11-06T03:09:42Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:09:42Z- TNavPF::First Term of Map Covariance Matrix is 0.36 2024-11-06T03:09:42Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T03:09:42Z- TerrainNav::Resampling particle filter... 2024-11-06T03:09:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862580.87 sec. 2024-11-06T03:09:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:09:42Z- variances([0], [2], and psi_berg) = 0.755989 0.352931 0.000000 2024-11-06T03:09:42Z- publishEstimatesDecoder() - at 1730862582.47 sec 2024-11-06T03:09:42Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:09:42Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061754.32 594777.63 54.48 2024-11-06T03:09:42Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061755.21 594777.11 54.26 lat/lon: 36.70 -121.94 2024-11-06T03:09:42Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.76 0.35 0.61 0.00 2024-11-06T03:09:42Z- handleDepth()- Depth_Keller msg: 1730862582.88 epoch sec; seqNo:3028; depth 49.44 2024-11-06T03:09:42Z- handleDVL() - DVL msg epochMillisec: 1730862582.90 2024-11-06T03:09:42Z- handleDvl() - RDI_Pathfinder msg: 1730862582.90 sec; seqNo:1021; #beams:4 2024-11-06T03:09:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.08 , 32.26 , 34.09 , 33.48 2024-11-06T03:09:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.04 , 0.03 2024-11-06T03:09:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862582.92 epoch sec; seqNo:3028; 4061722.33 north; 594795.14 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:09:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862582.47 2024-11-06T03:09:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:43Z- waiting for 1730862586.47, time is 1730862582.92 2024-11-06T03:09:43Z- AHRS_M2 msg: 1730862583.26 epoch sec; seqNo:2845 2024-11-06T03:09:43Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.45 psi 2024-11-06T03:09:43Z- handleDepth()- Depth_Keller msg: 1730862583.28 epoch sec; seqNo:3029; depth 49.44 2024-11-06T03:09:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862583.30 epoch sec; seqNo:3029; 4061722.33 north; 594795.14 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:09:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862582.47 2024-11-06T03:09:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:43Z- waiting for 1730862586.47, time is 1730862583.30 2024-11-06T03:09:43Z- AHRS_M2 msg: 1730862583.67 epoch sec; seqNo:2846 2024-11-06T03:09:43Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.44 psi 2024-11-06T03:09:43Z- handleDepth()- Depth_Keller msg: 1730862583.68 epoch sec; seqNo:3030; depth 49.44 2024-11-06T03:09:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862583.70 epoch sec; seqNo:3030; 4061722.33 north; 594795.14 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:09:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862582.47 2024-11-06T03:09:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:43Z- waiting for 1730862586.47, time is 1730862583.70 2024-11-06T03:09:44Z- AHRS_M2 msg: 1730862584.07 epoch sec; seqNo:2847 2024-11-06T03:09:44Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.44 psi 2024-11-06T03:09:44Z- handleDepth()- Depth_Keller msg: 1730862584.08 epoch sec; seqNo:3031; depth 49.44 2024-11-06T03:09:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862584.09 epoch sec; seqNo:3031; 4061722.33 north; 594795.14 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:09:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862582.47 2024-11-06T03:09:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:44Z- waiting for 1730862586.47, time is 1730862584.09 2024-11-06T03:09:44Z- AHRS_M2 msg: 1730862584.48 epoch sec; seqNo:2848 2024-11-06T03:09:44Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.44 psi 2024-11-06T03:09:44Z- handleDepth()- Depth_Keller msg: 1730862584.49 epoch sec; seqNo:3032; depth 49.45 2024-11-06T03:09:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862584.50 epoch sec; seqNo:3032; 4061722.35 north; 594796.36 east; 36.70 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:09:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862582.47 2024-11-06T03:09:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:44Z- waiting for 1730862586.47, time is 1730862584.50 2024-11-06T03:09:44Z- AHRS_M2 msg: 1730862584.88 epoch sec; seqNo:2849 2024-11-06T03:09:44Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.44 psi 2024-11-06T03:09:44Z- handleDepth()- Depth_Keller msg: 1730862584.88 epoch sec; seqNo:3033; depth 49.45 2024-11-06T03:09:44Z- handleDVL() - DVL msg epochMillisec: 1730862584.91 2024-11-06T03:09:44Z- handleDvl() - RDI_Pathfinder msg: 1730862584.91 sec; seqNo:1022; #beams:4 2024-11-06T03:09:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.16 , 32.56 , 33.79 , 33.94 2024-11-06T03:09:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.03 , 0.07 2024-11-06T03:09:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862584.93 epoch sec; seqNo:3033; 4061722.35 north; 594796.36 east; 36.70 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:09:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862582.47 2024-11-06T03:09:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:45Z- waiting for 1730862586.47, time is 1730862584.93 2024-11-06T03:09:45Z- AHRS_M2 msg: 1730862585.28 epoch sec; seqNo:2850 2024-11-06T03:09:45Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.44 psi 2024-11-06T03:09:45Z- handleDepth()- Depth_Keller msg: 1730862585.29 epoch sec; seqNo:3034; depth 49.44 2024-11-06T03:09:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862585.30 epoch sec; seqNo:3034; 4061722.35 north; 594796.36 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:09:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862582.47 2024-11-06T03:09:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:45Z- waiting for 1730862586.47, time is 1730862585.30 2024-11-06T03:09:45Z- AHRS_M2 msg: 1730862585.69 epoch sec; seqNo:2851 2024-11-06T03:09:45Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.44 psi 2024-11-06T03:09:45Z- handleDepth()- Depth_Keller msg: 1730862585.69 epoch sec; seqNo:3035; depth 49.46 2024-11-06T03:09:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862585.70 epoch sec; seqNo:3035; 4061722.73 north; 594796.35 east; 36.70 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T03:09:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862582.47 2024-11-06T03:09:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:45Z- waiting for 1730862586.47, time is 1730862585.70 2024-11-06T03:09:46Z- AHRS_M2 msg: 1730862586.09 epoch sec; seqNo:2852 2024-11-06T03:09:46Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.44 psi 2024-11-06T03:09:46Z- handleDepth()- Depth_Keller msg: 1730862586.10 epoch sec; seqNo:3036; depth 49.46 2024-11-06T03:09:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862586.11 epoch sec; seqNo:3036; 4061722.74 north; 594797.57 east; 36.70 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T03:09:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862582.47 2024-11-06T03:09:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:46Z- waiting for 1730862586.47, time is 1730862586.11 2024-11-06T03:09:46Z- AHRS_M2 msg: 1730862586.49 epoch sec; seqNo:2853 2024-11-06T03:09:46Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.45 psi 2024-11-06T03:09:46Z- handleDepth()- Depth_Keller msg: 1730862586.50 epoch sec; seqNo:3037; depth 49.47 2024-11-06T03:09:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862586.52 epoch sec; seqNo:3037; 4061722.74 north; 594797.57 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:09:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862582.47 2024-11-06T03:09:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:46Z- waiting for 1730862586.47, time is 1730862586.52 2024-11-06T03:09:46Z- LcmTrn::updateTrn() >>>> pose time 1730862586.52, meas time 1730862584.91 <<<< 2024-11-06T03:09:46Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862586.52 2024-11-06T03:09:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862584.91 sec, ping # 0; waiting for INS data more recent than 1730862582.47... 2024-11-06T03:09:46Z- TerrainNav::motionUpdate() - currEstimate = (4061723, 594797.57, 49.47) 2024-11-06T03:09:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:09:46Z- TerrainNav:: SubcloudNIS is 0.80, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:09:46Z- TNavPF::Projecting Measurements 2024-11-06T03:09:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:09:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:09:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:09:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:09:46Z- TNavFilter::4 good beams. 2024-11-06T03:09:46Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:09:46Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:09:46Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:09:46Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:09:46Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:09:46Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:09:46Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:09:46Z- TerrainMapOctree: (vehN, vehE ) = (4061722.42, 594796.12). 2024-11-06T03:09:46Z- TerrainMapOctree: Min Distance = 53.74. 2024-11-06T03:09:46Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:09:46Z- Forcing Subcloud Comparison 2024-11-06T03:09:46Z- Weighting particles with subcloud comparison 2024-11-06T03:09:46Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:46Z- meanExpectedMeasDiff: -2.147075 delta_rms_squared: 1.352252 alpha: 0.985425 2024-11-06T03:09:46Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:46Z- meanExpectedMeasDiff: 0.839393 delta_rms_squared: 0.974913 alpha: 0.979898 2024-11-06T03:09:46Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:46Z- meanExpectedMeasDiff: -1.761308 delta_rms_squared: 1.148519 alpha: 0.982884 2024-11-06T03:09:46Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:46Z- meanExpectedMeasDiff: 0.970155 delta_rms_squared: 1.076709 alpha: 0.981764 2024-11-06T03:09:46Z- Subcloud NIS: 0.785891 2024-11-06T03:09:46Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:09:46Z- TNavPF:: sumWeights = 0.206924 2024-11-06T03:09:46Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:09:46Z- TNavPF::First Term of Map Covariance Matrix is 0.54 2024-11-06T03:09:46Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T03:09:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862584.91 sec. 2024-11-06T03:09:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:09:46Z- variances([0], [2], and psi_berg) = 0.715964 0.346492 0.000000 2024-11-06T03:09:46Z- publishEstimatesDecoder() - at 1730862586.52 sec 2024-11-06T03:09:46Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:09:46Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061757.00 594782.18 55.81 2024-11-06T03:09:46Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061756.04 594780.77 54.56 lat/lon: 36.70 -121.94 2024-11-06T03:09:46Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.72 0.35 0.63 0.00 2024-11-06T03:09:46Z- handleDepth()- Depth_Keller msg: 1730862586.92 epoch sec; seqNo:3038; depth 49.48 2024-11-06T03:09:46Z- handleDVL() - DVL msg epochMillisec: 1730862586.94 2024-11-06T03:09:46Z- handleDvl() - RDI_Pathfinder msg: 1730862586.94 sec; seqNo:1023; #beams:4 2024-11-06T03:09:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.16 , 32.87 , 34.09 , 34.55 2024-11-06T03:09:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.00 , 0.06 2024-11-06T03:09:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862586.96 epoch sec; seqNo:3038; 4061722.74 north; 594797.57 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:09:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862586.52 2024-11-06T03:09:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:47Z- waiting for 1730862590.52, time is 1730862586.96 2024-11-06T03:09:47Z- AHRS_M2 msg: 1730862587.30 epoch sec; seqNo:2854 2024-11-06T03:09:47Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.45 psi 2024-11-06T03:09:47Z- handleDepth()- Depth_Keller msg: 1730862587.31 epoch sec; seqNo:3039; depth 49.48 2024-11-06T03:09:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862587.33 epoch sec; seqNo:3039; 4061722.75 north; 594798.79 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:09:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862586.52 2024-11-06T03:09:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:47Z- waiting for 1730862590.52, time is 1730862587.33 2024-11-06T03:09:47Z- AHRS_M2 msg: 1730862587.71 epoch sec; seqNo:2855 2024-11-06T03:09:47Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.45 psi 2024-11-06T03:09:47Z- handleDepth()- Depth_Keller msg: 1730862587.71 epoch sec; seqNo:3040; depth 49.49 2024-11-06T03:09:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862587.72 epoch sec; seqNo:3040; 4061722.75 north; 594798.79 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:09:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862586.52 2024-11-06T03:09:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:47Z- waiting for 1730862590.52, time is 1730862587.72 2024-11-06T03:09:48Z- AHRS_M2 msg: 1730862588.11 epoch sec; seqNo:2856 2024-11-06T03:09:48Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.45 psi 2024-11-06T03:09:48Z- handleDepth()- Depth_Keller msg: 1730862588.12 epoch sec; seqNo:3041; depth 49.49 2024-11-06T03:09:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862588.13 epoch sec; seqNo:3041; 4061722.75 north; 594798.79 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:09:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862586.52 2024-11-06T03:09:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:48Z- waiting for 1730862590.52, time is 1730862588.13 2024-11-06T03:09:48Z- AHRS_M2 msg: 1730862588.52 epoch sec; seqNo:2857 2024-11-06T03:09:48Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.46 psi 2024-11-06T03:09:48Z- handleDepth()- Depth_Keller msg: 1730862588.52 epoch sec; seqNo:3042; depth 49.50 2024-11-06T03:09:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862588.53 epoch sec; seqNo:3042; 4061723.13 north; 594798.79 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:09:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862586.52 2024-11-06T03:09:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:48Z- waiting for 1730862590.52, time is 1730862588.53 2024-11-06T03:09:48Z- handleDepth()- Depth_Keller msg: 1730862588.94 epoch sec; seqNo:3043; depth 49.48 2024-11-06T03:09:48Z- handleDVL() - DVL msg epochMillisec: 1730862588.96 2024-11-06T03:09:48Z- handleDvl() - RDI_Pathfinder msg: 1730862588.96 sec; seqNo:1024; #beams:4 2024-11-06T03:09:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.93 , 32.26 , 33.94 , 35.78 2024-11-06T03:09:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , 0.03 2024-11-06T03:09:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862588.98 epoch sec; seqNo:3043; 4061723.14 north; 594800.01 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:09:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862586.52 2024-11-06T03:09:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:49Z- waiting for 1730862590.52, time is 1730862588.98 2024-11-06T03:09:49Z- AHRS_M2 msg: 1730862589.33 epoch sec; seqNo:2858 2024-11-06T03:09:49Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.46 psi 2024-11-06T03:09:49Z- handleDepth()- Depth_Keller msg: 1730862589.34 epoch sec; seqNo:3044; depth 49.49 2024-11-06T03:09:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862589.35 epoch sec; seqNo:3044; 4061723.14 north; 594800.01 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:09:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862586.52 2024-11-06T03:09:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:49Z- waiting for 1730862590.52, time is 1730862589.35 2024-11-06T03:09:49Z- AHRS_M2 msg: 1730862589.73 epoch sec; seqNo:2859 2024-11-06T03:09:49Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.46 psi 2024-11-06T03:09:49Z- handleDepth()- Depth_Keller msg: 1730862589.73 epoch sec; seqNo:3045; depth 49.48 2024-11-06T03:09:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862589.75 epoch sec; seqNo:3045; 4061723.14 north; 594800.01 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:09:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862586.52 2024-11-06T03:09:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:49Z- waiting for 1730862590.52, time is 1730862589.75 2024-11-06T03:09:50Z- AHRS_M2 msg: 1730862590.13 epoch sec; seqNo:2860 2024-11-06T03:09:50Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.46 psi 2024-11-06T03:09:50Z- handleDepth()- Depth_Keller msg: 1730862590.14 epoch sec; seqNo:3046; depth 49.48 2024-11-06T03:09:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862590.15 epoch sec; seqNo:3046; 4061723.16 north; 594801.23 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:09:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862586.52 2024-11-06T03:09:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:50Z- waiting for 1730862590.52, time is 1730862590.15 2024-11-06T03:09:50Z- AHRS_M2 msg: 1730862590.54 epoch sec; seqNo:2861 2024-11-06T03:09:50Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.46 psi 2024-11-06T03:09:50Z- handleDepth()- Depth_Keller msg: 1730862590.54 epoch sec; seqNo:3047; depth 49.46 2024-11-06T03:09:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862590.55 epoch sec; seqNo:3047; 4061723.16 north; 594801.23 east; 36.70 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T03:09:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862586.52 2024-11-06T03:09:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:50Z- waiting for 1730862590.52, time is 1730862590.55 2024-11-06T03:09:50Z- LcmTrn::updateTrn() >>>> pose time 1730862590.55, meas time 1730862588.96 <<<< 2024-11-06T03:09:50Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862590.55 2024-11-06T03:09:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862588.96 sec, ping # 0; waiting for INS data more recent than 1730862586.52... 2024-11-06T03:09:50Z- TerrainNav::motionUpdate() - currEstimate = (4061723, 594801.23, 49.46) 2024-11-06T03:09:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:09:50Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:09:50Z- TNavPF::Projecting Measurements 2024-11-06T03:09:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:09:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:09:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:09:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:09:50Z- TNavFilter::4 good beams. 2024-11-06T03:09:50Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:09:50Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:09:50Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:09:50Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:09:50Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:09:50Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:09:50Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:09:50Z- TerrainMapOctree: (vehN, vehE ) = (4061722.99, 594799.79). 2024-11-06T03:09:50Z- TerrainMapOctree: Min Distance = 56.64. 2024-11-06T03:09:50Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:09:50Z- Forcing Subcloud Comparison 2024-11-06T03:09:50Z- Weighting particles with subcloud comparison 2024-11-06T03:09:50Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:50Z- meanExpectedMeasDiff: -2.012319 delta_rms_squared: 2.009103 alpha: 0.990143 2024-11-06T03:09:50Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:50Z- meanExpectedMeasDiff: 0.958533 delta_rms_squared: 1.013170 alpha: 0.980642 2024-11-06T03:09:50Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:50Z- meanExpectedMeasDiff: -1.573111 delta_rms_squared: 1.966114 alpha: 0.989930 2024-11-06T03:09:50Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:50Z- meanExpectedMeasDiff: 2.001889 delta_rms_squared: 1.224635 alpha: 0.983931 2024-11-06T03:09:50Z- Subcloud NIS: 0.787052 2024-11-06T03:09:50Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:09:50Z- TNavPF:: sumWeights = 0.140115 2024-11-06T03:09:50Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:09:50Z- TNavPF::First Term of Map Covariance Matrix is 0.43 2024-11-06T03:09:50Z- TNavPF::Calculated NIS Value : 0.07 numBeams normalized NIS: 0.02 2024-11-06T03:09:50Z- TerrainNav::Resampling particle filter... 2024-11-06T03:09:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862588.96 sec. 2024-11-06T03:09:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:09:50Z- variances([0], [2], and psi_berg) = 0.623394 0.341980 0.000000 2024-11-06T03:09:50Z- publishEstimatesDecoder() - at 1730862590.55 sec 2024-11-06T03:09:50Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:09:50Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061755.48 594784.98 53.60 2024-11-06T03:09:50Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061756.47 594784.33 54.69 lat/lon: 36.70 -121.94 2024-11-06T03:09:50Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.62 0.34 0.65 0.00 2024-11-06T03:09:50Z- AHRS_M2 msg: 1730862590.94 epoch sec; seqNo:2862 2024-11-06T03:09:50Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.46 psi 2024-11-06T03:09:50Z- handleDepth()- Depth_Keller msg: 1730862590.95 epoch sec; seqNo:3048; depth 49.46 2024-11-06T03:09:50Z- handleDVL() - DVL msg epochMillisec: 1730862590.98 2024-11-06T03:09:50Z- handleDvl() - RDI_Pathfinder msg: 1730862590.98 sec; seqNo:1025; #beams:4 2024-11-06T03:09:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.67 , 32.11 , 34.14 , 36.52 2024-11-06T03:09:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , 0.00 2024-11-06T03:09:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862590.99 epoch sec; seqNo:3048; 4061723.16 north; 594801.23 east; 36.70 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T03:09:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862590.55 2024-11-06T03:09:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:51Z- waiting for 1730862594.55, time is 1730862590.99 2024-11-06T03:09:51Z- AHRS_M2 msg: 1730862591.34 epoch sec; seqNo:2863 2024-11-06T03:09:51Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.46 psi 2024-11-06T03:09:51Z- handleDepth()- Depth_Keller msg: 1730862591.35 epoch sec; seqNo:3049; depth 49.43 2024-11-06T03:09:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862591.37 epoch sec; seqNo:3049; 4061723.17 north; 594802.45 east; 36.70 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T03:09:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862590.55 2024-11-06T03:09:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:51Z- waiting for 1730862594.55, time is 1730862591.37 2024-11-06T03:09:51Z- AHRS_M2 msg: 1730862591.75 epoch sec; seqNo:2864 2024-11-06T03:09:51Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.46 psi 2024-11-06T03:09:51Z- handleDepth()- Depth_Keller msg: 1730862591.76 epoch sec; seqNo:3050; depth 49.42 2024-11-06T03:09:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862591.77 epoch sec; seqNo:3050; 4061723.17 north; 594802.45 east; 36.70 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T03:09:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862590.55 2024-11-06T03:09:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:51Z- waiting for 1730862594.55, time is 1730862591.77 2024-11-06T03:09:52Z- AHRS_M2 msg: 1730862592.15 epoch sec; seqNo:2865 2024-11-06T03:09:52Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.46 psi 2024-11-06T03:09:52Z- handleDepth()- Depth_Keller msg: 1730862592.16 epoch sec; seqNo:3051; depth 49.40 2024-11-06T03:09:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862592.17 epoch sec; seqNo:3051; 4061723.17 north; 594802.45 east; 36.70 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T03:09:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862590.55 2024-11-06T03:09:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:52Z- waiting for 1730862594.55, time is 1730862592.17 2024-11-06T03:09:52Z- AHRS_M2 msg: 1730862592.56 epoch sec; seqNo:2866 2024-11-06T03:09:52Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.46 psi 2024-11-06T03:09:52Z- handleDepth()- Depth_Keller msg: 1730862592.58 epoch sec; seqNo:3052; depth 49.40 2024-11-06T03:09:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862592.59 epoch sec; seqNo:3052; 4061723.17 north; 594802.45 east; 36.70 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T03:09:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862590.55 2024-11-06T03:09:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:52Z- waiting for 1730862594.55, time is 1730862592.59 2024-11-06T03:09:52Z- AHRS_M2 msg: 1730862592.96 epoch sec; seqNo:2867 2024-11-06T03:09:52Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.46 psi 2024-11-06T03:09:52Z- handleDepth()- Depth_Keller msg: 1730862592.97 epoch sec; seqNo:3053; depth 49.39 2024-11-06T03:09:53Z- handleDVL() - DVL msg epochMillisec: 1730862593.00 2024-11-06T03:09:53Z- handleDvl() - RDI_Pathfinder msg: 1730862593.00 sec; seqNo:1026; #beams:4 2024-11-06T03:09:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.18 , 32.62 , 33.80 , 36.69 2024-11-06T03:09:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.04 , -0.02 2024-11-06T03:09:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862593.01 epoch sec; seqNo:3053; 4061723.19 north; 594803.67 east; 36.70 lat; -121.94 lon; 49.39 depth; utmZone: 10 2024-11-06T03:09:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862590.55 2024-11-06T03:09:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:53Z- waiting for 1730862594.55, time is 1730862593.01 2024-11-06T03:09:53Z- AHRS_M2 msg: 1730862593.36 epoch sec; seqNo:2868 2024-11-06T03:09:53Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.46 psi 2024-11-06T03:09:53Z- handleDepth()- Depth_Keller msg: 1730862593.37 epoch sec; seqNo:3054; depth 49.36 2024-11-06T03:09:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862593.38 epoch sec; seqNo:3054; 4061723.19 north; 594803.67 east; 36.70 lat; -121.94 lon; 49.36 depth; utmZone: 10 2024-11-06T03:09:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862590.55 2024-11-06T03:09:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:53Z- waiting for 1730862594.55, time is 1730862593.38 2024-11-06T03:09:53Z- AHRS_M2 msg: 1730862593.77 epoch sec; seqNo:2869 2024-11-06T03:09:53Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.46 psi 2024-11-06T03:09:53Z- handleDepth()- Depth_Keller msg: 1730862593.77 epoch sec; seqNo:3055; depth 49.36 2024-11-06T03:09:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862593.79 epoch sec; seqNo:3055; 4061723.56 north; 594803.67 east; 36.70 lat; -121.94 lon; 49.36 depth; utmZone: 10 2024-11-06T03:09:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862590.55 2024-11-06T03:09:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:53Z- waiting for 1730862594.55, time is 1730862593.79 2024-11-06T03:09:54Z- AHRS_M2 msg: 1730862594.17 epoch sec; seqNo:2870 2024-11-06T03:09:54Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.46 psi 2024-11-06T03:09:54Z- handleDepth()- Depth_Keller msg: 1730862594.18 epoch sec; seqNo:3056; depth 49.33 2024-11-06T03:09:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862594.19 epoch sec; seqNo:3056; 4061723.58 north; 594804.89 east; 36.70 lat; -121.94 lon; 49.33 depth; utmZone: 10 2024-11-06T03:09:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862590.55 2024-11-06T03:09:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:54Z- waiting for 1730862594.55, time is 1730862594.19 2024-11-06T03:09:54Z- AHRS_M2 msg: 1730862594.58 epoch sec; seqNo:2871 2024-11-06T03:09:54Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.47 psi 2024-11-06T03:09:54Z- handleDepth()- Depth_Keller msg: 1730862594.59 epoch sec; seqNo:3057; depth 49.32 2024-11-06T03:09:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862594.59 epoch sec; seqNo:3057; 4061723.58 north; 594804.89 east; 36.70 lat; -121.94 lon; 49.32 depth; utmZone: 10 2024-11-06T03:09:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862590.55 2024-11-06T03:09:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:54Z- waiting for 1730862594.55, time is 1730862594.59 2024-11-06T03:09:54Z- LcmTrn::updateTrn() >>>> pose time 1730862594.59, meas time 1730862593.00 <<<< 2024-11-06T03:09:54Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862594.59 2024-11-06T03:09:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862593.00 sec, ping # 0; waiting for INS data more recent than 1730862590.55... 2024-11-06T03:09:54Z- TerrainNav::motionUpdate() - currEstimate = (4061724, 594804.89, 49.32) 2024-11-06T03:09:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:09:54Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:09:54Z- TNavPF::Projecting Measurements 2024-11-06T03:09:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:09:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:09:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:09:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:09:54Z- TNavFilter::4 good beams. 2024-11-06T03:09:54Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:09:54Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:09:54Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:09:54Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:09:54Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:09:54Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:09:54Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:09:54Z- TerrainMapOctree: (vehN, vehE ) = (4061723.41, 594803.44). 2024-11-06T03:09:54Z- TerrainMapOctree: Min Distance = 59.69. 2024-11-06T03:09:54Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:09:54Z- Forcing Subcloud Comparison 2024-11-06T03:09:54Z- Weighting particles with subcloud comparison 2024-11-06T03:09:54Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:54Z- meanExpectedMeasDiff: -3.384938 delta_rms_squared: 2.742039 alpha: 0.992759 2024-11-06T03:09:54Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:54Z- meanExpectedMeasDiff: 1.378127 delta_rms_squared: 1.034780 alpha: 0.981039 2024-11-06T03:09:54Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:54Z- meanExpectedMeasDiff: -2.371096 delta_rms_squared: 1.217387 alpha: 0.983837 2024-11-06T03:09:54Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:54Z- meanExpectedMeasDiff: 2.593247 delta_rms_squared: 1.234992 alpha: 0.984064 2024-11-06T03:09:54Z- Subcloud NIS: 0.803555 2024-11-06T03:09:54Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:09:54Z- TNavPF:: sumWeights = 0.026013 2024-11-06T03:09:54Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:09:54Z- TNavPF::First Term of Map Covariance Matrix is 0.20 2024-11-06T03:09:54Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:09:54Z- TerrainNav::Resampling particle filter... 2024-11-06T03:09:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862593.00 sec. 2024-11-06T03:09:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:09:54Z- variances([0], [2], and psi_berg) = 0.777944 0.348509 0.000000 2024-11-06T03:09:54Z- publishEstimatesDecoder() - at 1730862594.59 sec 2024-11-06T03:09:54Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:09:54Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061755.84 594788.60 54.84 2024-11-06T03:09:54Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061756.66 594787.87 54.84 lat/lon: 36.70 -121.94 2024-11-06T03:09:54Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.78 0.35 0.65 0.00 2024-11-06T03:09:55Z- handleDepth()- Depth_Keller msg: 1730862595.00 epoch sec; seqNo:3058; depth 49.31 2024-11-06T03:09:55Z- handleDVL() - DVL msg epochMillisec: 1730862595.02 2024-11-06T03:09:55Z- handleDvl() - RDI_Pathfinder msg: 1730862595.02 sec; seqNo:1027; #beams:4 2024-11-06T03:09:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.86 , 32.28 , 33.80 , 36.35 2024-11-06T03:09:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.02 , -0.04 2024-11-06T03:09:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862595.04 epoch sec; seqNo:3058; 4061723.58 north; 594804.89 east; 36.70 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T03:09:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862594.59 2024-11-06T03:09:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:55Z- waiting for 1730862598.59, time is 1730862595.04 2024-11-06T03:09:55Z- AHRS_M2 msg: 1730862595.38 epoch sec; seqNo:2872 2024-11-06T03:09:55Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.47 psi 2024-11-06T03:09:55Z- handleDepth()- Depth_Keller msg: 1730862595.39 epoch sec; seqNo:3059; depth 49.29 2024-11-06T03:09:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862595.42 epoch sec; seqNo:3059; 4061723.58 north; 594804.89 east; 36.70 lat; -121.94 lon; 49.29 depth; utmZone: 10 2024-11-06T03:09:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862594.59 2024-11-06T03:09:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:55Z- waiting for 1730862598.59, time is 1730862595.42 2024-11-06T03:09:55Z- AHRS_M2 msg: 1730862595.79 epoch sec; seqNo:2873 2024-11-06T03:09:55Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.47 psi 2024-11-06T03:09:55Z- handleDepth()- Depth_Keller msg: 1730862595.79 epoch sec; seqNo:3060; depth 49.28 2024-11-06T03:09:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862595.82 epoch sec; seqNo:3060; 4061723.59 north; 594806.11 east; 36.70 lat; -121.94 lon; 49.28 depth; utmZone: 10 2024-11-06T03:09:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862594.59 2024-11-06T03:09:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:55Z- waiting for 1730862598.59, time is 1730862595.82 2024-11-06T03:09:56Z- AHRS_M2 msg: 1730862596.19 epoch sec; seqNo:2874 2024-11-06T03:09:56Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.47 psi 2024-11-06T03:09:56Z- handleDepth()- Depth_Keller msg: 1730862596.19 epoch sec; seqNo:3061; depth 49.27 2024-11-06T03:09:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862596.21 epoch sec; seqNo:3061; 4061723.59 north; 594806.11 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T03:09:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862594.59 2024-11-06T03:09:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:56Z- waiting for 1730862598.59, time is 1730862596.21 2024-11-06T03:09:56Z- AHRS_M2 msg: 1730862596.66 epoch sec; seqNo:2875 2024-11-06T03:09:56Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.47 psi 2024-11-06T03:09:56Z- handleDepth()- Depth_Keller msg: 1730862596.72 epoch sec; seqNo:3062; depth 49.25 2024-11-06T03:09:56Z- handleDVL() - DVL msg epochMillisec: 1730862596.86 2024-11-06T03:09:56Z- handleDvl() - RDI_Pathfinder msg: 1730862596.86 sec; seqNo:1028; #beams:4 2024-11-06T03:09:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.86 , 32.44 , 33.97 , 36.35 2024-11-06T03:09:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.02 , -0.04 2024-11-06T03:09:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862594.59 2024-11-06T03:09:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:56Z- waiting for 1730862598.59, time is 1730862596.86 2024-11-06T03:09:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862596.87 epoch sec; seqNo:3062; 4061723.59 north; 594806.11 east; 36.70 lat; -121.94 lon; 49.25 depth; utmZone: 10 2024-11-06T03:09:57Z- 1 msgs in 200 ms and 2 handle calls 2024-11-06T03:09:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862594.59 2024-11-06T03:09:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:57Z- waiting for 1730862598.59, time is 1730862596.87 2024-11-06T03:09:57Z- AHRS_M2 msg: 1730862597.07 epoch sec; seqNo:2876 2024-11-06T03:09:57Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.47 psi 2024-11-06T03:09:57Z- handleDepth()- Depth_Keller msg: 1730862597.08 epoch sec; seqNo:3063; depth 49.26 2024-11-06T03:09:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862597.10 epoch sec; seqNo:3063; 4061723.60 north; 594807.33 east; 36.70 lat; -121.94 lon; 49.26 depth; utmZone: 10 2024-11-06T03:09:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862594.59 2024-11-06T03:09:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:57Z- waiting for 1730862598.59, time is 1730862597.10 2024-11-06T03:09:57Z- AHRS_M2 msg: 1730862597.47 epoch sec; seqNo:2877 2024-11-06T03:09:57Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.47 psi 2024-11-06T03:09:57Z- handleDepth()- Depth_Keller msg: 1730862597.48 epoch sec; seqNo:3064; depth 49.25 2024-11-06T03:09:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862597.49 epoch sec; seqNo:3064; 4061723.60 north; 594807.33 east; 36.70 lat; -121.94 lon; 49.25 depth; utmZone: 10 2024-11-06T03:09:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862594.59 2024-11-06T03:09:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:57Z- waiting for 1730862598.59, time is 1730862597.49 2024-11-06T03:09:57Z- AHRS_M2 msg: 1730862597.87 epoch sec; seqNo:2878 2024-11-06T03:09:57Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.48 psi 2024-11-06T03:09:57Z- handleDepth()- Depth_Keller msg: 1730862597.88 epoch sec; seqNo:3065; depth 49.25 2024-11-06T03:09:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862597.90 epoch sec; seqNo:3065; 4061723.60 north; 594807.33 east; 36.70 lat; -121.94 lon; 49.25 depth; utmZone: 10 2024-11-06T03:09:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862594.59 2024-11-06T03:09:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:58Z- waiting for 1730862598.59, time is 1730862597.90 2024-11-06T03:09:58Z- AHRS_M2 msg: 1730862598.28 epoch sec; seqNo:2879 2024-11-06T03:09:58Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.48 psi 2024-11-06T03:09:58Z- handleDepth()- Depth_Keller msg: 1730862598.29 epoch sec; seqNo:3066; depth 49.24 2024-11-06T03:09:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862598.30 epoch sec; seqNo:3066; 4061723.60 north; 594807.33 east; 36.70 lat; -121.94 lon; 49.24 depth; utmZone: 10 2024-11-06T03:09:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862594.59 2024-11-06T03:09:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:58Z- waiting for 1730862598.59, time is 1730862598.30 2024-11-06T03:09:58Z- AHRS_M2 msg: 1730862598.69 epoch sec; seqNo:2880 2024-11-06T03:09:58Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.48 psi 2024-11-06T03:09:58Z- handleDepth()- Depth_Keller msg: 1730862598.70 epoch sec; seqNo:3067; depth 49.25 2024-11-06T03:09:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862598.75 epoch sec; seqNo:3067; 4061724.00 north; 594808.54 east; 36.70 lat; -121.94 lon; 49.25 depth; utmZone: 10 2024-11-06T03:09:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:09:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862594.59 2024-11-06T03:09:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:58Z- waiting for 1730862598.59, time is 1730862598.75 2024-11-06T03:09:58Z- LcmTrn::updateTrn() >>>> pose time 1730862598.75, meas time 1730862596.86 <<<< 2024-11-06T03:09:58Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862598.75 2024-11-06T03:09:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862596.86 sec, ping # 0; waiting for INS data more recent than 1730862594.59... 2024-11-06T03:09:58Z- TerrainNav::motionUpdate() - currEstimate = (4061724, 594808.54, 49.25) 2024-11-06T03:09:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:09:58Z- TerrainNav:: SubcloudNIS is 0.80, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:09:58Z- TNavPF::Projecting Measurements 2024-11-06T03:09:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:09:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:09:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:09:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:09:58Z- TNavFilter::4 good beams. 2024-11-06T03:09:58Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:09:58Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:09:58Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:09:58Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:09:58Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:09:58Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:09:58Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:09:58Z- TerrainMapOctree: (vehN, vehE ) = (4061723.81, 594806.88). 2024-11-06T03:09:58Z- TerrainMapOctree: Min Distance = 62.62. 2024-11-06T03:09:58Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:09:58Z- Forcing Subcloud Comparison 2024-11-06T03:09:58Z- Weighting particles with subcloud comparison 2024-11-06T03:09:58Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:58Z- meanExpectedMeasDiff: -3.462774 delta_rms_squared: 2.386317 alpha: 0.991689 2024-11-06T03:09:58Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:58Z- meanExpectedMeasDiff: 1.363065 delta_rms_squared: 1.036932 alpha: 0.981077 2024-11-06T03:09:58Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:58Z- meanExpectedMeasDiff: -1.779929 delta_rms_squared: 1.214509 alpha: 0.983799 2024-11-06T03:09:58Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:09:58Z- meanExpectedMeasDiff: 2.356036 delta_rms_squared: 1.255667 alpha: 0.984322 2024-11-06T03:09:59Z- Subcloud NIS: 0.822795 2024-11-06T03:09:59Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:09:59Z- TNavPF:: sumWeights = 0.037885 2024-11-06T03:09:59Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:09:59Z- TNavPF::First Term of Map Covariance Matrix is 0.33 2024-11-06T03:09:59Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:09:59Z- TerrainNav::Resampling particle filter... 2024-11-06T03:09:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862596.86 sec. 2024-11-06T03:09:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:09:59Z- variances([0], [2], and psi_berg) = 1.011391 0.414340 0.000000 2024-11-06T03:09:59Z- publishEstimatesDecoder() - at 1730862598.75 sec 2024-11-06T03:09:59Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:09:59Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061756.27 594792.30 54.03 2024-11-06T03:09:59Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061756.78 594791.62 55.02 lat/lon: 36.70 -121.94 2024-11-06T03:09:59Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.01 0.41 0.62 0.00 2024-11-06T03:09:59Z- AHRS_M2 msg: 1730862599.09 epoch sec; seqNo:2881 2024-11-06T03:09:59Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.47 psi 2024-11-06T03:09:59Z- handleDepth()- Depth_Keller msg: 1730862599.10 epoch sec; seqNo:3068; depth 49.25 2024-11-06T03:09:59Z- handleDVL() - DVL msg epochMillisec: 1730862599.13 2024-11-06T03:09:59Z- handleDvl() - RDI_Pathfinder msg: 1730862599.13 sec; seqNo:1029; #beams:4 2024-11-06T03:09:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.69 , 32.28 , 34.31 , 36.35 2024-11-06T03:09:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.07 , 0.01 2024-11-06T03:09:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862599.16 epoch sec; seqNo:3068; 4061724.00 north; 594808.54 east; 36.70 lat; -121.94 lon; 49.25 depth; utmZone: 10 2024-11-06T03:09:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:09:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862598.75 2024-11-06T03:09:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:59Z- waiting for 1730862602.75, time is 1730862599.16 2024-11-06T03:09:59Z- handleDepth()- Depth_Keller msg: 1730862599.51 epoch sec; seqNo:3069; depth 49.27 2024-11-06T03:09:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862599.53 epoch sec; seqNo:3069; 4061724.00 north; 594808.54 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T03:09:59Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:09:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862598.75 2024-11-06T03:09:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:09:59Z- waiting for 1730862602.75, time is 1730862599.53 2024-11-06T03:09:59Z- AHRS_M2 msg: 1730862599.89 epoch sec; seqNo:2882 2024-11-06T03:09:59Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.47 psi 2024-11-06T03:09:59Z- handleDepth()- Depth_Keller msg: 1730862599.90 epoch sec; seqNo:3070; depth 49.27 2024-11-06T03:09:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862599.92 epoch sec; seqNo:3070; 4061724.00 north; 594808.54 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T03:10:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862598.75 2024-11-06T03:10:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:00Z- waiting for 1730862602.75, time is 1730862599.92 2024-11-06T03:10:00Z- AHRS_M2 msg: 1730862600.31 epoch sec; seqNo:2883 2024-11-06T03:10:00Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.46 psi 2024-11-06T03:10:00Z- handleDepth()- Depth_Keller msg: 1730862600.31 epoch sec; seqNo:3071; depth 49.26 2024-11-06T03:10:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862600.32 epoch sec; seqNo:3071; 4061724.01 north; 594809.76 east; 36.70 lat; -121.94 lon; 49.26 depth; utmZone: 10 2024-11-06T03:10:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862598.75 2024-11-06T03:10:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:00Z- waiting for 1730862602.75, time is 1730862600.32 2024-11-06T03:10:00Z- AHRS_M2 msg: 1730862600.73 epoch sec; seqNo:2884 2024-11-06T03:10:00Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.46 psi 2024-11-06T03:10:00Z- handleDepth()- Depth_Keller msg: 1730862600.75 epoch sec; seqNo:3072; depth 49.28 2024-11-06T03:10:00Z- handleDVL() - DVL msg epochMillisec: 1730862600.78 2024-11-06T03:10:00Z- handleDvl() - RDI_Pathfinder msg: 1730862600.78 sec; seqNo:1030; #beams:4 2024-11-06T03:10:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.18 , 32.44 , 33.97 , 37.03 2024-11-06T03:10:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.07 , 0.09 2024-11-06T03:10:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862600.80 epoch sec; seqNo:3072; 4061724.01 north; 594809.76 east; 36.70 lat; -121.94 lon; 49.28 depth; utmZone: 10 2024-11-06T03:10:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862598.75 2024-11-06T03:10:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:00Z- waiting for 1730862602.75, time is 1730862600.80 2024-11-06T03:10:01Z- AHRS_M2 msg: 1730862601.11 epoch sec; seqNo:2885 2024-11-06T03:10:01Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.46 psi 2024-11-06T03:10:01Z- handleDepth()- Depth_Keller msg: 1730862601.11 epoch sec; seqNo:3073; depth 49.29 2024-11-06T03:10:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862601.13 epoch sec; seqNo:3073; 4061724.39 north; 594809.76 east; 36.70 lat; -121.94 lon; 49.29 depth; utmZone: 10 2024-11-06T03:10:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862598.75 2024-11-06T03:10:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:01Z- waiting for 1730862602.75, time is 1730862601.13 2024-11-06T03:10:01Z- AHRS_M2 msg: 1730862601.51 epoch sec; seqNo:2886 2024-11-06T03:10:01Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.46 psi 2024-11-06T03:10:01Z- handleDepth()- Depth_Keller msg: 1730862601.52 epoch sec; seqNo:3074; depth 49.29 2024-11-06T03:10:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862601.53 epoch sec; seqNo:3074; 4061724.39 north; 594809.76 east; 36.70 lat; -121.94 lon; 49.29 depth; utmZone: 10 2024-11-06T03:10:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862598.75 2024-11-06T03:10:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:01Z- waiting for 1730862602.75, time is 1730862601.53 2024-11-06T03:10:01Z- AHRS_M2 msg: 1730862601.92 epoch sec; seqNo:2887 2024-11-06T03:10:01Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.45 psi 2024-11-06T03:10:01Z- handleDepth()- Depth_Keller msg: 1730862601.92 epoch sec; seqNo:3075; depth 49.31 2024-11-06T03:10:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862601.93 epoch sec; seqNo:3075; 4061724.40 north; 594810.98 east; 36.70 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T03:10:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862598.75 2024-11-06T03:10:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:02Z- waiting for 1730862602.75, time is 1730862601.93 2024-11-06T03:10:02Z- AHRS_M2 msg: 1730862602.32 epoch sec; seqNo:2888 2024-11-06T03:10:02Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.45 psi 2024-11-06T03:10:02Z- handleDepth()- Depth_Keller msg: 1730862602.34 epoch sec; seqNo:3076; depth 49.32 2024-11-06T03:10:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862602.35 epoch sec; seqNo:3076; 4061724.40 north; 594810.98 east; 36.70 lat; -121.94 lon; 49.32 depth; utmZone: 10 2024-11-06T03:10:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862598.75 2024-11-06T03:10:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:02Z- waiting for 1730862602.75, time is 1730862602.35 2024-11-06T03:10:02Z- AHRS_M2 msg: 1730862602.76 epoch sec; seqNo:2889 2024-11-06T03:10:02Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.45 psi 2024-11-06T03:10:02Z- handleDepth()- Depth_Keller msg: 1730862602.77 epoch sec; seqNo:3077; depth 49.32 2024-11-06T03:10:02Z- handleDVL() - DVL msg epochMillisec: 1730862602.84 2024-11-06T03:10:02Z- handleDvl() - RDI_Pathfinder msg: 1730862602.84 sec; seqNo:1031; #beams:4 2024-11-06T03:10:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.52 , 32.62 , 33.97 , 37.37 2024-11-06T03:10:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , 0.11 2024-11-06T03:10:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862602.87 epoch sec; seqNo:3077; 4061724.40 north; 594810.98 east; 36.70 lat; -121.94 lon; 49.32 depth; utmZone: 10 2024-11-06T03:10:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862598.75 2024-11-06T03:10:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:02Z- waiting for 1730862602.75, time is 1730862602.87 2024-11-06T03:10:02Z- LcmTrn::updateTrn() >>>> pose time 1730862602.87, meas time 1730862602.84 <<<< 2024-11-06T03:10:02Z- LcmTrn::updateTrn() equating pose 1730862602.84 and meas times 1730862602.84 2024-11-06T03:10:02Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862602.84 2024-11-06T03:10:02Z- TerrainNav::motionUpdate() - currEstimate = (4061724, 594810.98, 49.32) 2024-11-06T03:10:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:10:02Z- TerrainNav:: SubcloudNIS is 0.82, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:10:02Z- TNavPF::Projecting Measurements 2024-11-06T03:10:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:10:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:10:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:10:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:10:02Z- TNavFilter::4 good beams. 2024-11-06T03:10:02Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:10:02Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:10:02Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:10:02Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:10:02Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:10:02Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:10:02Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:10:02Z- TerrainMapOctree: (vehN, vehE ) = (4061724.40, 594810.98). 2024-11-06T03:10:02Z- TerrainMapOctree: Min Distance = 66.13. 2024-11-06T03:10:02Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:10:03Z- Forcing Subcloud Comparison 2024-11-06T03:10:03Z- Weighting particles with subcloud comparison 2024-11-06T03:10:03Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:03Z- meanExpectedMeasDiff: -2.100053 delta_rms_squared: 2.153497 alpha: 0.990798 2024-11-06T03:10:03Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:03Z- meanExpectedMeasDiff: 0.593219 delta_rms_squared: 1.173077 alpha: 0.983237 2024-11-06T03:10:03Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:03Z- meanExpectedMeasDiff: -0.983937 delta_rms_squared: 1.173224 alpha: 0.983239 2024-11-06T03:10:03Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:03Z- meanExpectedMeasDiff: 3.451427 delta_rms_squared: 1.223083 alpha: 0.983911 2024-11-06T03:10:03Z- Subcloud NIS: 0.841361 2024-11-06T03:10:03Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:10:03Z- TNavPF:: sumWeights = 0.071813 2024-11-06T03:10:03Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:10:03Z- TNavPF::First Term of Map Covariance Matrix is 0.31 2024-11-06T03:10:03Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T03:10:03Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862602.84 sec, ping # 0. 2024-11-06T03:10:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:10:03Z- variances([0], [2], and psi_berg) = 0.947859 0.396670 0.000000 2024-11-06T03:10:03Z- publishEstimatesDecoder() - at 1730862602.84 sec 2024-11-06T03:10:03Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:10:03Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061755.88 594794.96 54.91 2024-11-06T03:10:03Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061757.05 594794.06 55.05 lat/lon: 36.70 -121.94 2024-11-06T03:10:03Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.95 0.40 0.37 0.00 2024-11-06T03:10:03Z- AHRS_M2 msg: 1730862603.16 epoch sec; seqNo:2890 2024-11-06T03:10:03Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.46 psi 2024-11-06T03:10:03Z- handleDepth()- Depth_Keller msg: 1730862603.17 epoch sec; seqNo:3078; depth 49.34 2024-11-06T03:10:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862603.19 epoch sec; seqNo:3078; 4061724.40 north; 594810.98 east; 36.70 lat; -121.94 lon; 49.34 depth; utmZone: 10 2024-11-06T03:10:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862602.84 2024-11-06T03:10:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:03Z- waiting for 1730862606.84, time is 1730862603.19 2024-11-06T03:10:03Z- AHRS_M2 msg: 1730862603.53 epoch sec; seqNo:2891 2024-11-06T03:10:03Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 1.46 psi 2024-11-06T03:10:03Z- handleDepth()- Depth_Keller msg: 1730862603.54 epoch sec; seqNo:3079; depth 49.34 2024-11-06T03:10:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862603.55 epoch sec; seqNo:3079; 4061724.42 north; 594812.20 east; 36.70 lat; -121.94 lon; 49.34 depth; utmZone: 10 2024-11-06T03:10:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862602.84 2024-11-06T03:10:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:03Z- waiting for 1730862606.84, time is 1730862603.55 2024-11-06T03:10:03Z- AHRS_M2 msg: 1730862603.93 epoch sec; seqNo:2892 2024-11-06T03:10:03Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.46 psi 2024-11-06T03:10:03Z- handleDepth()- Depth_Keller msg: 1730862603.94 epoch sec; seqNo:3080; depth 49.34 2024-11-06T03:10:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862603.96 epoch sec; seqNo:3080; 4061724.80 north; 594812.20 east; 36.70 lat; -121.94 lon; 49.34 depth; utmZone: 10 2024-11-06T03:10:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862602.84 2024-11-06T03:10:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:04Z- waiting for 1730862606.84, time is 1730862603.96 2024-11-06T03:10:04Z- AHRS_M2 msg: 1730862604.34 epoch sec; seqNo:2893 2024-11-06T03:10:04Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.46 psi 2024-11-06T03:10:04Z- handleDepth()- Depth_Keller msg: 1730862604.35 epoch sec; seqNo:3081; depth 49.35 2024-11-06T03:10:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862604.35 epoch sec; seqNo:3081; 4061724.80 north; 594812.20 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:10:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862602.84 2024-11-06T03:10:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:04Z- waiting for 1730862606.84, time is 1730862604.35 2024-11-06T03:10:04Z- AHRS_M2 msg: 1730862604.77 epoch sec; seqNo:2894 2024-11-06T03:10:04Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T03:10:04Z- handleDepth()- Depth_Keller msg: 1730862604.78 epoch sec; seqNo:3082; depth 49.36 2024-11-06T03:10:04Z- handleDVL() - DVL msg epochMillisec: 1730862604.80 2024-11-06T03:10:04Z- handleDvl() - RDI_Pathfinder msg: 1730862604.80 sec; seqNo:1032; #beams:4 2024-11-06T03:10:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.04 , 31.94 , 33.64 , 37.54 2024-11-06T03:10:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.05 , 0.05 2024-11-06T03:10:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862604.82 epoch sec; seqNo:3082; 4061724.81 north; 594813.42 east; 36.70 lat; -121.94 lon; 49.36 depth; utmZone: 10 2024-11-06T03:10:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862602.84 2024-11-06T03:10:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:04Z- waiting for 1730862606.84, time is 1730862604.82 2024-11-06T03:10:05Z- AHRS_M2 msg: 1730862605.15 epoch sec; seqNo:2895 2024-11-06T03:10:05Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T03:10:05Z- handleDepth()- Depth_Keller msg: 1730862605.15 epoch sec; seqNo:3083; depth 49.35 2024-11-06T03:10:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862605.17 epoch sec; seqNo:3083; 4061724.81 north; 594813.42 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:10:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862602.84 2024-11-06T03:10:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:05Z- waiting for 1730862606.84, time is 1730862605.17 2024-11-06T03:10:05Z- AHRS_M2 msg: 1730862605.56 epoch sec; seqNo:2896 2024-11-06T03:10:05Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.47 psi 2024-11-06T03:10:05Z- handleDepth()- Depth_Keller msg: 1730862605.57 epoch sec; seqNo:3084; depth 49.34 2024-11-06T03:10:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862605.58 epoch sec; seqNo:3084; 4061724.81 north; 594813.42 east; 36.70 lat; -121.94 lon; 49.34 depth; utmZone: 10 2024-11-06T03:10:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862602.84 2024-11-06T03:10:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:05Z- waiting for 1730862606.84, time is 1730862605.58 2024-11-06T03:10:05Z- AHRS_M2 msg: 1730862605.95 epoch sec; seqNo:2897 2024-11-06T03:10:05Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.48 psi 2024-11-06T03:10:05Z- handleDepth()- Depth_Keller msg: 1730862605.97 epoch sec; seqNo:3085; depth 49.35 2024-11-06T03:10:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862605.97 epoch sec; seqNo:3085; 4061724.82 north; 594814.64 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:10:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862602.84 2024-11-06T03:10:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:06Z- waiting for 1730862606.84, time is 1730862605.97 2024-11-06T03:10:06Z- AHRS_M2 msg: 1730862606.37 epoch sec; seqNo:2898 2024-11-06T03:10:06Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.48 psi 2024-11-06T03:10:06Z- handleDepth()- Depth_Keller msg: 1730862606.37 epoch sec; seqNo:3086; depth 49.34 2024-11-06T03:10:06Z- handleDVL() - DVL msg epochMillisec: 1730862606.37 2024-11-06T03:10:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:10:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:10:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:10:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:10:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:10:06Z- handleDvl() - RDI_Pathfinder msg: 1730862606.37 sec; seqNo:1033; #beams:0 2024-11-06T03:10:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:10:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.05 , 0.05 2024-11-06T03:10:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862606.38 epoch sec; seqNo:3086; 4061724.82 north; 594814.64 east; 36.70 lat; -121.94 lon; 49.34 depth; utmZone: 10 2024-11-06T03:10:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862602.84 2024-11-06T03:10:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:06Z- waiting for 1730862606.84, time is 1730862606.38 2024-11-06T03:10:06Z- AHRS_M2 msg: 1730862606.77 epoch sec; seqNo:2899 2024-11-06T03:10:06Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.48 psi 2024-11-06T03:10:06Z- handleDepth()- Depth_Keller msg: 1730862606.78 epoch sec; seqNo:3087; depth 49.35 2024-11-06T03:10:06Z- handleDVL() - DVL msg epochMillisec: 1730862606.80 2024-11-06T03:10:06Z- handleDvl() - RDI_Pathfinder msg: 1730862606.80 sec; seqNo:1034; #beams:4 2024-11-06T03:10:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.24 , 31.77 , 33.63 , 37.37 2024-11-06T03:10:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , -0.05 2024-11-06T03:10:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862606.82 epoch sec; seqNo:3087; 4061724.82 north; 594814.64 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:10:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862602.84 2024-11-06T03:10:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:06Z- waiting for 1730862606.84, time is 1730862606.82 2024-11-06T03:10:07Z- AHRS_M2 msg: 1730862607.17 epoch sec; seqNo:2900 2024-11-06T03:10:07Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.48 psi 2024-11-06T03:10:07Z- handleDepth()- Depth_Keller msg: 1730862607.17 epoch sec; seqNo:3088; depth 49.34 2024-11-06T03:10:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862607.20 epoch sec; seqNo:3088; 4061724.82 north; 594814.64 east; 36.70 lat; -121.94 lon; 49.34 depth; utmZone: 10 2024-11-06T03:10:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862602.84 2024-11-06T03:10:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:07Z- waiting for 1730862606.84, time is 1730862607.20 2024-11-06T03:10:07Z- LcmTrn::updateTrn() >>>> pose time 1730862607.20, meas time 1730862606.80 <<<< 2024-11-06T03:10:07Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862607.20 2024-11-06T03:10:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862606.80 sec, ping # 0; waiting for INS data more recent than 1730862602.84... 2024-11-06T03:10:07Z- TerrainNav::motionUpdate() - currEstimate = (4061725, 594814.64, 49.34) 2024-11-06T03:10:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:10:07Z- TerrainNav:: SubcloudNIS is 0.84, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:10:07Z- TNavPF::Projecting Measurements 2024-11-06T03:10:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:10:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:10:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:10:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:10:07Z- TNavFilter::4 good beams. 2024-11-06T03:10:07Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:10:07Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:10:07Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:10:07Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:10:07Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:10:07Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:10:07Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:10:07Z- TerrainMapOctree: (vehN, vehE ) = (4061724.78, 594814.30). 2024-11-06T03:10:07Z- TerrainMapOctree: Min Distance = 69.07. 2024-11-06T03:10:07Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:10:07Z- Forcing Subcloud Comparison 2024-11-06T03:10:07Z- Weighting particles with subcloud comparison 2024-11-06T03:10:07Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:07Z- meanExpectedMeasDiff: -2.770364 delta_rms_squared: 3.063429 alpha: 0.993514 2024-11-06T03:10:07Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:07Z- meanExpectedMeasDiff: 0.034596 delta_rms_squared: 0.695384 alpha: 0.972043 2024-11-06T03:10:07Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:07Z- meanExpectedMeasDiff: -3.716866 delta_rms_squared: 3.811109 alpha: 0.994780 2024-11-06T03:10:07Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:07Z- meanExpectedMeasDiff: 2.940005 delta_rms_squared: 0.879654 alpha: 0.977769 2024-11-06T03:10:07Z- Subcloud NIS: 0.858659 2024-11-06T03:10:07Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:10:07Z- TNavPF:: sumWeights = 0.033895 2024-11-06T03:10:07Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:10:07Z- TNavPF::First Term of Map Covariance Matrix is 0.12 2024-11-06T03:10:07Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:10:07Z- TerrainNav::Resampling particle filter... 2024-11-06T03:10:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862606.80 sec. 2024-11-06T03:10:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:10:07Z- variances([0], [2], and psi_berg) = 1.195041 0.423799 0.000000 2024-11-06T03:10:07Z- publishEstimatesDecoder() - at 1730862607.20 sec 2024-11-06T03:10:07Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:10:07Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061757.12 594798.43 54.63 2024-11-06T03:10:07Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061756.88 594797.45 55.41 lat/lon: 36.70 -121.94 2024-11-06T03:10:07Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.20 0.42 0.56 0.00 2024-11-06T03:10:07Z- AHRS_M2 msg: 1730862607.57 epoch sec; seqNo:2901 2024-11-06T03:10:07Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.48 psi 2024-11-06T03:10:07Z- handleDepth()- Depth_Keller msg: 1730862607.59 epoch sec; seqNo:3089; depth 49.35 2024-11-06T03:10:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862607.63 epoch sec; seqNo:3089; 4061724.84 north; 594815.86 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:10:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862607.20 2024-11-06T03:10:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:07Z- waiting for 1730862611.20, time is 1730862607.63 2024-11-06T03:10:07Z- AHRS_M2 msg: 1730862607.97 epoch sec; seqNo:2902 2024-11-06T03:10:07Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.48 psi 2024-11-06T03:10:07Z- handleDepth()- Depth_Keller msg: 1730862607.99 epoch sec; seqNo:3090; depth 49.33 2024-11-06T03:10:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862608.00 epoch sec; seqNo:3090; 4061724.84 north; 594815.86 east; 36.70 lat; -121.94 lon; 49.33 depth; utmZone: 10 2024-11-06T03:10:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862607.20 2024-11-06T03:10:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:08Z- waiting for 1730862611.20, time is 1730862608.00 2024-11-06T03:10:08Z- AHRS_M2 msg: 1730862608.38 epoch sec; seqNo:2903 2024-11-06T03:10:08Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.49 psi 2024-11-06T03:10:08Z- handleDepth()- Depth_Keller msg: 1730862608.38 epoch sec; seqNo:3091; depth 49.33 2024-11-06T03:10:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862608.39 epoch sec; seqNo:3091; 4061724.84 north; 594815.86 east; 36.70 lat; -121.94 lon; 49.33 depth; utmZone: 10 2024-11-06T03:10:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862607.20 2024-11-06T03:10:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:08Z- waiting for 1730862611.20, time is 1730862608.39 2024-11-06T03:10:08Z- AHRS_M2 msg: 1730862608.78 epoch sec; seqNo:2904 2024-11-06T03:10:08Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.49 psi 2024-11-06T03:10:08Z- handleDepth()- Depth_Keller msg: 1730862608.79 epoch sec; seqNo:3092; depth 49.32 2024-11-06T03:10:08Z- handleDVL() - DVL msg epochMillisec: 1730862608.82 2024-11-06T03:10:08Z- handleDvl() - RDI_Pathfinder msg: 1730862608.82 sec; seqNo:1035; #beams:4 2024-11-06T03:10:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.88 , 31.43 , 33.64 , 37.38 2024-11-06T03:10:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.00 , -0.08 2024-11-06T03:10:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862608.84 epoch sec; seqNo:3092; 4061724.85 north; 594817.08 east; 36.70 lat; -121.94 lon; 49.32 depth; utmZone: 10 2024-11-06T03:10:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862607.20 2024-11-06T03:10:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:08Z- waiting for 1730862611.20, time is 1730862608.84 2024-11-06T03:10:09Z- AHRS_M2 msg: 1730862609.19 epoch sec; seqNo:2905 2024-11-06T03:10:09Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.49 psi 2024-11-06T03:10:09Z- handleDepth()- Depth_Keller msg: 1730862609.19 epoch sec; seqNo:3093; depth 49.33 2024-11-06T03:10:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862609.21 epoch sec; seqNo:3093; 4061724.85 north; 594817.08 east; 36.70 lat; -121.94 lon; 49.33 depth; utmZone: 10 2024-11-06T03:10:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862607.20 2024-11-06T03:10:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:09Z- waiting for 1730862611.20, time is 1730862609.21 2024-11-06T03:10:09Z- AHRS_M2 msg: 1730862609.59 epoch sec; seqNo:2906 2024-11-06T03:10:09Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:09Z- handleDepth()- Depth_Keller msg: 1730862609.60 epoch sec; seqNo:3094; depth 49.32 2024-11-06T03:10:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862609.61 epoch sec; seqNo:3094; 4061724.85 north; 594817.08 east; 36.70 lat; -121.94 lon; 49.32 depth; utmZone: 10 2024-11-06T03:10:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862607.20 2024-11-06T03:10:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:09Z- waiting for 1730862611.20, time is 1730862609.61 2024-11-06T03:10:10Z- AHRS_M2 msg: 1730862609.99 epoch sec; seqNo:2907 2024-11-06T03:10:10Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:10Z- handleDepth()- Depth_Keller msg: 1730862610.00 epoch sec; seqNo:3095; depth 49.33 2024-11-06T03:10:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862610.01 epoch sec; seqNo:3095; 4061724.85 north; 594817.08 east; 36.70 lat; -121.94 lon; 49.33 depth; utmZone: 10 2024-11-06T03:10:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862607.20 2024-11-06T03:10:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:10Z- waiting for 1730862611.20, time is 1730862610.01 2024-11-06T03:10:10Z- AHRS_M2 msg: 1730862610.40 epoch sec; seqNo:2908 2024-11-06T03:10:10Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:10Z- handleDepth()- Depth_Keller msg: 1730862610.40 epoch sec; seqNo:3096; depth 49.33 2024-11-06T03:10:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862610.42 epoch sec; seqNo:3096; 4061724.86 north; 594818.30 east; 36.70 lat; -121.94 lon; 49.33 depth; utmZone: 10 2024-11-06T03:10:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862607.20 2024-11-06T03:10:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:10Z- waiting for 1730862611.20, time is 1730862610.42 2024-11-06T03:10:10Z- AHRS_M2 msg: 1730862610.81 epoch sec; seqNo:2909 2024-11-06T03:10:10Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.48 psi 2024-11-06T03:10:10Z- handleDepth()- Depth_Keller msg: 1730862610.83 epoch sec; seqNo:3097; depth 49.33 2024-11-06T03:10:10Z- handleDVL() - DVL msg epochMillisec: 1730862610.85 2024-11-06T03:10:10Z- handleDvl() - RDI_Pathfinder msg: 1730862610.85 sec; seqNo:1036; #beams:4 2024-11-06T03:10:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.71 , 31.09 , 33.80 , 37.37 2024-11-06T03:10:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.05 , -0.03 2024-11-06T03:10:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862610.88 epoch sec; seqNo:3097; 4061725.24 north; 594818.29 east; 36.70 lat; -121.94 lon; 49.33 depth; utmZone: 10 2024-11-06T03:10:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862607.20 2024-11-06T03:10:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:10Z- waiting for 1730862611.20, time is 1730862610.88 2024-11-06T03:10:11Z- AHRS_M2 msg: 1730862611.21 epoch sec; seqNo:2910 2024-11-06T03:10:11Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.48 psi 2024-11-06T03:10:11Z- handleDepth()- Depth_Keller msg: 1730862611.22 epoch sec; seqNo:3098; depth 49.34 2024-11-06T03:10:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862611.24 epoch sec; seqNo:3098; 4061725.24 north; 594818.29 east; 36.70 lat; -121.94 lon; 49.34 depth; utmZone: 10 2024-11-06T03:10:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862607.20 2024-11-06T03:10:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:11Z- waiting for 1730862611.20, time is 1730862611.24 2024-11-06T03:10:11Z- LcmTrn::updateTrn() >>>> pose time 1730862611.24, meas time 1730862610.85 <<<< 2024-11-06T03:10:11Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862611.24 2024-11-06T03:10:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862610.85 sec, ping # 0; waiting for INS data more recent than 1730862607.20... 2024-11-06T03:10:11Z- TerrainNav::motionUpdate() - currEstimate = (4061725, 594818.29, 49.34) 2024-11-06T03:10:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:10:11Z- TerrainNav:: SubcloudNIS is 0.86, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:10:11Z- TNavPF::Projecting Measurements 2024-11-06T03:10:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:10:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:10:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:10:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:10:11Z- TNavFilter::4 good beams. 2024-11-06T03:10:11Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:10:11Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:10:11Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:10:11Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:10:11Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:10:11Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:10:11Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:10:11Z- TerrainMapOctree: (vehN, vehE ) = (4061725.20, 594817.94). 2024-11-06T03:10:11Z- TerrainMapOctree: Min Distance = 72.33. 2024-11-06T03:10:11Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:10:11Z- Forcing Subcloud Comparison 2024-11-06T03:10:11Z- Weighting particles with subcloud comparison 2024-11-06T03:10:11Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:11Z- meanExpectedMeasDiff: -5.286985 delta_rms_squared: 2.824227 alpha: 0.992968 2024-11-06T03:10:11Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:11Z- meanExpectedMeasDiff: -0.319377 delta_rms_squared: 0.922565 alpha: 0.978781 2024-11-06T03:10:11Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:11Z- meanExpectedMeasDiff: -5.596834 delta_rms_squared: 3.153396 alpha: 0.993698 2024-11-06T03:10:11Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:11Z- meanExpectedMeasDiff: 3.109021 delta_rms_squared: 1.180920 alpha: 0.983346 2024-11-06T03:10:11Z- Subcloud NIS: 0.920666 2024-11-06T03:10:11Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:10:11Z- TNavPF:: sumWeights = 0.002428 2024-11-06T03:10:11Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:10:11Z- TNavPF::First Term of Map Covariance Matrix is 0.02 2024-11-06T03:10:11Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:10:11Z- TerrainNav::Resampling particle filter... 2024-11-06T03:10:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862610.85 sec. 2024-11-06T03:10:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:10:11Z- variances([0], [2], and psi_berg) = 0.565022 0.230227 0.000000 2024-11-06T03:10:11Z- publishEstimatesDecoder() - at 1730862611.24 sec 2024-11-06T03:10:11Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:10:11Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061758.08 594801.70 57.09 2024-11-06T03:10:11Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061756.33 594800.95 56.09 lat/lon: 36.70 -121.94 2024-11-06T03:10:11Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.23 0.60 0.00 2024-11-06T03:10:11Z- AHRS_M2 msg: 1730862611.61 epoch sec; seqNo:2911 2024-11-06T03:10:11Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.48 psi 2024-11-06T03:10:11Z- handleDepth()- Depth_Keller msg: 1730862611.64 epoch sec; seqNo:3099; depth 49.34 2024-11-06T03:10:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862611.66 epoch sec; seqNo:3099; 4061725.26 north; 594819.51 east; 36.70 lat; -121.94 lon; 49.34 depth; utmZone: 10 2024-11-06T03:10:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862611.24 2024-11-06T03:10:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:11Z- waiting for 1730862615.24, time is 1730862611.66 2024-11-06T03:10:12Z- AHRS_M2 msg: 1730862612.02 epoch sec; seqNo:2912 2024-11-06T03:10:12Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.47 psi 2024-11-06T03:10:12Z- handleDepth()- Depth_Keller msg: 1730862612.03 epoch sec; seqNo:3100; depth 49.35 2024-11-06T03:10:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862612.04 epoch sec; seqNo:3100; 4061725.26 north; 594819.51 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:10:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862611.24 2024-11-06T03:10:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:12Z- waiting for 1730862615.24, time is 1730862612.04 2024-11-06T03:10:12Z- AHRS_M2 msg: 1730862612.42 epoch sec; seqNo:2913 2024-11-06T03:10:12Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.47 psi 2024-11-06T03:10:12Z- handleDepth()- Depth_Keller msg: 1730862612.43 epoch sec; seqNo:3101; depth 49.37 2024-11-06T03:10:12Z- handleDVL() - DVL msg epochMillisec: 1730862612.44 2024-11-06T03:10:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:10:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:10:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:10:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:10:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:10:12Z- handleDvl() - RDI_Pathfinder msg: 1730862612.44 sec; seqNo:1037; #beams:0 2024-11-06T03:10:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:10:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.05 , -0.03 2024-11-06T03:10:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862612.45 epoch sec; seqNo:3101; 4061725.26 north; 594819.51 east; 36.70 lat; -121.94 lon; 49.37 depth; utmZone: 10 2024-11-06T03:10:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862611.24 2024-11-06T03:10:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:12Z- waiting for 1730862615.24, time is 1730862612.45 2024-11-06T03:10:12Z- AHRS_M2 msg: 1730862612.84 epoch sec; seqNo:2914 2024-11-06T03:10:12Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.47 psi 2024-11-06T03:10:12Z- handleDepth()- Depth_Keller msg: 1730862612.85 epoch sec; seqNo:3102; depth 49.40 2024-11-06T03:10:12Z- handleDVL() - DVL msg epochMillisec: 1730862612.88 2024-11-06T03:10:12Z- handleDvl() - RDI_Pathfinder msg: 1730862612.88 sec; seqNo:1038; #beams:4 2024-11-06T03:10:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.53 , 31.43 , 33.30 , 38.06 2024-11-06T03:10:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.06 , 0.05 2024-11-06T03:10:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862612.89 epoch sec; seqNo:3102; 4061725.26 north; 594819.51 east; 36.70 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T03:10:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862611.24 2024-11-06T03:10:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:13Z- waiting for 1730862615.24, time is 1730862612.89 2024-11-06T03:10:13Z- AHRS_M2 msg: 1730862613.23 epoch sec; seqNo:2915 2024-11-06T03:10:13Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.47 psi 2024-11-06T03:10:13Z- handleDepth()- Depth_Keller msg: 1730862613.24 epoch sec; seqNo:3103; depth 49.40 2024-11-06T03:10:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862613.26 epoch sec; seqNo:3103; 4061725.26 north; 594819.51 east; 36.70 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T03:10:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862611.24 2024-11-06T03:10:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:13Z- waiting for 1730862615.24, time is 1730862613.26 2024-11-06T03:10:13Z- AHRS_M2 msg: 1730862613.64 epoch sec; seqNo:2916 2024-11-06T03:10:13Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.46 psi 2024-11-06T03:10:13Z- handleDepth()- Depth_Keller msg: 1730862613.65 epoch sec; seqNo:3104; depth 49.42 2024-11-06T03:10:13Z- handleDVL() - DVL msg epochMillisec: 1730862613.65 2024-11-06T03:10:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:10:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:10:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:10:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:10:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:10:13Z- handleDvl() - RDI_Pathfinder msg: 1730862613.65 sec; seqNo:1039; #beams:0 2024-11-06T03:10:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:10:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.06 , 0.05 2024-11-06T03:10:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862613.67 epoch sec; seqNo:3104; 4061725.27 north; 594820.73 east; 36.70 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T03:10:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862611.24 2024-11-06T03:10:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:13Z- waiting for 1730862615.24, time is 1730862613.67 2024-11-06T03:10:14Z- AHRS_M2 msg: 1730862614.04 epoch sec; seqNo:2917 2024-11-06T03:10:14Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.46 psi 2024-11-06T03:10:14Z- handleDepth()- Depth_Keller msg: 1730862614.04 epoch sec; seqNo:3105; depth 49.45 2024-11-06T03:10:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862614.06 epoch sec; seqNo:3105; 4061725.65 north; 594820.73 east; 36.70 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:10:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862611.24 2024-11-06T03:10:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:14Z- waiting for 1730862615.24, time is 1730862614.06 2024-11-06T03:10:14Z- AHRS_M2 msg: 1730862614.44 epoch sec; seqNo:2918 2024-11-06T03:10:14Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.46 psi 2024-11-06T03:10:14Z- handleDepth()- Depth_Keller msg: 1730862614.45 epoch sec; seqNo:3106; depth 49.47 2024-11-06T03:10:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862614.46 epoch sec; seqNo:3106; 4061725.65 north; 594820.73 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:10:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862611.24 2024-11-06T03:10:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:14Z- waiting for 1730862615.24, time is 1730862614.46 2024-11-06T03:10:14Z- AHRS_M2 msg: 1730862614.85 epoch sec; seqNo:2919 2024-11-06T03:10:14Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.46 psi 2024-11-06T03:10:14Z- handleDepth()- Depth_Keller msg: 1730862614.85 epoch sec; seqNo:3107; depth 49.48 2024-11-06T03:10:14Z- handleDVL() - DVL msg epochMillisec: 1730862614.89 2024-11-06T03:10:14Z- handleDvl() - RDI_Pathfinder msg: 1730862614.89 sec; seqNo:1040; #beams:4 2024-11-06T03:10:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.35 , 31.43 , 33.12 , 37.54 2024-11-06T03:10:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , 0.08 2024-11-06T03:10:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862614.90 epoch sec; seqNo:3107; 4061725.65 north; 594820.73 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:10:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862611.24 2024-11-06T03:10:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:15Z- waiting for 1730862615.24, time is 1730862614.90 2024-11-06T03:10:15Z- AHRS_M2 msg: 1730862615.25 epoch sec; seqNo:2920 2024-11-06T03:10:15Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.46 psi 2024-11-06T03:10:15Z- handleDepth()- Depth_Keller msg: 1730862615.26 epoch sec; seqNo:3108; depth 49.52 2024-11-06T03:10:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862615.28 epoch sec; seqNo:3108; 4061725.66 north; 594821.95 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T03:10:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862611.24 2024-11-06T03:10:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:15Z- waiting for 1730862615.24, time is 1730862615.28 2024-11-06T03:10:15Z- LcmTrn::updateTrn() >>>> pose time 1730862615.28, meas time 1730862614.89 <<<< 2024-11-06T03:10:15Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862615.28 2024-11-06T03:10:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862614.89 sec, ping # 0; waiting for INS data more recent than 1730862611.24... 2024-11-06T03:10:15Z- TerrainNav::motionUpdate() - currEstimate = (4061726, 594821.95, 49.52) 2024-11-06T03:10:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:10:15Z- TerrainNav:: SubcloudNIS is 0.92, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:10:15Z- TNavPF::Projecting Measurements 2024-11-06T03:10:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:10:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:10:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:10:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:10:15Z- TNavFilter::4 good beams. 2024-11-06T03:10:15Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:10:15Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:10:15Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:10:15Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:10:15Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:10:15Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:10:15Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:10:15Z- TerrainMapOctree: (vehN, vehE ) = (4061725.62, 594821.60). 2024-11-06T03:10:15Z- TerrainMapOctree: Min Distance = 75.63. 2024-11-06T03:10:15Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:10:15Z- Forcing Subcloud Comparison 2024-11-06T03:10:15Z- Weighting particles with subcloud comparison 2024-11-06T03:10:15Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:15Z- meanExpectedMeasDiff: -5.871577 delta_rms_squared: 2.370718 alpha: 0.991634 2024-11-06T03:10:15Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:15Z- meanExpectedMeasDiff: -0.374649 delta_rms_squared: 0.965031 alpha: 0.979696 2024-11-06T03:10:15Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:15Z- meanExpectedMeasDiff: -6.736731 delta_rms_squared: 1.229087 alpha: 0.983988 2024-11-06T03:10:15Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:15Z- meanExpectedMeasDiff: 4.204998 delta_rms_squared: 1.229825 alpha: 0.983998 2024-11-06T03:10:15Z- Subcloud NIS: 1.049361 2024-11-06T03:10:15Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:10:15Z- TNavPF:: sumWeights = 0.000017 2024-11-06T03:10:15Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:10:15Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:10:15Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:10:15Z- TerrainNav::Resampling particle filter... 2024-11-06T03:10:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862614.89 sec. 2024-11-06T03:10:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:10:15Z- variances([0], [2], and psi_berg) = 0.456154 0.222200 0.000000 2024-11-06T03:10:15Z- publishEstimatesDecoder() - at 1730862615.28 sec 2024-11-06T03:10:15Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:10:15Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061758.46 594805.35 57.72 2024-11-06T03:10:15Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061756.61 594804.52 57.49 lat/lon: 36.70 -121.94 2024-11-06T03:10:15Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.46 0.22 0.60 0.00 2024-11-06T03:10:15Z- AHRS_M2 msg: 1730862615.67 epoch sec; seqNo:2921 2024-11-06T03:10:15Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.46 psi 2024-11-06T03:10:15Z- handleDepth()- Depth_Keller msg: 1730862615.68 epoch sec; seqNo:3109; depth 49.53 2024-11-06T03:10:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862615.70 epoch sec; seqNo:3109; 4061725.66 north; 594821.95 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T03:10:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862615.28 2024-11-06T03:10:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:15Z- waiting for 1730862619.28, time is 1730862615.70 2024-11-06T03:10:16Z- AHRS_M2 msg: 1730862616.06 epoch sec; seqNo:2922 2024-11-06T03:10:16Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.46 psi 2024-11-06T03:10:16Z- handleDepth()- Depth_Keller msg: 1730862616.06 epoch sec; seqNo:3110; depth 49.56 2024-11-06T03:10:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862616.07 epoch sec; seqNo:3110; 4061725.66 north; 594821.95 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:10:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862615.28 2024-11-06T03:10:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:16Z- waiting for 1730862619.28, time is 1730862616.07 2024-11-06T03:10:16Z- AHRS_M2 msg: 1730862616.46 epoch sec; seqNo:2923 2024-11-06T03:10:16Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.47 psi 2024-11-06T03:10:16Z- handleDepth()- Depth_Keller msg: 1730862616.47 epoch sec; seqNo:3111; depth 49.58 2024-11-06T03:10:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862616.48 epoch sec; seqNo:3111; 4061725.66 north; 594821.95 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:10:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862615.28 2024-11-06T03:10:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:16Z- waiting for 1730862619.28, time is 1730862616.48 2024-11-06T03:10:16Z- AHRS_M2 msg: 1730862616.87 epoch sec; seqNo:2924 2024-11-06T03:10:16Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.47 psi 2024-11-06T03:10:16Z- handleDepth()- Depth_Keller msg: 1730862616.87 epoch sec; seqNo:3112; depth 49.61 2024-11-06T03:10:16Z- handleDVL() - DVL msg epochMillisec: 1730862616.90 2024-11-06T03:10:16Z- handleDvl() - RDI_Pathfinder msg: 1730862616.90 sec; seqNo:1041; #beams:4 2024-11-06T03:10:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.18 , 31.94 , 33.12 , 37.71 2024-11-06T03:10:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.04 2024-11-06T03:10:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862616.92 epoch sec; seqNo:3112; 4061726.05 north; 594823.17 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:10:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862615.28 2024-11-06T03:10:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:17Z- waiting for 1730862619.28, time is 1730862616.92 2024-11-06T03:10:17Z- AHRS_M2 msg: 1730862617.27 epoch sec; seqNo:2925 2024-11-06T03:10:17Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.47 psi 2024-11-06T03:10:17Z- handleDepth()- Depth_Keller msg: 1730862617.28 epoch sec; seqNo:3113; depth 49.64 2024-11-06T03:10:17Z- handleDVL() - DVL msg epochMillisec: 1730862617.28 2024-11-06T03:10:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:10:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:10:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:10:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:10:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:10:17Z- handleDvl() - RDI_Pathfinder msg: 1730862617.28 sec; seqNo:1042; #beams:0 2024-11-06T03:10:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:10:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.04 2024-11-06T03:10:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862617.30 epoch sec; seqNo:3113; 4061726.05 north; 594823.17 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:10:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862615.28 2024-11-06T03:10:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:17Z- waiting for 1730862619.28, time is 1730862617.30 2024-11-06T03:10:17Z- AHRS_M2 msg: 1730862617.68 epoch sec; seqNo:2926 2024-11-06T03:10:17Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.47 psi 2024-11-06T03:10:17Z- handleDepth()- Depth_Keller msg: 1730862617.68 epoch sec; seqNo:3114; depth 49.66 2024-11-06T03:10:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862617.69 epoch sec; seqNo:3114; 4061726.05 north; 594823.17 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:10:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862615.28 2024-11-06T03:10:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:17Z- waiting for 1730862619.28, time is 1730862617.69 2024-11-06T03:10:18Z- AHRS_M2 msg: 1730862618.08 epoch sec; seqNo:2927 2024-11-06T03:10:18Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.47 psi 2024-11-06T03:10:18Z- handleDepth()- Depth_Keller msg: 1730862618.08 epoch sec; seqNo:3115; depth 49.68 2024-11-06T03:10:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862618.09 epoch sec; seqNo:3115; 4061726.07 north; 594824.39 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:10:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862615.28 2024-11-06T03:10:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:18Z- waiting for 1730862619.28, time is 1730862618.09 2024-11-06T03:10:18Z- AHRS_M2 msg: 1730862618.48 epoch sec; seqNo:2928 2024-11-06T03:10:18Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.48 psi 2024-11-06T03:10:18Z- handleDepth()- Depth_Keller msg: 1730862618.50 epoch sec; seqNo:3116; depth 49.70 2024-11-06T03:10:18Z- handleDVL() - DVL msg epochMillisec: 1730862618.50 2024-11-06T03:10:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:10:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:10:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:10:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:10:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:10:18Z- handleDvl() - RDI_Pathfinder msg: 1730862618.50 sec; seqNo:1043; #beams:0 2024-11-06T03:10:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:10:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.04 2024-11-06T03:10:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862618.51 epoch sec; seqNo:3116; 4061726.07 north; 594824.39 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:10:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862615.28 2024-11-06T03:10:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:18Z- waiting for 1730862619.28, time is 1730862618.51 2024-11-06T03:10:18Z- handleDepth()- Depth_Keller msg: 1730862618.91 epoch sec; seqNo:3117; depth 49.73 2024-11-06T03:10:18Z- handleDVL() - DVL msg epochMillisec: 1730862618.93 2024-11-06T03:10:18Z- handleDvl() - RDI_Pathfinder msg: 1730862618.93 sec; seqNo:1044; #beams:4 2024-11-06T03:10:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.70 , 31.43 , 33.13 , 37.54 2024-11-06T03:10:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.00 2024-11-06T03:10:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862618.95 epoch sec; seqNo:3117; 4061726.07 north; 594824.39 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:10:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862615.28 2024-11-06T03:10:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:19Z- waiting for 1730862619.28, time is 1730862618.95 2024-11-06T03:10:19Z- AHRS_M2 msg: 1730862619.29 epoch sec; seqNo:2929 2024-11-06T03:10:19Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 1.48 psi 2024-11-06T03:10:19Z- handleDepth()- Depth_Keller msg: 1730862619.30 epoch sec; seqNo:3118; depth 49.77 2024-11-06T03:10:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862619.31 epoch sec; seqNo:3118; 4061726.08 north; 594825.61 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:10:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862615.28 2024-11-06T03:10:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:19Z- waiting for 1730862619.28, time is 1730862619.31 2024-11-06T03:10:19Z- LcmTrn::updateTrn() >>>> pose time 1730862619.31, meas time 1730862618.93 <<<< 2024-11-06T03:10:19Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862619.31 2024-11-06T03:10:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862618.93 sec, ping # 0; waiting for INS data more recent than 1730862615.28... 2024-11-06T03:10:19Z- TerrainNav::motionUpdate() - currEstimate = (4061726, 594825.61, 49.77) 2024-11-06T03:10:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:10:19Z- TerrainNav:: SubcloudNIS is 1.05, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:10:19Z- TNavPF::Projecting Measurements 2024-11-06T03:10:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:10:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:10:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:10:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:10:19Z- TNavFilter::4 good beams. 2024-11-06T03:10:19Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:10:19Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:10:19Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:10:19Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:10:19Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:10:19Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:10:19Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:10:19Z- TerrainMapOctree: (vehN, vehE ) = (4061726.04, 594825.26). 2024-11-06T03:10:19Z- TerrainMapOctree: Min Distance = 78.99. 2024-11-06T03:10:19Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:10:19Z- Forcing Subcloud Comparison 2024-11-06T03:10:19Z- Weighting particles with subcloud comparison 2024-11-06T03:10:19Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:19Z- meanExpectedMeasDiff: -5.611982 delta_rms_squared: 1.672932 alpha: 0.988186 2024-11-06T03:10:19Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:19Z- meanExpectedMeasDiff: 1.308333 delta_rms_squared: 0.981629 alpha: 0.980033 2024-11-06T03:10:19Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:19Z- meanExpectedMeasDiff: -4.983428 delta_rms_squared: 1.066212 alpha: 0.981587 2024-11-06T03:10:19Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:19Z- meanExpectedMeasDiff: 5.489267 delta_rms_squared: 0.860606 alpha: 0.977288 2024-11-06T03:10:19Z- Subcloud NIS: 1.017441 2024-11-06T03:10:19Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:10:19Z- TNavPF:: sumWeights = 0.000018 2024-11-06T03:10:19Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:10:19Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:10:19Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:10:19Z- TerrainNav::Resampling particle filter... 2024-11-06T03:10:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862618.93 sec. 2024-11-06T03:10:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:10:19Z- variances([0], [2], and psi_berg) = 0.259134 0.217465 0.000000 2024-11-06T03:10:19Z- publishEstimatesDecoder() - at 1730862619.31 sec 2024-11-06T03:10:19Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:10:19Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061758.88 594809.07 59.31 2024-11-06T03:10:19Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061756.87 594808.37 58.62 lat/lon: 36.70 -121.94 2024-11-06T03:10:19Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.22 0.63 0.00 2024-11-06T03:10:19Z- AHRS_M2 msg: 1730862619.70 epoch sec; seqNo:2930 2024-11-06T03:10:19Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 1.48 psi 2024-11-06T03:10:19Z- handleDepth()- Depth_Keller msg: 1730862619.71 epoch sec; seqNo:3119; depth 49.77 2024-11-06T03:10:19Z- handleDVL() - DVL msg epochMillisec: 1730862619.72 2024-11-06T03:10:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:10:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:10:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:10:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:10:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:10:19Z- handleDvl() - RDI_Pathfinder msg: 1730862619.72 sec; seqNo:1045; #beams:0 2024-11-06T03:10:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:10:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.00 2024-11-06T03:10:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862619.74 epoch sec; seqNo:3119; 4061726.08 north; 594825.61 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:10:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862619.31 2024-11-06T03:10:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:19Z- waiting for 1730862623.31, time is 1730862619.74 2024-11-06T03:10:20Z- AHRS_M2 msg: 1730862620.10 epoch sec; seqNo:2931 2024-11-06T03:10:20Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 1.48 psi 2024-11-06T03:10:20Z- handleDepth()- Depth_Keller msg: 1730862620.10 epoch sec; seqNo:3120; depth 49.78 2024-11-06T03:10:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862620.11 epoch sec; seqNo:3120; 4061726.08 north; 594825.61 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:10:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862619.31 2024-11-06T03:10:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:20Z- waiting for 1730862623.31, time is 1730862620.11 2024-11-06T03:10:20Z- AHRS_M2 msg: 1730862620.50 epoch sec; seqNo:2932 2024-11-06T03:10:20Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 1.48 psi 2024-11-06T03:10:20Z- handleDepth()- Depth_Keller msg: 1730862620.51 epoch sec; seqNo:3121; depth 49.81 2024-11-06T03:10:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862620.52 epoch sec; seqNo:3121; 4061726.46 north; 594825.60 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:10:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862619.31 2024-11-06T03:10:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:20Z- waiting for 1730862623.31, time is 1730862620.52 2024-11-06T03:10:20Z- AHRS_M2 msg: 1730862620.91 epoch sec; seqNo:2933 2024-11-06T03:10:20Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 1.48 psi 2024-11-06T03:10:20Z- handleDepth()- Depth_Keller msg: 1730862620.92 epoch sec; seqNo:3122; depth 49.82 2024-11-06T03:10:20Z- handleDVL() - DVL msg epochMillisec: 1730862620.94 2024-11-06T03:10:20Z- handleDvl() - RDI_Pathfinder msg: 1730862620.94 sec; seqNo:1046; #beams:4 2024-11-06T03:10:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.03 , 32.11 , 33.47 , 37.71 2024-11-06T03:10:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.00 2024-11-06T03:10:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862620.95 epoch sec; seqNo:3122; 4061726.47 north; 594826.82 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:10:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862619.31 2024-11-06T03:10:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:21Z- waiting for 1730862623.31, time is 1730862620.95 2024-11-06T03:10:21Z- AHRS_M2 msg: 1730862621.31 epoch sec; seqNo:2934 2024-11-06T03:10:21Z- AHRS_M2 msg: 0.14 phi; -0.08 theta; 1.48 psi 2024-11-06T03:10:21Z- handleDepth()- Depth_Keller msg: 1730862621.33 epoch sec; seqNo:3123; depth 49.84 2024-11-06T03:10:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862621.36 epoch sec; seqNo:3123; 4061726.47 north; 594826.82 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:10:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862619.31 2024-11-06T03:10:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:21Z- waiting for 1730862623.31, time is 1730862621.36 2024-11-06T03:10:21Z- AHRS_M2 msg: 1730862621.72 epoch sec; seqNo:2935 2024-11-06T03:10:21Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.48 psi 2024-11-06T03:10:21Z- handleDepth()- Depth_Keller msg: 1730862621.72 epoch sec; seqNo:3124; depth 49.85 2024-11-06T03:10:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862621.73 epoch sec; seqNo:3124; 4061726.47 north; 594826.82 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:10:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862619.31 2024-11-06T03:10:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:21Z- waiting for 1730862623.31, time is 1730862621.73 2024-11-06T03:10:22Z- AHRS_M2 msg: 1730862622.12 epoch sec; seqNo:2936 2024-11-06T03:10:22Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.48 psi 2024-11-06T03:10:22Z- handleDepth()- Depth_Keller msg: 1730862622.12 epoch sec; seqNo:3125; depth 49.87 2024-11-06T03:10:22Z- handleDVL() - DVL msg epochMillisec: 1730862622.13 2024-11-06T03:10:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:10:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:10:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:10:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:10:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:10:22Z- handleDvl() - RDI_Pathfinder msg: 1730862622.13 sec; seqNo:1047; #beams:0 2024-11-06T03:10:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:10:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.00 2024-11-06T03:10:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862622.13 epoch sec; seqNo:3125; 4061726.49 north; 594828.04 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:10:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862619.31 2024-11-06T03:10:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:22Z- waiting for 1730862623.31, time is 1730862622.13 2024-11-06T03:10:22Z- AHRS_M2 msg: 1730862622.53 epoch sec; seqNo:2937 2024-11-06T03:10:22Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.48 psi 2024-11-06T03:10:22Z- handleDepth()- Depth_Keller msg: 1730862622.53 epoch sec; seqNo:3126; depth 49.87 2024-11-06T03:10:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862622.54 epoch sec; seqNo:3126; 4061726.49 north; 594828.04 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:10:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862619.31 2024-11-06T03:10:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:22Z- waiting for 1730862623.31, time is 1730862622.54 2024-11-06T03:10:22Z- AHRS_M2 msg: 1730862622.93 epoch sec; seqNo:2938 2024-11-06T03:10:22Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.48 psi 2024-11-06T03:10:22Z- handleDepth()- Depth_Keller msg: 1730862622.94 epoch sec; seqNo:3127; depth 49.89 2024-11-06T03:10:22Z- handleDVL() - DVL msg epochMillisec: 1730862622.96 2024-11-06T03:10:22Z- handleDvl() - RDI_Pathfinder msg: 1730862622.96 sec; seqNo:1048; #beams:4 2024-11-06T03:10:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.19 , 31.60 , 33.81 , 37.04 2024-11-06T03:10:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , 0.01 2024-11-06T03:10:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862622.97 epoch sec; seqNo:3127; 4061726.49 north; 594828.04 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:10:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862619.31 2024-11-06T03:10:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:23Z- waiting for 1730862623.31, time is 1730862622.97 2024-11-06T03:10:23Z- AHRS_M2 msg: 1730862623.33 epoch sec; seqNo:2939 2024-11-06T03:10:23Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.48 psi 2024-11-06T03:10:23Z- handleDepth()- Depth_Keller msg: 1730862623.34 epoch sec; seqNo:3128; depth 49.90 2024-11-06T03:10:23Z- handleDVL() - DVL msg epochMillisec: 1730862623.34 2024-11-06T03:10:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:10:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:10:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:10:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:10:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:10:23Z- handleDvl() - RDI_Pathfinder msg: 1730862623.34 sec; seqNo:1049; #beams:0 2024-11-06T03:10:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:10:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , 0.01 2024-11-06T03:10:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862623.36 epoch sec; seqNo:3128; 4061726.49 north; 594828.04 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:10:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862619.31 2024-11-06T03:10:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:23Z- waiting for 1730862623.31, time is 1730862623.36 2024-11-06T03:10:23Z- LcmTrn::updateTrn() >>>> pose time 1730862623.36, meas time 1730862623.34 <<<< 2024-11-06T03:10:23Z- LcmTrn::updateTrn() equating pose 1730862623.34 and meas times 1730862623.34 2024-11-06T03:10:23Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862623.34 2024-11-06T03:10:23Z- TerrainNav::motionUpdate() - currEstimate = (4061726, 594828.04, 49.90) 2024-11-06T03:10:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:10:23Z- TerrainNav:: SubcloudNIS is 1.02, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:10:23Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:10:23Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:10:23Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:10:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:10:23Z- variances([0], [2], and psi_berg) = 0.260390 0.218334 0.000000 2024-11-06T03:10:23Z- publishEstimatesDecoder() - at 1730862623.34 sec 2024-11-06T03:10:23Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:10:23Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061759.28 594811.47 59.06 2024-11-06T03:10:23Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061757.28 594810.81 58.75 lat/lon: 36.70 -121.94 2024-11-06T03:10:23Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.22 0.89 0.00 2024-11-06T03:10:23Z- AHRS_M2 msg: 1730862623.73 epoch sec; seqNo:2940 2024-11-06T03:10:23Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.49 psi 2024-11-06T03:10:23Z- handleDepth()- Depth_Keller msg: 1730862623.75 epoch sec; seqNo:3129; depth 49.91 2024-11-06T03:10:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862623.76 epoch sec; seqNo:3129; 4061726.50 north; 594829.26 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:10:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862623.34 2024-11-06T03:10:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:23Z- waiting for 1730862627.34, time is 1730862623.76 2024-11-06T03:10:24Z- AHRS_M2 msg: 1730862624.14 epoch sec; seqNo:2941 2024-11-06T03:10:24Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.49 psi 2024-11-06T03:10:24Z- handleDepth()- Depth_Keller msg: 1730862624.15 epoch sec; seqNo:3130; depth 49.92 2024-11-06T03:10:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862624.16 epoch sec; seqNo:3130; 4061726.50 north; 594829.26 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:10:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862623.34 2024-11-06T03:10:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:24Z- waiting for 1730862627.34, time is 1730862624.16 2024-11-06T03:10:24Z- AHRS_M2 msg: 1730862624.54 epoch sec; seqNo:2942 2024-11-06T03:10:24Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.49 psi 2024-11-06T03:10:24Z- handleDepth()- Depth_Keller msg: 1730862624.56 epoch sec; seqNo:3131; depth 49.93 2024-11-06T03:10:24Z- handleDVL() - DVL msg epochMillisec: 1730862624.56 2024-11-06T03:10:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:10:24Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:10:24Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:10:24Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:10:24Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:10:24Z- handleDvl() - RDI_Pathfinder msg: 1730862624.56 sec; seqNo:1050; #beams:0 2024-11-06T03:10:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:10:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , 0.01 2024-11-06T03:10:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862624.57 epoch sec; seqNo:3131; 4061726.50 north; 594829.26 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:10:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862623.34 2024-11-06T03:10:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:24Z- waiting for 1730862627.34, time is 1730862624.57 2024-11-06T03:10:24Z- AHRS_M2 msg: 1730862624.95 epoch sec; seqNo:2943 2024-11-06T03:10:24Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.49 psi 2024-11-06T03:10:24Z- handleDepth()- Depth_Keller msg: 1730862624.95 epoch sec; seqNo:3132; depth 49.94 2024-11-06T03:10:24Z- handleDVL() - DVL msg epochMillisec: 1730862624.98 2024-11-06T03:10:24Z- handleDvl() - RDI_Pathfinder msg: 1730862624.98 sec; seqNo:1051; #beams:4 2024-11-06T03:10:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.36 , 31.26 , 33.98 , 35.00 2024-11-06T03:10:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.03 , -0.02 2024-11-06T03:10:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862625.00 epoch sec; seqNo:3132; 4061726.51 north; 594830.48 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:10:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862623.34 2024-11-06T03:10:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:25Z- waiting for 1730862627.34, time is 1730862625.00 2024-11-06T03:10:25Z- AHRS_M2 msg: 1730862625.35 epoch sec; seqNo:2944 2024-11-06T03:10:25Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:25Z- handleDepth()- Depth_Keller msg: 1730862625.36 epoch sec; seqNo:3133; depth 49.95 2024-11-06T03:10:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862625.37 epoch sec; seqNo:3133; 4061726.89 north; 594830.48 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:10:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862623.34 2024-11-06T03:10:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:25Z- waiting for 1730862627.34, time is 1730862625.37 2024-11-06T03:10:25Z- AHRS_M2 msg: 1730862625.76 epoch sec; seqNo:2945 2024-11-06T03:10:25Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:25Z- handleDepth()- Depth_Keller msg: 1730862625.77 epoch sec; seqNo:3134; depth 49.96 2024-11-06T03:10:25Z- handleDVL() - DVL msg epochMillisec: 1730862625.77 2024-11-06T03:10:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:10:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:10:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:10:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:10:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:10:25Z- handleDvl() - RDI_Pathfinder msg: 1730862625.77 sec; seqNo:1052; #beams:0 2024-11-06T03:10:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:10:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.03 , -0.02 2024-11-06T03:10:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862625.78 epoch sec; seqNo:3134; 4061726.89 north; 594830.48 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:10:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862623.34 2024-11-06T03:10:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:25Z- waiting for 1730862627.34, time is 1730862625.78 2024-11-06T03:10:26Z- AHRS_M2 msg: 1730862626.16 epoch sec; seqNo:2946 2024-11-06T03:10:26Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:26Z- handleDepth()- Depth_Keller msg: 1730862626.17 epoch sec; seqNo:3135; depth 49.97 2024-11-06T03:10:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862626.19 epoch sec; seqNo:3135; 4061726.89 north; 594830.48 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:10:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862623.34 2024-11-06T03:10:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:26Z- waiting for 1730862627.34, time is 1730862626.19 2024-11-06T03:10:26Z- AHRS_M2 msg: 1730862626.56 epoch sec; seqNo:2947 2024-11-06T03:10:26Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:26Z- handleDepth()- Depth_Keller msg: 1730862626.57 epoch sec; seqNo:3136; depth 49.97 2024-11-06T03:10:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862626.58 epoch sec; seqNo:3136; 4061726.91 north; 594831.70 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:10:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862623.34 2024-11-06T03:10:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:26Z- waiting for 1730862627.34, time is 1730862626.58 2024-11-06T03:10:26Z- AHRS_M2 msg: 1730862626.98 epoch sec; seqNo:2948 2024-11-06T03:10:26Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:26Z- handleDepth()- Depth_Keller msg: 1730862626.99 epoch sec; seqNo:3137; depth 50.00 2024-11-06T03:10:27Z- handleDVL() - DVL msg epochMillisec: 1730862627.02 2024-11-06T03:10:27Z- handleDvl() - RDI_Pathfinder msg: 1730862627.02 sec; seqNo:1053; #beams:4 2024-11-06T03:10:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.93 , 30.89 , 33.64 , 34.10 2024-11-06T03:10:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.01 , -0.04 2024-11-06T03:10:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862627.04 epoch sec; seqNo:3137; 4061726.91 north; 594831.70 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:10:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862623.34 2024-11-06T03:10:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:27Z- waiting for 1730862627.34, time is 1730862627.04 2024-11-06T03:10:27Z- AHRS_M2 msg: 1730862627.37 epoch sec; seqNo:2949 2024-11-06T03:10:27Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:27Z- handleDepth()- Depth_Keller msg: 1730862627.39 epoch sec; seqNo:3138; depth 50.01 2024-11-06T03:10:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862627.41 epoch sec; seqNo:3138; 4061726.91 north; 594831.70 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:10:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862623.34 2024-11-06T03:10:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:27Z- waiting for 1730862627.34, time is 1730862627.41 2024-11-06T03:10:27Z- LcmTrn::updateTrn() >>>> pose time 1730862627.41, meas time 1730862627.02 <<<< 2024-11-06T03:10:27Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862627.41 2024-11-06T03:10:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862627.02 sec, ping # 0; waiting for INS data more recent than 1730862623.34... 2024-11-06T03:10:27Z- TerrainNav::motionUpdate() - currEstimate = (4061727, 594831.70, 50.01) 2024-11-06T03:10:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:10:27Z- TerrainNav:: SubcloudNIS is 1.02, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:10:27Z- TNavPF::Projecting Measurements 2024-11-06T03:10:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:10:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:10:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:10:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:10:27Z- TNavFilter::4 good beams. 2024-11-06T03:10:27Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:10:27Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:10:27Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:10:27Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:10:27Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:10:27Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:10:27Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:10:27Z- TerrainMapOctree: (vehN, vehE ) = (4061726.86, 594831.34). 2024-11-06T03:10:27Z- TerrainMapOctree: Min Distance = 84.57. 2024-11-06T03:10:27Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:10:27Z- Forcing Subcloud Comparison 2024-11-06T03:10:27Z- Weighting particles with subcloud comparison 2024-11-06T03:10:27Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:27Z- meanExpectedMeasDiff: -5.937712 delta_rms_squared: 1.879975 alpha: 0.989474 2024-11-06T03:10:27Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:27Z- meanExpectedMeasDiff: 1.942767 delta_rms_squared: 1.323670 alpha: 0.985115 2024-11-06T03:10:27Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:27Z- meanExpectedMeasDiff: -3.412079 delta_rms_squared: 1.469978 alpha: 0.986577 2024-11-06T03:10:27Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:27Z- meanExpectedMeasDiff: 2.798575 delta_rms_squared: 1.696326 alpha: 0.988347 2024-11-06T03:10:27Z- Subcloud NIS: 1.090008 2024-11-06T03:10:27Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:10:27Z- TNavPF:: sumWeights = 0.000371 2024-11-06T03:10:27Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:10:27Z- TNavPF::First Term of Map Covariance Matrix is 0.01 2024-11-06T03:10:27Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:10:27Z- TerrainNav::Resampling particle filter... 2024-11-06T03:10:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862627.02 sec. 2024-11-06T03:10:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:10:27Z- variances([0], [2], and psi_berg) = 0.260370 0.217338 0.000000 2024-11-06T03:10:27Z- publishEstimatesDecoder() - at 1730862627.41 sec 2024-11-06T03:10:27Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:10:27Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061759.68 594815.22 59.73 2024-11-06T03:10:27Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061757.70 594814.47 59.50 lat/lon: 36.70 -121.94 2024-11-06T03:10:27Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.22 0.66 0.00 2024-11-06T03:10:27Z- AHRS_M2 msg: 1730862627.79 epoch sec; seqNo:2950 2024-11-06T03:10:27Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:27Z- handleDepth()- Depth_Keller msg: 1730862627.81 epoch sec; seqNo:3139; depth 50.01 2024-11-06T03:10:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862627.86 epoch sec; seqNo:3139; 4061726.92 north; 594832.92 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:10:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862627.41 2024-11-06T03:10:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:27Z- waiting for 1730862631.41, time is 1730862627.86 2024-11-06T03:10:28Z- handleDepth()- Depth_Keller msg: 1730862628.21 epoch sec; seqNo:3140; depth 50.01 2024-11-06T03:10:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862628.22 epoch sec; seqNo:3140; 4061726.92 north; 594832.92 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:10:28Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:10:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862627.41 2024-11-06T03:10:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:28Z- waiting for 1730862631.41, time is 1730862628.22 2024-11-06T03:10:28Z- AHRS_M2 msg: 1730862628.59 epoch sec; seqNo:2951 2024-11-06T03:10:28Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:28Z- handleDepth()- Depth_Keller msg: 1730862628.59 epoch sec; seqNo:3141; depth 50.03 2024-11-06T03:10:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862628.60 epoch sec; seqNo:3141; 4061726.92 north; 594832.92 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:10:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862627.41 2024-11-06T03:10:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:28Z- waiting for 1730862631.41, time is 1730862628.60 2024-11-06T03:10:28Z- AHRS_M2 msg: 1730862628.99 epoch sec; seqNo:2952 2024-11-06T03:10:28Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:28Z- handleDepth()- Depth_Keller msg: 1730862628.99 epoch sec; seqNo:3142; depth 50.04 2024-11-06T03:10:29Z- handleDVL() - DVL msg epochMillisec: 1730862629.01 2024-11-06T03:10:29Z- handleDvl() - RDI_Pathfinder msg: 1730862629.01 sec; seqNo:1054; #beams:4 2024-11-06T03:10:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.08 , 30.89 , 33.49 , 33.94 2024-11-06T03:10:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.04 , 0.00 2024-11-06T03:10:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862629.03 epoch sec; seqNo:3142; 4061726.92 north; 594832.92 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:10:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862627.41 2024-11-06T03:10:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:29Z- waiting for 1730862631.41, time is 1730862629.03 2024-11-06T03:10:29Z- AHRS_M2 msg: 1730862629.39 epoch sec; seqNo:2953 2024-11-06T03:10:29Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:29Z- handleDepth()- Depth_Keller msg: 1730862629.40 epoch sec; seqNo:3143; depth 50.06 2024-11-06T03:10:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862629.43 epoch sec; seqNo:3143; 4061726.93 north; 594834.14 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:10:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862627.41 2024-11-06T03:10:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:29Z- waiting for 1730862631.41, time is 1730862629.43 2024-11-06T03:10:29Z- AHRS_M2 msg: 1730862629.80 epoch sec; seqNo:2954 2024-11-06T03:10:29Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:29Z- handleDepth()- Depth_Keller msg: 1730862629.81 epoch sec; seqNo:3144; depth 50.05 2024-11-06T03:10:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862629.82 epoch sec; seqNo:3144; 4061726.93 north; 594834.14 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:10:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862627.41 2024-11-06T03:10:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:29Z- waiting for 1730862631.41, time is 1730862629.82 2024-11-06T03:10:30Z- AHRS_M2 msg: 1730862630.20 epoch sec; seqNo:2955 2024-11-06T03:10:30Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:30Z- handleDepth()- Depth_Keller msg: 1730862630.21 epoch sec; seqNo:3145; depth 50.08 2024-11-06T03:10:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862630.22 epoch sec; seqNo:3145; 4061726.93 north; 594834.14 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:10:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862627.41 2024-11-06T03:10:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:30Z- waiting for 1730862631.41, time is 1730862630.22 2024-11-06T03:10:30Z- AHRS_M2 msg: 1730862630.60 epoch sec; seqNo:2956 2024-11-06T03:10:30Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:30Z- handleDepth()- Depth_Keller msg: 1730862630.61 epoch sec; seqNo:3146; depth 50.08 2024-11-06T03:10:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862630.62 epoch sec; seqNo:3146; 4061727.31 north; 594834.14 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:10:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862627.41 2024-11-06T03:10:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:30Z- waiting for 1730862631.41, time is 1730862630.62 2024-11-06T03:10:31Z- AHRS_M2 msg: 1730862631.01 epoch sec; seqNo:2957 2024-11-06T03:10:31Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:31Z- handleDepth()- Depth_Keller msg: 1730862631.02 epoch sec; seqNo:3147; depth 50.13 2024-11-06T03:10:31Z- handleDVL() - DVL msg epochMillisec: 1730862631.04 2024-11-06T03:10:31Z- handleDvl() - RDI_Pathfinder msg: 1730862631.04 sec; seqNo:1055; #beams:4 2024-11-06T03:10:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.70 , 31.19 , 34.10 , 34.25 2024-11-06T03:10:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , 0.05 2024-11-06T03:10:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862631.05 epoch sec; seqNo:3147; 4061727.33 north; 594835.36 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:10:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862627.41 2024-11-06T03:10:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:31Z- waiting for 1730862631.41, time is 1730862631.05 2024-11-06T03:10:31Z- AHRS_M2 msg: 1730862631.41 epoch sec; seqNo:2958 2024-11-06T03:10:31Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:31Z- handleDepth()- Depth_Keller msg: 1730862631.41 epoch sec; seqNo:3148; depth 50.15 2024-11-06T03:10:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862631.43 epoch sec; seqNo:3148; 4061727.33 north; 594835.36 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:10:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862627.41 2024-11-06T03:10:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:31Z- waiting for 1730862631.41, time is 1730862631.43 2024-11-06T03:10:31Z- LcmTrn::updateTrn() >>>> pose time 1730862631.43, meas time 1730862631.04 <<<< 2024-11-06T03:10:31Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862631.43 2024-11-06T03:10:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862631.04 sec, ping # 0; waiting for INS data more recent than 1730862627.41... 2024-11-06T03:10:31Z- TerrainNav::motionUpdate() - currEstimate = (4061727, 594835.36, 50.15) 2024-11-06T03:10:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:10:31Z- TerrainNav:: SubcloudNIS is 1.09, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:10:31Z- TNavPF::Projecting Measurements 2024-11-06T03:10:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:10:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:10:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:10:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:10:31Z- TNavFilter::4 good beams. 2024-11-06T03:10:31Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:10:31Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:10:31Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:10:31Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:10:31Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:10:31Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:10:31Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:10:31Z- TerrainMapOctree: (vehN, vehE ) = (4061727.28, 594835.00). 2024-11-06T03:10:31Z- TerrainMapOctree: Min Distance = 87.98. 2024-11-06T03:10:31Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:10:31Z- Forcing Subcloud Comparison 2024-11-06T03:10:31Z- Weighting particles with subcloud comparison 2024-11-06T03:10:31Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:31Z- meanExpectedMeasDiff: -4.612478 delta_rms_squared: 1.515363 alpha: 0.986974 2024-11-06T03:10:31Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:31Z- meanExpectedMeasDiff: 2.806412 delta_rms_squared: 1.014779 alpha: 0.980672 2024-11-06T03:10:31Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:31Z- meanExpectedMeasDiff: -2.182744 delta_rms_squared: 1.166586 alpha: 0.983145 2024-11-06T03:10:31Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:31Z- meanExpectedMeasDiff: 3.517257 delta_rms_squared: 0.823405 alpha: 0.976287 2024-11-06T03:10:31Z- Subcloud NIS: 1.073917 2024-11-06T03:10:31Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:10:31Z- TNavPF:: sumWeights = 0.001532 2024-11-06T03:10:31Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:10:31Z- TNavPF::First Term of Map Covariance Matrix is 0.03 2024-11-06T03:10:31Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:10:31Z- TerrainNav::Resampling particle filter... 2024-11-06T03:10:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862631.04 sec. 2024-11-06T03:10:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:10:31Z- variances([0], [2], and psi_berg) = 0.332715 0.305400 0.000000 2024-11-06T03:10:31Z- publishEstimatesDecoder() - at 1730862631.43 sec 2024-11-06T03:10:31Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:10:31Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061760.20 594818.80 61.25 2024-11-06T03:10:31Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061758.22 594818.21 59.95 lat/lon: 36.70 -121.94 2024-11-06T03:10:31Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.31 0.73 0.00 2024-11-06T03:10:31Z- AHRS_M2 msg: 1730862631.82 epoch sec; seqNo:2959 2024-11-06T03:10:31Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:31Z- handleDepth()- Depth_Keller msg: 1730862631.83 epoch sec; seqNo:3149; depth 50.15 2024-11-06T03:10:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862631.85 epoch sec; seqNo:3149; 4061727.33 north; 594835.36 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:10:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862631.43 2024-11-06T03:10:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:31Z- waiting for 1730862635.43, time is 1730862631.85 2024-11-06T03:10:32Z- AHRS_M2 msg: 1730862632.22 epoch sec; seqNo:2960 2024-11-06T03:10:32Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:32Z- handleDepth()- Depth_Keller msg: 1730862632.22 epoch sec; seqNo:3150; depth 50.18 2024-11-06T03:10:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862632.24 epoch sec; seqNo:3150; 4061727.33 north; 594835.36 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:10:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862631.43 2024-11-06T03:10:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:32Z- waiting for 1730862635.43, time is 1730862632.24 2024-11-06T03:10:32Z- AHRS_M2 msg: 1730862632.63 epoch sec; seqNo:2961 2024-11-06T03:10:32Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:32Z- handleDepth()- Depth_Keller msg: 1730862632.64 epoch sec; seqNo:3151; depth 50.20 2024-11-06T03:10:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862632.65 epoch sec; seqNo:3151; 4061727.34 north; 594836.58 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:10:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862631.43 2024-11-06T03:10:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:32Z- waiting for 1730862635.43, time is 1730862632.65 2024-11-06T03:10:33Z- AHRS_M2 msg: 1730862633.03 epoch sec; seqNo:2962 2024-11-06T03:10:33Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:33Z- handleDepth()- Depth_Keller msg: 1730862633.03 epoch sec; seqNo:3152; depth 50.23 2024-11-06T03:10:33Z- handleDVL() - DVL msg epochMillisec: 1730862633.05 2024-11-06T03:10:33Z- handleDvl() - RDI_Pathfinder msg: 1730862633.05 sec; seqNo:1056; #beams:4 2024-11-06T03:10:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.70 , 30.89 , 0.00 , 34.25 2024-11-06T03:10:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , 0.06 2024-11-06T03:10:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862633.08 epoch sec; seqNo:3152; 4061727.34 north; 594836.58 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:10:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862631.43 2024-11-06T03:10:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:33Z- waiting for 1730862635.43, time is 1730862633.08 2024-11-06T03:10:33Z- AHRS_M2 msg: 1730862633.43 epoch sec; seqNo:2963 2024-11-06T03:10:33Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:33Z- handleDepth()- Depth_Keller msg: 1730862633.44 epoch sec; seqNo:3153; depth 50.25 2024-11-06T03:10:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862633.45 epoch sec; seqNo:3153; 4061727.34 north; 594836.58 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:10:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862631.43 2024-11-06T03:10:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:33Z- waiting for 1730862635.43, time is 1730862633.45 2024-11-06T03:10:33Z- AHRS_M2 msg: 1730862633.83 epoch sec; seqNo:2964 2024-11-06T03:10:33Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:33Z- handleDepth()- Depth_Keller msg: 1730862633.84 epoch sec; seqNo:3154; depth 50.26 2024-11-06T03:10:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862633.86 epoch sec; seqNo:3154; 4061727.72 north; 594836.57 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:10:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862631.43 2024-11-06T03:10:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:33Z- waiting for 1730862635.43, time is 1730862633.86 2024-11-06T03:10:34Z- AHRS_M2 msg: 1730862634.24 epoch sec; seqNo:2965 2024-11-06T03:10:34Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.49 psi 2024-11-06T03:10:34Z- handleDepth()- Depth_Keller msg: 1730862634.25 epoch sec; seqNo:3155; depth 50.28 2024-11-06T03:10:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862634.26 epoch sec; seqNo:3155; 4061727.73 north; 594837.79 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:10:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862631.43 2024-11-06T03:10:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:34Z- waiting for 1730862635.43, time is 1730862634.26 2024-11-06T03:10:34Z- AHRS_M2 msg: 1730862634.65 epoch sec; seqNo:2966 2024-11-06T03:10:34Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.50 psi 2024-11-06T03:10:34Z- handleDepth()- Depth_Keller msg: 1730862634.66 epoch sec; seqNo:3156; depth 50.29 2024-11-06T03:10:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862634.66 epoch sec; seqNo:3156; 4061727.73 north; 594837.79 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:10:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862631.43 2024-11-06T03:10:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:34Z- waiting for 1730862635.43, time is 1730862634.66 2024-11-06T03:10:35Z- AHRS_M2 msg: 1730862635.05 epoch sec; seqNo:2967 2024-11-06T03:10:35Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.50 psi 2024-11-06T03:10:35Z- handleDepth()- Depth_Keller msg: 1730862635.06 epoch sec; seqNo:3157; depth 50.31 2024-11-06T03:10:35Z- handleDVL() - DVL msg epochMillisec: 1730862635.09 2024-11-06T03:10:35Z- handleDvl() - RDI_Pathfinder msg: 1730862635.09 sec; seqNo:1057; #beams:4 2024-11-06T03:10:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.00 , 31.19 , 37.46 , 34.10 2024-11-06T03:10:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , 0.03 2024-11-06T03:10:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862635.10 epoch sec; seqNo:3157; 4061727.73 north; 594837.79 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:10:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862631.43 2024-11-06T03:10:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:35Z- waiting for 1730862635.43, time is 1730862635.10 2024-11-06T03:10:35Z- AHRS_M2 msg: 1730862635.45 epoch sec; seqNo:2968 2024-11-06T03:10:35Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.50 psi 2024-11-06T03:10:35Z- handleDepth()- Depth_Keller msg: 1730862635.46 epoch sec; seqNo:3158; depth 50.34 2024-11-06T03:10:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862635.47 epoch sec; seqNo:3158; 4061727.74 north; 594839.01 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:10:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862631.43 2024-11-06T03:10:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:35Z- waiting for 1730862635.43, time is 1730862635.47 2024-11-06T03:10:35Z- LcmTrn::updateTrn() >>>> pose time 1730862635.47, meas time 1730862635.09 <<<< 2024-11-06T03:10:35Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862635.47 2024-11-06T03:10:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862635.09 sec, ping # 0; waiting for INS data more recent than 1730862631.43... 2024-11-06T03:10:35Z- TerrainNav::motionUpdate() - currEstimate = (4061728, 594839.01, 50.34) 2024-11-06T03:10:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:10:35Z- TerrainNav:: SubcloudNIS is 1.07, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:10:35Z- TNavPF::Projecting Measurements 2024-11-06T03:10:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:10:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:10:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:10:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:10:35Z- TNavFilter::4 good beams. 2024-11-06T03:10:35Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:10:35Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:10:35Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:10:35Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:10:35Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:10:35Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:10:35Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:10:35Z- TerrainMapOctree: (vehN, vehE ) = (4061727.70, 594838.67). 2024-11-06T03:10:35Z- TerrainMapOctree: Min Distance = 91.43. 2024-11-06T03:10:35Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:10:35Z- Forcing Subcloud Comparison 2024-11-06T03:10:35Z- Weighting particles with subcloud comparison 2024-11-06T03:10:35Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:35Z- meanExpectedMeasDiff: -3.898534 delta_rms_squared: 1.789332 alpha: 0.988946 2024-11-06T03:10:35Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:35Z- meanExpectedMeasDiff: 2.183568 delta_rms_squared: 0.929899 alpha: 0.978945 2024-11-06T03:10:35Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:35Z- meanExpectedMeasDiff: 1.780990 delta_rms_squared: 1.222787 alpha: 0.983907 2024-11-06T03:10:35Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:35Z- meanExpectedMeasDiff: 3.086899 delta_rms_squared: 1.409586 alpha: 0.986010 2024-11-06T03:10:35Z- Subcloud NIS: 1.049597 2024-11-06T03:10:35Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:10:35Z- TNavPF:: sumWeights = 0.010474 2024-11-06T03:10:35Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:10:35Z- TNavPF::First Term of Map Covariance Matrix is 0.19 2024-11-06T03:10:35Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:10:35Z- TerrainNav::Resampling particle filter... 2024-11-06T03:10:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862635.09 sec. 2024-11-06T03:10:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:10:35Z- variances([0], [2], and psi_berg) = 0.436542 0.247580 0.000000 2024-11-06T03:10:35Z- publishEstimatesDecoder() - at 1730862635.47 sec 2024-11-06T03:10:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:10:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061760.53 594822.52 59.55 2024-11-06T03:10:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061758.75 594821.73 59.72 lat/lon: 36.70 -121.94 2024-11-06T03:10:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.44 0.25 0.71 0.00 2024-11-06T03:10:35Z- AHRS_M2 msg: 1730862635.87 epoch sec; seqNo:2969 2024-11-06T03:10:35Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.50 psi 2024-11-06T03:10:35Z- handleDepth()- Depth_Keller msg: 1730862635.89 epoch sec; seqNo:3159; depth 50.34 2024-11-06T03:10:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862635.91 epoch sec; seqNo:3159; 4061727.74 north; 594839.01 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:10:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862635.47 2024-11-06T03:10:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:36Z- waiting for 1730862639.47, time is 1730862635.91 2024-11-06T03:10:36Z- AHRS_M2 msg: 1730862636.26 epoch sec; seqNo:2970 2024-11-06T03:10:36Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.50 psi 2024-11-06T03:10:36Z- handleDepth()- Depth_Keller msg: 1730862636.26 epoch sec; seqNo:3160; depth 50.35 2024-11-06T03:10:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862636.28 epoch sec; seqNo:3160; 4061727.74 north; 594839.01 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:10:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862635.47 2024-11-06T03:10:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:36Z- waiting for 1730862639.47, time is 1730862636.28 2024-11-06T03:10:36Z- AHRS_M2 msg: 1730862636.66 epoch sec; seqNo:2971 2024-11-06T03:10:36Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.50 psi 2024-11-06T03:10:36Z- handleDepth()- Depth_Keller msg: 1730862636.68 epoch sec; seqNo:3161; depth 50.36 2024-11-06T03:10:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862636.70 epoch sec; seqNo:3161; 4061727.76 north; 594840.23 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:10:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862635.47 2024-11-06T03:10:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:36Z- waiting for 1730862639.47, time is 1730862636.70 2024-11-06T03:10:37Z- AHRS_M2 msg: 1730862637.07 epoch sec; seqNo:2972 2024-11-06T03:10:37Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.50 psi 2024-11-06T03:10:37Z- handleDepth()- Depth_Keller msg: 1730862637.07 epoch sec; seqNo:3162; depth 50.38 2024-11-06T03:10:37Z- handleDVL() - DVL msg epochMillisec: 1730862637.10 2024-11-06T03:10:37Z- handleDvl() - RDI_Pathfinder msg: 1730862637.10 sec; seqNo:1058; #beams:4 2024-11-06T03:10:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.38 , 31.43 , 38.06 , 34.15 2024-11-06T03:10:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.01 2024-11-06T03:10:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862637.14 epoch sec; seqNo:3162; 4061727.76 north; 594840.23 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:10:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862635.47 2024-11-06T03:10:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:37Z- waiting for 1730862639.47, time is 1730862637.14 2024-11-06T03:10:37Z- AHRS_M2 msg: 1730862637.47 epoch sec; seqNo:2973 2024-11-06T03:10:37Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.50 psi 2024-11-06T03:10:37Z- handleDepth()- Depth_Keller msg: 1730862637.48 epoch sec; seqNo:3163; depth 50.39 2024-11-06T03:10:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862637.49 epoch sec; seqNo:3163; 4061727.76 north; 594840.23 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:10:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862635.47 2024-11-06T03:10:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:37Z- waiting for 1730862639.47, time is 1730862637.49 2024-11-06T03:10:37Z- AHRS_M2 msg: 1730862637.88 epoch sec; seqNo:2974 2024-11-06T03:10:37Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.50 psi 2024-11-06T03:10:37Z- handleDepth()- Depth_Keller msg: 1730862637.88 epoch sec; seqNo:3164; depth 50.40 2024-11-06T03:10:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862637.90 epoch sec; seqNo:3164; 4061727.76 north; 594840.23 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:10:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862635.47 2024-11-06T03:10:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:38Z- waiting for 1730862639.47, time is 1730862637.90 2024-11-06T03:10:38Z- handleDepth()- Depth_Keller msg: 1730862638.30 epoch sec; seqNo:3165; depth 50.40 2024-11-06T03:10:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862638.32 epoch sec; seqNo:3165; 4061727.77 north; 594841.46 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:10:38Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:10:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862635.47 2024-11-06T03:10:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:38Z- waiting for 1730862639.47, time is 1730862638.32 2024-11-06T03:10:38Z- AHRS_M2 msg: 1730862638.69 epoch sec; seqNo:2975 2024-11-06T03:10:38Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.50 psi 2024-11-06T03:10:38Z- handleDepth()- Depth_Keller msg: 1730862638.70 epoch sec; seqNo:3166; depth 50.40 2024-11-06T03:10:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862638.77 epoch sec; seqNo:3166; 4061727.77 north; 594841.46 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:10:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862635.47 2024-11-06T03:10:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:38Z- waiting for 1730862639.47, time is 1730862638.77 2024-11-06T03:10:39Z- AHRS_M2 msg: 1730862639.09 epoch sec; seqNo:2976 2024-11-06T03:10:39Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.50 psi 2024-11-06T03:10:39Z- handleDepth()- Depth_Keller msg: 1730862639.09 epoch sec; seqNo:3167; depth 50.42 2024-11-06T03:10:39Z- handleDVL() - DVL msg epochMillisec: 1730862639.11 2024-11-06T03:10:39Z- handleDvl() - RDI_Pathfinder msg: 1730862639.11 sec; seqNo:1059; #beams:4 2024-11-06T03:10:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.04 , 31.77 , 38.74 , 33.47 2024-11-06T03:10:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , -0.00 2024-11-06T03:10:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862639.13 epoch sec; seqNo:3167; 4061728.15 north; 594841.45 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:10:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862635.47 2024-11-06T03:10:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:39Z- waiting for 1730862639.47, time is 1730862639.13 2024-11-06T03:10:39Z- AHRS_M2 msg: 1730862639.49 epoch sec; seqNo:2977 2024-11-06T03:10:39Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.50 psi 2024-11-06T03:10:39Z- handleDepth()- Depth_Keller msg: 1730862639.50 epoch sec; seqNo:3168; depth 50.43 2024-11-06T03:10:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862639.52 epoch sec; seqNo:3168; 4061728.16 north; 594842.67 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:10:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862635.47 2024-11-06T03:10:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:39Z- waiting for 1730862639.47, time is 1730862639.52 2024-11-06T03:10:39Z- LcmTrn::updateTrn() >>>> pose time 1730862639.52, meas time 1730862639.11 <<<< 2024-11-06T03:10:39Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862639.52 2024-11-06T03:10:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862639.11 sec, ping # 0; waiting for INS data more recent than 1730862635.47... 2024-11-06T03:10:39Z- TerrainNav::motionUpdate() - currEstimate = (4061728, 594842.67, 50.43) 2024-11-06T03:10:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:10:39Z- TerrainNav:: SubcloudNIS is 1.05, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:10:39Z- TNavPF::Projecting Measurements 2024-11-06T03:10:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:10:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:10:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:10:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:10:39Z- TNavFilter::4 good beams. 2024-11-06T03:10:39Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:10:39Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:10:39Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:10:39Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:10:39Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:10:39Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:10:39Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:10:39Z- TerrainMapOctree: (vehN, vehE ) = (4061728.12, 594842.31). 2024-11-06T03:10:39Z- TerrainMapOctree: Min Distance = 94.86. 2024-11-06T03:10:39Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:10:39Z- Forcing Subcloud Comparison 2024-11-06T03:10:39Z- Weighting particles with subcloud comparison 2024-11-06T03:10:39Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:39Z- meanExpectedMeasDiff: -4.202914 delta_rms_squared: 2.421432 alpha: 0.991808 2024-11-06T03:10:39Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:39Z- meanExpectedMeasDiff: -0.447588 delta_rms_squared: 1.065808 alpha: 0.981581 2024-11-06T03:10:39Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:39Z- meanExpectedMeasDiff: 2.503147 delta_rms_squared: 1.234574 alpha: 0.984058 2024-11-06T03:10:39Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:39Z- meanExpectedMeasDiff: 2.269400 delta_rms_squared: 1.368472 alpha: 0.985596 2024-11-06T03:10:39Z- Subcloud NIS: 0.931113 2024-11-06T03:10:39Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:10:39Z- TNavPF:: sumWeights = 0.017543 2024-11-06T03:10:39Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:10:39Z- TNavPF::First Term of Map Covariance Matrix is 0.27 2024-11-06T03:10:39Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:10:39Z- TerrainNav::Resampling particle filter... 2024-11-06T03:10:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862639.11 sec. 2024-11-06T03:10:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:10:39Z- variances([0], [2], and psi_berg) = 0.263331 0.203304 0.000000 2024-11-06T03:10:39Z- publishEstimatesDecoder() - at 1730862639.52 sec 2024-11-06T03:10:39Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:10:39Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061760.99 594826.03 60.08 2024-11-06T03:10:39Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061759.03 594825.42 59.87 lat/lon: 36.70 -121.94 2024-11-06T03:10:39Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.20 0.62 0.00 2024-11-06T03:10:39Z- AHRS_M2 msg: 1730862639.91 epoch sec; seqNo:2978 2024-11-06T03:10:39Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.50 psi 2024-11-06T03:10:39Z- handleDepth()- Depth_Keller msg: 1730862639.92 epoch sec; seqNo:3169; depth 50.42 2024-11-06T03:10:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862639.94 epoch sec; seqNo:3169; 4061728.16 north; 594842.67 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:10:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862639.52 2024-11-06T03:10:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:40Z- waiting for 1730862643.52, time is 1730862639.94 2024-11-06T03:10:40Z- handleDepth()- Depth_Keller msg: 1730862640.33 epoch sec; seqNo:3170; depth 50.43 2024-11-06T03:10:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862640.34 epoch sec; seqNo:3170; 4061728.16 north; 594842.67 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:10:40Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:10:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862639.52 2024-11-06T03:10:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:40Z- waiting for 1730862643.52, time is 1730862640.34 2024-11-06T03:10:40Z- AHRS_M2 msg: 1730862640.73 epoch sec; seqNo:2979 2024-11-06T03:10:40Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:10:40Z- handleDepth()- Depth_Keller msg: 1730862640.75 epoch sec; seqNo:3171; depth 50.42 2024-11-06T03:10:40Z- handleDVL() - DVL msg epochMillisec: 1730862640.79 2024-11-06T03:10:40Z- handleDvl() - RDI_Pathfinder msg: 1730862640.79 sec; seqNo:1060; #beams:4 2024-11-06T03:10:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.04 , 32.28 , 39.41 , 33.47 2024-11-06T03:10:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , -0.03 2024-11-06T03:10:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862640.80 epoch sec; seqNo:3171; 4061728.18 north; 594843.89 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:10:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862639.52 2024-11-06T03:10:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:40Z- waiting for 1730862643.52, time is 1730862640.80 2024-11-06T03:10:41Z- AHRS_M2 msg: 1730862641.11 epoch sec; seqNo:2980 2024-11-06T03:10:41Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:10:41Z- handleDepth()- Depth_Keller msg: 1730862641.11 epoch sec; seqNo:3172; depth 50.43 2024-11-06T03:10:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862641.14 epoch sec; seqNo:3172; 4061728.18 north; 594843.89 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:10:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862639.52 2024-11-06T03:10:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:41Z- waiting for 1730862643.52, time is 1730862641.14 2024-11-06T03:10:41Z- AHRS_M2 msg: 1730862641.51 epoch sec; seqNo:2981 2024-11-06T03:10:41Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:10:41Z- handleDepth()- Depth_Keller msg: 1730862641.51 epoch sec; seqNo:3173; depth 50.42 2024-11-06T03:10:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862641.52 epoch sec; seqNo:3173; 4061728.18 north; 594843.89 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:10:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862639.52 2024-11-06T03:10:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:41Z- waiting for 1730862643.52, time is 1730862641.52 2024-11-06T03:10:41Z- AHRS_M2 msg: 1730862641.92 epoch sec; seqNo:2982 2024-11-06T03:10:41Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:10:41Z- handleDepth()- Depth_Keller msg: 1730862641.92 epoch sec; seqNo:3174; depth 50.43 2024-11-06T03:10:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862641.93 epoch sec; seqNo:3174; 4061728.18 north; 594843.89 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:10:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862639.52 2024-11-06T03:10:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:42Z- waiting for 1730862643.52, time is 1730862641.93 2024-11-06T03:10:42Z- AHRS_M2 msg: 1730862642.32 epoch sec; seqNo:2983 2024-11-06T03:10:42Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.52 psi 2024-11-06T03:10:42Z- handleDepth()- Depth_Keller msg: 1730862642.33 epoch sec; seqNo:3175; depth 50.43 2024-11-06T03:10:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862642.35 epoch sec; seqNo:3175; 4061728.19 north; 594845.11 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:10:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862639.52 2024-11-06T03:10:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:42Z- waiting for 1730862643.52, time is 1730862642.35 2024-11-06T03:10:42Z- AHRS_M2 msg: 1730862642.76 epoch sec; seqNo:2984 2024-11-06T03:10:42Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.52 psi 2024-11-06T03:10:42Z- handleDepth()- Depth_Keller msg: 1730862642.77 epoch sec; seqNo:3176; depth 50.44 2024-11-06T03:10:42Z- handleDVL() - DVL msg epochMillisec: 1730862642.81 2024-11-06T03:10:42Z- handleDvl() - RDI_Pathfinder msg: 1730862642.81 sec; seqNo:1061; #beams:4 2024-11-06T03:10:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.21 , 32.45 , 39.24 , 33.81 2024-11-06T03:10:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.01 , -0.04 2024-11-06T03:10:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862642.83 epoch sec; seqNo:3176; 4061728.19 north; 594845.11 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:10:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862639.52 2024-11-06T03:10:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:42Z- waiting for 1730862643.52, time is 1730862642.83 2024-11-06T03:10:43Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:10:43Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:10:43Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:10:43Z- AHRS_M2 msg: 1730862643.12 epoch sec; seqNo:2985 2024-11-06T03:10:43Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.52 psi 2024-11-06T03:10:43Z- handleDepth()- Depth_Keller msg: 1730862643.15 epoch sec; seqNo:3177; depth 50.44 2024-11-06T03:10:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862643.18 epoch sec; seqNo:3177; 4061728.19 north; 594845.11 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:10:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862639.52 2024-11-06T03:10:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:43Z- waiting for 1730862643.52, time is 1730862643.18 2024-11-06T03:10:43Z- AHRS_M2 msg: 1730862643.53 epoch sec; seqNo:2986 2024-11-06T03:10:43Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T03:10:43Z- handleDepth()- Depth_Keller msg: 1730862643.55 epoch sec; seqNo:3178; depth 50.44 2024-11-06T03:10:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862643.55 epoch sec; seqNo:3178; 4061728.19 north; 594845.11 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:10:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862639.52 2024-11-06T03:10:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:43Z- waiting for 1730862643.52, time is 1730862643.55 2024-11-06T03:10:43Z- LcmTrn::updateTrn() >>>> pose time 1730862643.55, meas time 1730862642.81 <<<< 2024-11-06T03:10:43Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862643.55 2024-11-06T03:10:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862642.81 sec, ping # 0; waiting for INS data more recent than 1730862639.52... 2024-11-06T03:10:43Z- TerrainNav::motionUpdate() - currEstimate = (4061728, 594845.11, 50.44) 2024-11-06T03:10:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:10:43Z- TerrainNav:: SubcloudNIS is 0.93, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:10:43Z- TNavPF::Projecting Measurements 2024-11-06T03:10:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:10:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:10:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:10:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:10:43Z- TNavFilter::4 good beams. 2024-11-06T03:10:43Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:10:43Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:10:43Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:10:43Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:10:43Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:10:43Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:10:43Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:10:43Z- TerrainMapOctree: (vehN, vehE ) = (4061728.19, 594844.66). 2024-11-06T03:10:43Z- TerrainMapOctree: Min Distance = 97.14. 2024-11-06T03:10:43Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:10:43Z- Forcing Subcloud Comparison 2024-11-06T03:10:43Z- Weighting particles with subcloud comparison 2024-11-06T03:10:43Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:43Z- meanExpectedMeasDiff: -3.698614 delta_rms_squared: 1.458764 alpha: 0.986475 2024-11-06T03:10:43Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:43Z- meanExpectedMeasDiff: -0.943638 delta_rms_squared: 1.067457 alpha: 0.981608 2024-11-06T03:10:43Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:43Z- meanExpectedMeasDiff: 2.143700 delta_rms_squared: 1.096280 alpha: 0.982083 2024-11-06T03:10:43Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:43Z- meanExpectedMeasDiff: 2.905834 delta_rms_squared: 1.216773 alpha: 0.983829 2024-11-06T03:10:43Z- Subcloud NIS: 0.902797 2024-11-06T03:10:43Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:10:43Z- TNavPF:: sumWeights = 0.019809 2024-11-06T03:10:43Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:10:43Z- TNavPF::First Term of Map Covariance Matrix is 0.27 2024-11-06T03:10:43Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:10:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862642.81 sec. 2024-11-06T03:10:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:10:43Z- variances([0], [2], and psi_berg) = 0.204783 0.162974 0.000000 2024-11-06T03:10:43Z- publishEstimatesDecoder() - at 1730862643.55 sec 2024-11-06T03:10:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:10:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061758.69 594827.82 59.76 2024-11-06T03:10:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061758.98 594827.80 59.88 lat/lon: 36.70 -121.94 2024-11-06T03:10:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.20 0.16 0.65 0.00 2024-11-06T03:10:43Z- AHRS_M2 msg: 1730862643.93 epoch sec; seqNo:2987 2024-11-06T03:10:43Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T03:10:43Z- handleDepth()- Depth_Keller msg: 1730862643.94 epoch sec; seqNo:3179; depth 50.46 2024-11-06T03:10:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862643.96 epoch sec; seqNo:3179; 4061728.20 north; 594846.33 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:10:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862643.55 2024-11-06T03:10:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:44Z- waiting for 1730862647.55, time is 1730862643.96 2024-11-06T03:10:44Z- AHRS_M2 msg: 1730862644.34 epoch sec; seqNo:2988 2024-11-06T03:10:44Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T03:10:44Z- handleDepth()- Depth_Keller msg: 1730862644.35 epoch sec; seqNo:3180; depth 50.46 2024-11-06T03:10:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862644.37 epoch sec; seqNo:3180; 4061728.20 north; 594846.33 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:10:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862643.55 2024-11-06T03:10:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:44Z- waiting for 1730862647.55, time is 1730862644.37 2024-11-06T03:10:44Z- AHRS_M2 msg: 1730862644.77 epoch sec; seqNo:2989 2024-11-06T03:10:44Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T03:10:44Z- handleDepth()- Depth_Keller msg: 1730862644.77 epoch sec; seqNo:3181; depth 50.47 2024-11-06T03:10:44Z- handleDVL() - DVL msg epochMillisec: 1730862644.81 2024-11-06T03:10:44Z- handleDvl() - RDI_Pathfinder msg: 1730862644.81 sec; seqNo:1062; #beams:4 2024-11-06T03:10:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.70 , 34.49 , 39.24 , 33.30 2024-11-06T03:10:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.05 , -0.01 2024-11-06T03:10:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862644.82 epoch sec; seqNo:3181; 4061728.20 north; 594846.33 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:10:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862643.55 2024-11-06T03:10:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:44Z- waiting for 1730862647.55, time is 1730862644.82 2024-11-06T03:10:45Z- handleDepth()- Depth_Keller msg: 1730862645.17 epoch sec; seqNo:3182; depth 50.48 2024-11-06T03:10:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862645.18 epoch sec; seqNo:3182; 4061728.20 north; 594846.33 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:10:45Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:10:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862643.55 2024-11-06T03:10:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:45Z- waiting for 1730862647.55, time is 1730862645.18 2024-11-06T03:10:45Z- AHRS_M2 msg: 1730862645.56 epoch sec; seqNo:2990 2024-11-06T03:10:45Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T03:10:45Z- handleDepth()- Depth_Keller msg: 1730862645.56 epoch sec; seqNo:3183; depth 50.48 2024-11-06T03:10:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862645.58 epoch sec; seqNo:3183; 4061728.22 north; 594847.55 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:10:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862643.55 2024-11-06T03:10:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:45Z- waiting for 1730862647.55, time is 1730862645.58 2024-11-06T03:10:45Z- AHRS_M2 msg: 1730862645.95 epoch sec; seqNo:2991 2024-11-06T03:10:45Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T03:10:45Z- handleDepth()- Depth_Keller msg: 1730862645.96 epoch sec; seqNo:3184; depth 50.51 2024-11-06T03:10:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862645.98 epoch sec; seqNo:3184; 4061728.22 north; 594847.55 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:10:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862643.55 2024-11-06T03:10:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:46Z- waiting for 1730862647.55, time is 1730862645.98 2024-11-06T03:10:46Z- AHRS_M2 msg: 1730862646.36 epoch sec; seqNo:2992 2024-11-06T03:10:46Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T03:10:46Z- handleDepth()- Depth_Keller msg: 1730862646.37 epoch sec; seqNo:3185; depth 50.51 2024-11-06T03:10:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862646.38 epoch sec; seqNo:3185; 4061728.22 north; 594847.55 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:10:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862643.55 2024-11-06T03:10:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:46Z- waiting for 1730862647.55, time is 1730862646.38 2024-11-06T03:10:46Z- AHRS_M2 msg: 1730862646.77 epoch sec; seqNo:2993 2024-11-06T03:10:46Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.50 psi 2024-11-06T03:10:46Z- handleDepth()- Depth_Keller msg: 1730862646.78 epoch sec; seqNo:3186; depth 50.53 2024-11-06T03:10:46Z- handleDVL() - DVL msg epochMillisec: 1730862646.82 2024-11-06T03:10:46Z- handleDvl() - RDI_Pathfinder msg: 1730862646.82 sec; seqNo:1063; #beams:4 2024-11-06T03:10:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.87 , 35.68 , 39.41 , 33.47 2024-11-06T03:10:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.07 , 0.06 2024-11-06T03:10:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862646.83 epoch sec; seqNo:3186; 4061728.60 north; 594847.55 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:10:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862643.55 2024-11-06T03:10:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:46Z- waiting for 1730862647.55, time is 1730862646.83 2024-11-06T03:10:47Z- AHRS_M2 msg: 1730862647.17 epoch sec; seqNo:2994 2024-11-06T03:10:47Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.50 psi 2024-11-06T03:10:47Z- handleDepth()- Depth_Keller msg: 1730862647.18 epoch sec; seqNo:3187; depth 50.53 2024-11-06T03:10:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862647.19 epoch sec; seqNo:3187; 4061728.61 north; 594848.77 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:10:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862643.55 2024-11-06T03:10:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:47Z- waiting for 1730862647.55, time is 1730862647.19 2024-11-06T03:10:47Z- AHRS_M2 msg: 1730862647.57 epoch sec; seqNo:2995 2024-11-06T03:10:47Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.50 psi 2024-11-06T03:10:47Z- handleDepth()- Depth_Keller msg: 1730862647.58 epoch sec; seqNo:3188; depth 50.55 2024-11-06T03:10:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862647.60 epoch sec; seqNo:3188; 4061728.61 north; 594848.77 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:10:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862643.55 2024-11-06T03:10:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:47Z- waiting for 1730862647.55, time is 1730862647.60 2024-11-06T03:10:47Z- LcmTrn::updateTrn() >>>> pose time 1730862647.60, meas time 1730862646.82 <<<< 2024-11-06T03:10:47Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862647.60 2024-11-06T03:10:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862646.82 sec, ping # 0; waiting for INS data more recent than 1730862643.55... 2024-11-06T03:10:47Z- TerrainNav::motionUpdate() - currEstimate = (4061729, 594848.77, 50.55) 2024-11-06T03:10:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:10:47Z- TerrainNav:: SubcloudNIS is 0.90, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:10:47Z- TNavPF::Projecting Measurements 2024-11-06T03:10:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:10:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:10:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:10:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:10:47Z- TNavFilter::4 good beams. 2024-11-06T03:10:47Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:10:47Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:10:47Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:10:47Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:10:47Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:10:47Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:10:47Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:10:47Z- TerrainMapOctree: (vehN, vehE ) = (4061728.53, 594848.06). 2024-11-06T03:10:47Z- TerrainMapOctree: Min Distance = 100.39. 2024-11-06T03:10:47Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:10:47Z- Forcing Subcloud Comparison 2024-11-06T03:10:47Z- Weighting particles with subcloud comparison 2024-11-06T03:10:47Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:47Z- meanExpectedMeasDiff: -3.583150 delta_rms_squared: 1.465986 alpha: 0.986541 2024-11-06T03:10:47Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:47Z- meanExpectedMeasDiff: 1.529421 delta_rms_squared: 1.041922 alpha: 0.981166 2024-11-06T03:10:47Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:47Z- meanExpectedMeasDiff: 2.894778 delta_rms_squared: 1.192972 alpha: 0.983512 2024-11-06T03:10:47Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:47Z- meanExpectedMeasDiff: 1.556026 delta_rms_squared: 1.188665 alpha: 0.983453 2024-11-06T03:10:47Z- Subcloud NIS: 0.904468 2024-11-06T03:10:47Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:10:47Z- TNavPF:: sumWeights = 0.027204 2024-11-06T03:10:47Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:10:47Z- TNavPF::First Term of Map Covariance Matrix is 0.40 2024-11-06T03:10:47Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:10:47Z- TerrainNav::Resampling particle filter... 2024-11-06T03:10:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862646.82 sec. 2024-11-06T03:10:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:10:47Z- variances([0], [2], and psi_berg) = 0.243817 0.174967 0.000000 2024-11-06T03:10:47Z- publishEstimatesDecoder() - at 1730862647.60 sec 2024-11-06T03:10:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:10:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061761.45 594832.11 58.85 2024-11-06T03:10:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061759.44 594831.46 59.76 lat/lon: 36.70 -121.94 2024-11-06T03:10:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.17 0.68 0.00 2024-11-06T03:10:47Z- AHRS_M2 msg: 1730862647.97 epoch sec; seqNo:2996 2024-11-06T03:10:47Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.50 psi 2024-11-06T03:10:47Z- handleDepth()- Depth_Keller msg: 1730862647.98 epoch sec; seqNo:3189; depth 50.54 2024-11-06T03:10:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862648.00 epoch sec; seqNo:3189; 4061728.61 north; 594848.77 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:10:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862647.60 2024-11-06T03:10:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:48Z- waiting for 1730862651.60, time is 1730862648.00 2024-11-06T03:10:48Z- AHRS_M2 msg: 1730862648.39 epoch sec; seqNo:2997 2024-11-06T03:10:48Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.49 psi 2024-11-06T03:10:48Z- handleDepth()- Depth_Keller msg: 1730862648.40 epoch sec; seqNo:3190; depth 50.56 2024-11-06T03:10:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862648.41 epoch sec; seqNo:3190; 4061728.61 north; 594848.77 east; 36.70 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T03:10:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862647.60 2024-11-06T03:10:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:48Z- waiting for 1730862651.60, time is 1730862648.41 2024-11-06T03:10:48Z- AHRS_M2 msg: 1730862648.78 epoch sec; seqNo:2998 2024-11-06T03:10:48Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.49 psi 2024-11-06T03:10:48Z- handleDepth()- Depth_Keller msg: 1730862648.79 epoch sec; seqNo:3191; depth 50.55 2024-11-06T03:10:48Z- handleDVL() - DVL msg epochMillisec: 1730862648.82 2024-11-06T03:10:48Z- handleDvl() - RDI_Pathfinder msg: 1730862648.82 sec; seqNo:1064; #beams:4 2024-11-06T03:10:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.72 , 35.68 , 39.75 , 33.30 2024-11-06T03:10:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.08 2024-11-06T03:10:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862648.83 epoch sec; seqNo:3191; 4061728.62 north; 594849.99 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:10:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862647.60 2024-11-06T03:10:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:48Z- waiting for 1730862651.60, time is 1730862648.83 2024-11-06T03:10:49Z- AHRS_M2 msg: 1730862649.19 epoch sec; seqNo:2999 2024-11-06T03:10:49Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T03:10:49Z- handleDepth()- Depth_Keller msg: 1730862649.19 epoch sec; seqNo:3192; depth 50.59 2024-11-06T03:10:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862649.21 epoch sec; seqNo:3192; 4061728.62 north; 594849.99 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:10:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862647.60 2024-11-06T03:10:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:49Z- waiting for 1730862651.60, time is 1730862649.21 2024-11-06T03:10:49Z- AHRS_M2 msg: 1730862649.59 epoch sec; seqNo:3000 2024-11-06T03:10:49Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T03:10:49Z- handleDepth()- Depth_Keller msg: 1730862649.60 epoch sec; seqNo:3193; depth 50.58 2024-11-06T03:10:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862649.61 epoch sec; seqNo:3193; 4061728.62 north; 594849.99 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T03:10:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862647.60 2024-11-06T03:10:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:49Z- waiting for 1730862651.60, time is 1730862649.61 2024-11-06T03:10:50Z- AHRS_M2 msg: 1730862649.99 epoch sec; seqNo:3001 2024-11-06T03:10:50Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T03:10:50Z- handleDepth()- Depth_Keller msg: 1730862650.00 epoch sec; seqNo:3194; depth 50.59 2024-11-06T03:10:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862650.01 epoch sec; seqNo:3194; 4061729.02 north; 594851.21 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:10:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862647.60 2024-11-06T03:10:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:50Z- waiting for 1730862651.60, time is 1730862650.01 2024-11-06T03:10:50Z- AHRS_M2 msg: 1730862650.40 epoch sec; seqNo:3002 2024-11-06T03:10:50Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T03:10:50Z- handleDepth()- Depth_Keller msg: 1730862650.41 epoch sec; seqNo:3195; depth 50.59 2024-11-06T03:10:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862650.41 epoch sec; seqNo:3195; 4061729.02 north; 594851.21 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:10:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862647.60 2024-11-06T03:10:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:50Z- waiting for 1730862651.60, time is 1730862650.41 2024-11-06T03:10:50Z- AHRS_M2 msg: 1730862650.81 epoch sec; seqNo:3003 2024-11-06T03:10:50Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T03:10:50Z- handleDepth()- Depth_Keller msg: 1730862650.82 epoch sec; seqNo:3196; depth 50.59 2024-11-06T03:10:50Z- handleDVL() - DVL msg epochMillisec: 1730862650.86 2024-11-06T03:10:50Z- handleDvl() - RDI_Pathfinder msg: 1730862650.86 sec; seqNo:1065; #beams:4 2024-11-06T03:10:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.38 , 36.70 , 40.94 , 33.13 2024-11-06T03:10:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , 0.03 2024-11-06T03:10:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862650.87 epoch sec; seqNo:3196; 4061729.02 north; 594851.21 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:10:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862647.60 2024-11-06T03:10:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:50Z- waiting for 1730862651.60, time is 1730862650.87 2024-11-06T03:10:51Z- AHRS_M2 msg: 1730862651.21 epoch sec; seqNo:3004 2024-11-06T03:10:51Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T03:10:51Z- handleDepth()- Depth_Keller msg: 1730862651.22 epoch sec; seqNo:3197; depth 50.59 2024-11-06T03:10:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862651.23 epoch sec; seqNo:3197; 4061729.02 north; 594851.21 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:10:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862647.60 2024-11-06T03:10:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:51Z- waiting for 1730862651.60, time is 1730862651.23 2024-11-06T03:10:51Z- AHRS_M2 msg: 1730862651.61 epoch sec; seqNo:3005 2024-11-06T03:10:51Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T03:10:51Z- handleDepth()- Depth_Keller msg: 1730862651.62 epoch sec; seqNo:3198; depth 50.59 2024-11-06T03:10:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862651.64 epoch sec; seqNo:3198; 4061729.03 north; 594852.43 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:10:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862647.60 2024-11-06T03:10:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:51Z- waiting for 1730862651.60, time is 1730862651.64 2024-11-06T03:10:51Z- LcmTrn::updateTrn() >>>> pose time 1730862651.64, meas time 1730862650.86 <<<< 2024-11-06T03:10:51Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862651.64 2024-11-06T03:10:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862650.86 sec, ping # 0; waiting for INS data more recent than 1730862647.60... 2024-11-06T03:10:51Z- TerrainNav::motionUpdate() - currEstimate = (4061729, 594852.43, 50.59) 2024-11-06T03:10:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:10:51Z- TerrainNav:: SubcloudNIS is 0.90, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:10:51Z- TNavPF::Projecting Measurements 2024-11-06T03:10:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:10:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:10:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:10:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:10:51Z- TNavFilter::4 good beams. 2024-11-06T03:10:51Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:10:51Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:10:51Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:10:51Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:10:51Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:10:51Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:10:51Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:10:51Z- TerrainMapOctree: (vehN, vehE ) = (4061728.95, 594851.72). 2024-11-06T03:10:51Z- TerrainMapOctree: Min Distance = 103.87. 2024-11-06T03:10:51Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:10:51Z- Forcing Subcloud Comparison 2024-11-06T03:10:51Z- Weighting particles with subcloud comparison 2024-11-06T03:10:51Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:51Z- meanExpectedMeasDiff: -3.063859 delta_rms_squared: 1.516212 alpha: 0.986981 2024-11-06T03:10:51Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:51Z- meanExpectedMeasDiff: 2.227984 delta_rms_squared: 1.150835 alpha: 0.982918 2024-11-06T03:10:51Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:51Z- meanExpectedMeasDiff: 3.952931 delta_rms_squared: 1.266990 alpha: 0.984460 2024-11-06T03:10:51Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:51Z- meanExpectedMeasDiff: -0.872520 delta_rms_squared: 0.925734 alpha: 0.978852 2024-11-06T03:10:51Z- Subcloud NIS: 0.912978 2024-11-06T03:10:51Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:10:51Z- TNavPF:: sumWeights = 0.018530 2024-11-06T03:10:51Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:10:51Z- TNavPF::First Term of Map Covariance Matrix is 0.21 2024-11-06T03:10:51Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:10:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862650.86 sec. 2024-11-06T03:10:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:10:51Z- variances([0], [2], and psi_berg) = 0.196862 0.168088 0.000000 2024-11-06T03:10:51Z- publishEstimatesDecoder() - at 1730862651.64 sec 2024-11-06T03:10:51Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:10:51Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061759.23 594835.95 59.49 2024-11-06T03:10:51Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061759.80 594835.10 59.57 lat/lon: 36.70 -121.94 2024-11-06T03:10:51Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.20 0.17 0.69 0.00 2024-11-06T03:10:52Z- AHRS_M2 msg: 1730862652.02 epoch sec; seqNo:3006 2024-11-06T03:10:52Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.50 psi 2024-11-06T03:10:52Z- handleDepth()- Depth_Keller msg: 1730862652.03 epoch sec; seqNo:3199; depth 50.58 2024-11-06T03:10:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862652.05 epoch sec; seqNo:3199; 4061729.03 north; 594852.43 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T03:10:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862651.64 2024-11-06T03:10:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:52Z- waiting for 1730862655.64, time is 1730862652.05 2024-11-06T03:10:52Z- AHRS_M2 msg: 1730862652.42 epoch sec; seqNo:3007 2024-11-06T03:10:52Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.50 psi 2024-11-06T03:10:52Z- handleDepth()- Depth_Keller msg: 1730862652.43 epoch sec; seqNo:3200; depth 50.57 2024-11-06T03:10:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862652.44 epoch sec; seqNo:3200; 4061729.03 north; 594852.43 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T03:10:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862651.64 2024-11-06T03:10:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:52Z- waiting for 1730862655.64, time is 1730862652.44 2024-11-06T03:10:52Z- AHRS_M2 msg: 1730862652.83 epoch sec; seqNo:3008 2024-11-06T03:10:52Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.50 psi 2024-11-06T03:10:52Z- handleDepth()- Depth_Keller msg: 1730862652.87 epoch sec; seqNo:3201; depth 50.56 2024-11-06T03:10:52Z- handleDVL() - DVL msg epochMillisec: 1730862652.90 2024-11-06T03:10:52Z- handleDvl() - RDI_Pathfinder msg: 1730862652.90 sec; seqNo:1066; #beams:4 2024-11-06T03:10:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.88 , 36.53 , 42.64 , 32.96 2024-11-06T03:10:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , -0.03 2024-11-06T03:10:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862652.92 epoch sec; seqNo:3201; 4061729.04 north; 594853.65 east; 36.70 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T03:10:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862651.64 2024-11-06T03:10:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:52Z- waiting for 1730862655.64, time is 1730862652.92 2024-11-06T03:10:53Z- AHRS_M2 msg: 1730862653.23 epoch sec; seqNo:3009 2024-11-06T03:10:53Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.50 psi 2024-11-06T03:10:53Z- handleDepth()- Depth_Keller msg: 1730862653.23 epoch sec; seqNo:3202; depth 50.55 2024-11-06T03:10:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862653.25 epoch sec; seqNo:3202; 4061729.04 north; 594853.65 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:10:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862651.64 2024-11-06T03:10:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:53Z- waiting for 1730862655.64, time is 1730862653.25 2024-11-06T03:10:53Z- AHRS_M2 msg: 1730862653.64 epoch sec; seqNo:3010 2024-11-06T03:10:53Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T03:10:53Z- handleDepth()- Depth_Keller msg: 1730862653.64 epoch sec; seqNo:3203; depth 50.54 2024-11-06T03:10:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862653.65 epoch sec; seqNo:3203; 4061729.04 north; 594853.65 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:10:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862651.64 2024-11-06T03:10:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:53Z- waiting for 1730862655.64, time is 1730862653.65 2024-11-06T03:10:54Z- AHRS_M2 msg: 1730862654.05 epoch sec; seqNo:3011 2024-11-06T03:10:54Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T03:10:54Z- handleDepth()- Depth_Keller msg: 1730862654.08 epoch sec; seqNo:3204; depth 50.52 2024-11-06T03:10:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862654.11 epoch sec; seqNo:3204; 4061729.06 north; 594854.87 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:10:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862651.64 2024-11-06T03:10:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:54Z- waiting for 1730862655.64, time is 1730862654.11 2024-11-06T03:10:54Z- AHRS_M2 msg: 1730862654.44 epoch sec; seqNo:3012 2024-11-06T03:10:54Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T03:10:54Z- handleDepth()- Depth_Keller msg: 1730862654.44 epoch sec; seqNo:3205; depth 50.50 2024-11-06T03:10:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862654.45 epoch sec; seqNo:3205; 4061729.06 north; 594854.87 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:10:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862651.64 2024-11-06T03:10:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:54Z- waiting for 1730862655.64, time is 1730862654.45 2024-11-06T03:10:54Z- AHRS_M2 msg: 1730862654.85 epoch sec; seqNo:3013 2024-11-06T03:10:54Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T03:10:54Z- handleDepth()- Depth_Keller msg: 1730862654.86 epoch sec; seqNo:3206; depth 50.50 2024-11-06T03:10:54Z- handleDVL() - DVL msg epochMillisec: 1730862654.88 2024-11-06T03:10:54Z- handleDvl() - RDI_Pathfinder msg: 1730862654.88 sec; seqNo:1067; #beams:4 2024-11-06T03:10:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.40 , 36.70 , 43.49 , 32.96 2024-11-06T03:10:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.00 , -0.05 2024-11-06T03:10:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862654.91 epoch sec; seqNo:3206; 4061729.06 north; 594854.87 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:10:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862651.64 2024-11-06T03:10:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:55Z- waiting for 1730862655.64, time is 1730862654.91 2024-11-06T03:10:55Z- AHRS_M2 msg: 1730862655.25 epoch sec; seqNo:3014 2024-11-06T03:10:55Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T03:10:55Z- handleDepth()- Depth_Keller msg: 1730862655.25 epoch sec; seqNo:3207; depth 50.47 2024-11-06T03:10:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862655.27 epoch sec; seqNo:3207; 4061729.06 north; 594854.87 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:10:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862651.64 2024-11-06T03:10:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:55Z- waiting for 1730862655.64, time is 1730862655.27 2024-11-06T03:10:55Z- AHRS_M2 msg: 1730862655.65 epoch sec; seqNo:3015 2024-11-06T03:10:55Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T03:10:55Z- handleDepth()- Depth_Keller msg: 1730862655.67 epoch sec; seqNo:3208; depth 50.47 2024-11-06T03:10:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862655.68 epoch sec; seqNo:3208; 4061729.07 north; 594856.09 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:10:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862651.64 2024-11-06T03:10:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:55Z- waiting for 1730862655.64, time is 1730862655.68 2024-11-06T03:10:55Z- LcmTrn::updateTrn() >>>> pose time 1730862655.68, meas time 1730862654.88 <<<< 2024-11-06T03:10:55Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862655.68 2024-11-06T03:10:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862654.88 sec, ping # 0; waiting for INS data more recent than 1730862651.64... 2024-11-06T03:10:55Z- TerrainNav::motionUpdate() - currEstimate = (4061729, 594856.09, 50.47) 2024-11-06T03:10:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:10:55Z- TerrainNav:: SubcloudNIS is 0.91, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:10:55Z- TNavPF::Projecting Measurements 2024-11-06T03:10:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:10:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:10:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:10:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:10:55Z- TNavFilter::4 good beams. 2024-11-06T03:10:55Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:10:55Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:10:55Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:10:55Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:10:55Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:10:55Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:10:55Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:10:55Z- TerrainMapOctree: (vehN, vehE ) = (4061729.06, 594855.36). 2024-11-06T03:10:55Z- TerrainMapOctree: Min Distance = 107.42. 2024-11-06T03:10:55Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:10:55Z- Forcing Subcloud Comparison 2024-11-06T03:10:55Z- Weighting particles with subcloud comparison 2024-11-06T03:10:55Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:55Z- meanExpectedMeasDiff: -2.826714 delta_rms_squared: 1.535770 alpha: 0.987145 2024-11-06T03:10:55Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:55Z- meanExpectedMeasDiff: 1.040601 delta_rms_squared: 1.077882 alpha: 0.981783 2024-11-06T03:10:55Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:55Z- meanExpectedMeasDiff: 5.935557 delta_rms_squared: 1.273035 alpha: 0.984533 2024-11-06T03:10:55Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:10:55Z- meanExpectedMeasDiff: -3.858469 delta_rms_squared: 0.930287 alpha: 0.978954 2024-11-06T03:10:55Z- Subcloud NIS: 0.950427 2024-11-06T03:10:55Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:10:55Z- TNavPF:: sumWeights = 0.000802 2024-11-06T03:10:55Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:10:55Z- TNavPF::First Term of Map Covariance Matrix is 0.01 2024-11-06T03:10:55Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:10:55Z- TerrainNav::Resampling particle filter... 2024-11-06T03:10:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862654.88 sec. 2024-11-06T03:10:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:10:55Z- variances([0], [2], and psi_berg) = 0.157368 0.124240 0.000000 2024-11-06T03:10:55Z- publishEstimatesDecoder() - at 1730862655.68 sec 2024-11-06T03:10:55Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:10:55Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061761.92 594839.44 60.68 2024-11-06T03:10:55Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061759.77 594838.69 59.52 lat/lon: 36.70 -121.94 2024-11-06T03:10:55Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.12 0.67 0.00 2024-11-06T03:10:56Z- AHRS_M2 msg: 1730862656.06 epoch sec; seqNo:3016 2024-11-06T03:10:56Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T03:10:56Z- handleDepth()- Depth_Keller msg: 1730862656.07 epoch sec; seqNo:3209; depth 50.44 2024-11-06T03:10:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862656.08 epoch sec; seqNo:3209; 4061729.07 north; 594856.09 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:10:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862655.68 2024-11-06T03:10:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:56Z- waiting for 1730862659.68, time is 1730862656.08 2024-11-06T03:10:56Z- AHRS_M2 msg: 1730862656.46 epoch sec; seqNo:3017 2024-11-06T03:10:56Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T03:10:56Z- handleDepth()- Depth_Keller msg: 1730862656.47 epoch sec; seqNo:3210; depth 50.44 2024-11-06T03:10:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862656.48 epoch sec; seqNo:3210; 4061729.07 north; 594856.09 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:10:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862655.68 2024-11-06T03:10:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:56Z- waiting for 1730862659.68, time is 1730862656.48 2024-11-06T03:10:56Z- AHRS_M2 msg: 1730862656.86 epoch sec; seqNo:3018 2024-11-06T03:10:56Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.50 psi 2024-11-06T03:10:56Z- handleDepth()- Depth_Keller msg: 1730862656.87 epoch sec; seqNo:3211; depth 50.42 2024-11-06T03:10:56Z- handleDVL() - DVL msg epochMillisec: 1730862656.90 2024-11-06T03:10:56Z- handleDvl() - RDI_Pathfinder msg: 1730862656.90 sec; seqNo:1068; #beams:4 2024-11-06T03:10:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.07 , 36.70 , 43.66 , 33.13 2024-11-06T03:10:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.04 , -0.00 2024-11-06T03:10:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862656.91 epoch sec; seqNo:3211; 4061729.46 north; 594857.30 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:10:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862655.68 2024-11-06T03:10:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:57Z- waiting for 1730862659.68, time is 1730862656.91 2024-11-06T03:10:57Z- AHRS_M2 msg: 1730862657.27 epoch sec; seqNo:3019 2024-11-06T03:10:57Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.50 psi 2024-11-06T03:10:57Z- handleDepth()- Depth_Keller msg: 1730862657.28 epoch sec; seqNo:3212; depth 50.40 2024-11-06T03:10:57Z- handleDVL() - DVL msg epochMillisec: 1730862657.28 2024-11-06T03:10:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:10:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:10:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:10:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:10:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:10:57Z- handleDvl() - RDI_Pathfinder msg: 1730862657.28 sec; seqNo:1069; #beams:0 2024-11-06T03:10:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:10:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.04 , -0.00 2024-11-06T03:10:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862657.29 epoch sec; seqNo:3212; 4061729.46 north; 594857.30 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:10:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862655.68 2024-11-06T03:10:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:57Z- waiting for 1730862659.68, time is 1730862657.29 2024-11-06T03:10:57Z- AHRS_M2 msg: 1730862657.67 epoch sec; seqNo:3020 2024-11-06T03:10:57Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.50 psi 2024-11-06T03:10:57Z- handleDepth()- Depth_Keller msg: 1730862657.69 epoch sec; seqNo:3213; depth 50.40 2024-11-06T03:10:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862657.70 epoch sec; seqNo:3213; 4061729.46 north; 594857.30 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:10:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862655.68 2024-11-06T03:10:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:57Z- waiting for 1730862659.68, time is 1730862657.70 2024-11-06T03:10:58Z- AHRS_M2 msg: 1730862658.08 epoch sec; seqNo:3021 2024-11-06T03:10:58Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T03:10:58Z- handleDepth()- Depth_Keller msg: 1730862658.08 epoch sec; seqNo:3214; depth 50.39 2024-11-06T03:10:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862658.09 epoch sec; seqNo:3214; 4061729.46 north; 594857.30 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:10:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862655.68 2024-11-06T03:10:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:58Z- waiting for 1730862659.68, time is 1730862658.09 2024-11-06T03:10:58Z- AHRS_M2 msg: 1730862658.48 epoch sec; seqNo:3022 2024-11-06T03:10:58Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.50 psi 2024-11-06T03:10:58Z- handleDepth()- Depth_Keller msg: 1730862658.48 epoch sec; seqNo:3215; depth 50.38 2024-11-06T03:10:58Z- handleDVL() - DVL msg epochMillisec: 1730862658.49 2024-11-06T03:10:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:10:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:10:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:10:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:10:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:10:58Z- handleDvl() - RDI_Pathfinder msg: 1730862658.49 sec; seqNo:1070; #beams:0 2024-11-06T03:10:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:10:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.04 , -0.00 2024-11-06T03:10:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862658.49 epoch sec; seqNo:3215; 4061729.48 north; 594858.52 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:10:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862655.68 2024-11-06T03:10:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:58Z- waiting for 1730862659.68, time is 1730862658.49 2024-11-06T03:10:58Z- AHRS_M2 msg: 1730862658.88 epoch sec; seqNo:3023 2024-11-06T03:10:58Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.50 psi 2024-11-06T03:10:58Z- handleDepth()- Depth_Keller msg: 1730862658.90 epoch sec; seqNo:3216; depth 50.34 2024-11-06T03:10:58Z- handleDVL() - DVL msg epochMillisec: 1730862658.92 2024-11-06T03:10:58Z- handleDvl() - RDI_Pathfinder msg: 1730862658.92 sec; seqNo:1071; #beams:4 2024-11-06T03:10:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.43 , 36.81 , 43.92 , 33.45 2024-11-06T03:10:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , 0.04 2024-11-06T03:10:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862658.94 epoch sec; seqNo:3216; 4061729.48 north; 594858.52 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:10:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862655.68 2024-11-06T03:10:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:59Z- waiting for 1730862659.68, time is 1730862658.94 2024-11-06T03:10:59Z- AHRS_M2 msg: 1730862659.29 epoch sec; seqNo:3024 2024-11-06T03:10:59Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.50 psi 2024-11-06T03:10:59Z- handleDepth()- Depth_Keller msg: 1730862659.29 epoch sec; seqNo:3217; depth 50.33 2024-11-06T03:10:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862659.32 epoch sec; seqNo:3217; 4061729.48 north; 594858.52 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:10:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:10:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862655.68 2024-11-06T03:10:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:59Z- waiting for 1730862659.68, time is 1730862659.32 2024-11-06T03:10:59Z- AHRS_M2 msg: 1730862659.70 epoch sec; seqNo:3025 2024-11-06T03:10:59Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.50 psi 2024-11-06T03:10:59Z- handleDepth()- Depth_Keller msg: 1730862659.70 epoch sec; seqNo:3218; depth 50.32 2024-11-06T03:10:59Z- handleDVL() - DVL msg epochMillisec: 1730862659.70 2024-11-06T03:10:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:10:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:10:59Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:10:59Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:10:59Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:10:59Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:10:59Z- handleDvl() - RDI_Pathfinder msg: 1730862659.70 sec; seqNo:1072; #beams:0 2024-11-06T03:10:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:10:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , 0.04 2024-11-06T03:10:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862659.71 epoch sec; seqNo:3218; 4061729.48 north; 594858.52 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:10:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:10:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862655.68 2024-11-06T03:10:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:10:59Z- waiting for 1730862659.68, time is 1730862659.71 2024-11-06T03:10:59Z- LcmTrn::updateTrn() >>>> pose time 1730862659.71, meas time 1730862659.70 <<<< 2024-11-06T03:10:59Z- LcmTrn::updateTrn() equating pose 1730862659.70 and meas times 1730862659.70 2024-11-06T03:10:59Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862659.70 2024-11-06T03:10:59Z- TerrainNav::motionUpdate() - currEstimate = (4061729, 594858.52, 50.32) 2024-11-06T03:10:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:10:59Z- TerrainNav:: SubcloudNIS is 0.95, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:10:59Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:10:59Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:10:59Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:10:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:10:59Z- variances([0], [2], and psi_berg) = 0.158203 0.125202 0.000000 2024-11-06T03:10:59Z- publishEstimatesDecoder() - at 1730862659.70 sec 2024-11-06T03:10:59Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:10:59Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061762.34 594841.87 61.12 2024-11-06T03:10:59Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061760.17 594841.13 59.37 lat/lon: 36.70 -121.94 2024-11-06T03:10:59Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.13 0.91 0.00 2024-11-06T03:11:00Z- AHRS_M2 msg: 1730862660.10 epoch sec; seqNo:3026 2024-11-06T03:11:00Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:00Z- handleDepth()- Depth_Keller msg: 1730862660.11 epoch sec; seqNo:3219; depth 50.32 2024-11-06T03:11:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862660.12 epoch sec; seqNo:3219; 4061729.49 north; 594859.74 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:11:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862659.70 2024-11-06T03:11:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:00Z- waiting for 1730862663.70, time is 1730862660.12 2024-11-06T03:11:00Z- AHRS_M2 msg: 1730862660.50 epoch sec; seqNo:3027 2024-11-06T03:11:00Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:00Z- handleDepth()- Depth_Keller msg: 1730862660.51 epoch sec; seqNo:3220; depth 50.31 2024-11-06T03:11:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862660.52 epoch sec; seqNo:3220; 4061729.49 north; 594859.74 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:11:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862659.70 2024-11-06T03:11:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:00Z- waiting for 1730862663.70, time is 1730862660.52 2024-11-06T03:11:00Z- AHRS_M2 msg: 1730862660.90 epoch sec; seqNo:3028 2024-11-06T03:11:00Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:00Z- handleDepth()- Depth_Keller msg: 1730862660.91 epoch sec; seqNo:3221; depth 50.29 2024-11-06T03:11:00Z- handleDVL() - DVL msg epochMillisec: 1730862660.94 2024-11-06T03:11:00Z- handleDvl() - RDI_Pathfinder msg: 1730862660.94 sec; seqNo:1073; #beams:4 2024-11-06T03:11:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.11 , 37.00 , 43.92 , 33.83 2024-11-06T03:11:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , 0.04 2024-11-06T03:11:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862660.96 epoch sec; seqNo:3221; 4061729.87 north; 594859.74 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:11:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862659.70 2024-11-06T03:11:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:01Z- waiting for 1730862663.70, time is 1730862660.96 2024-11-06T03:11:01Z- AHRS_M2 msg: 1730862661.31 epoch sec; seqNo:3029 2024-11-06T03:11:01Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:01Z- handleDepth()- Depth_Keller msg: 1730862661.31 epoch sec; seqNo:3222; depth 50.29 2024-11-06T03:11:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862661.34 epoch sec; seqNo:3222; 4061729.87 north; 594859.74 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:11:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862659.70 2024-11-06T03:11:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:01Z- waiting for 1730862663.70, time is 1730862661.34 2024-11-06T03:11:01Z- handleDepth()- Depth_Keller msg: 1730862661.74 epoch sec; seqNo:3223; depth 50.29 2024-11-06T03:11:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862661.75 epoch sec; seqNo:3223; 4061729.88 north; 594860.96 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:11:01Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:11:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862659.70 2024-11-06T03:11:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:01Z- waiting for 1730862663.70, time is 1730862661.75 2024-11-06T03:11:02Z- AHRS_M2 msg: 1730862662.11 epoch sec; seqNo:3030 2024-11-06T03:11:02Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:02Z- handleDepth()- Depth_Keller msg: 1730862662.13 epoch sec; seqNo:3224; depth 50.27 2024-11-06T03:11:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862662.14 epoch sec; seqNo:3224; 4061729.88 north; 594860.96 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:11:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862659.70 2024-11-06T03:11:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:02Z- waiting for 1730862663.70, time is 1730862662.14 2024-11-06T03:11:02Z- AHRS_M2 msg: 1730862662.53 epoch sec; seqNo:3031 2024-11-06T03:11:02Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:02Z- handleDepth()- Depth_Keller msg: 1730862662.53 epoch sec; seqNo:3225; depth 50.26 2024-11-06T03:11:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862662.55 epoch sec; seqNo:3225; 4061729.88 north; 594860.96 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:11:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862659.70 2024-11-06T03:11:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:02Z- waiting for 1730862663.70, time is 1730862662.55 2024-11-06T03:11:02Z- AHRS_M2 msg: 1730862662.93 epoch sec; seqNo:3032 2024-11-06T03:11:02Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:02Z- handleDepth()- Depth_Keller msg: 1730862662.94 epoch sec; seqNo:3226; depth 50.26 2024-11-06T03:11:02Z- handleDVL() - DVL msg epochMillisec: 1730862662.96 2024-11-06T03:11:02Z- handleDvl() - RDI_Pathfinder msg: 1730862662.96 sec; seqNo:1074; #beams:4 2024-11-06T03:11:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.30 , 37.38 , 43.92 , 33.83 2024-11-06T03:11:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , 0.01 2024-11-06T03:11:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862662.99 epoch sec; seqNo:3226; 4061729.90 north; 594862.18 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:11:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862659.70 2024-11-06T03:11:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:03Z- waiting for 1730862663.70, time is 1730862662.99 2024-11-06T03:11:03Z- AHRS_M2 msg: 1730862663.33 epoch sec; seqNo:3033 2024-11-06T03:11:03Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:03Z- handleDepth()- Depth_Keller msg: 1730862663.34 epoch sec; seqNo:3227; depth 50.24 2024-11-06T03:11:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862663.36 epoch sec; seqNo:3227; 4061729.90 north; 594862.18 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:11:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862659.70 2024-11-06T03:11:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:03Z- waiting for 1730862663.70, time is 1730862663.36 2024-11-06T03:11:03Z- AHRS_M2 msg: 1730862663.73 epoch sec; seqNo:3034 2024-11-06T03:11:03Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:03Z- handleDepth()- Depth_Keller msg: 1730862663.74 epoch sec; seqNo:3228; depth 50.23 2024-11-06T03:11:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862663.75 epoch sec; seqNo:3228; 4061729.90 north; 594862.18 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:11:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862659.70 2024-11-06T03:11:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:03Z- waiting for 1730862663.70, time is 1730862663.75 2024-11-06T03:11:03Z- LcmTrn::updateTrn() >>>> pose time 1730862663.75, meas time 1730862662.96 <<<< 2024-11-06T03:11:03Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862663.75 2024-11-06T03:11:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862662.96 sec, ping # 0; waiting for INS data more recent than 1730862659.70... 2024-11-06T03:11:03Z- TerrainNav::motionUpdate() - currEstimate = (4061730, 594862.18, 50.23) 2024-11-06T03:11:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:11:03Z- TerrainNav:: SubcloudNIS is 0.95, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:11:03Z- TNavPF::Projecting Measurements 2024-11-06T03:11:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:11:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:11:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:11:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:11:03Z- TNavFilter::4 good beams. 2024-11-06T03:11:03Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:11:03Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:11:03Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:11:03Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:11:03Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:11:03Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:11:03Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:11:03Z- TerrainMapOctree: (vehN, vehE ) = (4061729.81, 594861.47). 2024-11-06T03:11:03Z- TerrainMapOctree: Min Distance = 113.28. 2024-11-06T03:11:03Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:11:04Z- Forcing Subcloud Comparison 2024-11-06T03:11:04Z- Weighting particles with subcloud comparison 2024-11-06T03:11:04Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:04Z- meanExpectedMeasDiff: 0.087562 delta_rms_squared: 1.860371 alpha: 0.989364 2024-11-06T03:11:04Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:04Z- meanExpectedMeasDiff: 1.426899 delta_rms_squared: 1.329073 alpha: 0.985175 2024-11-06T03:11:04Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:04Z- meanExpectedMeasDiff: 6.006680 delta_rms_squared: 1.572571 alpha: 0.987442 2024-11-06T03:11:04Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:04Z- meanExpectedMeasDiff: -3.821677 delta_rms_squared: 1.550718 alpha: 0.987267 2024-11-06T03:11:04Z- Subcloud NIS: 0.961683 2024-11-06T03:11:04Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:11:04Z- TNavPF:: sumWeights = 0.001754 2024-11-06T03:11:04Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:11:04Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:11:04Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:11:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862662.96 sec. 2024-11-06T03:11:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:11:04Z- variances([0], [2], and psi_berg) = 0.157657 0.123690 0.000000 2024-11-06T03:11:04Z- publishEstimatesDecoder() - at 1730862663.75 sec 2024-11-06T03:11:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:11:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061760.79 594844.30 58.43 2024-11-06T03:11:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061760.59 594844.78 59.00 lat/lon: 36.70 -121.94 2024-11-06T03:11:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.12 0.71 0.00 2024-11-06T03:11:04Z- AHRS_M2 msg: 1730862664.13 epoch sec; seqNo:3035 2024-11-06T03:11:04Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:04Z- handleDepth()- Depth_Keller msg: 1730862664.14 epoch sec; seqNo:3229; depth 50.22 2024-11-06T03:11:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862664.15 epoch sec; seqNo:3229; 4061729.90 north; 594862.18 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:11:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862663.75 2024-11-06T03:11:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:04Z- waiting for 1730862667.75, time is 1730862664.15 2024-11-06T03:11:04Z- AHRS_M2 msg: 1730862664.54 epoch sec; seqNo:3036 2024-11-06T03:11:04Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:04Z- handleDepth()- Depth_Keller msg: 1730862664.54 epoch sec; seqNo:3230; depth 50.21 2024-11-06T03:11:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862664.55 epoch sec; seqNo:3230; 4061729.91 north; 594863.40 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:11:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862663.75 2024-11-06T03:11:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:04Z- waiting for 1730862667.75, time is 1730862664.55 2024-11-06T03:11:04Z- AHRS_M2 msg: 1730862664.94 epoch sec; seqNo:3037 2024-11-06T03:11:04Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:04Z- handleDepth()- Depth_Keller msg: 1730862664.95 epoch sec; seqNo:3231; depth 50.21 2024-11-06T03:11:04Z- handleDVL() - DVL msg epochMillisec: 1730862664.97 2024-11-06T03:11:04Z- handleDvl() - RDI_Pathfinder msg: 1730862664.97 sec; seqNo:1075; #beams:4 2024-11-06T03:11:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.86 , 37.38 , 44.48 , 34.20 2024-11-06T03:11:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , -0.00 2024-11-06T03:11:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862664.99 epoch sec; seqNo:3231; 4061730.29 north; 594863.40 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:11:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862663.75 2024-11-06T03:11:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:05Z- waiting for 1730862667.75, time is 1730862664.99 2024-11-06T03:11:05Z- AHRS_M2 msg: 1730862665.35 epoch sec; seqNo:3038 2024-11-06T03:11:05Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:05Z- handleDepth()- Depth_Keller msg: 1730862665.35 epoch sec; seqNo:3232; depth 50.21 2024-11-06T03:11:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862665.37 epoch sec; seqNo:3232; 4061730.29 north; 594863.40 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:11:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862663.75 2024-11-06T03:11:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:05Z- waiting for 1730862667.75, time is 1730862665.37 2024-11-06T03:11:05Z- AHRS_M2 msg: 1730862665.76 epoch sec; seqNo:3039 2024-11-06T03:11:05Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:05Z- handleDepth()- Depth_Keller msg: 1730862665.76 epoch sec; seqNo:3233; depth 50.20 2024-11-06T03:11:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862665.77 epoch sec; seqNo:3233; 4061730.30 north; 594864.62 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:11:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862663.75 2024-11-06T03:11:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:05Z- waiting for 1730862667.75, time is 1730862665.77 2024-11-06T03:11:06Z- AHRS_M2 msg: 1730862666.15 epoch sec; seqNo:3040 2024-11-06T03:11:06Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:06Z- handleDepth()- Depth_Keller msg: 1730862666.17 epoch sec; seqNo:3234; depth 50.18 2024-11-06T03:11:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862666.18 epoch sec; seqNo:3234; 4061730.30 north; 594864.62 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:11:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862663.75 2024-11-06T03:11:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:06Z- waiting for 1730862667.75, time is 1730862666.18 2024-11-06T03:11:06Z- AHRS_M2 msg: 1730862666.56 epoch sec; seqNo:3041 2024-11-06T03:11:06Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:06Z- handleDepth()- Depth_Keller msg: 1730862666.57 epoch sec; seqNo:3235; depth 50.17 2024-11-06T03:11:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862666.59 epoch sec; seqNo:3235; 4061730.30 north; 594864.62 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:11:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862663.75 2024-11-06T03:11:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:06Z- waiting for 1730862667.75, time is 1730862666.59 2024-11-06T03:11:06Z- AHRS_M2 msg: 1730862666.96 epoch sec; seqNo:3042 2024-11-06T03:11:06Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:06Z- handleDepth()- Depth_Keller msg: 1730862666.97 epoch sec; seqNo:3236; depth 50.17 2024-11-06T03:11:07Z- handleDVL() - DVL msg epochMillisec: 1730862666.99 2024-11-06T03:11:07Z- handleDvl() - RDI_Pathfinder msg: 1730862666.99 sec; seqNo:1076; #beams:4 2024-11-06T03:11:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.86 , 37.19 , 44.29 , 34.38 2024-11-06T03:11:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , 0.01 2024-11-06T03:11:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862667.01 epoch sec; seqNo:3236; 4061730.30 north; 594864.62 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:11:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862663.75 2024-11-06T03:11:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:07Z- waiting for 1730862667.75, time is 1730862667.01 2024-11-06T03:11:07Z- AHRS_M2 msg: 1730862667.37 epoch sec; seqNo:3043 2024-11-06T03:11:07Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:07Z- handleDepth()- Depth_Keller msg: 1730862667.37 epoch sec; seqNo:3237; depth 50.15 2024-11-06T03:11:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862667.39 epoch sec; seqNo:3237; 4061730.32 north; 594865.84 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:11:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862663.75 2024-11-06T03:11:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:07Z- waiting for 1730862667.75, time is 1730862667.39 2024-11-06T03:11:07Z- AHRS_M2 msg: 1730862667.78 epoch sec; seqNo:3044 2024-11-06T03:11:07Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:07Z- handleDepth()- Depth_Keller msg: 1730862667.78 epoch sec; seqNo:3238; depth 50.14 2024-11-06T03:11:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862667.79 epoch sec; seqNo:3238; 4061730.32 north; 594865.84 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:11:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862663.75 2024-11-06T03:11:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:07Z- waiting for 1730862667.75, time is 1730862667.79 2024-11-06T03:11:07Z- LcmTrn::updateTrn() >>>> pose time 1730862667.79, meas time 1730862666.99 <<<< 2024-11-06T03:11:07Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862667.79 2024-11-06T03:11:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862666.99 sec, ping # 0; waiting for INS data more recent than 1730862663.75... 2024-11-06T03:11:07Z- TerrainNav::motionUpdate() - currEstimate = (4061730, 594865.84, 50.14) 2024-11-06T03:11:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:11:07Z- TerrainNav:: SubcloudNIS is 0.96, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:11:07Z- TNavPF::Projecting Measurements 2024-11-06T03:11:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:11:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:11:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:11:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:11:07Z- TNavFilter::4 good beams. 2024-11-06T03:11:07Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:11:07Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:11:07Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:11:07Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:11:07Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:11:07Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:11:07Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:11:07Z- TerrainMapOctree: (vehN, vehE ) = (4061730.23, 594865.11). 2024-11-06T03:11:07Z- TerrainMapOctree: Min Distance = 116.80. 2024-11-06T03:11:07Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:11:08Z- Forcing Subcloud Comparison 2024-11-06T03:11:08Z- Weighting particles with subcloud comparison 2024-11-06T03:11:08Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:08Z- meanExpectedMeasDiff: 0.197577 delta_rms_squared: 1.522804 alpha: 0.987037 2024-11-06T03:11:08Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:08Z- meanExpectedMeasDiff: 0.848209 delta_rms_squared: 1.088132 alpha: 0.981952 2024-11-06T03:11:08Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:08Z- meanExpectedMeasDiff: 5.947837 delta_rms_squared: 1.288228 alpha: 0.984712 2024-11-06T03:11:08Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:08Z- meanExpectedMeasDiff: -3.667848 delta_rms_squared: 1.268710 alpha: 0.984481 2024-11-06T03:11:08Z- Subcloud NIS: 0.990775 2024-11-06T03:11:08Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:11:08Z- TNavPF:: sumWeights = 0.002919 2024-11-06T03:11:08Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:11:08Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:11:08Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:11:08Z- TerrainNav::Resampling particle filter... 2024-11-06T03:11:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862666.99 sec. 2024-11-06T03:11:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:11:08Z- variances([0], [2], and psi_berg) = 0.155168 0.123826 0.000000 2024-11-06T03:11:08Z- publishEstimatesDecoder() - at 1730862667.79 sec 2024-11-06T03:11:08Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:11:08Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061762.64 594849.23 58.66 2024-11-06T03:11:08Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061761.01 594848.44 58.69 lat/lon: 36.70 -121.94 2024-11-06T03:11:08Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.12 0.68 0.00 2024-11-06T03:11:08Z- AHRS_M2 msg: 1730862668.19 epoch sec; seqNo:3045 2024-11-06T03:11:08Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:08Z- handleDepth()- Depth_Keller msg: 1730862668.20 epoch sec; seqNo:3239; depth 50.14 2024-11-06T03:11:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862668.23 epoch sec; seqNo:3239; 4061730.32 north; 594865.84 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:11:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862667.79 2024-11-06T03:11:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:08Z- waiting for 1730862671.79, time is 1730862668.23 2024-11-06T03:11:08Z- AHRS_M2 msg: 1730862668.58 epoch sec; seqNo:3046 2024-11-06T03:11:08Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:08Z- handleDepth()- Depth_Keller msg: 1730862668.59 epoch sec; seqNo:3240; depth 50.12 2024-11-06T03:11:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862668.60 epoch sec; seqNo:3240; 4061730.32 north; 594865.84 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:11:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862667.79 2024-11-06T03:11:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:08Z- waiting for 1730862671.79, time is 1730862668.60 2024-11-06T03:11:08Z- AHRS_M2 msg: 1730862668.98 epoch sec; seqNo:3047 2024-11-06T03:11:08Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:08Z- handleDepth()- Depth_Keller msg: 1730862668.99 epoch sec; seqNo:3241; depth 50.13 2024-11-06T03:11:09Z- handleDVL() - DVL msg epochMillisec: 1730862669.01 2024-11-06T03:11:09Z- handleDvl() - RDI_Pathfinder msg: 1730862669.01 sec; seqNo:1077; #beams:4 2024-11-06T03:11:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.23 , 37.38 , 44.66 , 34.57 2024-11-06T03:11:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , 0.03 2024-11-06T03:11:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862669.03 epoch sec; seqNo:3241; 4061730.33 north; 594867.06 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:11:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862667.79 2024-11-06T03:11:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:09Z- waiting for 1730862671.79, time is 1730862669.03 2024-11-06T03:11:09Z- AHRS_M2 msg: 1730862669.39 epoch sec; seqNo:3048 2024-11-06T03:11:09Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:09Z- handleDepth()- Depth_Keller msg: 1730862669.40 epoch sec; seqNo:3242; depth 50.12 2024-11-06T03:11:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862669.42 epoch sec; seqNo:3242; 4061730.33 north; 594867.06 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:11:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862667.79 2024-11-06T03:11:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:09Z- waiting for 1730862671.79, time is 1730862669.42 2024-11-06T03:11:09Z- handleDepth()- Depth_Keller msg: 1730862669.82 epoch sec; seqNo:3243; depth 50.10 2024-11-06T03:11:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862669.83 epoch sec; seqNo:3243; 4061730.33 north; 594867.06 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:11:09Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:11:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862667.79 2024-11-06T03:11:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:09Z- waiting for 1730862671.79, time is 1730862669.83 2024-11-06T03:11:10Z- AHRS_M2 msg: 1730862670.20 epoch sec; seqNo:3049 2024-11-06T03:11:10Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:10Z- handleDepth()- Depth_Keller msg: 1730862670.20 epoch sec; seqNo:3244; depth 50.09 2024-11-06T03:11:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862670.21 epoch sec; seqNo:3244; 4061730.34 north; 594868.28 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:11:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862667.79 2024-11-06T03:11:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:10Z- waiting for 1730862671.79, time is 1730862670.21 2024-11-06T03:11:10Z- AHRS_M2 msg: 1730862670.60 epoch sec; seqNo:3050 2024-11-06T03:11:10Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.51 psi 2024-11-06T03:11:10Z- handleDepth()- Depth_Keller msg: 1730862670.61 epoch sec; seqNo:3245; depth 50.09 2024-11-06T03:11:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862670.62 epoch sec; seqNo:3245; 4061730.72 north; 594868.27 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:11:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862667.79 2024-11-06T03:11:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:10Z- waiting for 1730862671.79, time is 1730862670.62 2024-11-06T03:11:11Z- AHRS_M2 msg: 1730862671.00 epoch sec; seqNo:3051 2024-11-06T03:11:11Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.51 psi 2024-11-06T03:11:11Z- handleDepth()- Depth_Keller msg: 1730862671.01 epoch sec; seqNo:3246; depth 50.07 2024-11-06T03:11:11Z- handleDVL() - DVL msg epochMillisec: 1730862671.04 2024-11-06T03:11:11Z- handleDvl() - RDI_Pathfinder msg: 1730862671.04 sec; seqNo:1078; #beams:4 2024-11-06T03:11:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.05 , 37.94 , 44.66 , 34.76 2024-11-06T03:11:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.02 2024-11-06T03:11:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862671.05 epoch sec; seqNo:3246; 4061730.72 north; 594868.27 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:11:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862667.79 2024-11-06T03:11:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:11Z- waiting for 1730862671.79, time is 1730862671.05 2024-11-06T03:11:11Z- AHRS_M2 msg: 1730862671.41 epoch sec; seqNo:3052 2024-11-06T03:11:11Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.51 psi 2024-11-06T03:11:11Z- handleDepth()- Depth_Keller msg: 1730862671.42 epoch sec; seqNo:3247; depth 50.07 2024-11-06T03:11:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862671.43 epoch sec; seqNo:3247; 4061730.72 north; 594868.27 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:11:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862667.79 2024-11-06T03:11:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:11Z- waiting for 1730862671.79, time is 1730862671.43 2024-11-06T03:11:11Z- AHRS_M2 msg: 1730862671.81 epoch sec; seqNo:3053 2024-11-06T03:11:11Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.51 psi 2024-11-06T03:11:11Z- handleDepth()- Depth_Keller msg: 1730862671.83 epoch sec; seqNo:3248; depth 50.05 2024-11-06T03:11:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862671.84 epoch sec; seqNo:3248; 4061730.73 north; 594869.49 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:11:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862667.79 2024-11-06T03:11:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:11Z- waiting for 1730862671.79, time is 1730862671.84 2024-11-06T03:11:11Z- LcmTrn::updateTrn() >>>> pose time 1730862671.84, meas time 1730862671.04 <<<< 2024-11-06T03:11:11Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862671.84 2024-11-06T03:11:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862671.04 sec, ping # 0; waiting for INS data more recent than 1730862667.79... 2024-11-06T03:11:11Z- TerrainNav::motionUpdate() - currEstimate = (4061731, 594869.49, 50.05) 2024-11-06T03:11:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:11:11Z- TerrainNav:: SubcloudNIS is 0.99, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:11:11Z- TNavPF::Projecting Measurements 2024-11-06T03:11:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:11:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:11:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:11:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:11:11Z- TNavFilter::4 good beams. 2024-11-06T03:11:11Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:11:11Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:11:11Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:11:11Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:11:11Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:11:11Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:11:11Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:11:11Z- TerrainMapOctree: (vehN, vehE ) = (4061730.65, 594868.76). 2024-11-06T03:11:11Z- TerrainMapOctree: Min Distance = 120.33. 2024-11-06T03:11:11Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:11:12Z- Forcing Subcloud Comparison 2024-11-06T03:11:12Z- Weighting particles with subcloud comparison 2024-11-06T03:11:12Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:12Z- meanExpectedMeasDiff: -0.302228 delta_rms_squared: 1.526329 alpha: 0.987066 2024-11-06T03:11:12Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:12Z- meanExpectedMeasDiff: 1.366895 delta_rms_squared: 1.069298 alpha: 0.981640 2024-11-06T03:11:12Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:12Z- meanExpectedMeasDiff: 5.929215 delta_rms_squared: 1.275273 alpha: 0.984559 2024-11-06T03:11:12Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:12Z- meanExpectedMeasDiff: -3.748258 delta_rms_squared: 1.260218 alpha: 0.984378 2024-11-06T03:11:12Z- Subcloud NIS: 1.015846 2024-11-06T03:11:12Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:11:12Z- TNavPF:: sumWeights = 0.002534 2024-11-06T03:11:12Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:11:12Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:11:12Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:11:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862671.04 sec. 2024-11-06T03:11:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:11:12Z- variances([0], [2], and psi_berg) = 0.153630 0.126595 0.000000 2024-11-06T03:11:12Z- publishEstimatesDecoder() - at 1730862671.84 sec 2024-11-06T03:11:12Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:11:12Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061761.39 594851.65 58.54 2024-11-06T03:11:12Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061761.42 594852.11 58.39 lat/lon: 36.70 -121.94 2024-11-06T03:11:12Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.13 0.67 0.00 2024-11-06T03:11:12Z- AHRS_M2 msg: 1730862672.21 epoch sec; seqNo:3054 2024-11-06T03:11:12Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 1.51 psi 2024-11-06T03:11:12Z- handleDepth()- Depth_Keller msg: 1730862672.23 epoch sec; seqNo:3249; depth 50.04 2024-11-06T03:11:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862672.24 epoch sec; seqNo:3249; 4061730.73 north; 594869.49 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:11:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862671.84 2024-11-06T03:11:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:12Z- waiting for 1730862675.84, time is 1730862672.24 2024-11-06T03:11:12Z- AHRS_M2 msg: 1730862672.62 epoch sec; seqNo:3055 2024-11-06T03:11:12Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 1.51 psi 2024-11-06T03:11:12Z- handleDepth()- Depth_Keller msg: 1730862672.63 epoch sec; seqNo:3250; depth 50.03 2024-11-06T03:11:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862672.65 epoch sec; seqNo:3250; 4061730.73 north; 594869.49 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:11:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862671.84 2024-11-06T03:11:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:12Z- waiting for 1730862675.84, time is 1730862672.65 2024-11-06T03:11:13Z- AHRS_M2 msg: 1730862673.02 epoch sec; seqNo:3056 2024-11-06T03:11:13Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 1.51 psi 2024-11-06T03:11:13Z- handleDepth()- Depth_Keller msg: 1730862673.03 epoch sec; seqNo:3251; depth 50.02 2024-11-06T03:11:13Z- handleDVL() - DVL msg epochMillisec: 1730862673.05 2024-11-06T03:11:13Z- handleDvl() - RDI_Pathfinder msg: 1730862673.05 sec; seqNo:1079; #beams:4 2024-11-06T03:11:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.98 , 37.94 , 44.85 , 34.76 2024-11-06T03:11:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.00 2024-11-06T03:11:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862673.08 epoch sec; seqNo:3251; 4061730.75 north; 594870.71 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:11:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862671.84 2024-11-06T03:11:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:13Z- waiting for 1730862675.84, time is 1730862673.08 2024-11-06T03:11:13Z- AHRS_M2 msg: 1730862673.43 epoch sec; seqNo:3057 2024-11-06T03:11:13Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 1.51 psi 2024-11-06T03:11:13Z- handleDepth()- Depth_Keller msg: 1730862673.43 epoch sec; seqNo:3252; depth 50.02 2024-11-06T03:11:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862673.45 epoch sec; seqNo:3252; 4061730.75 north; 594870.71 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:11:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862671.84 2024-11-06T03:11:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:13Z- waiting for 1730862675.84, time is 1730862673.45 2024-11-06T03:11:13Z- AHRS_M2 msg: 1730862673.83 epoch sec; seqNo:3058 2024-11-06T03:11:13Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.51 psi 2024-11-06T03:11:13Z- handleDepth()- Depth_Keller msg: 1730862673.84 epoch sec; seqNo:3253; depth 50.02 2024-11-06T03:11:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862673.85 epoch sec; seqNo:3253; 4061730.75 north; 594870.71 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:11:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862671.84 2024-11-06T03:11:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:13Z- waiting for 1730862675.84, time is 1730862673.85 2024-11-06T03:11:14Z- AHRS_M2 msg: 1730862674.23 epoch sec; seqNo:3059 2024-11-06T03:11:14Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 1.51 psi 2024-11-06T03:11:14Z- handleDepth()- Depth_Keller msg: 1730862674.24 epoch sec; seqNo:3254; depth 50.01 2024-11-06T03:11:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862674.25 epoch sec; seqNo:3254; 4061730.75 north; 594870.71 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:11:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862671.84 2024-11-06T03:11:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:14Z- waiting for 1730862675.84, time is 1730862674.25 2024-11-06T03:11:14Z- AHRS_M2 msg: 1730862674.65 epoch sec; seqNo:3060 2024-11-06T03:11:14Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 1.51 psi 2024-11-06T03:11:14Z- handleDepth()- Depth_Keller msg: 1730862674.65 epoch sec; seqNo:3255; depth 49.98 2024-11-06T03:11:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862674.66 epoch sec; seqNo:3255; 4061730.76 north; 594871.93 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:11:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862671.84 2024-11-06T03:11:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:14Z- waiting for 1730862675.84, time is 1730862674.66 2024-11-06T03:11:15Z- AHRS_M2 msg: 1730862675.04 epoch sec; seqNo:3061 2024-11-06T03:11:15Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.51 psi 2024-11-06T03:11:15Z- handleDepth()- Depth_Keller msg: 1730862675.06 epoch sec; seqNo:3256; depth 49.99 2024-11-06T03:11:15Z- handleDVL() - DVL msg epochMillisec: 1730862675.08 2024-11-06T03:11:15Z- handleDvl() - RDI_Pathfinder msg: 1730862675.08 sec; seqNo:1080; #beams:4 2024-11-06T03:11:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.35 , 38.31 , 44.85 , 35.32 2024-11-06T03:11:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , -0.01 2024-11-06T03:11:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862675.09 epoch sec; seqNo:3256; 4061730.76 north; 594871.93 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:11:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862671.84 2024-11-06T03:11:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:15Z- waiting for 1730862675.84, time is 1730862675.09 2024-11-06T03:11:15Z- AHRS_M2 msg: 1730862675.46 epoch sec; seqNo:3062 2024-11-06T03:11:15Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.51 psi 2024-11-06T03:11:15Z- handleDepth()- Depth_Keller msg: 1730862675.47 epoch sec; seqNo:3257; depth 49.98 2024-11-06T03:11:15Z- handleDVL() - DVL msg epochMillisec: 1730862675.47 2024-11-06T03:11:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:11:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:11:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:11:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:11:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:11:15Z- handleDvl() - RDI_Pathfinder msg: 1730862675.47 sec; seqNo:1081; #beams:0 2024-11-06T03:11:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:11:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , -0.01 2024-11-06T03:11:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862675.49 epoch sec; seqNo:3257; 4061730.76 north; 594871.93 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:11:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862671.84 2024-11-06T03:11:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:15Z- waiting for 1730862675.84, time is 1730862675.49 2024-11-06T03:11:15Z- AHRS_M2 msg: 1730862675.85 epoch sec; seqNo:3063 2024-11-06T03:11:15Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.51 psi 2024-11-06T03:11:15Z- handleDepth()- Depth_Keller msg: 1730862675.86 epoch sec; seqNo:3258; depth 49.96 2024-11-06T03:11:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862675.87 epoch sec; seqNo:3258; 4061731.15 north; 594873.15 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:11:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862671.84 2024-11-06T03:11:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:16Z- waiting for 1730862675.84, time is 1730862675.87 2024-11-06T03:11:16Z- LcmTrn::updateTrn() >>>> pose time 1730862675.87, meas time 1730862675.47 <<<< 2024-11-06T03:11:16Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862675.87 2024-11-06T03:11:16Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:11:16Z- TerrainNav::motionUpdate() - currEstimate = (4061731, 594873.15, 49.96) 2024-11-06T03:11:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:11:16Z- TerrainNav:: SubcloudNIS is 1.02, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:11:16Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:11:16Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:11:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:11:16Z- variances([0], [2], and psi_berg) = 0.156020 0.129016 0.000000 2024-11-06T03:11:16Z- publishEstimatesDecoder() - at 1730862675.87 sec 2024-11-06T03:11:16Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:11:16Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061761.80 594855.27 58.23 2024-11-06T03:11:16Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061761.84 594855.76 58.29 lat/lon: 36.70 -121.94 2024-11-06T03:11:16Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.13 0.93 0.00 2024-11-06T03:11:16Z- AHRS_M2 msg: 1730862676.25 epoch sec; seqNo:3064 2024-11-06T03:11:16Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.51 psi 2024-11-06T03:11:16Z- handleDepth()- Depth_Keller msg: 1730862676.26 epoch sec; seqNo:3259; depth 49.96 2024-11-06T03:11:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862676.27 epoch sec; seqNo:3259; 4061731.15 north; 594873.15 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:11:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862675.87 2024-11-06T03:11:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:16Z- waiting for 1730862679.87, time is 1730862676.27 2024-11-06T03:11:16Z- handleDepth()- Depth_Keller msg: 1730862676.69 epoch sec; seqNo:3260; depth 49.94 2024-11-06T03:11:16Z- handleDVL() - DVL msg epochMillisec: 1730862676.69 2024-11-06T03:11:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:11:16Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:11:16Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:11:16Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:11:16Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:11:16Z- handleDvl() - RDI_Pathfinder msg: 1730862676.69 sec; seqNo:1082; #beams:0 2024-11-06T03:11:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:11:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , -0.01 2024-11-06T03:11:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862676.72 epoch sec; seqNo:3260; 4061731.15 north; 594873.15 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:11:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862675.87 2024-11-06T03:11:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:16Z- waiting for 1730862679.87, time is 1730862676.72 2024-11-06T03:11:17Z- handleDepth()- Depth_Keller msg: 1730862677.08 epoch sec; seqNo:3261; depth 49.93 2024-11-06T03:11:17Z- handleDVL() - DVL msg epochMillisec: 1730862677.10 2024-11-06T03:11:17Z- handleDvl() - RDI_Pathfinder msg: 1730862677.10 sec; seqNo:1083; #beams:4 2024-11-06T03:11:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.10 , 39.24 , 44.85 , 36.63 2024-11-06T03:11:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , -0.01 2024-11-06T03:11:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862677.12 epoch sec; seqNo:3261; 4061731.15 north; 594873.15 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:11:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862675.87 2024-11-06T03:11:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:17Z- waiting for 1730862679.87, time is 1730862677.12 2024-11-06T03:11:17Z- AHRS_M2 msg: 1730862677.47 epoch sec; seqNo:3065 2024-11-06T03:11:17Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:17Z- handleDepth()- Depth_Keller msg: 1730862677.48 epoch sec; seqNo:3262; depth 49.92 2024-11-06T03:11:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862677.49 epoch sec; seqNo:3262; 4061731.17 north; 594874.37 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:11:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862675.87 2024-11-06T03:11:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:17Z- waiting for 1730862679.87, time is 1730862677.49 2024-11-06T03:11:17Z- AHRS_M2 msg: 1730862677.90 epoch sec; seqNo:3066 2024-11-06T03:11:17Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:17Z- handleDepth()- Depth_Keller msg: 1730862677.90 epoch sec; seqNo:3263; depth 49.91 2024-11-06T03:11:17Z- handleDVL() - DVL msg epochMillisec: 1730862677.90 2024-11-06T03:11:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:11:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:11:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:11:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:11:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:11:17Z- handleDvl() - RDI_Pathfinder msg: 1730862677.90 sec; seqNo:1084; #beams:0 2024-11-06T03:11:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:11:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , -0.01 2024-11-06T03:11:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862677.92 epoch sec; seqNo:3263; 4061731.17 north; 594874.37 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:11:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862675.87 2024-11-06T03:11:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:18Z- waiting for 1730862679.87, time is 1730862677.92 2024-11-06T03:11:18Z- AHRS_M2 msg: 1730862678.28 epoch sec; seqNo:3067 2024-11-06T03:11:18Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:18Z- handleDepth()- Depth_Keller msg: 1730862678.28 epoch sec; seqNo:3264; depth 49.90 2024-11-06T03:11:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862678.29 epoch sec; seqNo:3264; 4061731.17 north; 594874.37 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:11:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862675.87 2024-11-06T03:11:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:18Z- waiting for 1730862679.87, time is 1730862678.29 2024-11-06T03:11:18Z- AHRS_M2 msg: 1730862678.68 epoch sec; seqNo:3068 2024-11-06T03:11:18Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:18Z- handleDepth()- Depth_Keller msg: 1730862678.69 epoch sec; seqNo:3265; depth 49.89 2024-11-06T03:11:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862678.74 epoch sec; seqNo:3265; 4061731.18 north; 594875.59 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:11:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862675.87 2024-11-06T03:11:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:18Z- waiting for 1730862679.87, time is 1730862678.74 2024-11-06T03:11:19Z- AHRS_M2 msg: 1730862679.08 epoch sec; seqNo:3069 2024-11-06T03:11:19Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:19Z- handleDepth()- Depth_Keller msg: 1730862679.10 epoch sec; seqNo:3266; depth 49.88 2024-11-06T03:11:19Z- handleDVL() - DVL msg epochMillisec: 1730862679.12 2024-11-06T03:11:19Z- handleDvl() - RDI_Pathfinder msg: 1730862679.12 sec; seqNo:1085; #beams:4 2024-11-06T03:11:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.73 , 40.18 , 45.22 , 37.56 2024-11-06T03:11:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.00 , 0.00 2024-11-06T03:11:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862679.13 epoch sec; seqNo:3266; 4061731.18 north; 594875.59 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:11:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862675.87 2024-11-06T03:11:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:19Z- waiting for 1730862679.87, time is 1730862679.13 2024-11-06T03:11:19Z- AHRS_M2 msg: 1730862679.49 epoch sec; seqNo:3070 2024-11-06T03:11:19Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:19Z- handleDepth()- Depth_Keller msg: 1730862679.50 epoch sec; seqNo:3267; depth 49.88 2024-11-06T03:11:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862679.52 epoch sec; seqNo:3267; 4061731.18 north; 594875.59 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:11:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862675.87 2024-11-06T03:11:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:19Z- waiting for 1730862679.87, time is 1730862679.52 2024-11-06T03:11:19Z- AHRS_M2 msg: 1730862679.89 epoch sec; seqNo:3071 2024-11-06T03:11:19Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:19Z- handleDepth()- Depth_Keller msg: 1730862679.90 epoch sec; seqNo:3268; depth 49.87 2024-11-06T03:11:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862679.91 epoch sec; seqNo:3268; 4061731.18 north; 594875.59 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:11:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862675.87 2024-11-06T03:11:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:20Z- waiting for 1730862679.87, time is 1730862679.91 2024-11-06T03:11:20Z- LcmTrn::updateTrn() >>>> pose time 1730862679.91, meas time 1730862679.12 <<<< 2024-11-06T03:11:20Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862679.91 2024-11-06T03:11:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862679.12 sec, ping # 0; waiting for INS data more recent than 1730862675.87... 2024-11-06T03:11:20Z- TerrainNav::motionUpdate() - currEstimate = (4061731, 594875.59, 49.87) 2024-11-06T03:11:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:11:20Z- TerrainNav:: SubcloudNIS is 1.02, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:11:20Z- TNavPF::Projecting Measurements 2024-11-06T03:11:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:11:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:11:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:11:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:11:20Z- TNavFilter::4 good beams. 2024-11-06T03:11:20Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:11:20Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:11:20Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:11:20Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:11:20Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:11:20Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:11:20Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:11:20Z- TerrainMapOctree: (vehN, vehE ) = (4061731.18, 594875.11). 2024-11-06T03:11:20Z- TerrainMapOctree: Min Distance = 126.52. 2024-11-06T03:11:20Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:11:20Z- Forcing Subcloud Comparison 2024-11-06T03:11:20Z- Weighting particles with subcloud comparison 2024-11-06T03:11:20Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:20Z- meanExpectedMeasDiff: 1.072004 delta_rms_squared: 1.885592 alpha: 0.989505 2024-11-06T03:11:20Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:20Z- meanExpectedMeasDiff: 3.100553 delta_rms_squared: 1.340293 alpha: 0.985297 2024-11-06T03:11:20Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:20Z- meanExpectedMeasDiff: 6.027591 delta_rms_squared: 1.590505 alpha: 0.987582 2024-11-06T03:11:20Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:20Z- meanExpectedMeasDiff: -1.339152 delta_rms_squared: 1.566483 alpha: 0.987393 2024-11-06T03:11:20Z- Subcloud NIS: 1.029150 2024-11-06T03:11:20Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:11:20Z- TNavPF:: sumWeights = 0.011818 2024-11-06T03:11:20Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:11:20Z- TNavPF::First Term of Map Covariance Matrix is 0.01 2024-11-06T03:11:20Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:11:20Z- TerrainNav::Resampling particle filter... 2024-11-06T03:11:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862679.12 sec. 2024-11-06T03:11:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:11:20Z- variances([0], [2], and psi_berg) = 0.167113 0.131708 0.000000 2024-11-06T03:11:20Z- publishEstimatesDecoder() - at 1730862679.91 sec 2024-11-06T03:11:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:11:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061763.55 594859.03 58.48 2024-11-06T03:11:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061761.87 594858.21 57.21 lat/lon: 36.70 -121.94 2024-11-06T03:11:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.13 0.72 0.00 2024-11-06T03:11:20Z- AHRS_M2 msg: 1730862680.30 epoch sec; seqNo:3072 2024-11-06T03:11:20Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:20Z- handleDepth()- Depth_Keller msg: 1730862680.30 epoch sec; seqNo:3269; depth 49.88 2024-11-06T03:11:20Z- handleDVL() - DVL msg epochMillisec: 1730862680.31 2024-11-06T03:11:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:11:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:11:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:11:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:11:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:11:20Z- handleDvl() - RDI_Pathfinder msg: 1730862680.31 sec; seqNo:1086; #beams:0 2024-11-06T03:11:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:11:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.00 , 0.00 2024-11-06T03:11:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862680.32 epoch sec; seqNo:3269; 4061731.19 north; 594876.81 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:11:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862679.91 2024-11-06T03:11:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:20Z- waiting for 1730862683.91, time is 1730862680.32 2024-11-06T03:11:20Z- AHRS_M2 msg: 1730862680.76 epoch sec; seqNo:3073 2024-11-06T03:11:20Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:20Z- handleDepth()- Depth_Keller msg: 1730862680.77 epoch sec; seqNo:3270; depth 49.86 2024-11-06T03:11:20Z- handleDVL() - DVL msg epochMillisec: 1730862680.79 2024-11-06T03:11:20Z- handleDvl() - RDI_Pathfinder msg: 1730862680.79 sec; seqNo:1087; #beams:4 2024-11-06T03:11:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.54 , 40.74 , 45.04 , 37.75 2024-11-06T03:11:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.00 , 0.01 2024-11-06T03:11:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862680.80 epoch sec; seqNo:3270; 4061731.19 north; 594876.81 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:11:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862679.91 2024-11-06T03:11:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:20Z- waiting for 1730862683.91, time is 1730862680.80 2024-11-06T03:11:21Z- AHRS_M2 msg: 1730862681.11 epoch sec; seqNo:3074 2024-11-06T03:11:21Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.50 psi 2024-11-06T03:11:21Z- handleDepth()- Depth_Keller msg: 1730862681.11 epoch sec; seqNo:3271; depth 49.85 2024-11-06T03:11:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862681.13 epoch sec; seqNo:3271; 4061731.19 north; 594876.81 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:11:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862679.91 2024-11-06T03:11:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:21Z- waiting for 1730862683.91, time is 1730862681.13 2024-11-06T03:11:21Z- AHRS_M2 msg: 1730862681.51 epoch sec; seqNo:3075 2024-11-06T03:11:21Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.50 psi 2024-11-06T03:11:21Z- handleDepth()- Depth_Keller msg: 1730862681.51 epoch sec; seqNo:3272; depth 49.85 2024-11-06T03:11:21Z- handleDVL() - DVL msg epochMillisec: 1730862681.52 2024-11-06T03:11:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:11:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:11:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:11:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:11:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:11:21Z- handleDvl() - RDI_Pathfinder msg: 1730862681.52 sec; seqNo:1088; #beams:0 2024-11-06T03:11:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:11:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.00 , 0.01 2024-11-06T03:11:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862681.52 epoch sec; seqNo:3272; 4061731.57 north; 594876.81 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:11:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862679.91 2024-11-06T03:11:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:21Z- waiting for 1730862683.91, time is 1730862681.52 2024-11-06T03:11:21Z- AHRS_M2 msg: 1730862681.91 epoch sec; seqNo:3076 2024-11-06T03:11:21Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.50 psi 2024-11-06T03:11:21Z- handleDepth()- Depth_Keller msg: 1730862681.92 epoch sec; seqNo:3273; depth 49.85 2024-11-06T03:11:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862681.94 epoch sec; seqNo:3273; 4061731.59 north; 594878.03 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:11:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862679.91 2024-11-06T03:11:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:22Z- waiting for 1730862683.91, time is 1730862681.94 2024-11-06T03:11:22Z- AHRS_M2 msg: 1730862682.31 epoch sec; seqNo:3077 2024-11-06T03:11:22Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.50 psi 2024-11-06T03:11:22Z- handleDepth()- Depth_Keller msg: 1730862682.33 epoch sec; seqNo:3274; depth 49.83 2024-11-06T03:11:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862682.35 epoch sec; seqNo:3274; 4061731.59 north; 594878.03 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:11:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862679.91 2024-11-06T03:11:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:22Z- waiting for 1730862683.91, time is 1730862682.35 2024-11-06T03:11:22Z- AHRS_M2 msg: 1730862682.77 epoch sec; seqNo:3078 2024-11-06T03:11:22Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.50 psi 2024-11-06T03:11:22Z- handleDepth()- Depth_Keller msg: 1730862682.77 epoch sec; seqNo:3275; depth 49.85 2024-11-06T03:11:22Z- handleDVL() - DVL msg epochMillisec: 1730862682.79 2024-11-06T03:11:22Z- handleDvl() - RDI_Pathfinder msg: 1730862682.79 sec; seqNo:1089; #beams:4 2024-11-06T03:11:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.24 , 41.18 , 45.05 , 38.74 2024-11-06T03:11:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.02 2024-11-06T03:11:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862682.80 epoch sec; seqNo:3275; 4061731.59 north; 594878.03 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:11:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862679.91 2024-11-06T03:11:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:22Z- waiting for 1730862683.91, time is 1730862682.80 2024-11-06T03:11:23Z- AHRS_M2 msg: 1730862683.13 epoch sec; seqNo:3079 2024-11-06T03:11:23Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.50 psi 2024-11-06T03:11:23Z- handleDepth()- Depth_Keller msg: 1730862683.13 epoch sec; seqNo:3276; depth 49.85 2024-11-06T03:11:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862683.15 epoch sec; seqNo:3276; 4061731.60 north; 594879.25 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:11:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862679.91 2024-11-06T03:11:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:23Z- waiting for 1730862683.91, time is 1730862683.15 2024-11-06T03:11:23Z- AHRS_M2 msg: 1730862683.53 epoch sec; seqNo:3080 2024-11-06T03:11:23Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 1.50 psi 2024-11-06T03:11:23Z- handleDepth()- Depth_Keller msg: 1730862683.53 epoch sec; seqNo:3277; depth 49.82 2024-11-06T03:11:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862683.55 epoch sec; seqNo:3277; 4061731.60 north; 594879.25 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:11:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862679.91 2024-11-06T03:11:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:23Z- waiting for 1730862683.91, time is 1730862683.55 2024-11-06T03:11:23Z- AHRS_M2 msg: 1730862683.93 epoch sec; seqNo:3081 2024-11-06T03:11:23Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 1.50 psi 2024-11-06T03:11:23Z- handleDepth()- Depth_Keller msg: 1730862683.94 epoch sec; seqNo:3278; depth 49.84 2024-11-06T03:11:23Z- handleDVL() - DVL msg epochMillisec: 1730862683.94 2024-11-06T03:11:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:11:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:11:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:11:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:11:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:11:23Z- handleDvl() - RDI_Pathfinder msg: 1730862683.94 sec; seqNo:1090; #beams:0 2024-11-06T03:11:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:11:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.02 2024-11-06T03:11:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862683.96 epoch sec; seqNo:3278; 4061731.60 north; 594879.25 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:11:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862679.91 2024-11-06T03:11:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:24Z- waiting for 1730862683.91, time is 1730862683.96 2024-11-06T03:11:24Z- LcmTrn::updateTrn() >>>> pose time 1730862683.96, meas time 1730862683.94 <<<< 2024-11-06T03:11:24Z- LcmTrn::updateTrn() equating pose 1730862683.94 and meas times 1730862683.94 2024-11-06T03:11:24Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862683.94 2024-11-06T03:11:24Z- TerrainNav::motionUpdate() - currEstimate = (4061732, 594879.25, 49.84) 2024-11-06T03:11:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:11:24Z- TerrainNav:: SubcloudNIS is 1.03, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:11:24Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:11:24Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:11:24Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:11:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:11:24Z- variances([0], [2], and psi_berg) = 0.169087 0.134280 0.000000 2024-11-06T03:11:24Z- publishEstimatesDecoder() - at 1730862683.94 sec 2024-11-06T03:11:24Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:11:24Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061763.98 594862.66 57.74 2024-11-06T03:11:24Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061762.29 594861.86 57.18 lat/lon: 36.70 -121.94 2024-11-06T03:11:24Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.13 0.98 0.00 2024-11-06T03:11:24Z- AHRS_M2 msg: 1730862684.34 epoch sec; seqNo:3082 2024-11-06T03:11:24Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 1.50 psi 2024-11-06T03:11:24Z- handleDepth()- Depth_Keller msg: 1730862684.35 epoch sec; seqNo:3279; depth 49.84 2024-11-06T03:11:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862684.36 epoch sec; seqNo:3279; 4061731.60 north; 594879.25 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:11:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862683.94 2024-11-06T03:11:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:24Z- waiting for 1730862687.94, time is 1730862684.36 2024-11-06T03:11:24Z- AHRS_M2 msg: 1730862684.77 epoch sec; seqNo:3083 2024-11-06T03:11:24Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 1.50 psi 2024-11-06T03:11:24Z- handleDepth()- Depth_Keller msg: 1730862684.77 epoch sec; seqNo:3280; depth 49.86 2024-11-06T03:11:24Z- handleDVL() - DVL msg epochMillisec: 1730862684.80 2024-11-06T03:11:24Z- handleDvl() - RDI_Pathfinder msg: 1730862684.80 sec; seqNo:1091; #beams:4 2024-11-06T03:11:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.85 , 41.18 , 44.85 , 39.14 2024-11-06T03:11:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , 0.03 2024-11-06T03:11:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862684.81 epoch sec; seqNo:3280; 4061731.61 north; 594880.47 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:11:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862683.94 2024-11-06T03:11:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:24Z- waiting for 1730862687.94, time is 1730862684.81 2024-11-06T03:11:25Z- AHRS_M2 msg: 1730862685.14 epoch sec; seqNo:3084 2024-11-06T03:11:25Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 1.50 psi 2024-11-06T03:11:25Z- handleDepth()- Depth_Keller msg: 1730862685.16 epoch sec; seqNo:3281; depth 49.84 2024-11-06T03:11:25Z- handleDVL() - DVL msg epochMillisec: 1730862685.17 2024-11-06T03:11:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:11:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:11:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:11:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:11:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:11:25Z- handleDvl() - RDI_Pathfinder msg: 1730862685.17 sec; seqNo:1092; #beams:0 2024-11-06T03:11:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:11:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , 0.03 2024-11-06T03:11:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862685.18 epoch sec; seqNo:3281; 4061731.61 north; 594880.47 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:11:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862683.94 2024-11-06T03:11:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:25Z- waiting for 1730862687.94, time is 1730862685.18 2024-11-06T03:11:25Z- AHRS_M2 msg: 1730862685.55 epoch sec; seqNo:3085 2024-11-06T03:11:25Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T03:11:25Z- handleDepth()- Depth_Keller msg: 1730862685.55 epoch sec; seqNo:3282; depth 49.84 2024-11-06T03:11:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862685.56 epoch sec; seqNo:3282; 4061731.61 north; 594880.47 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:11:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862683.94 2024-11-06T03:11:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:25Z- waiting for 1730862687.94, time is 1730862685.56 2024-11-06T03:11:25Z- AHRS_M2 msg: 1730862685.95 epoch sec; seqNo:3086 2024-11-06T03:11:25Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T03:11:25Z- handleDepth()- Depth_Keller msg: 1730862685.97 epoch sec; seqNo:3283; depth 49.83 2024-11-06T03:11:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862685.97 epoch sec; seqNo:3283; 4061731.99 north; 594880.46 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:11:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862683.94 2024-11-06T03:11:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:26Z- waiting for 1730862687.94, time is 1730862685.97 2024-11-06T03:11:26Z- AHRS_M2 msg: 1730862686.36 epoch sec; seqNo:3087 2024-11-06T03:11:26Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T03:11:26Z- handleDepth()- Depth_Keller msg: 1730862686.36 epoch sec; seqNo:3284; depth 49.85 2024-11-06T03:11:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862686.37 epoch sec; seqNo:3284; 4061732.01 north; 594881.68 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:11:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862683.94 2024-11-06T03:11:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:26Z- waiting for 1730862687.94, time is 1730862686.37 2024-11-06T03:11:26Z- AHRS_M2 msg: 1730862686.77 epoch sec; seqNo:3088 2024-11-06T03:11:26Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T03:11:26Z- handleDepth()- Depth_Keller msg: 1730862686.78 epoch sec; seqNo:3285; depth 49.83 2024-11-06T03:11:26Z- handleDVL() - DVL msg epochMillisec: 1730862686.80 2024-11-06T03:11:26Z- handleDvl() - RDI_Pathfinder msg: 1730862686.80 sec; seqNo:1093; #beams:4 2024-11-06T03:11:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.46 , 41.18 , 44.85 , 39.75 2024-11-06T03:11:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , 0.03 2024-11-06T03:11:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862686.81 epoch sec; seqNo:3285; 4061732.01 north; 594881.68 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:11:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862683.94 2024-11-06T03:11:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:26Z- waiting for 1730862687.94, time is 1730862686.81 2024-11-06T03:11:27Z- AHRS_M2 msg: 1730862687.18 epoch sec; seqNo:3089 2024-11-06T03:11:27Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T03:11:27Z- handleDepth()- Depth_Keller msg: 1730862687.19 epoch sec; seqNo:3286; depth 49.84 2024-11-06T03:11:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862687.22 epoch sec; seqNo:3286; 4061732.01 north; 594881.68 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:11:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862683.94 2024-11-06T03:11:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:27Z- waiting for 1730862687.94, time is 1730862687.22 2024-11-06T03:11:27Z- AHRS_M2 msg: 1730862687.57 epoch sec; seqNo:3090 2024-11-06T03:11:27Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T03:11:27Z- handleDepth()- Depth_Keller msg: 1730862687.58 epoch sec; seqNo:3287; depth 49.83 2024-11-06T03:11:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862687.58 epoch sec; seqNo:3287; 4061732.02 north; 594882.90 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:11:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862683.94 2024-11-06T03:11:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:27Z- waiting for 1730862687.94, time is 1730862687.58 2024-11-06T03:11:27Z- AHRS_M2 msg: 1730862687.97 epoch sec; seqNo:3091 2024-11-06T03:11:27Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T03:11:27Z- handleDepth()- Depth_Keller msg: 1730862687.98 epoch sec; seqNo:3288; depth 49.83 2024-11-06T03:11:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862688.00 epoch sec; seqNo:3288; 4061732.02 north; 594882.90 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:11:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862683.94 2024-11-06T03:11:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:28Z- waiting for 1730862687.94, time is 1730862688.00 2024-11-06T03:11:28Z- LcmTrn::updateTrn() >>>> pose time 1730862688.00, meas time 1730862686.80 <<<< 2024-11-06T03:11:28Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862688.00 2024-11-06T03:11:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862686.80 sec, ping # 0; waiting for INS data more recent than 1730862683.94... 2024-11-06T03:11:28Z- TerrainNav::motionUpdate() - currEstimate = (4061732, 594882.90, 49.83) 2024-11-06T03:11:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:11:28Z- TerrainNav:: SubcloudNIS is 1.03, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:11:28Z- TNavPF::Projecting Measurements 2024-11-06T03:11:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:11:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:11:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:11:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:11:28Z- TNavFilter::4 good beams. 2024-11-06T03:11:28Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:11:28Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:11:28Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:11:28Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:11:28Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:11:28Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:11:28Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:11:28Z- TerrainMapOctree: (vehN, vehE ) = (4061731.90, 594881.82). 2024-11-06T03:11:28Z- TerrainMapOctree: Min Distance = 133.05. 2024-11-06T03:11:28Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:11:28Z- Forcing Subcloud Comparison 2024-11-06T03:11:28Z- Weighting particles with subcloud comparison 2024-11-06T03:11:28Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:28Z- meanExpectedMeasDiff: 1.202595 delta_rms_squared: 2.018315 alpha: 0.990188 2024-11-06T03:11:28Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:28Z- meanExpectedMeasDiff: 3.092146 delta_rms_squared: 1.408747 alpha: 0.986002 2024-11-06T03:11:28Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:28Z- meanExpectedMeasDiff: 4.639762 delta_rms_squared: 1.664246 alpha: 0.988125 2024-11-06T03:11:28Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:28Z- meanExpectedMeasDiff: -0.665246 delta_rms_squared: 1.681614 alpha: 0.988246 2024-11-06T03:11:28Z- Subcloud NIS: 1.015574 2024-11-06T03:11:28Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:11:28Z- TNavPF:: sumWeights = 0.055652 2024-11-06T03:11:28Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:11:28Z- TNavPF::First Term of Map Covariance Matrix is 0.04 2024-11-06T03:11:28Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T03:11:28Z- TerrainNav::Resampling particle filter... 2024-11-06T03:11:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862686.80 sec. 2024-11-06T03:11:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:11:28Z- variances([0], [2], and psi_berg) = 0.178538 0.135845 0.000000 2024-11-06T03:11:28Z- publishEstimatesDecoder() - at 1730862688.00 sec 2024-11-06T03:11:28Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:11:28Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061764.34 594866.11 56.97 2024-11-06T03:11:28Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061762.71 594865.52 56.20 lat/lon: 36.70 -121.94 2024-11-06T03:11:28Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.14 0.73 0.00 2024-11-06T03:11:28Z- AHRS_M2 msg: 1730862688.37 epoch sec; seqNo:3092 2024-11-06T03:11:28Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T03:11:28Z- handleDepth()- Depth_Keller msg: 1730862688.39 epoch sec; seqNo:3289; depth 49.83 2024-11-06T03:11:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862688.39 epoch sec; seqNo:3289; 4061732.02 north; 594882.90 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:11:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862688.00 2024-11-06T03:11:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:28Z- waiting for 1730862692.00, time is 1730862688.39 2024-11-06T03:11:28Z- AHRS_M2 msg: 1730862688.78 epoch sec; seqNo:3093 2024-11-06T03:11:28Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T03:11:28Z- handleDepth()- Depth_Keller msg: 1730862688.79 epoch sec; seqNo:3290; depth 49.83 2024-11-06T03:11:28Z- handleDVL() - DVL msg epochMillisec: 1730862688.81 2024-11-06T03:11:28Z- handleDvl() - RDI_Pathfinder msg: 1730862688.81 sec; seqNo:1094; #beams:4 2024-11-06T03:11:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 41.59 , 45.05 , 40.16 2024-11-06T03:11:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , 0.00 2024-11-06T03:11:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862688.82 epoch sec; seqNo:3290; 4061732.03 north; 594884.12 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:11:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862688.00 2024-11-06T03:11:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:28Z- waiting for 1730862692.00, time is 1730862688.82 2024-11-06T03:11:29Z- AHRS_M2 msg: 1730862689.18 epoch sec; seqNo:3094 2024-11-06T03:11:29Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T03:11:29Z- handleDepth()- Depth_Keller msg: 1730862689.19 epoch sec; seqNo:3291; depth 49.82 2024-11-06T03:11:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862689.21 epoch sec; seqNo:3291; 4061732.03 north; 594884.12 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:11:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862688.00 2024-11-06T03:11:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:29Z- waiting for 1730862692.00, time is 1730862689.21 2024-11-06T03:11:29Z- AHRS_M2 msg: 1730862689.59 epoch sec; seqNo:3095 2024-11-06T03:11:29Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T03:11:29Z- handleDepth()- Depth_Keller msg: 1730862689.59 epoch sec; seqNo:3292; depth 49.82 2024-11-06T03:11:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862689.60 epoch sec; seqNo:3292; 4061732.03 north; 594884.12 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:11:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862688.00 2024-11-06T03:11:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:29Z- waiting for 1730862692.00, time is 1730862689.60 2024-11-06T03:11:29Z- AHRS_M2 msg: 1730862689.99 epoch sec; seqNo:3096 2024-11-06T03:11:29Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T03:11:30Z- handleDepth()- Depth_Keller msg: 1730862690.00 epoch sec; seqNo:3293; depth 49.81 2024-11-06T03:11:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862690.01 epoch sec; seqNo:3293; 4061732.03 north; 594884.12 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:11:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862688.00 2024-11-06T03:11:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:30Z- waiting for 1730862692.00, time is 1730862690.01 2024-11-06T03:11:30Z- AHRS_M2 msg: 1730862690.40 epoch sec; seqNo:3097 2024-11-06T03:11:30Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T03:11:30Z- handleDepth()- Depth_Keller msg: 1730862690.41 epoch sec; seqNo:3294; depth 49.80 2024-11-06T03:11:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862690.42 epoch sec; seqNo:3294; 4061732.05 north; 594885.34 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:11:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862688.00 2024-11-06T03:11:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:30Z- waiting for 1730862692.00, time is 1730862690.42 2024-11-06T03:11:30Z- AHRS_M2 msg: 1730862690.80 epoch sec; seqNo:3098 2024-11-06T03:11:30Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T03:11:30Z- handleDepth()- Depth_Keller msg: 1730862690.80 epoch sec; seqNo:3295; depth 49.80 2024-11-06T03:11:30Z- handleDVL() - DVL msg epochMillisec: 1730862690.83 2024-11-06T03:11:30Z- handleDvl() - RDI_Pathfinder msg: 1730862690.83 sec; seqNo:1095; #beams:4 2024-11-06T03:11:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 41.79 , 45.26 , 40.16 2024-11-06T03:11:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.01 , -0.02 2024-11-06T03:11:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862690.85 epoch sec; seqNo:3295; 4061732.05 north; 594885.34 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:11:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862688.00 2024-11-06T03:11:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:30Z- waiting for 1730862692.00, time is 1730862690.85 2024-11-06T03:11:31Z- AHRS_M2 msg: 1730862691.20 epoch sec; seqNo:3099 2024-11-06T03:11:31Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T03:11:31Z- handleDepth()- Depth_Keller msg: 1730862691.21 epoch sec; seqNo:3296; depth 49.80 2024-11-06T03:11:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862691.26 epoch sec; seqNo:3296; 4061732.05 north; 594885.34 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:11:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862688.00 2024-11-06T03:11:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:31Z- waiting for 1730862692.00, time is 1730862691.26 2024-11-06T03:11:31Z- AHRS_M2 msg: 1730862691.61 epoch sec; seqNo:3100 2024-11-06T03:11:31Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.51 psi 2024-11-06T03:11:31Z- handleDepth()- Depth_Keller msg: 1730862691.62 epoch sec; seqNo:3297; depth 49.77 2024-11-06T03:11:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862691.63 epoch sec; seqNo:3297; 4061732.06 north; 594886.56 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:11:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862688.00 2024-11-06T03:11:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:31Z- waiting for 1730862692.00, time is 1730862691.63 2024-11-06T03:11:32Z- AHRS_M2 msg: 1730862692.01 epoch sec; seqNo:3101 2024-11-06T03:11:32Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.51 psi 2024-11-06T03:11:32Z- handleDepth()- Depth_Keller msg: 1730862692.02 epoch sec; seqNo:3298; depth 49.79 2024-11-06T03:11:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862692.04 epoch sec; seqNo:3298; 4061732.06 north; 594886.56 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:11:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862688.00 2024-11-06T03:11:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:32Z- waiting for 1730862692.00, time is 1730862692.04 2024-11-06T03:11:32Z- LcmTrn::updateTrn() >>>> pose time 1730862692.04, meas time 1730862690.83 <<<< 2024-11-06T03:11:32Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862692.04 2024-11-06T03:11:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862690.83 sec, ping # 0; waiting for INS data more recent than 1730862688.00... 2024-11-06T03:11:32Z- TerrainNav::motionUpdate() - currEstimate = (4061732, 594886.56, 49.79) 2024-11-06T03:11:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:11:32Z- TerrainNav:: SubcloudNIS is 1.02, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:11:32Z- TNavPF::Projecting Measurements 2024-11-06T03:11:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:11:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:11:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:11:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:11:32Z- TNavFilter::4 good beams. 2024-11-06T03:11:32Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:11:32Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:11:32Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:11:32Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:11:32Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:11:32Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:11:32Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:11:32Z- TerrainMapOctree: (vehN, vehE ) = (4061732.05, 594885.47). 2024-11-06T03:11:32Z- TerrainMapOctree: Min Distance = 136.65. 2024-11-06T03:11:32Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:11:32Z- Forcing Subcloud Comparison 2024-11-06T03:11:32Z- Weighting particles with subcloud comparison 2024-11-06T03:11:32Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:32Z- meanExpectedMeasDiff: 0.597886 delta_rms_squared: 1.574794 alpha: 0.987459 2024-11-06T03:11:32Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:32Z- meanExpectedMeasDiff: 2.413469 delta_rms_squared: 1.134598 alpha: 0.982678 2024-11-06T03:11:32Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:32Z- meanExpectedMeasDiff: 4.128743 delta_rms_squared: 1.309884 alpha: 0.984961 2024-11-06T03:11:32Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:32Z- meanExpectedMeasDiff: -1.532359 delta_rms_squared: 1.346403 alpha: 0.985363 2024-11-06T03:11:32Z- Subcloud NIS: 0.978808 2024-11-06T03:11:32Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:11:32Z- TNavPF:: sumWeights = 0.071902 2024-11-06T03:11:32Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:11:32Z- TNavPF::First Term of Map Covariance Matrix is 0.05 2024-11-06T03:11:32Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.01 2024-11-06T03:11:32Z- TerrainNav::Resampling particle filter... 2024-11-06T03:11:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862690.83 sec. 2024-11-06T03:11:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:11:32Z- variances([0], [2], and psi_berg) = 0.182027 0.135680 0.000000 2024-11-06T03:11:32Z- publishEstimatesDecoder() - at 1730862692.04 sec 2024-11-06T03:11:32Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:11:32Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061764.81 594870.04 55.46 2024-11-06T03:11:32Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061762.76 594869.18 55.59 lat/lon: 36.70 -121.94 2024-11-06T03:11:32Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.14 0.70 0.00 2024-11-06T03:11:32Z- AHRS_M2 msg: 1730862692.43 epoch sec; seqNo:3102 2024-11-06T03:11:32Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.50 psi 2024-11-06T03:11:32Z- handleDepth()- Depth_Keller msg: 1730862692.44 epoch sec; seqNo:3299; depth 49.75 2024-11-06T03:11:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862692.46 epoch sec; seqNo:3299; 4061732.06 north; 594886.56 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:11:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862692.04 2024-11-06T03:11:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:32Z- waiting for 1730862696.04, time is 1730862692.46 2024-11-06T03:11:32Z- AHRS_M2 msg: 1730862692.82 epoch sec; seqNo:3103 2024-11-06T03:11:32Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.50 psi 2024-11-06T03:11:32Z- handleDepth()- Depth_Keller msg: 1730862692.82 epoch sec; seqNo:3300; depth 49.77 2024-11-06T03:11:32Z- handleDVL() - DVL msg epochMillisec: 1730862692.84 2024-11-06T03:11:32Z- handleDvl() - RDI_Pathfinder msg: 1730862692.84 sec; seqNo:1096; #beams:4 2024-11-06T03:11:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.87 , 42.20 , 44.65 , 40.37 2024-11-06T03:11:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , -0.03 2024-11-06T03:11:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862692.90 epoch sec; seqNo:3300; 4061732.44 north; 594886.56 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:11:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862692.04 2024-11-06T03:11:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:32Z- waiting for 1730862696.04, time is 1730862692.90 2024-11-06T03:11:33Z- AHRS_M2 msg: 1730862693.22 epoch sec; seqNo:3104 2024-11-06T03:11:33Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.50 psi 2024-11-06T03:11:33Z- handleDepth()- Depth_Keller msg: 1730862693.23 epoch sec; seqNo:3301; depth 49.75 2024-11-06T03:11:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862693.24 epoch sec; seqNo:3301; 4061732.45 north; 594887.78 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:11:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862692.04 2024-11-06T03:11:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:33Z- waiting for 1730862696.04, time is 1730862693.24 2024-11-06T03:11:33Z- AHRS_M2 msg: 1730862693.63 epoch sec; seqNo:3105 2024-11-06T03:11:33Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.50 psi 2024-11-06T03:11:33Z- handleDepth()- Depth_Keller msg: 1730862693.65 epoch sec; seqNo:3302; depth 49.76 2024-11-06T03:11:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862693.66 epoch sec; seqNo:3302; 4061732.45 north; 594887.78 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:11:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862692.04 2024-11-06T03:11:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:33Z- waiting for 1730862696.04, time is 1730862693.66 2024-11-06T03:11:34Z- AHRS_M2 msg: 1730862694.03 epoch sec; seqNo:3106 2024-11-06T03:11:34Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.50 psi 2024-11-06T03:11:34Z- handleDepth()- Depth_Keller msg: 1730862694.04 epoch sec; seqNo:3303; depth 49.75 2024-11-06T03:11:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862694.05 epoch sec; seqNo:3303; 4061732.45 north; 594887.78 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:11:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862692.04 2024-11-06T03:11:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:34Z- waiting for 1730862696.04, time is 1730862694.05 2024-11-06T03:11:34Z- AHRS_M2 msg: 1730862694.43 epoch sec; seqNo:3107 2024-11-06T03:11:34Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.50 psi 2024-11-06T03:11:34Z- handleDepth()- Depth_Keller msg: 1730862694.44 epoch sec; seqNo:3304; depth 49.75 2024-11-06T03:11:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862694.45 epoch sec; seqNo:3304; 4061732.47 north; 594889.00 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:11:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862692.04 2024-11-06T03:11:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:34Z- waiting for 1730862696.04, time is 1730862694.45 2024-11-06T03:11:34Z- AHRS_M2 msg: 1730862694.84 epoch sec; seqNo:3108 2024-11-06T03:11:34Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.50 psi 2024-11-06T03:11:34Z- handleDepth()- Depth_Keller msg: 1730862694.85 epoch sec; seqNo:3305; depth 49.73 2024-11-06T03:11:34Z- handleDVL() - DVL msg epochMillisec: 1730862694.87 2024-11-06T03:11:34Z- handleDvl() - RDI_Pathfinder msg: 1730862694.87 sec; seqNo:1097; #beams:4 2024-11-06T03:11:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.26 , 42.20 , 44.44 , 40.57 2024-11-06T03:11:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.01 , -0.02 2024-11-06T03:11:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862694.89 epoch sec; seqNo:3305; 4061732.47 north; 594889.00 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:11:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862692.04 2024-11-06T03:11:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:34Z- waiting for 1730862696.04, time is 1730862694.89 2024-11-06T03:11:35Z- AHRS_M2 msg: 1730862695.24 epoch sec; seqNo:3109 2024-11-06T03:11:35Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.50 psi 2024-11-06T03:11:35Z- handleDepth()- Depth_Keller msg: 1730862695.25 epoch sec; seqNo:3306; depth 49.72 2024-11-06T03:11:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862695.28 epoch sec; seqNo:3306; 4061732.47 north; 594889.00 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:11:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862692.04 2024-11-06T03:11:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:35Z- waiting for 1730862696.04, time is 1730862695.28 2024-11-06T03:11:35Z- AHRS_M2 msg: 1730862695.65 epoch sec; seqNo:3110 2024-11-06T03:11:35Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.50 psi 2024-11-06T03:11:35Z- handleDepth()- Depth_Keller msg: 1730862695.66 epoch sec; seqNo:3307; depth 49.74 2024-11-06T03:11:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862695.66 epoch sec; seqNo:3307; 4061732.47 north; 594889.00 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:11:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862692.04 2024-11-06T03:11:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:35Z- waiting for 1730862696.04, time is 1730862695.66 2024-11-06T03:11:36Z- AHRS_M2 msg: 1730862696.06 epoch sec; seqNo:3111 2024-11-06T03:11:36Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.50 psi 2024-11-06T03:11:36Z- handleDepth()- Depth_Keller msg: 1730862696.06 epoch sec; seqNo:3308; depth 49.74 2024-11-06T03:11:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862696.08 epoch sec; seqNo:3308; 4061732.48 north; 594890.22 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:11:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862692.04 2024-11-06T03:11:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:36Z- waiting for 1730862696.04, time is 1730862696.08 2024-11-06T03:11:36Z- LcmTrn::updateTrn() >>>> pose time 1730862696.08, meas time 1730862694.87 <<<< 2024-11-06T03:11:36Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862696.08 2024-11-06T03:11:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862694.87 sec, ping # 0; waiting for INS data more recent than 1730862692.04... 2024-11-06T03:11:36Z- TerrainNav::motionUpdate() - currEstimate = (4061732, 594890.22, 49.74) 2024-11-06T03:11:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:11:36Z- TerrainNav:: SubcloudNIS is 0.98, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:11:36Z- TNavPF::Projecting Measurements 2024-11-06T03:11:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:11:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:11:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:11:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:11:36Z- TNavFilter::4 good beams. 2024-11-06T03:11:36Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:11:36Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:11:36Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:11:36Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:11:36Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:11:36Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:11:36Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:11:36Z- TerrainMapOctree: (vehN, vehE ) = (4061732.35, 594889.13). 2024-11-06T03:11:36Z- TerrainMapOctree: Min Distance = 140.25. 2024-11-06T03:11:36Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:11:36Z- Forcing Subcloud Comparison 2024-11-06T03:11:36Z- Weighting particles with subcloud comparison 2024-11-06T03:11:36Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:36Z- meanExpectedMeasDiff: -0.018924 delta_rms_squared: 1.523991 alpha: 0.987047 2024-11-06T03:11:36Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:36Z- meanExpectedMeasDiff: 1.890199 delta_rms_squared: 1.133390 alpha: 0.982660 2024-11-06T03:11:36Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:36Z- meanExpectedMeasDiff: 2.846277 delta_rms_squared: 1.270728 alpha: 0.984505 2024-11-06T03:11:36Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:36Z- meanExpectedMeasDiff: -1.904001 delta_rms_squared: 1.322305 alpha: 0.985100 2024-11-06T03:11:36Z- Subcloud NIS: 0.960648 2024-11-06T03:11:36Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:11:36Z- TNavPF:: sumWeights = 0.149184 2024-11-06T03:11:36Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:11:36Z- TNavPF::First Term of Map Covariance Matrix is 0.12 2024-11-06T03:11:36Z- TNavPF::Calculated NIS Value : 0.07 numBeams normalized NIS: 0.02 2024-11-06T03:11:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862694.87 sec. 2024-11-06T03:11:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:11:36Z- variances([0], [2], and psi_berg) = 0.186476 0.137547 0.000000 2024-11-06T03:11:36Z- publishEstimatesDecoder() - at 1730862696.08 sec 2024-11-06T03:11:36Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:11:36Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061762.17 594873.77 55.58 2024-11-06T03:11:36Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061763.18 594872.83 55.27 lat/lon: 36.70 -121.94 2024-11-06T03:11:36Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.19 0.14 0.73 0.00 2024-11-06T03:11:36Z- AHRS_M2 msg: 1730862696.46 epoch sec; seqNo:3112 2024-11-06T03:11:36Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.50 psi 2024-11-06T03:11:36Z- handleDepth()- Depth_Keller msg: 1730862696.47 epoch sec; seqNo:3309; depth 49.73 2024-11-06T03:11:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862696.48 epoch sec; seqNo:3309; 4061732.48 north; 594890.22 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:11:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862696.08 2024-11-06T03:11:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:36Z- waiting for 1730862700.08, time is 1730862696.48 2024-11-06T03:11:36Z- AHRS_M2 msg: 1730862696.88 epoch sec; seqNo:3113 2024-11-06T03:11:36Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.50 psi 2024-11-06T03:11:36Z- handleDepth()- Depth_Keller msg: 1730862696.88 epoch sec; seqNo:3310; depth 49.74 2024-11-06T03:11:36Z- handleDVL() - DVL msg epochMillisec: 1730862696.91 2024-11-06T03:11:36Z- handleDvl() - RDI_Pathfinder msg: 1730862696.91 sec; seqNo:1098; #beams:4 2024-11-06T03:11:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 42.40 , 44.24 , 40.77 2024-11-06T03:11:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.01 , -0.02 2024-11-06T03:11:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862696.92 epoch sec; seqNo:3310; 4061732.48 north; 594890.22 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:11:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862696.08 2024-11-06T03:11:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:37Z- waiting for 1730862700.08, time is 1730862696.92 2024-11-06T03:11:37Z- AHRS_M2 msg: 1730862697.27 epoch sec; seqNo:3114 2024-11-06T03:11:37Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.49 psi 2024-11-06T03:11:37Z- handleDepth()- Depth_Keller msg: 1730862697.27 epoch sec; seqNo:3311; depth 49.74 2024-11-06T03:11:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862697.29 epoch sec; seqNo:3311; 4061732.48 north; 594890.22 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:11:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862696.08 2024-11-06T03:11:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:37Z- waiting for 1730862700.08, time is 1730862697.29 2024-11-06T03:11:37Z- AHRS_M2 msg: 1730862697.67 epoch sec; seqNo:3115 2024-11-06T03:11:37Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.49 psi 2024-11-06T03:11:37Z- handleDepth()- Depth_Keller msg: 1730862697.68 epoch sec; seqNo:3312; depth 49.75 2024-11-06T03:11:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862697.68 epoch sec; seqNo:3312; 4061732.49 north; 594891.44 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:11:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862696.08 2024-11-06T03:11:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:37Z- waiting for 1730862700.08, time is 1730862697.68 2024-11-06T03:11:38Z- AHRS_M2 msg: 1730862698.08 epoch sec; seqNo:3116 2024-11-06T03:11:38Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.49 psi 2024-11-06T03:11:38Z- handleDepth()- Depth_Keller msg: 1730862698.08 epoch sec; seqNo:3313; depth 49.76 2024-11-06T03:11:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862698.09 epoch sec; seqNo:3313; 4061732.49 north; 594891.44 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:11:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862696.08 2024-11-06T03:11:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:38Z- waiting for 1730862700.08, time is 1730862698.09 2024-11-06T03:11:38Z- AHRS_M2 msg: 1730862698.48 epoch sec; seqNo:3117 2024-11-06T03:11:38Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.49 psi 2024-11-06T03:11:38Z- handleDepth()- Depth_Keller msg: 1730862698.48 epoch sec; seqNo:3314; depth 49.76 2024-11-06T03:11:38Z- handleDVL() - DVL msg epochMillisec: 1730862698.48 2024-11-06T03:11:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:11:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:11:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:11:38Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:11:38Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:11:38Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:11:38Z- handleDvl() - RDI_Pathfinder msg: 1730862698.48 sec; seqNo:1099; #beams:0 2024-11-06T03:11:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:11:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.01 , -0.02 2024-11-06T03:11:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862698.51 epoch sec; seqNo:3314; 4061732.87 north; 594891.44 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:11:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862696.08 2024-11-06T03:11:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:38Z- waiting for 1730862700.08, time is 1730862698.51 2024-11-06T03:11:38Z- AHRS_M2 msg: 1730862698.88 epoch sec; seqNo:3118 2024-11-06T03:11:38Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.49 psi 2024-11-06T03:11:38Z- handleDepth()- Depth_Keller msg: 1730862698.89 epoch sec; seqNo:3315; depth 49.77 2024-11-06T03:11:38Z- handleDVL() - DVL msg epochMillisec: 1730862698.91 2024-11-06T03:11:38Z- handleDvl() - RDI_Pathfinder msg: 1730862698.91 sec; seqNo:1100; #beams:4 2024-11-06T03:11:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.46 , 42.40 , 44.24 , 40.57 2024-11-06T03:11:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.02 , -0.01 2024-11-06T03:11:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862698.93 epoch sec; seqNo:3315; 4061732.87 north; 594891.44 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:11:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862696.08 2024-11-06T03:11:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:39Z- waiting for 1730862700.08, time is 1730862698.93 2024-11-06T03:11:39Z- AHRS_M2 msg: 1730862699.29 epoch sec; seqNo:3119 2024-11-06T03:11:39Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.49 psi 2024-11-06T03:11:39Z- handleDepth()- Depth_Keller msg: 1730862699.30 epoch sec; seqNo:3316; depth 49.80 2024-11-06T03:11:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862699.31 epoch sec; seqNo:3316; 4061732.89 north; 594892.66 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:11:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862696.08 2024-11-06T03:11:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:39Z- waiting for 1730862700.08, time is 1730862699.31 2024-11-06T03:11:39Z- handleDepth()- Depth_Keller msg: 1730862699.71 epoch sec; seqNo:3317; depth 49.80 2024-11-06T03:11:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862699.72 epoch sec; seqNo:3317; 4061732.89 north; 594892.66 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:11:39Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:11:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862696.08 2024-11-06T03:11:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:39Z- waiting for 1730862700.08, time is 1730862699.72 2024-11-06T03:11:40Z- AHRS_M2 msg: 1730862700.09 epoch sec; seqNo:3120 2024-11-06T03:11:40Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.49 psi 2024-11-06T03:11:40Z- handleDepth()- Depth_Keller msg: 1730862700.10 epoch sec; seqNo:3318; depth 49.82 2024-11-06T03:11:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862700.12 epoch sec; seqNo:3318; 4061732.89 north; 594892.66 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:11:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862696.08 2024-11-06T03:11:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:40Z- waiting for 1730862700.08, time is 1730862700.12 2024-11-06T03:11:40Z- LcmTrn::updateTrn() >>>> pose time 1730862700.12, meas time 1730862698.91 <<<< 2024-11-06T03:11:40Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862700.12 2024-11-06T03:11:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862698.91 sec, ping # 0; waiting for INS data more recent than 1730862696.08... 2024-11-06T03:11:40Z- TerrainNav::motionUpdate() - currEstimate = (4061733, 594892.66, 49.82) 2024-11-06T03:11:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:11:40Z- TerrainNav:: SubcloudNIS is 0.96, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:11:40Z- TNavPF::Projecting Measurements 2024-11-06T03:11:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:11:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:11:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:11:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:11:40Z- TNavFilter::4 good beams. 2024-11-06T03:11:40Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:11:40Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:11:40Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:11:40Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:11:40Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:11:40Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:11:40Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:11:40Z- TerrainMapOctree: (vehN, vehE ) = (4061732.76, 594891.93). 2024-11-06T03:11:40Z- TerrainMapOctree: Min Distance = 142.98. 2024-11-06T03:11:40Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:11:40Z- Forcing Subcloud Comparison 2024-11-06T03:11:40Z- Weighting particles with subcloud comparison 2024-11-06T03:11:40Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:40Z- meanExpectedMeasDiff: -0.153596 delta_rms_squared: 2.345627 alpha: 0.991546 2024-11-06T03:11:40Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:40Z- meanExpectedMeasDiff: 1.729748 delta_rms_squared: 1.279031 alpha: 0.984604 2024-11-06T03:11:40Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:40Z- meanExpectedMeasDiff: 2.083948 delta_rms_squared: 1.562911 alpha: 0.987365 2024-11-06T03:11:40Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:40Z- meanExpectedMeasDiff: -2.210521 delta_rms_squared: 1.337088 alpha: 0.985263 2024-11-06T03:11:40Z- Subcloud NIS: 0.943078 2024-11-06T03:11:40Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:11:40Z- TNavPF:: sumWeights = 0.183247 2024-11-06T03:11:40Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:11:40Z- TNavPF::First Term of Map Covariance Matrix is 0.18 2024-11-06T03:11:40Z- TNavPF::Calculated NIS Value : 0.09 numBeams normalized NIS: 0.02 2024-11-06T03:11:40Z- TerrainNav::Resampling particle filter... 2024-11-06T03:11:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862698.91 sec. 2024-11-06T03:11:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:11:40Z- variances([0], [2], and psi_berg) = 0.169374 0.135617 0.000000 2024-11-06T03:11:40Z- publishEstimatesDecoder() - at 1730862700.12 sec 2024-11-06T03:11:40Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:11:40Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061765.63 594876.12 55.13 2024-11-06T03:11:40Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061763.52 594875.30 55.24 lat/lon: 36.70 -121.94 2024-11-06T03:11:40Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.14 0.64 0.00 2024-11-06T03:11:40Z- AHRS_M2 msg: 1730862700.50 epoch sec; seqNo:3121 2024-11-06T03:11:40Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.49 psi 2024-11-06T03:11:40Z- handleDepth()- Depth_Keller msg: 1730862700.51 epoch sec; seqNo:3319; depth 49.84 2024-11-06T03:11:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862700.52 epoch sec; seqNo:3319; 4061732.89 north; 594892.66 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:11:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862700.12 2024-11-06T03:11:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:40Z- waiting for 1730862704.12, time is 1730862700.52 2024-11-06T03:11:40Z- AHRS_M2 msg: 1730862700.90 epoch sec; seqNo:3122 2024-11-06T03:11:40Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.48 psi 2024-11-06T03:11:40Z- handleDepth()- Depth_Keller msg: 1730862700.91 epoch sec; seqNo:3320; depth 49.86 2024-11-06T03:11:40Z- handleDVL() - DVL msg epochMillisec: 1730862700.93 2024-11-06T03:11:40Z- handleDvl() - RDI_Pathfinder msg: 1730862700.93 sec; seqNo:1101; #beams:4 2024-11-06T03:11:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.87 , 42.40 , 44.24 , 40.77 2024-11-06T03:11:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.04 , 0.03 2024-11-06T03:11:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862700.96 epoch sec; seqNo:3320; 4061732.90 north; 594893.88 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:11:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862700.12 2024-11-06T03:11:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:41Z- waiting for 1730862704.12, time is 1730862700.96 2024-11-06T03:11:41Z- AHRS_M2 msg: 1730862701.31 epoch sec; seqNo:3123 2024-11-06T03:11:41Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.48 psi 2024-11-06T03:11:41Z- handleDepth()- Depth_Keller msg: 1730862701.32 epoch sec; seqNo:3321; depth 49.86 2024-11-06T03:11:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862701.33 epoch sec; seqNo:3321; 4061732.90 north; 594893.88 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:11:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862700.12 2024-11-06T03:11:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:41Z- waiting for 1730862704.12, time is 1730862701.33 2024-11-06T03:11:41Z- AHRS_M2 msg: 1730862701.71 epoch sec; seqNo:3124 2024-11-06T03:11:41Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.48 psi 2024-11-06T03:11:41Z- handleDepth()- Depth_Keller msg: 1730862701.72 epoch sec; seqNo:3322; depth 49.89 2024-11-06T03:11:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862701.73 epoch sec; seqNo:3322; 4061732.90 north; 594893.88 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:11:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862700.12 2024-11-06T03:11:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:41Z- waiting for 1730862704.12, time is 1730862701.73 2024-11-06T03:11:42Z- AHRS_M2 msg: 1730862702.11 epoch sec; seqNo:3125 2024-11-06T03:11:42Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.48 psi 2024-11-06T03:11:42Z- handleDepth()- Depth_Keller msg: 1730862702.13 epoch sec; seqNo:3323; depth 49.90 2024-11-06T03:11:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862702.13 epoch sec; seqNo:3323; 4061732.90 north; 594893.88 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:11:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862700.12 2024-11-06T03:11:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:42Z- waiting for 1730862704.12, time is 1730862702.13 2024-11-06T03:11:42Z- AHRS_M2 msg: 1730862702.53 epoch sec; seqNo:3126 2024-11-06T03:11:42Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.48 psi 2024-11-06T03:11:42Z- handleDepth()- Depth_Keller msg: 1730862702.53 epoch sec; seqNo:3324; depth 49.92 2024-11-06T03:11:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862702.54 epoch sec; seqNo:3324; 4061733.29 north; 594895.09 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:11:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862700.12 2024-11-06T03:11:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:42Z- waiting for 1730862704.12, time is 1730862702.54 2024-11-06T03:11:42Z- AHRS_M2 msg: 1730862702.92 epoch sec; seqNo:3127 2024-11-06T03:11:42Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.48 psi 2024-11-06T03:11:42Z- handleDepth()- Depth_Keller msg: 1730862702.93 epoch sec; seqNo:3325; depth 49.93 2024-11-06T03:11:42Z- handleDVL() - DVL msg epochMillisec: 1730862702.96 2024-11-06T03:11:42Z- handleDvl() - RDI_Pathfinder msg: 1730862702.96 sec; seqNo:1102; #beams:4 2024-11-06T03:11:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.87 , 42.61 , 44.03 , 41.18 2024-11-06T03:11:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , 0.08 2024-11-06T03:11:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862702.97 epoch sec; seqNo:3325; 4061733.29 north; 594895.09 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:11:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862700.12 2024-11-06T03:11:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:43Z- waiting for 1730862704.12, time is 1730862702.97 2024-11-06T03:11:43Z- AHRS_M2 msg: 1730862703.33 epoch sec; seqNo:3128 2024-11-06T03:11:43Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.48 psi 2024-11-06T03:11:43Z- handleDepth()- Depth_Keller msg: 1730862703.33 epoch sec; seqNo:3326; depth 49.94 2024-11-06T03:11:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862703.35 epoch sec; seqNo:3326; 4061733.29 north; 594895.09 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:11:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862700.12 2024-11-06T03:11:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:43Z- waiting for 1730862704.12, time is 1730862703.35 2024-11-06T03:11:43Z- AHRS_M2 msg: 1730862703.73 epoch sec; seqNo:3129 2024-11-06T03:11:43Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.48 psi 2024-11-06T03:11:43Z- handleDepth()- Depth_Keller msg: 1730862703.74 epoch sec; seqNo:3327; depth 49.95 2024-11-06T03:11:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862703.75 epoch sec; seqNo:3327; 4061733.31 north; 594896.31 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:11:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862700.12 2024-11-06T03:11:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:43Z- waiting for 1730862704.12, time is 1730862703.75 2024-11-06T03:11:44Z- AHRS_M2 msg: 1730862704.13 epoch sec; seqNo:3130 2024-11-06T03:11:44Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.48 psi 2024-11-06T03:11:44Z- handleDepth()- Depth_Keller msg: 1730862704.15 epoch sec; seqNo:3328; depth 49.96 2024-11-06T03:11:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862704.16 epoch sec; seqNo:3328; 4061733.31 north; 594896.31 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:11:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862700.12 2024-11-06T03:11:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:44Z- waiting for 1730862704.12, time is 1730862704.16 2024-11-06T03:11:44Z- LcmTrn::updateTrn() >>>> pose time 1730862704.16, meas time 1730862702.96 <<<< 2024-11-06T03:11:44Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862704.16 2024-11-06T03:11:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862702.96 sec, ping # 0; waiting for INS data more recent than 1730862700.12... 2024-11-06T03:11:44Z- TerrainNav::motionUpdate() - currEstimate = (4061733, 594896.31, 49.96) 2024-11-06T03:11:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:11:44Z- TerrainNav:: SubcloudNIS is 0.94, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:11:44Z- TNavPF::Projecting Measurements 2024-11-06T03:11:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:11:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:11:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:11:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:11:44Z- TNavFilter::4 good beams. 2024-11-06T03:11:44Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:11:44Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:11:44Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:11:44Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:11:44Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:11:44Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:11:44Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:11:44Z- TerrainMapOctree: (vehN, vehE ) = (4061733.18, 594895.23). 2024-11-06T03:11:44Z- TerrainMapOctree: Min Distance = 146.20. 2024-11-06T03:11:44Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:11:44Z- Forcing Subcloud Comparison 2024-11-06T03:11:44Z- Weighting particles with subcloud comparison 2024-11-06T03:11:44Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:44Z- meanExpectedMeasDiff: -0.363599 delta_rms_squared: 2.617610 alpha: 0.992417 2024-11-06T03:11:44Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:44Z- meanExpectedMeasDiff: 1.961611 delta_rms_squared: 1.049212 alpha: 0.981295 2024-11-06T03:11:44Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:44Z- meanExpectedMeasDiff: 2.254718 delta_rms_squared: 1.091246 alpha: 0.982002 2024-11-06T03:11:44Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:44Z- meanExpectedMeasDiff: -1.650730 delta_rms_squared: 1.226371 alpha: 0.983953 2024-11-06T03:11:44Z- Subcloud NIS: 0.951423 2024-11-06T03:11:44Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:11:44Z- TNavPF:: sumWeights = 0.195737 2024-11-06T03:11:44Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:11:44Z- TNavPF::First Term of Map Covariance Matrix is 0.33 2024-11-06T03:11:44Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.02 2024-11-06T03:11:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862702.96 sec. 2024-11-06T03:11:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:11:44Z- variances([0], [2], and psi_berg) = 0.172579 0.136177 0.000000 2024-11-06T03:11:44Z- publishEstimatesDecoder() - at 1730862704.16 sec 2024-11-06T03:11:44Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:11:44Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061763.21 594878.99 55.80 2024-11-06T03:11:44Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061763.93 594878.95 55.20 lat/lon: 36.70 -121.94 2024-11-06T03:11:44Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.14 0.68 0.00 2024-11-06T03:11:44Z- AHRS_M2 msg: 1730862704.54 epoch sec; seqNo:3131 2024-11-06T03:11:44Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.48 psi 2024-11-06T03:11:44Z- handleDepth()- Depth_Keller msg: 1730862704.55 epoch sec; seqNo:3329; depth 49.97 2024-11-06T03:11:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862704.57 epoch sec; seqNo:3329; 4061733.31 north; 594896.31 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:11:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862704.16 2024-11-06T03:11:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:44Z- waiting for 1730862708.16, time is 1730862704.57 2024-11-06T03:11:44Z- handleDepth()- Depth_Keller msg: 1730862704.97 epoch sec; seqNo:3330; depth 49.97 2024-11-06T03:11:44Z- handleDVL() - DVL msg epochMillisec: 1730862704.99 2024-11-06T03:11:44Z- handleDvl() - RDI_Pathfinder msg: 1730862704.99 sec; seqNo:1103; #beams:4 2024-11-06T03:11:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.87 , 42.61 , 43.83 , 41.59 2024-11-06T03:11:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , 0.05 2024-11-06T03:11:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862705.00 epoch sec; seqNo:3330; 4061733.31 north; 594896.31 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:11:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862704.16 2024-11-06T03:11:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:45Z- waiting for 1730862708.16, time is 1730862705.00 2024-11-06T03:11:45Z- AHRS_M2 msg: 1730862705.35 epoch sec; seqNo:3132 2024-11-06T03:11:45Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.48 psi 2024-11-06T03:11:45Z- handleDepth()- Depth_Keller msg: 1730862705.36 epoch sec; seqNo:3331; depth 49.98 2024-11-06T03:11:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862705.39 epoch sec; seqNo:3331; 4061733.32 north; 594897.54 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:11:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862704.16 2024-11-06T03:11:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:45Z- waiting for 1730862708.16, time is 1730862705.39 2024-11-06T03:11:45Z- AHRS_M2 msg: 1730862705.75 epoch sec; seqNo:3133 2024-11-06T03:11:45Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.48 psi 2024-11-06T03:11:45Z- handleDepth()- Depth_Keller msg: 1730862705.76 epoch sec; seqNo:3332; depth 49.99 2024-11-06T03:11:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862705.77 epoch sec; seqNo:3332; 4061733.32 north; 594897.54 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:11:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862704.16 2024-11-06T03:11:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:45Z- waiting for 1730862708.16, time is 1730862705.77 2024-11-06T03:11:46Z- AHRS_M2 msg: 1730862706.16 epoch sec; seqNo:3134 2024-11-06T03:11:46Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.48 psi 2024-11-06T03:11:46Z- handleDepth()- Depth_Keller msg: 1730862706.17 epoch sec; seqNo:3333; depth 49.99 2024-11-06T03:11:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862706.17 epoch sec; seqNo:3333; 4061733.32 north; 594897.54 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:11:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862704.16 2024-11-06T03:11:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:46Z- waiting for 1730862708.16, time is 1730862706.17 2024-11-06T03:11:46Z- AHRS_M2 msg: 1730862706.56 epoch sec; seqNo:3135 2024-11-06T03:11:46Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.48 psi 2024-11-06T03:11:46Z- handleDepth()- Depth_Keller msg: 1730862706.57 epoch sec; seqNo:3334; depth 49.99 2024-11-06T03:11:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862706.58 epoch sec; seqNo:3334; 4061733.71 north; 594898.75 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:11:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862704.16 2024-11-06T03:11:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:46Z- waiting for 1730862708.16, time is 1730862706.58 2024-11-06T03:11:46Z- handleDepth()- Depth_Keller msg: 1730862706.98 epoch sec; seqNo:3335; depth 49.99 2024-11-06T03:11:47Z- handleDVL() - DVL msg epochMillisec: 1730862707.00 2024-11-06T03:11:47Z- handleDvl() - RDI_Pathfinder msg: 1730862707.00 sec; seqNo:1104; #beams:4 2024-11-06T03:11:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.07 , 42.61 , 44.03 , 42.00 2024-11-06T03:11:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.03 , -0.03 2024-11-06T03:11:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862707.03 epoch sec; seqNo:3335; 4061733.71 north; 594898.75 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:11:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862704.16 2024-11-06T03:11:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:47Z- waiting for 1730862708.16, time is 1730862707.03 2024-11-06T03:11:47Z- AHRS_M2 msg: 1730862707.37 epoch sec; seqNo:3136 2024-11-06T03:11:47Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.48 psi 2024-11-06T03:11:47Z- handleDepth()- Depth_Keller msg: 1730862707.37 epoch sec; seqNo:3336; depth 49.98 2024-11-06T03:11:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862707.39 epoch sec; seqNo:3336; 4061733.71 north; 594898.75 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:11:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862704.16 2024-11-06T03:11:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:47Z- waiting for 1730862708.16, time is 1730862707.39 2024-11-06T03:11:47Z- AHRS_M2 msg: 1730862707.77 epoch sec; seqNo:3137 2024-11-06T03:11:47Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.48 psi 2024-11-06T03:11:47Z- handleDepth()- Depth_Keller msg: 1730862707.78 epoch sec; seqNo:3337; depth 49.98 2024-11-06T03:11:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862707.79 epoch sec; seqNo:3337; 4061733.72 north; 594899.97 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:11:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862704.16 2024-11-06T03:11:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:47Z- waiting for 1730862708.16, time is 1730862707.79 2024-11-06T03:11:48Z- AHRS_M2 msg: 1730862708.17 epoch sec; seqNo:3138 2024-11-06T03:11:48Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.48 psi 2024-11-06T03:11:48Z- handleDepth()- Depth_Keller msg: 1730862708.19 epoch sec; seqNo:3338; depth 49.98 2024-11-06T03:11:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862708.20 epoch sec; seqNo:3338; 4061733.72 north; 594899.97 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:11:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862704.16 2024-11-06T03:11:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:48Z- waiting for 1730862708.16, time is 1730862708.20 2024-11-06T03:11:48Z- LcmTrn::updateTrn() >>>> pose time 1730862708.20, meas time 1730862707.00 <<<< 2024-11-06T03:11:48Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862708.20 2024-11-06T03:11:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862707.00 sec, ping # 0; waiting for INS data more recent than 1730862704.16... 2024-11-06T03:11:48Z- TerrainNav::motionUpdate() - currEstimate = (4061734, 594899.97, 49.98) 2024-11-06T03:11:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:11:48Z- TerrainNav:: SubcloudNIS is 0.95, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:11:48Z- TNavPF::Projecting Measurements 2024-11-06T03:11:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:11:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:11:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:11:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:11:48Z- TNavFilter::4 good beams. 2024-11-06T03:11:48Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:11:48Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:11:48Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:11:48Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:11:48Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:11:48Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:11:48Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:11:48Z- TerrainMapOctree: (vehN, vehE ) = (4061733.60, 594898.89). 2024-11-06T03:11:48Z- TerrainMapOctree: Min Distance = 149.79. 2024-11-06T03:11:48Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:11:48Z- Forcing Subcloud Comparison 2024-11-06T03:11:48Z- Weighting particles with subcloud comparison 2024-11-06T03:11:48Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:48Z- meanExpectedMeasDiff: -0.673895 delta_rms_squared: 2.034562 alpha: 0.990266 2024-11-06T03:11:48Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:48Z- meanExpectedMeasDiff: 2.935613 delta_rms_squared: 1.108311 alpha: 0.982274 2024-11-06T03:11:48Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:48Z- meanExpectedMeasDiff: 2.225858 delta_rms_squared: 1.208879 alpha: 0.983725 2024-11-06T03:11:48Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:48Z- meanExpectedMeasDiff: -1.087017 delta_rms_squared: 1.292098 alpha: 0.984757 2024-11-06T03:11:48Z- Subcloud NIS: 0.924671 2024-11-06T03:11:48Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:11:48Z- TNavPF:: sumWeights = 0.157312 2024-11-06T03:11:48Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:11:48Z- TNavPF::First Term of Map Covariance Matrix is 0.28 2024-11-06T03:11:48Z- TNavPF::Calculated NIS Value : 0.07 numBeams normalized NIS: 0.02 2024-11-06T03:11:48Z- TerrainNav::Resampling particle filter... 2024-11-06T03:11:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862707.00 sec. 2024-11-06T03:11:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:11:48Z- variances([0], [2], and psi_berg) = 0.172583 0.140337 0.000000 2024-11-06T03:11:48Z- publishEstimatesDecoder() - at 1730862708.20 sec 2024-11-06T03:11:48Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:11:48Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061766.39 594883.37 55.07 2024-11-06T03:11:48Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061764.35 594882.61 54.88 lat/lon: 36.70 -121.94 2024-11-06T03:11:48Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.14 0.69 0.00 2024-11-06T03:11:48Z- AHRS_M2 msg: 1730862708.58 epoch sec; seqNo:3139 2024-11-06T03:11:48Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.48 psi 2024-11-06T03:11:48Z- handleDepth()- Depth_Keller msg: 1730862708.59 epoch sec; seqNo:3339; depth 49.97 2024-11-06T03:11:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862708.60 epoch sec; seqNo:3339; 4061733.72 north; 594899.97 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:11:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862708.20 2024-11-06T03:11:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:48Z- waiting for 1730862712.20, time is 1730862708.60 2024-11-06T03:11:48Z- AHRS_M2 msg: 1730862708.99 epoch sec; seqNo:3140 2024-11-06T03:11:49Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.48 psi 2024-11-06T03:11:49Z- handleDepth()- Depth_Keller msg: 1730862709.00 epoch sec; seqNo:3340; depth 49.96 2024-11-06T03:11:49Z- handleDVL() - DVL msg epochMillisec: 1730862709.02 2024-11-06T03:11:49Z- handleDvl() - RDI_Pathfinder msg: 1730862709.02 sec; seqNo:1105; #beams:4 2024-11-06T03:11:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 42.61 , 44.03 , 42.61 2024-11-06T03:11:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , -0.10 2024-11-06T03:11:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862709.03 epoch sec; seqNo:3340; 4061733.74 north; 594901.19 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:11:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862708.20 2024-11-06T03:11:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:49Z- waiting for 1730862712.20, time is 1730862709.03 2024-11-06T03:11:49Z- AHRS_M2 msg: 1730862709.39 epoch sec; seqNo:3141 2024-11-06T03:11:49Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.48 psi 2024-11-06T03:11:49Z- handleDepth()- Depth_Keller msg: 1730862709.39 epoch sec; seqNo:3341; depth 49.96 2024-11-06T03:11:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862709.40 epoch sec; seqNo:3341; 4061733.74 north; 594901.19 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:11:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862708.20 2024-11-06T03:11:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:49Z- waiting for 1730862712.20, time is 1730862709.40 2024-11-06T03:11:49Z- AHRS_M2 msg: 1730862709.80 epoch sec; seqNo:3142 2024-11-06T03:11:49Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.48 psi 2024-11-06T03:11:49Z- handleDepth()- Depth_Keller msg: 1730862709.80 epoch sec; seqNo:3342; depth 49.95 2024-11-06T03:11:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862709.81 epoch sec; seqNo:3342; 4061733.74 north; 594901.19 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:11:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862708.20 2024-11-06T03:11:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:49Z- waiting for 1730862712.20, time is 1730862709.81 2024-11-06T03:11:50Z- AHRS_M2 msg: 1730862710.20 epoch sec; seqNo:3143 2024-11-06T03:11:50Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.48 psi 2024-11-06T03:11:50Z- handleDepth()- Depth_Keller msg: 1730862710.20 epoch sec; seqNo:3343; depth 49.95 2024-11-06T03:11:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862710.21 epoch sec; seqNo:3343; 4061733.74 north; 594901.19 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:11:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862708.20 2024-11-06T03:11:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:50Z- waiting for 1730862712.20, time is 1730862710.21 2024-11-06T03:11:50Z- AHRS_M2 msg: 1730862710.60 epoch sec; seqNo:3144 2024-11-06T03:11:50Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.48 psi 2024-11-06T03:11:50Z- handleDepth()- Depth_Keller msg: 1730862710.62 epoch sec; seqNo:3344; depth 49.96 2024-11-06T03:11:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862710.63 epoch sec; seqNo:3344; 4061733.75 north; 594902.41 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:11:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862708.20 2024-11-06T03:11:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:50Z- waiting for 1730862712.20, time is 1730862710.63 2024-11-06T03:11:51Z- AHRS_M2 msg: 1730862711.00 epoch sec; seqNo:3145 2024-11-06T03:11:51Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.48 psi 2024-11-06T03:11:51Z- handleDepth()- Depth_Keller msg: 1730862711.01 epoch sec; seqNo:3345; depth 49.96 2024-11-06T03:11:51Z- handleDVL() - DVL msg epochMillisec: 1730862711.03 2024-11-06T03:11:51Z- handleDvl() - RDI_Pathfinder msg: 1730862711.03 sec; seqNo:1106; #beams:4 2024-11-06T03:11:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 42.40 , 43.63 , 42.61 2024-11-06T03:11:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.01 , -0.10 2024-11-06T03:11:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862711.08 epoch sec; seqNo:3345; 4061733.75 north; 594902.41 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:11:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862708.20 2024-11-06T03:11:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:51Z- waiting for 1730862712.20, time is 1730862711.08 2024-11-06T03:11:51Z- handleDepth()- Depth_Keller msg: 1730862711.43 epoch sec; seqNo:3346; depth 49.96 2024-11-06T03:11:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862711.44 epoch sec; seqNo:3346; 4061733.75 north; 594902.41 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:11:51Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:11:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862708.20 2024-11-06T03:11:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:51Z- waiting for 1730862712.20, time is 1730862711.44 2024-11-06T03:11:51Z- AHRS_M2 msg: 1730862711.81 epoch sec; seqNo:3146 2024-11-06T03:11:51Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.48 psi 2024-11-06T03:11:51Z- handleDepth()- Depth_Keller msg: 1730862711.82 epoch sec; seqNo:3347; depth 49.97 2024-11-06T03:11:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862711.84 epoch sec; seqNo:3347; 4061733.75 north; 594902.41 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:11:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862708.20 2024-11-06T03:11:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:51Z- waiting for 1730862712.20, time is 1730862711.84 2024-11-06T03:11:52Z- AHRS_M2 msg: 1730862712.21 epoch sec; seqNo:3147 2024-11-06T03:11:52Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.48 psi 2024-11-06T03:11:52Z- handleDepth()- Depth_Keller msg: 1730862712.23 epoch sec; seqNo:3348; depth 49.97 2024-11-06T03:11:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862712.23 epoch sec; seqNo:3348; 4061733.76 north; 594903.63 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:11:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862708.20 2024-11-06T03:11:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:52Z- waiting for 1730862712.20, time is 1730862712.23 2024-11-06T03:11:52Z- LcmTrn::updateTrn() >>>> pose time 1730862712.23, meas time 1730862711.03 <<<< 2024-11-06T03:11:52Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862712.23 2024-11-06T03:11:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862711.03 sec, ping # 0; waiting for INS data more recent than 1730862708.20... 2024-11-06T03:11:52Z- TerrainNav::motionUpdate() - currEstimate = (4061734, 594903.63, 49.97) 2024-11-06T03:11:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:11:52Z- TerrainNav:: SubcloudNIS is 0.92, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:11:52Z- TNavPF::Projecting Measurements 2024-11-06T03:11:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:11:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:11:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:11:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:11:52Z- TNavFilter::4 good beams. 2024-11-06T03:11:52Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:11:52Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:11:52Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:11:52Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:11:52Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:11:52Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:11:52Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:11:52Z- TerrainMapOctree: (vehN, vehE ) = (4061733.75, 594902.54). 2024-11-06T03:11:52Z- TerrainMapOctree: Min Distance = 153.40. 2024-11-06T03:11:52Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:11:52Z- Forcing Subcloud Comparison 2024-11-06T03:11:52Z- Weighting particles with subcloud comparison 2024-11-06T03:11:52Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:52Z- meanExpectedMeasDiff: -1.788020 delta_rms_squared: 1.461050 alpha: 0.986496 2024-11-06T03:11:52Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:52Z- meanExpectedMeasDiff: 2.458956 delta_rms_squared: 1.086390 alpha: 0.981923 2024-11-06T03:11:52Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:52Z- meanExpectedMeasDiff: 0.490145 delta_rms_squared: 1.601943 alpha: 0.987669 2024-11-06T03:11:52Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:52Z- meanExpectedMeasDiff: -0.963517 delta_rms_squared: 1.294501 alpha: 0.984785 2024-11-06T03:11:52Z- Subcloud NIS: 0.877225 2024-11-06T03:11:52Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:11:52Z- TNavPF:: sumWeights = 0.233717 2024-11-06T03:11:52Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:11:52Z- TNavPF::First Term of Map Covariance Matrix is 0.60 2024-11-06T03:11:52Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T03:11:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862711.03 sec. 2024-11-06T03:11:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:11:52Z- variances([0], [2], and psi_berg) = 0.162137 0.138591 0.000000 2024-11-06T03:11:52Z- publishEstimatesDecoder() - at 1730862712.23 sec 2024-11-06T03:11:52Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:11:52Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061764.11 594886.37 54.99 2024-11-06T03:11:52Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061764.37 594886.28 54.87 lat/lon: 36.70 -121.94 2024-11-06T03:11:52Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.14 0.69 0.00 2024-11-06T03:11:52Z- AHRS_M2 msg: 1730862712.63 epoch sec; seqNo:3148 2024-11-06T03:11:52Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.47 psi 2024-11-06T03:11:52Z- handleDepth()- Depth_Keller msg: 1730862712.63 epoch sec; seqNo:3349; depth 49.98 2024-11-06T03:11:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862712.65 epoch sec; seqNo:3349; 4061733.76 north; 594903.63 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:11:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862712.23 2024-11-06T03:11:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:52Z- waiting for 1730862716.23, time is 1730862712.65 2024-11-06T03:11:53Z- AHRS_M2 msg: 1730862713.02 epoch sec; seqNo:3149 2024-11-06T03:11:53Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.47 psi 2024-11-06T03:11:53Z- handleDepth()- Depth_Keller msg: 1730862713.03 epoch sec; seqNo:3350; depth 50.00 2024-11-06T03:11:53Z- handleDVL() - DVL msg epochMillisec: 1730862713.06 2024-11-06T03:11:53Z- handleDvl() - RDI_Pathfinder msg: 1730862713.06 sec; seqNo:1107; #beams:4 2024-11-06T03:11:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 42.00 , 43.22 , 42.81 2024-11-06T03:11:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.05 , -0.04 2024-11-06T03:11:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862713.08 epoch sec; seqNo:3350; 4061734.14 north; 594903.63 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:11:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862712.23 2024-11-06T03:11:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:53Z- waiting for 1730862716.23, time is 1730862713.08 2024-11-06T03:11:53Z- AHRS_M2 msg: 1730862713.43 epoch sec; seqNo:3150 2024-11-06T03:11:53Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.47 psi 2024-11-06T03:11:53Z- handleDepth()- Depth_Keller msg: 1730862713.44 epoch sec; seqNo:3351; depth 50.01 2024-11-06T03:11:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862713.46 epoch sec; seqNo:3351; 4061734.14 north; 594903.63 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:11:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862712.23 2024-11-06T03:11:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:53Z- waiting for 1730862716.23, time is 1730862713.46 2024-11-06T03:11:53Z- AHRS_M2 msg: 1730862713.84 epoch sec; seqNo:3151 2024-11-06T03:11:53Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.46 psi 2024-11-06T03:11:53Z- handleDepth()- Depth_Keller msg: 1730862713.84 epoch sec; seqNo:3352; depth 50.03 2024-11-06T03:11:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862713.85 epoch sec; seqNo:3352; 4061734.16 north; 594904.85 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:11:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862712.23 2024-11-06T03:11:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:53Z- waiting for 1730862716.23, time is 1730862713.85 2024-11-06T03:11:54Z- AHRS_M2 msg: 1730862714.24 epoch sec; seqNo:3152 2024-11-06T03:11:54Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.46 psi 2024-11-06T03:11:54Z- handleDepth()- Depth_Keller msg: 1730862714.25 epoch sec; seqNo:3353; depth 50.04 2024-11-06T03:11:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862714.26 epoch sec; seqNo:3353; 4061734.16 north; 594904.85 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:11:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862712.23 2024-11-06T03:11:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:54Z- waiting for 1730862716.23, time is 1730862714.26 2024-11-06T03:11:54Z- AHRS_M2 msg: 1730862714.65 epoch sec; seqNo:3153 2024-11-06T03:11:54Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.46 psi 2024-11-06T03:11:54Z- handleDepth()- Depth_Keller msg: 1730862714.66 epoch sec; seqNo:3354; depth 50.08 2024-11-06T03:11:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862714.67 epoch sec; seqNo:3354; 4061734.16 north; 594904.85 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:11:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862712.23 2024-11-06T03:11:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:54Z- waiting for 1730862716.23, time is 1730862714.67 2024-11-06T03:11:55Z- AHRS_M2 msg: 1730862715.04 epoch sec; seqNo:3154 2024-11-06T03:11:55Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.46 psi 2024-11-06T03:11:55Z- handleDepth()- Depth_Keller msg: 1730862715.05 epoch sec; seqNo:3355; depth 50.09 2024-11-06T03:11:55Z- handleDVL() - DVL msg epochMillisec: 1730862715.08 2024-11-06T03:11:55Z- handleDvl() - RDI_Pathfinder msg: 1730862715.08 sec; seqNo:1108; #beams:4 2024-11-06T03:11:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 42.21 , 43.43 , 42.82 2024-11-06T03:11:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.06 , 0.05 2024-11-06T03:11:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862715.09 epoch sec; seqNo:3355; 4061734.16 north; 594904.85 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:11:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862712.23 2024-11-06T03:11:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:55Z- waiting for 1730862716.23, time is 1730862715.09 2024-11-06T03:11:55Z- AHRS_M2 msg: 1730862715.45 epoch sec; seqNo:3155 2024-11-06T03:11:55Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.46 psi 2024-11-06T03:11:55Z- handleDepth()- Depth_Keller msg: 1730862715.45 epoch sec; seqNo:3356; depth 50.14 2024-11-06T03:11:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862715.47 epoch sec; seqNo:3356; 4061734.16 north; 594904.85 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:11:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862712.23 2024-11-06T03:11:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:55Z- waiting for 1730862716.23, time is 1730862715.47 2024-11-06T03:11:55Z- AHRS_M2 msg: 1730862715.85 epoch sec; seqNo:3156 2024-11-06T03:11:55Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.45 psi 2024-11-06T03:11:55Z- handleDepth()- Depth_Keller msg: 1730862715.86 epoch sec; seqNo:3357; depth 50.14 2024-11-06T03:11:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862715.87 epoch sec; seqNo:3357; 4061734.17 north; 594906.07 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:11:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862712.23 2024-11-06T03:11:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:56Z- waiting for 1730862716.23, time is 1730862715.87 2024-11-06T03:11:56Z- AHRS_M2 msg: 1730862716.26 epoch sec; seqNo:3157 2024-11-06T03:11:56Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.45 psi 2024-11-06T03:11:56Z- handleDepth()- Depth_Keller msg: 1730862716.26 epoch sec; seqNo:3358; depth 50.17 2024-11-06T03:11:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862716.27 epoch sec; seqNo:3358; 4061734.55 north; 594906.06 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:11:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862712.23 2024-11-06T03:11:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:56Z- waiting for 1730862716.23, time is 1730862716.27 2024-11-06T03:11:56Z- LcmTrn::updateTrn() >>>> pose time 1730862716.27, meas time 1730862715.08 <<<< 2024-11-06T03:11:56Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862716.27 2024-11-06T03:11:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862715.08 sec, ping # 0; waiting for INS data more recent than 1730862712.23... 2024-11-06T03:11:56Z- TerrainNav::motionUpdate() - currEstimate = (4061735, 594906.06, 50.17) 2024-11-06T03:11:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:11:56Z- TerrainNav:: SubcloudNIS is 0.88, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:11:56Z- TNavPF::Projecting Measurements 2024-11-06T03:11:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:11:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:11:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:11:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:11:56Z- TNavFilter::4 good beams. 2024-11-06T03:11:56Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:11:56Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:11:56Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:11:56Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:11:56Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:11:56Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:11:56Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:11:56Z- TerrainMapOctree: (vehN, vehE ) = (4061734.32, 594905.35). 2024-11-06T03:11:56Z- TerrainMapOctree: Min Distance = 156.14. 2024-11-06T03:11:56Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:11:56Z- Forcing Subcloud Comparison 2024-11-06T03:11:56Z- Weighting particles with subcloud comparison 2024-11-06T03:11:56Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:56Z- meanExpectedMeasDiff: -1.664568 delta_rms_squared: 1.468327 alpha: 0.986562 2024-11-06T03:11:56Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:56Z- meanExpectedMeasDiff: 2.327018 delta_rms_squared: 1.041733 alpha: 0.981163 2024-11-06T03:11:56Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:56Z- meanExpectedMeasDiff: 0.248620 delta_rms_squared: 1.386134 alpha: 0.985777 2024-11-06T03:11:56Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:11:56Z- meanExpectedMeasDiff: -0.636413 delta_rms_squared: 1.304580 alpha: 0.984901 2024-11-06T03:11:56Z- Subcloud NIS: 0.816168 2024-11-06T03:11:56Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:11:56Z- TNavPF:: sumWeights = 0.283402 2024-11-06T03:11:56Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:11:56Z- TNavPF::First Term of Map Covariance Matrix is 0.61 2024-11-06T03:11:56Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T03:11:56Z- TerrainNav::Resampling particle filter... 2024-11-06T03:11:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862715.08 sec. 2024-11-06T03:11:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:11:56Z- variances([0], [2], and psi_berg) = 0.159455 0.147900 0.000000 2024-11-06T03:11:56Z- publishEstimatesDecoder() - at 1730862716.27 sec 2024-11-06T03:11:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:11:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061767.19 594889.40 54.35 2024-11-06T03:11:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061765.16 594888.73 55.06 lat/lon: 36.70 -121.94 2024-11-06T03:11:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.15 0.71 0.00 2024-11-06T03:11:56Z- AHRS_M2 msg: 1730862716.68 epoch sec; seqNo:3158 2024-11-06T03:11:56Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.45 psi 2024-11-06T03:11:56Z- handleDepth()- Depth_Keller msg: 1730862716.69 epoch sec; seqNo:3359; depth 50.21 2024-11-06T03:11:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862716.76 epoch sec; seqNo:3359; 4061734.55 north; 594906.06 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:11:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862716.27 2024-11-06T03:11:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:56Z- waiting for 1730862720.27, time is 1730862716.76 2024-11-06T03:11:57Z- AHRS_M2 msg: 1730862717.07 epoch sec; seqNo:3159 2024-11-06T03:11:57Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.45 psi 2024-11-06T03:11:57Z- handleDepth()- Depth_Keller msg: 1730862717.07 epoch sec; seqNo:3360; depth 50.25 2024-11-06T03:11:57Z- handleDVL() - DVL msg epochMillisec: 1730862717.10 2024-11-06T03:11:57Z- handleDvl() - RDI_Pathfinder msg: 1730862717.10 sec; seqNo:1109; #beams:4 2024-11-06T03:11:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.87 , 42.20 , 43.42 , 43.42 2024-11-06T03:11:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.11 2024-11-06T03:11:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862717.13 epoch sec; seqNo:3360; 4061734.55 north; 594906.06 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:11:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862716.27 2024-11-06T03:11:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:57Z- waiting for 1730862720.27, time is 1730862717.13 2024-11-06T03:11:57Z- handleDepth()- Depth_Keller msg: 1730862717.49 epoch sec; seqNo:3361; depth 50.27 2024-11-06T03:11:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862717.50 epoch sec; seqNo:3361; 4061734.56 north; 594907.29 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:11:57Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:11:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862716.27 2024-11-06T03:11:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:57Z- waiting for 1730862720.27, time is 1730862717.50 2024-11-06T03:11:57Z- AHRS_M2 msg: 1730862717.87 epoch sec; seqNo:3160 2024-11-06T03:11:57Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.45 psi 2024-11-06T03:11:57Z- handleDepth()- Depth_Keller msg: 1730862717.89 epoch sec; seqNo:3362; depth 50.28 2024-11-06T03:11:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862717.93 epoch sec; seqNo:3362; 4061734.56 north; 594907.29 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:11:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862716.27 2024-11-06T03:11:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:58Z- waiting for 1730862720.27, time is 1730862717.93 2024-11-06T03:11:58Z- AHRS_M2 msg: 1730862718.28 epoch sec; seqNo:3161 2024-11-06T03:11:58Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.45 psi 2024-11-06T03:11:58Z- handleDepth()- Depth_Keller msg: 1730862718.29 epoch sec; seqNo:3363; depth 50.31 2024-11-06T03:11:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862718.31 epoch sec; seqNo:3363; 4061734.56 north; 594907.29 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:11:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862716.27 2024-11-06T03:11:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:58Z- waiting for 1730862720.27, time is 1730862718.31 2024-11-06T03:11:58Z- AHRS_M2 msg: 1730862718.69 epoch sec; seqNo:3162 2024-11-06T03:11:58Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.45 psi 2024-11-06T03:11:58Z- handleDepth()- Depth_Keller msg: 1730862718.70 epoch sec; seqNo:3364; depth 50.33 2024-11-06T03:11:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862718.75 epoch sec; seqNo:3364; 4061734.96 north; 594908.50 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:11:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:11:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862716.27 2024-11-06T03:11:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:58Z- waiting for 1730862720.27, time is 1730862718.75 2024-11-06T03:11:59Z- AHRS_M2 msg: 1730862719.08 epoch sec; seqNo:3163 2024-11-06T03:11:59Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.46 psi 2024-11-06T03:11:59Z- handleDepth()- Depth_Keller msg: 1730862719.09 epoch sec; seqNo:3365; depth 50.35 2024-11-06T03:11:59Z- handleDVL() - DVL msg epochMillisec: 1730862719.13 2024-11-06T03:11:59Z- handleDvl() - RDI_Pathfinder msg: 1730862719.13 sec; seqNo:1110; #beams:4 2024-11-06T03:11:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.07 , 42.40 , 43.63 , 43.83 2024-11-06T03:11:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.03 , 0.07 2024-11-06T03:11:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862719.14 epoch sec; seqNo:3365; 4061734.96 north; 594908.50 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:11:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:11:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862716.27 2024-11-06T03:11:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:59Z- waiting for 1730862720.27, time is 1730862719.14 2024-11-06T03:11:59Z- handleDepth()- Depth_Keller msg: 1730862719.51 epoch sec; seqNo:3366; depth 50.37 2024-11-06T03:11:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862719.52 epoch sec; seqNo:3366; 4061734.96 north; 594908.50 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:11:59Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:11:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862716.27 2024-11-06T03:11:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:11:59Z- waiting for 1730862720.27, time is 1730862719.52 2024-11-06T03:11:59Z- AHRS_M2 msg: 1730862719.89 epoch sec; seqNo:3164 2024-11-06T03:11:59Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.46 psi 2024-11-06T03:11:59Z- handleDepth()- Depth_Keller msg: 1730862719.90 epoch sec; seqNo:3367; depth 50.38 2024-11-06T03:11:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862719.91 epoch sec; seqNo:3367; 4061734.97 north; 594909.72 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:12:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862716.27 2024-11-06T03:12:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:00Z- waiting for 1730862720.27, time is 1730862719.91 2024-11-06T03:12:00Z- AHRS_M2 msg: 1730862720.29 epoch sec; seqNo:3165 2024-11-06T03:12:00Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.46 psi 2024-11-06T03:12:00Z- handleDepth()- Depth_Keller msg: 1730862720.31 epoch sec; seqNo:3368; depth 50.40 2024-11-06T03:12:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862720.32 epoch sec; seqNo:3368; 4061734.97 north; 594909.72 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:12:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862716.27 2024-11-06T03:12:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:00Z- waiting for 1730862720.27, time is 1730862720.32 2024-11-06T03:12:00Z- LcmTrn::updateTrn() >>>> pose time 1730862720.32, meas time 1730862719.13 <<<< 2024-11-06T03:12:00Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862720.32 2024-11-06T03:12:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862719.13 sec, ping # 0; waiting for INS data more recent than 1730862716.27... 2024-11-06T03:12:00Z- TerrainNav::motionUpdate() - currEstimate = (4061735, 594909.72, 50.40) 2024-11-06T03:12:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:12:00Z- TerrainNav:: SubcloudNIS is 0.82, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:12:00Z- TNavPF::Projecting Measurements 2024-11-06T03:12:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:12:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:12:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:12:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:12:00Z- TNavFilter::4 good beams. 2024-11-06T03:12:00Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:12:00Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:12:00Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:12:00Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:12:00Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:12:00Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:12:00Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:12:00Z- TerrainMapOctree: (vehN, vehE ) = (4061734.85, 594908.65). 2024-11-06T03:12:00Z- TerrainMapOctree: Min Distance = 159.37. 2024-11-06T03:12:00Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:12:00Z- Forcing Subcloud Comparison 2024-11-06T03:12:00Z- Weighting particles with subcloud comparison 2024-11-06T03:12:00Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:00Z- meanExpectedMeasDiff: -1.576923 delta_rms_squared: 1.530775 alpha: 0.987103 2024-11-06T03:12:00Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:00Z- meanExpectedMeasDiff: 2.029108 delta_rms_squared: 1.112056 alpha: 0.982333 2024-11-06T03:12:00Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:00Z- meanExpectedMeasDiff: -0.732908 delta_rms_squared: 2.327819 alpha: 0.991481 2024-11-06T03:12:00Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:00Z- meanExpectedMeasDiff: 0.549057 delta_rms_squared: 1.331901 alpha: 0.985206 2024-11-06T03:12:00Z- Subcloud NIS: 0.779191 2024-11-06T03:12:00Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:12:00Z- TNavPF:: sumWeights = 0.307352 2024-11-06T03:12:00Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:12:00Z- TNavPF::First Term of Map Covariance Matrix is 0.57 2024-11-06T03:12:00Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T03:12:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862719.13 sec. 2024-11-06T03:12:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:12:00Z- variances([0], [2], and psi_berg) = 0.162337 0.149460 0.000000 2024-11-06T03:12:00Z- publishEstimatesDecoder() - at 1730862720.32 sec 2024-11-06T03:12:00Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:12:00Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061765.78 594893.52 55.41 2024-11-06T03:12:00Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061765.58 594892.37 55.32 lat/lon: 36.70 -121.94 2024-11-06T03:12:00Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.15 0.68 0.00 2024-11-06T03:12:00Z- AHRS_M2 msg: 1730862720.71 epoch sec; seqNo:3166 2024-11-06T03:12:00Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.46 psi 2024-11-06T03:12:00Z- handleDepth()- Depth_Keller msg: 1730862720.74 epoch sec; seqNo:3369; depth 50.41 2024-11-06T03:12:00Z- handleDVL() - DVL msg epochMillisec: 1730862720.78 2024-11-06T03:12:00Z- handleDvl() - RDI_Pathfinder msg: 1730862720.78 sec; seqNo:1111; #beams:4 2024-11-06T03:12:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 42.21 , 43.63 , 44.04 2024-11-06T03:12:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.07 , -0.02 2024-11-06T03:12:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862720.79 epoch sec; seqNo:3369; 4061734.97 north; 594909.72 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:12:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862720.32 2024-11-06T03:12:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:00Z- waiting for 1730862724.32, time is 1730862720.79 2024-11-06T03:12:01Z- AHRS_M2 msg: 1730862721.11 epoch sec; seqNo:3167 2024-11-06T03:12:01Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T03:12:01Z- handleDepth()- Depth_Keller msg: 1730862721.11 epoch sec; seqNo:3370; depth 50.43 2024-11-06T03:12:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862721.13 epoch sec; seqNo:3370; 4061734.97 north; 594909.72 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:12:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862720.32 2024-11-06T03:12:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:01Z- waiting for 1730862724.32, time is 1730862721.13 2024-11-06T03:12:01Z- handleDepth()- Depth_Keller msg: 1730862721.53 epoch sec; seqNo:3371; depth 50.42 2024-11-06T03:12:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862721.54 epoch sec; seqNo:3371; 4061734.98 north; 594910.94 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:12:01Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:12:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862720.32 2024-11-06T03:12:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:01Z- waiting for 1730862724.32, time is 1730862721.54 2024-11-06T03:12:01Z- AHRS_M2 msg: 1730862721.91 epoch sec; seqNo:3168 2024-11-06T03:12:01Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.47 psi 2024-11-06T03:12:01Z- handleDepth()- Depth_Keller msg: 1730862721.92 epoch sec; seqNo:3372; depth 50.44 2024-11-06T03:12:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862721.93 epoch sec; seqNo:3372; 4061734.98 north; 594910.94 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:12:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862720.32 2024-11-06T03:12:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:02Z- waiting for 1730862724.32, time is 1730862721.93 2024-11-06T03:12:02Z- handleDepth()- Depth_Keller msg: 1730862722.33 epoch sec; seqNo:3373; depth 50.44 2024-11-06T03:12:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862722.35 epoch sec; seqNo:3373; 4061734.98 north; 594910.94 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:12:02Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:12:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862720.32 2024-11-06T03:12:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:02Z- waiting for 1730862724.32, time is 1730862722.35 2024-11-06T03:12:02Z- AHRS_M2 msg: 1730862722.75 epoch sec; seqNo:3169 2024-11-06T03:12:02Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 1.48 psi 2024-11-06T03:12:02Z- handleDepth()- Depth_Keller msg: 1730862722.77 epoch sec; seqNo:3374; depth 50.45 2024-11-06T03:12:02Z- handleDVL() - DVL msg epochMillisec: 1730862722.80 2024-11-06T03:12:02Z- handleDvl() - RDI_Pathfinder msg: 1730862722.80 sec; seqNo:1112; #beams:4 2024-11-06T03:12:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 42.00 , 43.84 , 44.04 2024-11-06T03:12:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.06 , -0.11 2024-11-06T03:12:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862722.83 epoch sec; seqNo:3374; 4061735.00 north; 594912.16 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:12:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862720.32 2024-11-06T03:12:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:02Z- waiting for 1730862724.32, time is 1730862722.83 2024-11-06T03:12:03Z- AHRS_M2 msg: 1730862723.14 epoch sec; seqNo:3170 2024-11-06T03:12:03Z- AHRS_M2 msg: 0.19 phi; -0.03 theta; 1.49 psi 2024-11-06T03:12:03Z- handleDepth()- Depth_Keller msg: 1730862723.15 epoch sec; seqNo:3375; depth 50.44 2024-11-06T03:12:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862723.16 epoch sec; seqNo:3375; 4061735.00 north; 594912.16 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:12:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862720.32 2024-11-06T03:12:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:03Z- waiting for 1730862724.32, time is 1730862723.16 2024-11-06T03:12:03Z- AHRS_M2 msg: 1730862723.53 epoch sec; seqNo:3171 2024-11-06T03:12:03Z- AHRS_M2 msg: 0.20 phi; -0.03 theta; 1.50 psi 2024-11-06T03:12:03Z- handleDepth()- Depth_Keller msg: 1730862723.53 epoch sec; seqNo:3376; depth 50.43 2024-11-06T03:12:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862723.54 epoch sec; seqNo:3376; 4061735.00 north; 594912.16 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:12:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862720.32 2024-11-06T03:12:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:03Z- waiting for 1730862724.32, time is 1730862723.54 2024-11-06T03:12:03Z- handleDepth()- Depth_Keller msg: 1730862723.97 epoch sec; seqNo:3377; depth 50.44 2024-11-06T03:12:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862723.98 epoch sec; seqNo:3377; 4061735.01 north; 594913.38 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:12:04Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:12:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862720.32 2024-11-06T03:12:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:04Z- waiting for 1730862724.32, time is 1730862723.98 2024-11-06T03:12:04Z- AHRS_M2 msg: 1730862724.34 epoch sec; seqNo:3172 2024-11-06T03:12:04Z- AHRS_M2 msg: 0.23 phi; -0.03 theta; 1.52 psi 2024-11-06T03:12:04Z- handleDepth()- Depth_Keller msg: 1730862724.35 epoch sec; seqNo:3378; depth 50.43 2024-11-06T03:12:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862724.36 epoch sec; seqNo:3378; 4061735.01 north; 594913.38 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:12:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862720.32 2024-11-06T03:12:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:04Z- waiting for 1730862724.32, time is 1730862724.36 2024-11-06T03:12:04Z- LcmTrn::updateTrn() >>>> pose time 1730862724.36, meas time 1730862722.80 <<<< 2024-11-06T03:12:04Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862724.36 2024-11-06T03:12:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862722.80 sec, ping # 0; waiting for INS data more recent than 1730862720.32... 2024-11-06T03:12:04Z- TerrainNav::motionUpdate() - currEstimate = (4061735, 594913.38, 50.43) 2024-11-06T03:12:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:12:04Z- TerrainNav:: SubcloudNIS is 0.78, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:12:04Z- TNavPF::Projecting Measurements 2024-11-06T03:12:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:12:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:12:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:12:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:12:04Z- TNavFilter::4 good beams. 2024-11-06T03:12:04Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:12:04Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:12:04Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:12:04Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:12:04Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:12:04Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:12:04Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:12:04Z- TerrainMapOctree: (vehN, vehE ) = (4061734.99, 594911.97). 2024-11-06T03:12:04Z- TerrainMapOctree: Min Distance = 162.66. 2024-11-06T03:12:04Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:12:04Z- Forcing Subcloud Comparison 2024-11-06T03:12:04Z- Weighting particles with subcloud comparison 2024-11-06T03:12:04Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:04Z- meanExpectedMeasDiff: -2.864358 delta_rms_squared: 1.633689 alpha: 0.987906 2024-11-06T03:12:04Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:04Z- meanExpectedMeasDiff: 1.637033 delta_rms_squared: 1.096692 alpha: 0.982090 2024-11-06T03:12:04Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:04Z- meanExpectedMeasDiff: -2.643286 delta_rms_squared: 1.907114 alpha: 0.989622 2024-11-06T03:12:04Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:04Z- meanExpectedMeasDiff: 0.405963 delta_rms_squared: 1.324498 alpha: 0.985125 2024-11-06T03:12:04Z- Subcloud NIS: 0.739892 2024-11-06T03:12:04Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:12:04Z- TNavPF:: sumWeights = 0.133361 2024-11-06T03:12:04Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:12:04Z- TNavPF::First Term of Map Covariance Matrix is 0.63 2024-11-06T03:12:04Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.01 2024-11-06T03:12:04Z- TerrainNav::Resampling particle filter... 2024-11-06T03:12:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862722.80 sec. 2024-11-06T03:12:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:12:04Z- variances([0], [2], and psi_berg) = 0.157541 0.146773 0.000000 2024-11-06T03:12:04Z- publishEstimatesDecoder() - at 1730862724.36 sec 2024-11-06T03:12:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:12:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061767.69 594896.67 55.71 2024-11-06T03:12:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061765.62 594896.05 55.80 lat/lon: 36.70 -121.94 2024-11-06T03:12:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.15 0.70 0.00 2024-11-06T03:12:04Z- AHRS_M2 msg: 1730862724.77 epoch sec; seqNo:3173 2024-11-06T03:12:04Z- AHRS_M2 msg: 0.26 phi; -0.04 theta; 1.54 psi 2024-11-06T03:12:04Z- handleDepth()- Depth_Keller msg: 1730862724.79 epoch sec; seqNo:3379; depth 50.44 2024-11-06T03:12:04Z- handleDVL() - DVL msg epochMillisec: 1730862724.84 2024-11-06T03:12:04Z- handleDvl() - RDI_Pathfinder msg: 1730862724.84 sec; seqNo:1113; #beams:4 2024-11-06T03:12:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.73 , 40.78 , 44.04 , 44.24 2024-11-06T03:12:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.02 , -0.14 2024-11-06T03:12:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862724.85 epoch sec; seqNo:3379; 4061735.01 north; 594913.38 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:12:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862724.36 2024-11-06T03:12:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:04Z- waiting for 1730862728.36, time is 1730862724.85 2024-11-06T03:12:05Z- AHRS_M2 msg: 1730862725.15 epoch sec; seqNo:3174 2024-11-06T03:12:05Z- AHRS_M2 msg: 0.29 phi; -0.04 theta; 1.57 psi 2024-11-06T03:12:05Z- handleDepth()- Depth_Keller msg: 1730862725.16 epoch sec; seqNo:3380; depth 50.45 2024-11-06T03:12:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862725.18 epoch sec; seqNo:3380; 4061735.01 north; 594913.38 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:12:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862724.36 2024-11-06T03:12:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:05Z- waiting for 1730862728.36, time is 1730862725.18 2024-11-06T03:12:05Z- AHRS_M2 msg: 1730862725.55 epoch sec; seqNo:3175 2024-11-06T03:12:05Z- AHRS_M2 msg: 0.33 phi; -0.05 theta; 1.59 psi 2024-11-06T03:12:05Z- handleDepth()- Depth_Keller msg: 1730862725.56 epoch sec; seqNo:3381; depth 50.45 2024-11-06T03:12:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862725.57 epoch sec; seqNo:3381; 4061735.02 north; 594914.60 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:12:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862724.36 2024-11-06T03:12:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:05Z- waiting for 1730862728.36, time is 1730862725.57 2024-11-06T03:12:05Z- AHRS_M2 msg: 1730862725.95 epoch sec; seqNo:3176 2024-11-06T03:12:05Z- AHRS_M2 msg: 0.36 phi; -0.06 theta; 1.62 psi 2024-11-06T03:12:05Z- handleDepth()- Depth_Keller msg: 1730862725.96 epoch sec; seqNo:3382; depth 50.46 2024-11-06T03:12:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862725.97 epoch sec; seqNo:3382; 4061735.02 north; 594914.60 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:12:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862724.36 2024-11-06T03:12:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:06Z- waiting for 1730862728.36, time is 1730862725.97 2024-11-06T03:12:06Z- AHRS_M2 msg: 1730862726.36 epoch sec; seqNo:3177 2024-11-06T03:12:06Z- AHRS_M2 msg: 0.39 phi; -0.07 theta; 1.65 psi 2024-11-06T03:12:06Z- handleDepth()- Depth_Keller msg: 1730862726.36 epoch sec; seqNo:3383; depth 50.47 2024-11-06T03:12:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862726.37 epoch sec; seqNo:3383; 4061735.02 north; 594914.60 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:12:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862724.36 2024-11-06T03:12:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:06Z- waiting for 1730862728.36, time is 1730862726.37 2024-11-06T03:12:06Z- AHRS_M2 msg: 1730862726.77 epoch sec; seqNo:3178 2024-11-06T03:12:06Z- AHRS_M2 msg: 0.41 phi; -0.08 theta; 1.68 psi 2024-11-06T03:12:06Z- handleDepth()- Depth_Keller msg: 1730862726.78 epoch sec; seqNo:3384; depth 50.49 2024-11-06T03:12:06Z- handleDVL() - DVL msg epochMillisec: 1730862726.80 2024-11-06T03:12:06Z- handleDvl() - RDI_Pathfinder msg: 1730862726.80 sec; seqNo:1114; #beams:4 2024-11-06T03:12:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 57.09 , 38.94 , 45.67 , 46.49 2024-11-06T03:12:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.02 , -0.09 2024-11-06T03:12:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862726.83 epoch sec; seqNo:3384; 4061735.04 north; 594915.82 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:12:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862724.36 2024-11-06T03:12:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:06Z- waiting for 1730862728.36, time is 1730862726.83 2024-11-06T03:12:07Z- AHRS_M2 msg: 1730862727.23 epoch sec; seqNo:3179 2024-11-06T03:12:07Z- AHRS_M2 msg: 0.43 phi; -0.09 theta; 1.71 psi 2024-11-06T03:12:07Z- handleDepth()- Depth_Keller msg: 1730862727.23 epoch sec; seqNo:3385; depth 50.51 2024-11-06T03:12:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862727.26 epoch sec; seqNo:3385; 4061734.66 north; 594915.83 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:12:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862724.36 2024-11-06T03:12:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:07Z- waiting for 1730862728.36, time is 1730862727.26 2024-11-06T03:12:07Z- AHRS_M2 msg: 1730862727.62 epoch sec; seqNo:3180 2024-11-06T03:12:07Z- AHRS_M2 msg: 0.44 phi; -0.10 theta; 1.75 psi 2024-11-06T03:12:07Z- handleDepth()- Depth_Keller msg: 1730862727.63 epoch sec; seqNo:3386; depth 50.53 2024-11-06T03:12:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862727.67 epoch sec; seqNo:3386; 4061734.66 north; 594915.83 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:12:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862724.36 2024-11-06T03:12:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:07Z- waiting for 1730862728.36, time is 1730862727.67 2024-11-06T03:12:08Z- handleDepth()- Depth_Keller msg: 1730862728.05 epoch sec; seqNo:3387; depth 50.57 2024-11-06T03:12:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862728.06 epoch sec; seqNo:3387; 4061734.66 north; 594915.83 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T03:12:08Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:12:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862724.36 2024-11-06T03:12:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:08Z- waiting for 1730862728.36, time is 1730862728.06 2024-11-06T03:12:08Z- AHRS_M2 msg: 1730862728.45 epoch sec; seqNo:3181 2024-11-06T03:12:08Z- AHRS_M2 msg: 0.43 phi; -0.11 theta; 1.84 psi 2024-11-06T03:12:08Z- handleDepth()- Depth_Keller msg: 1730862728.47 epoch sec; seqNo:3388; depth 50.60 2024-11-06T03:12:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862728.49 epoch sec; seqNo:3388; 4061734.66 north; 594915.83 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:12:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862724.36 2024-11-06T03:12:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:08Z- waiting for 1730862728.36, time is 1730862728.49 2024-11-06T03:12:08Z- LcmTrn::updateTrn() >>>> pose time 1730862728.49, meas time 1730862726.80 <<<< 2024-11-06T03:12:08Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862728.49 2024-11-06T03:12:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862726.80 sec, ping # 0; waiting for INS data more recent than 1730862724.36... 2024-11-06T03:12:08Z- TerrainNav::motionUpdate() - currEstimate = (4061735, 594915.83, 50.60) 2024-11-06T03:12:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:12:08Z- TerrainNav:: SubcloudNIS is 0.74, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:12:08Z- TNavPF::Projecting Measurements 2024-11-06T03:12:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:12:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:12:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:12:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:12:08Z- TNavFilter::4 good beams. 2024-11-06T03:12:08Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:12:08Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:12:08Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:12:08Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:12:08Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:12:08Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:12:08Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:12:08Z- TerrainMapOctree: (vehN, vehE ) = (4061734.80, 594914.83). 2024-11-06T03:12:08Z- TerrainMapOctree: Min Distance = 165.53. 2024-11-06T03:12:08Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:12:08Z- Forcing Subcloud Comparison 2024-11-06T03:12:08Z- Weighting particles with subcloud comparison 2024-11-06T03:12:08Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:08Z- meanExpectedMeasDiff: -1.722156 delta_rms_squared: 2.299555 alpha: 0.991378 2024-11-06T03:12:08Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:08Z- meanExpectedMeasDiff: 0.766407 delta_rms_squared: 1.361086 alpha: 0.985519 2024-11-06T03:12:08Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:08Z- meanExpectedMeasDiff: -1.040751 delta_rms_squared: 1.304230 alpha: 0.984897 2024-11-06T03:12:08Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:08Z- meanExpectedMeasDiff: 0.143711 delta_rms_squared: 1.588897 alpha: 0.987569 2024-11-06T03:12:08Z- Subcloud NIS: 0.712652 2024-11-06T03:12:08Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:12:08Z- TNavPF:: sumWeights = 0.469808 2024-11-06T03:12:08Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:12:08Z- TNavPF::First Term of Map Covariance Matrix is 0.78 2024-11-06T03:12:08Z- TNavPF::Calculated NIS Value : 0.19 numBeams normalized NIS: 0.05 2024-11-06T03:12:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862726.80 sec. 2024-11-06T03:12:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:12:08Z- variances([0], [2], and psi_berg) = 0.151097 0.151957 0.000000 2024-11-06T03:12:08Z- publishEstimatesDecoder() - at 1730862728.49 sec 2024-11-06T03:12:08Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:12:08Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061764.74 594898.73 56.29 2024-11-06T03:12:08Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061765.23 594898.52 56.14 lat/lon: 36.70 -121.94 2024-11-06T03:12:08Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.15 0.68 0.00 2024-11-06T03:12:08Z- AHRS_M2 msg: 1730862728.86 epoch sec; seqNo:3182 2024-11-06T03:12:08Z- AHRS_M2 msg: 0.42 phi; -0.11 theta; 1.87 psi 2024-11-06T03:12:08Z- handleDepth()- Depth_Keller msg: 1730862728.88 epoch sec; seqNo:3389; depth 50.66 2024-11-06T03:12:08Z- handleDVL() - DVL msg epochMillisec: 1730862728.92 2024-11-06T03:12:08Z- handleDvl() - RDI_Pathfinder msg: 1730862728.92 sec; seqNo:1115; #beams:4 2024-11-06T03:12:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 38.65 , 45.50 , 49.03 2024-11-06T03:12:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.03 , 0.01 2024-11-06T03:12:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862728.94 epoch sec; seqNo:3389; 4061734.67 north; 594917.05 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T03:12:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862728.49 2024-11-06T03:12:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:09Z- waiting for 1730862732.49, time is 1730862728.94 2024-11-06T03:12:09Z- AHRS_M2 msg: 1730862729.29 epoch sec; seqNo:3183 2024-11-06T03:12:09Z- AHRS_M2 msg: 0.41 phi; -0.12 theta; 1.91 psi 2024-11-06T03:12:09Z- handleDepth()- Depth_Keller msg: 1730862729.30 epoch sec; seqNo:3390; depth 50.71 2024-11-06T03:12:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862729.31 epoch sec; seqNo:3390; 4061734.67 north; 594917.05 east; 36.70 lat; -121.94 lon; 50.71 depth; utmZone: 10 2024-11-06T03:12:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862728.49 2024-11-06T03:12:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:09Z- waiting for 1730862732.49, time is 1730862729.31 2024-11-06T03:12:09Z- AHRS_M2 msg: 1730862729.70 epoch sec; seqNo:3184 2024-11-06T03:12:09Z- AHRS_M2 msg: 0.40 phi; -0.12 theta; 1.94 psi 2024-11-06T03:12:09Z- handleDepth()- Depth_Keller msg: 1730862729.70 epoch sec; seqNo:3391; depth 50.74 2024-11-06T03:12:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862729.71 epoch sec; seqNo:3391; 4061734.29 north; 594917.05 east; 36.70 lat; -121.94 lon; 50.74 depth; utmZone: 10 2024-11-06T03:12:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862728.49 2024-11-06T03:12:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:09Z- waiting for 1730862732.49, time is 1730862729.71 2024-11-06T03:12:10Z- AHRS_M2 msg: 1730862730.10 epoch sec; seqNo:3185 2024-11-06T03:12:10Z- AHRS_M2 msg: 0.40 phi; -0.11 theta; 1.97 psi 2024-11-06T03:12:10Z- handleDepth()- Depth_Keller msg: 1730862730.11 epoch sec; seqNo:3392; depth 50.80 2024-11-06T03:12:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862730.12 epoch sec; seqNo:3392; 4061734.29 north; 594917.05 east; 36.70 lat; -121.94 lon; 50.80 depth; utmZone: 10 2024-11-06T03:12:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862728.49 2024-11-06T03:12:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:10Z- waiting for 1730862732.49, time is 1730862730.12 2024-11-06T03:12:10Z- AHRS_M2 msg: 1730862730.50 epoch sec; seqNo:3186 2024-11-06T03:12:10Z- AHRS_M2 msg: 0.40 phi; -0.11 theta; 2.01 psi 2024-11-06T03:12:10Z- handleDepth()- Depth_Keller msg: 1730862730.51 epoch sec; seqNo:3393; depth 50.89 2024-11-06T03:12:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862730.52 epoch sec; seqNo:3393; 4061734.29 north; 594917.05 east; 36.70 lat; -121.94 lon; 50.89 depth; utmZone: 10 2024-11-06T03:12:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862728.49 2024-11-06T03:12:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:10Z- waiting for 1730862732.49, time is 1730862730.52 2024-11-06T03:12:10Z- handleDepth()- Depth_Keller msg: 1730862730.93 epoch sec; seqNo:3394; depth 50.95 2024-11-06T03:12:10Z- handleDVL() - DVL msg epochMillisec: 1730862730.96 2024-11-06T03:12:10Z- handleDvl() - RDI_Pathfinder msg: 1730862730.96 sec; seqNo:1116; #beams:4 2024-11-06T03:12:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 59.53 , 38.74 , 44.65 , 48.93 2024-11-06T03:12:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.02 , 0.08 2024-11-06T03:12:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862730.98 epoch sec; seqNo:3394; 4061734.31 north; 594918.27 east; 36.70 lat; -121.94 lon; 50.95 depth; utmZone: 10 2024-11-06T03:12:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862728.49 2024-11-06T03:12:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:11Z- waiting for 1730862732.49, time is 1730862730.98 2024-11-06T03:12:11Z- AHRS_M2 msg: 1730862731.31 epoch sec; seqNo:3187 2024-11-06T03:12:11Z- AHRS_M2 msg: 0.40 phi; -0.11 theta; 2.08 psi 2024-11-06T03:12:11Z- handleDepth()- Depth_Keller msg: 1730862731.32 epoch sec; seqNo:3395; depth 51.02 2024-11-06T03:12:11Z- handleDVL() - DVL msg epochMillisec: 1730862731.32 2024-11-06T03:12:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:11Z- handleDvl() - RDI_Pathfinder msg: 1730862731.32 sec; seqNo:1117; #beams:0 2024-11-06T03:12:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.02 , 0.08 2024-11-06T03:12:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862731.34 epoch sec; seqNo:3395; 4061733.93 north; 594918.28 east; 36.70 lat; -121.94 lon; 51.02 depth; utmZone: 10 2024-11-06T03:12:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862728.49 2024-11-06T03:12:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:11Z- waiting for 1730862732.49, time is 1730862731.34 2024-11-06T03:12:11Z- AHRS_M2 msg: 1730862731.72 epoch sec; seqNo:3188 2024-11-06T03:12:11Z- AHRS_M2 msg: 0.40 phi; -0.10 theta; 2.12 psi 2024-11-06T03:12:11Z- handleDepth()- Depth_Keller msg: 1730862731.72 epoch sec; seqNo:3396; depth 51.06 2024-11-06T03:12:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862731.73 epoch sec; seqNo:3396; 4061733.93 north; 594918.28 east; 36.70 lat; -121.94 lon; 51.06 depth; utmZone: 10 2024-11-06T03:12:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862728.49 2024-11-06T03:12:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:11Z- waiting for 1730862732.49, time is 1730862731.73 2024-11-06T03:12:12Z- AHRS_M2 msg: 1730862732.12 epoch sec; seqNo:3189 2024-11-06T03:12:12Z- AHRS_M2 msg: 0.40 phi; -0.10 theta; 2.16 psi 2024-11-06T03:12:12Z- handleDepth()- Depth_Keller msg: 1730862732.12 epoch sec; seqNo:3397; depth 51.12 2024-11-06T03:12:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862732.14 epoch sec; seqNo:3397; 4061733.93 north; 594918.28 east; 36.70 lat; -121.94 lon; 51.12 depth; utmZone: 10 2024-11-06T03:12:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862728.49 2024-11-06T03:12:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:12Z- waiting for 1730862732.49, time is 1730862732.14 2024-11-06T03:12:12Z- AHRS_M2 msg: 1730862732.52 epoch sec; seqNo:3190 2024-11-06T03:12:12Z- AHRS_M2 msg: 0.40 phi; -0.10 theta; 2.20 psi 2024-11-06T03:12:12Z- handleDepth()- Depth_Keller msg: 1730862732.53 epoch sec; seqNo:3398; depth 51.18 2024-11-06T03:12:12Z- handleDVL() - DVL msg epochMillisec: 1730862732.53 2024-11-06T03:12:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:12Z- handleDvl() - RDI_Pathfinder msg: 1730862732.53 sec; seqNo:1118; #beams:0 2024-11-06T03:12:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.02 , 0.08 2024-11-06T03:12:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862732.54 epoch sec; seqNo:3398; 4061733.55 north; 594918.28 east; 36.70 lat; -121.94 lon; 51.18 depth; utmZone: 10 2024-11-06T03:12:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862728.49 2024-11-06T03:12:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:12Z- waiting for 1730862732.49, time is 1730862732.54 2024-11-06T03:12:12Z- LcmTrn::updateTrn() >>>> pose time 1730862732.54, meas time 1730862732.53 <<<< 2024-11-06T03:12:12Z- LcmTrn::updateTrn() equating pose 1730862732.53 and meas times 1730862732.53 2024-11-06T03:12:12Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862732.53 2024-11-06T03:12:12Z- TerrainNav::motionUpdate() - currEstimate = (4061734, 594918.28, 51.18) 2024-11-06T03:12:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:12:12Z- TerrainNav:: SubcloudNIS is 0.71, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:12:12Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:12:12Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:12:12Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:12:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:12:12Z- variances([0], [2], and psi_berg) = 0.152074 0.153428 0.000000 2024-11-06T03:12:12Z- publishEstimatesDecoder() - at 1730862732.53 sec 2024-11-06T03:12:12Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:12:12Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061763.68 594901.17 56.67 2024-11-06T03:12:12Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061764.12 594900.97 56.72 lat/lon: 36.70 -121.94 2024-11-06T03:12:12Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.15 0.95 0.00 2024-11-06T03:12:12Z- AHRS_M2 msg: 1730862732.93 epoch sec; seqNo:3191 2024-11-06T03:12:12Z- AHRS_M2 msg: 0.40 phi; -0.09 theta; 2.23 psi 2024-11-06T03:12:12Z- handleDepth()- Depth_Keller msg: 1730862732.96 epoch sec; seqNo:3399; depth 51.24 2024-11-06T03:12:13Z- handleDVL() - DVL msg epochMillisec: 1730862733.00 2024-11-06T03:12:13Z- handleDvl() - RDI_Pathfinder msg: 1730862733.00 sec; seqNo:1119; #beams:4 2024-11-06T03:12:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.50 , 38.87 , 45.50 , 48.37 2024-11-06T03:12:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.02 , 0.05 2024-11-06T03:12:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862733.01 epoch sec; seqNo:3399; 4061733.56 north; 594919.50 east; 36.70 lat; -121.94 lon; 51.24 depth; utmZone: 10 2024-11-06T03:12:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862732.53 2024-11-06T03:12:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:13Z- waiting for 1730862736.53, time is 1730862733.01 2024-11-06T03:12:13Z- AHRS_M2 msg: 1730862733.33 epoch sec; seqNo:3192 2024-11-06T03:12:13Z- AHRS_M2 msg: 0.41 phi; -0.09 theta; 2.27 psi 2024-11-06T03:12:13Z- handleDepth()- Depth_Keller msg: 1730862733.34 epoch sec; seqNo:3400; depth 51.30 2024-11-06T03:12:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862733.36 epoch sec; seqNo:3400; 4061733.18 north; 594919.51 east; 36.70 lat; -121.94 lon; 51.30 depth; utmZone: 10 2024-11-06T03:12:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862732.53 2024-11-06T03:12:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:13Z- waiting for 1730862736.53, time is 1730862733.36 2024-11-06T03:12:13Z- AHRS_M2 msg: 1730862733.73 epoch sec; seqNo:3193 2024-11-06T03:12:13Z- AHRS_M2 msg: 0.41 phi; -0.08 theta; 2.31 psi 2024-11-06T03:12:13Z- handleDepth()- Depth_Keller msg: 1730862733.75 epoch sec; seqNo:3401; depth 51.35 2024-11-06T03:12:13Z- handleDVL() - DVL msg epochMillisec: 1730862733.75 2024-11-06T03:12:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:13Z- handleDvl() - RDI_Pathfinder msg: 1730862733.75 sec; seqNo:1120; #beams:0 2024-11-06T03:12:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.02 , 0.05 2024-11-06T03:12:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862733.76 epoch sec; seqNo:3401; 4061733.18 north; 594919.51 east; 36.70 lat; -121.94 lon; 51.35 depth; utmZone: 10 2024-11-06T03:12:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862732.53 2024-11-06T03:12:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:13Z- waiting for 1730862736.53, time is 1730862733.76 2024-11-06T03:12:14Z- AHRS_M2 msg: 1730862734.14 epoch sec; seqNo:3194 2024-11-06T03:12:14Z- AHRS_M2 msg: 0.40 phi; -0.08 theta; 2.35 psi 2024-11-06T03:12:14Z- handleDepth()- Depth_Keller msg: 1730862734.14 epoch sec; seqNo:3402; depth 51.40 2024-11-06T03:12:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862734.15 epoch sec; seqNo:3402; 4061732.80 north; 594919.51 east; 36.70 lat; -121.94 lon; 51.40 depth; utmZone: 10 2024-11-06T03:12:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862732.53 2024-11-06T03:12:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:14Z- waiting for 1730862736.53, time is 1730862734.15 2024-11-06T03:12:14Z- AHRS_M2 msg: 1730862734.54 epoch sec; seqNo:3195 2024-11-06T03:12:14Z- AHRS_M2 msg: 0.40 phi; -0.07 theta; 2.39 psi 2024-11-06T03:12:14Z- handleDepth()- Depth_Keller msg: 1730862734.55 epoch sec; seqNo:3403; depth 51.45 2024-11-06T03:12:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862734.56 epoch sec; seqNo:3403; 4061732.44 north; 594920.73 east; 36.70 lat; -121.94 lon; 51.45 depth; utmZone: 10 2024-11-06T03:12:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862732.53 2024-11-06T03:12:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:14Z- waiting for 1730862736.53, time is 1730862734.56 2024-11-06T03:12:14Z- AHRS_M2 msg: 1730862734.95 epoch sec; seqNo:3196 2024-11-06T03:12:14Z- AHRS_M2 msg: 0.40 phi; -0.07 theta; 2.43 psi 2024-11-06T03:12:14Z- handleDepth()- Depth_Keller msg: 1730862734.95 epoch sec; seqNo:3404; depth 51.49 2024-11-06T03:12:14Z- handleDVL() - DVL msg epochMillisec: 1730862734.98 2024-11-06T03:12:14Z- handleDvl() - RDI_Pathfinder msg: 1730862734.98 sec; seqNo:1121; #beams:4 2024-11-06T03:12:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.29 , 38.65 , 46.38 , 47.71 2024-11-06T03:12:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.04 2024-11-06T03:12:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862735.00 epoch sec; seqNo:3404; 4061732.44 north; 594920.73 east; 36.70 lat; -121.94 lon; 51.49 depth; utmZone: 10 2024-11-06T03:12:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862732.53 2024-11-06T03:12:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:15Z- waiting for 1730862736.53, time is 1730862735.00 2024-11-06T03:12:15Z- AHRS_M2 msg: 1730862735.35 epoch sec; seqNo:3197 2024-11-06T03:12:15Z- AHRS_M2 msg: 0.40 phi; -0.06 theta; 2.47 psi 2024-11-06T03:12:15Z- handleDepth()- Depth_Keller msg: 1730862735.36 epoch sec; seqNo:3405; depth 51.56 2024-11-06T03:12:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862735.41 epoch sec; seqNo:3405; 4061732.06 north; 594920.74 east; 36.70 lat; -121.94 lon; 51.56 depth; utmZone: 10 2024-11-06T03:12:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862732.53 2024-11-06T03:12:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:15Z- waiting for 1730862736.53, time is 1730862735.41 2024-11-06T03:12:15Z- AHRS_M2 msg: 1730862735.78 epoch sec; seqNo:3198 2024-11-06T03:12:15Z- AHRS_M2 msg: 0.40 phi; -0.06 theta; 2.51 psi 2024-11-06T03:12:15Z- handleDepth()- Depth_Keller msg: 1730862735.79 epoch sec; seqNo:3406; depth 51.58 2024-11-06T03:12:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862735.81 epoch sec; seqNo:3406; 4061731.68 north; 594920.74 east; 36.70 lat; -121.94 lon; 51.58 depth; utmZone: 10 2024-11-06T03:12:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862732.53 2024-11-06T03:12:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:15Z- waiting for 1730862736.53, time is 1730862735.81 2024-11-06T03:12:16Z- AHRS_M2 msg: 1730862736.18 epoch sec; seqNo:3199 2024-11-06T03:12:16Z- AHRS_M2 msg: 0.40 phi; -0.06 theta; 2.55 psi 2024-11-06T03:12:16Z- handleDepth()- Depth_Keller msg: 1730862736.19 epoch sec; seqNo:3407; depth 51.62 2024-11-06T03:12:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862736.20 epoch sec; seqNo:3407; 4061731.30 north; 594920.75 east; 36.70 lat; -121.94 lon; 51.62 depth; utmZone: 10 2024-11-06T03:12:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862732.53 2024-11-06T03:12:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:16Z- waiting for 1730862736.53, time is 1730862736.20 2024-11-06T03:12:16Z- AHRS_M2 msg: 1730862736.59 epoch sec; seqNo:3200 2024-11-06T03:12:16Z- AHRS_M2 msg: 0.40 phi; -0.05 theta; 2.59 psi 2024-11-06T03:12:16Z- handleDepth()- Depth_Keller msg: 1730862736.61 epoch sec; seqNo:3408; depth 51.63 2024-11-06T03:12:16Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:12:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862732.53 2024-11-06T03:12:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:16Z- waiting for 1730862736.53, time is 1730862736.59 2024-11-06T03:12:16Z- LcmTrn::updateTrn() >>>> pose time 1730862736.59, meas time 1730862734.98 <<<< 2024-11-06T03:12:16Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862736.59 2024-11-06T03:12:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862734.98 sec, ping # 0; waiting for INS data more recent than 1730862732.53... 2024-11-06T03:12:16Z- TerrainNav::motionUpdate() - currEstimate = (4061731, 594920.75, 51.63) 2024-11-06T03:12:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:12:16Z- TerrainNav:: SubcloudNIS is 0.71, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:12:16Z- TNavPF::Projecting Measurements 2024-11-06T03:12:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:12:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:12:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:12:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:12:16Z- TNavFilter::4 good beams. 2024-11-06T03:12:16Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:12:16Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:12:16Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:12:16Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:12:16Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:12:16Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:12:16Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:12:16Z- TerrainMapOctree: (vehN, vehE ) = (4061732.19, 594919.77). 2024-11-06T03:12:16Z- TerrainMapOctree: Min Distance = 170.70. 2024-11-06T03:12:16Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:12:16Z- Forcing Subcloud Comparison 2024-11-06T03:12:16Z- Weighting particles with subcloud comparison 2024-11-06T03:12:16Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:16Z- meanExpectedMeasDiff: -1.100299 delta_rms_squared: 5.644160 alpha: 0.996469 2024-11-06T03:12:16Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:16Z- meanExpectedMeasDiff: 1.396152 delta_rms_squared: 1.272162 alpha: 0.984522 2024-11-06T03:12:16Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:16Z- meanExpectedMeasDiff: -0.248558 delta_rms_squared: 1.722090 alpha: 0.988520 2024-11-06T03:12:16Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:16Z- meanExpectedMeasDiff: 0.838812 delta_rms_squared: 2.180597 alpha: 0.990912 2024-11-06T03:12:16Z- Subcloud NIS: 0.692612 2024-11-06T03:12:16Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:12:16Z- TNavPF:: sumWeights = 0.406523 2024-11-06T03:12:16Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:12:16Z- TNavPF::First Term of Map Covariance Matrix is 1.22 2024-11-06T03:12:16Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T03:12:16Z- TerrainNav::Resampling particle filter... 2024-11-06T03:12:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862734.98 sec. 2024-11-06T03:12:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:12:16Z- variances([0], [2], and psi_berg) = 0.162799 0.166007 0.000000 2024-11-06T03:12:16Z- publishEstimatesDecoder() - at 1730862736.59 sec 2024-11-06T03:12:16Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:12:16Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061763.95 594904.02 57.91 2024-11-06T03:12:16Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061761.88 594903.43 57.09 lat/lon: 36.70 -121.94 2024-11-06T03:12:16Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.17 0.72 0.00 2024-11-06T03:12:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862736.73 epoch sec; seqNo:3408; 4061730.92 north; 594920.75 east; 36.70 lat; -121.94 lon; 51.63 depth; utmZone: 10 2024-11-06T03:12:17Z- handleDepth()- Depth_Keller msg: 1730862737.02 epoch sec; seqNo:3409; depth 51.68 2024-11-06T03:12:17Z- handleDVL() - DVL msg epochMillisec: 1730862737.06 2024-11-06T03:12:17Z- handleDvl() - RDI_Pathfinder msg: 1730862737.06 sec; seqNo:1122; #beams:4 2024-11-06T03:12:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.51 , 37.99 , 46.60 , 46.60 2024-11-06T03:12:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.02 , -0.09 2024-11-06T03:12:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862737.08 epoch sec; seqNo:3409; 4061730.94 north; 594921.97 east; 36.70 lat; -121.94 lon; 51.68 depth; utmZone: 10 2024-11-06T03:12:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862736.59 2024-11-06T03:12:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:17Z- waiting for 1730862740.59, time is 1730862737.08 2024-11-06T03:12:17Z- AHRS_M2 msg: 1730862737.39 epoch sec; seqNo:3201 2024-11-06T03:12:17Z- AHRS_M2 msg: 0.40 phi; -0.04 theta; 2.67 psi 2024-11-06T03:12:17Z- handleDepth()- Depth_Keller msg: 1730862737.40 epoch sec; seqNo:3410; depth 51.71 2024-11-06T03:12:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862737.42 epoch sec; seqNo:3410; 4061730.56 north; 594921.98 east; 36.70 lat; -121.94 lon; 51.71 depth; utmZone: 10 2024-11-06T03:12:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862736.59 2024-11-06T03:12:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:17Z- waiting for 1730862740.59, time is 1730862737.42 2024-11-06T03:12:17Z- AHRS_M2 msg: 1730862737.80 epoch sec; seqNo:3202 2024-11-06T03:12:17Z- AHRS_M2 msg: 0.40 phi; -0.03 theta; 2.70 psi 2024-11-06T03:12:17Z- handleDepth()- Depth_Keller msg: 1730862737.81 epoch sec; seqNo:3411; depth 51.72 2024-11-06T03:12:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862737.82 epoch sec; seqNo:3411; 4061730.18 north; 594921.98 east; 36.70 lat; -121.94 lon; 51.72 depth; utmZone: 10 2024-11-06T03:12:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862736.59 2024-11-06T03:12:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:17Z- waiting for 1730862740.59, time is 1730862737.82 2024-11-06T03:12:18Z- handleDepth()- Depth_Keller msg: 1730862738.23 epoch sec; seqNo:3412; depth 51.72 2024-11-06T03:12:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862738.24 epoch sec; seqNo:3412; 4061729.80 north; 594921.98 east; 36.70 lat; -121.94 lon; 51.72 depth; utmZone: 10 2024-11-06T03:12:18Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:12:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862736.59 2024-11-06T03:12:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:18Z- waiting for 1730862740.59, time is 1730862738.24 2024-11-06T03:12:18Z- AHRS_M2 msg: 1730862738.61 epoch sec; seqNo:3203 2024-11-06T03:12:18Z- AHRS_M2 msg: 0.40 phi; -0.02 theta; 2.78 psi 2024-11-06T03:12:18Z- handleDepth()- Depth_Keller msg: 1730862738.62 epoch sec; seqNo:3413; depth 51.76 2024-11-06T03:12:18Z- handleDVL() - DVL msg epochMillisec: 1730862738.62 2024-11-06T03:12:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:18Z- handleDvl() - RDI_Pathfinder msg: 1730862738.62 sec; seqNo:1123; #beams:0 2024-11-06T03:12:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.02 , -0.09 2024-11-06T03:12:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862738.63 epoch sec; seqNo:3413; 4061729.42 north; 594921.99 east; 36.70 lat; -121.94 lon; 51.76 depth; utmZone: 10 2024-11-06T03:12:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862736.59 2024-11-06T03:12:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:18Z- waiting for 1730862740.59, time is 1730862738.63 2024-11-06T03:12:19Z- handleDepth()- Depth_Keller msg: 1730862739.03 epoch sec; seqNo:3414; depth 51.78 2024-11-06T03:12:19Z- handleDVL() - DVL msg epochMillisec: 1730862739.05 2024-11-06T03:12:19Z- handleDvl() - RDI_Pathfinder msg: 1730862739.05 sec; seqNo:1124; #beams:4 2024-11-06T03:12:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.07 , 38.21 , 47.27 , 45.72 2024-11-06T03:12:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.04 , -0.08 2024-11-06T03:12:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862739.07 epoch sec; seqNo:3414; 4061729.42 north; 594921.99 east; 36.70 lat; -121.94 lon; 51.78 depth; utmZone: 10 2024-11-06T03:12:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862736.59 2024-11-06T03:12:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:19Z- waiting for 1730862740.59, time is 1730862739.07 2024-11-06T03:12:19Z- AHRS_M2 msg: 1730862739.41 epoch sec; seqNo:3204 2024-11-06T03:12:19Z- AHRS_M2 msg: 0.39 phi; -0.01 theta; 2.86 psi 2024-11-06T03:12:19Z- handleDepth()- Depth_Keller msg: 1730862739.42 epoch sec; seqNo:3415; depth 51.78 2024-11-06T03:12:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862739.43 epoch sec; seqNo:3415; 4061729.04 north; 594921.99 east; 36.70 lat; -121.94 lon; 51.78 depth; utmZone: 10 2024-11-06T03:12:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862736.59 2024-11-06T03:12:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:19Z- waiting for 1730862740.59, time is 1730862739.43 2024-11-06T03:12:19Z- AHRS_M2 msg: 1730862739.83 epoch sec; seqNo:3205 2024-11-06T03:12:19Z- AHRS_M2 msg: 0.39 phi; -0.01 theta; 2.89 psi 2024-11-06T03:12:19Z- handleDepth()- Depth_Keller msg: 1730862739.83 epoch sec; seqNo:3416; depth 51.79 2024-11-06T03:12:19Z- handleDVL() - DVL msg epochMillisec: 1730862739.83 2024-11-06T03:12:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:19Z- handleDvl() - RDI_Pathfinder msg: 1730862739.83 sec; seqNo:1125; #beams:0 2024-11-06T03:12:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.04 , -0.08 2024-11-06T03:12:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862739.84 epoch sec; seqNo:3416; 4061728.66 north; 594922.00 east; 36.70 lat; -121.94 lon; 51.79 depth; utmZone: 10 2024-11-06T03:12:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862736.59 2024-11-06T03:12:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:19Z- waiting for 1730862740.59, time is 1730862739.84 2024-11-06T03:12:20Z- AHRS_M2 msg: 1730862740.22 epoch sec; seqNo:3206 2024-11-06T03:12:20Z- AHRS_M2 msg: 0.39 phi; -0.00 theta; 2.93 psi 2024-11-06T03:12:20Z- handleDepth()- Depth_Keller msg: 1730862740.23 epoch sec; seqNo:3417; depth 51.79 2024-11-06T03:12:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862740.24 epoch sec; seqNo:3417; 4061728.29 north; 594922.00 east; 36.70 lat; -121.94 lon; 51.79 depth; utmZone: 10 2024-11-06T03:12:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862736.59 2024-11-06T03:12:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:20Z- waiting for 1730862740.59, time is 1730862740.24 2024-11-06T03:12:20Z- AHRS_M2 msg: 1730862740.63 epoch sec; seqNo:3207 2024-11-06T03:12:20Z- AHRS_M2 msg: 0.39 phi; 0.00 theta; 2.97 psi 2024-11-06T03:12:20Z- handleDepth()- Depth_Keller msg: 1730862740.64 epoch sec; seqNo:3418; depth 51.80 2024-11-06T03:12:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862740.66 epoch sec; seqNo:3418; 4061727.91 north; 594922.00 east; 36.70 lat; -121.94 lon; 51.80 depth; utmZone: 10 2024-11-06T03:12:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862736.59 2024-11-06T03:12:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:20Z- waiting for 1730862740.59, time is 1730862740.66 2024-11-06T03:12:20Z- LcmTrn::updateTrn() >>>> pose time 1730862740.66, meas time 1730862739.83 <<<< 2024-11-06T03:12:20Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862740.66 2024-11-06T03:12:20Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:12:20Z- TerrainNav::motionUpdate() - currEstimate = (4061728, 594922.00, 51.80) 2024-11-06T03:12:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:12:20Z- TerrainNav:: SubcloudNIS is 0.69, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:12:20Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:12:20Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:12:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:12:20Z- variances([0], [2], and psi_berg) = 0.164988 0.168355 0.000000 2024-11-06T03:12:20Z- publishEstimatesDecoder() - at 1730862740.66 sec 2024-11-06T03:12:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:12:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061760.54 594905.35 57.67 2024-11-06T03:12:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061758.48 594904.69 57.26 lat/lon: 36.70 -121.94 2024-11-06T03:12:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.17 0.97 0.00 2024-11-06T03:12:21Z- AHRS_M2 msg: 1730862741.03 epoch sec; seqNo:3208 2024-11-06T03:12:21Z- AHRS_M2 msg: 0.39 phi; 0.01 theta; 3.01 psi 2024-11-06T03:12:21Z- handleDepth()- Depth_Keller msg: 1730862741.04 epoch sec; seqNo:3419; depth 51.81 2024-11-06T03:12:21Z- handleDVL() - DVL msg epochMillisec: 1730862741.06 2024-11-06T03:12:21Z- handleDvl() - RDI_Pathfinder msg: 1730862741.06 sec; seqNo:1126; #beams:4 2024-11-06T03:12:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.85 , 38.43 , 47.71 , 45.28 2024-11-06T03:12:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.09 , -0.06 2024-11-06T03:12:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862741.09 epoch sec; seqNo:3419; 4061727.91 north; 594922.00 east; 36.70 lat; -121.94 lon; 51.81 depth; utmZone: 10 2024-11-06T03:12:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862740.66 2024-11-06T03:12:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:21Z- waiting for 1730862744.66, time is 1730862741.09 2024-11-06T03:12:21Z- AHRS_M2 msg: 1730862741.43 epoch sec; seqNo:3209 2024-11-06T03:12:21Z- AHRS_M2 msg: 0.39 phi; 0.01 theta; 3.04 psi 2024-11-06T03:12:21Z- handleDepth()- Depth_Keller msg: 1730862741.44 epoch sec; seqNo:3420; depth 51.81 2024-11-06T03:12:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862741.47 epoch sec; seqNo:3420; 4061727.53 north; 594922.01 east; 36.70 lat; -121.94 lon; 51.81 depth; utmZone: 10 2024-11-06T03:12:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862740.66 2024-11-06T03:12:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:21Z- waiting for 1730862744.66, time is 1730862741.47 2024-11-06T03:12:21Z- AHRS_M2 msg: 1730862741.84 epoch sec; seqNo:3210 2024-11-06T03:12:21Z- AHRS_M2 msg: 0.39 phi; 0.02 theta; 3.08 psi 2024-11-06T03:12:21Z- handleDepth()- Depth_Keller msg: 1730862741.84 epoch sec; seqNo:3421; depth 51.84 2024-11-06T03:12:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862741.87 epoch sec; seqNo:3421; 4061727.15 north; 594922.01 east; 36.70 lat; -121.94 lon; 51.84 depth; utmZone: 10 2024-11-06T03:12:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862740.66 2024-11-06T03:12:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:21Z- waiting for 1730862744.66, time is 1730862741.87 2024-11-06T03:12:22Z- handleDepth()- Depth_Keller msg: 1730862742.28 epoch sec; seqNo:3422; depth 51.84 2024-11-06T03:12:22Z- handleDVL() - DVL msg epochMillisec: 1730862742.28 2024-11-06T03:12:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:22Z- handleDvl() - RDI_Pathfinder msg: 1730862742.28 sec; seqNo:1127; #beams:0 2024-11-06T03:12:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.09 , -0.06 2024-11-06T03:12:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862742.29 epoch sec; seqNo:3422; 4061726.77 north; 594922.02 east; 36.70 lat; -121.94 lon; 51.84 depth; utmZone: 10 2024-11-06T03:12:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862740.66 2024-11-06T03:12:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:22Z- waiting for 1730862744.66, time is 1730862742.29 2024-11-06T03:12:22Z- AHRS_M2 msg: 1730862742.66 epoch sec; seqNo:3211 2024-11-06T03:12:22Z- AHRS_M2 msg: 0.39 phi; 0.03 theta; 3.19 psi 2024-11-06T03:12:22Z- handleDepth()- Depth_Keller msg: 1730862742.66 epoch sec; seqNo:3423; depth 51.86 2024-11-06T03:12:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862742.67 epoch sec; seqNo:3423; 4061726.39 north; 594922.02 east; 36.70 lat; -121.94 lon; 51.86 depth; utmZone: 10 2024-11-06T03:12:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862740.66 2024-11-06T03:12:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:22Z- waiting for 1730862744.66, time is 1730862742.67 2024-11-06T03:12:23Z- AHRS_M2 msg: 1730862743.05 epoch sec; seqNo:3212 2024-11-06T03:12:23Z- AHRS_M2 msg: 0.39 phi; 0.03 theta; 3.23 psi 2024-11-06T03:12:23Z- handleDepth()- Depth_Keller msg: 1730862743.06 epoch sec; seqNo:3424; depth 51.86 2024-11-06T03:12:23Z- handleDVL() - DVL msg epochMillisec: 1730862743.09 2024-11-06T03:12:23Z- handleDvl() - RDI_Pathfinder msg: 1730862743.09 sec; seqNo:1128; #beams:4 2024-11-06T03:12:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.07 , 38.21 , 48.15 , 45.06 2024-11-06T03:12:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.13 , -0.03 2024-11-06T03:12:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862743.11 epoch sec; seqNo:3424; 4061726.39 north; 594922.02 east; 36.70 lat; -121.94 lon; 51.86 depth; utmZone: 10 2024-11-06T03:12:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862740.66 2024-11-06T03:12:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:23Z- waiting for 1730862744.66, time is 1730862743.11 2024-11-06T03:12:23Z- AHRS_M2 msg: 1730862743.46 epoch sec; seqNo:3213 2024-11-06T03:12:23Z- AHRS_M2 msg: 0.39 phi; 0.04 theta; 3.26 psi 2024-11-06T03:12:23Z- handleDepth()- Depth_Keller msg: 1730862743.46 epoch sec; seqNo:3425; depth 51.88 2024-11-06T03:12:23Z- handleDVL() - DVL msg epochMillisec: 1730862743.46 2024-11-06T03:12:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:23Z- handleDvl() - RDI_Pathfinder msg: 1730862743.46 sec; seqNo:1129; #beams:0 2024-11-06T03:12:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.13 , -0.03 2024-11-06T03:12:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862743.48 epoch sec; seqNo:3425; 4061726.01 north; 594922.03 east; 36.70 lat; -121.94 lon; 51.88 depth; utmZone: 10 2024-11-06T03:12:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862740.66 2024-11-06T03:12:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:23Z- waiting for 1730862744.66, time is 1730862743.48 2024-11-06T03:12:23Z- AHRS_M2 msg: 1730862743.86 epoch sec; seqNo:3214 2024-11-06T03:12:23Z- AHRS_M2 msg: 0.40 phi; 0.04 theta; 3.30 psi 2024-11-06T03:12:23Z- handleDepth()- Depth_Keller msg: 1730862743.86 epoch sec; seqNo:3426; depth 51.89 2024-11-06T03:12:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862743.88 epoch sec; seqNo:3426; 4061725.63 north; 594922.03 east; 36.70 lat; -121.94 lon; 51.89 depth; utmZone: 10 2024-11-06T03:12:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862740.66 2024-11-06T03:12:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:24Z- waiting for 1730862744.66, time is 1730862743.88 2024-11-06T03:12:24Z- AHRS_M2 msg: 1730862744.26 epoch sec; seqNo:3215 2024-11-06T03:12:24Z- AHRS_M2 msg: 0.40 phi; 0.04 theta; 3.34 psi 2024-11-06T03:12:24Z- handleDepth()- Depth_Keller msg: 1730862744.27 epoch sec; seqNo:3427; depth 51.90 2024-11-06T03:12:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862744.28 epoch sec; seqNo:3427; 4061725.25 north; 594922.03 east; 36.70 lat; -121.94 lon; 51.90 depth; utmZone: 10 2024-11-06T03:12:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862740.66 2024-11-06T03:12:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:24Z- waiting for 1730862744.66, time is 1730862744.28 2024-11-06T03:12:24Z- AHRS_M2 msg: 1730862744.67 epoch sec; seqNo:3216 2024-11-06T03:12:24Z- AHRS_M2 msg: 0.40 phi; 0.04 theta; 3.37 psi 2024-11-06T03:12:24Z- handleDepth()- Depth_Keller msg: 1730862744.67 epoch sec; seqNo:3428; depth 51.91 2024-11-06T03:12:24Z- handleDVL() - DVL msg epochMillisec: 1730862744.68 2024-11-06T03:12:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:24Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:24Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:24Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:24Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:24Z- handleDvl() - RDI_Pathfinder msg: 1730862744.68 sec; seqNo:1130; #beams:0 2024-11-06T03:12:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.13 , -0.03 2024-11-06T03:12:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862744.69 epoch sec; seqNo:3428; 4061725.25 north; 594922.03 east; 36.70 lat; -121.94 lon; 51.91 depth; utmZone: 10 2024-11-06T03:12:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862740.66 2024-11-06T03:12:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:24Z- waiting for 1730862744.66, time is 1730862744.69 2024-11-06T03:12:24Z- LcmTrn::updateTrn() >>>> pose time 1730862744.69, meas time 1730862744.68 <<<< 2024-11-06T03:12:24Z- LcmTrn::updateTrn() equating pose 1730862744.68 and meas times 1730862744.68 2024-11-06T03:12:24Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862744.68 2024-11-06T03:12:24Z- TerrainNav::motionUpdate() - currEstimate = (4061725, 594922.03, 51.91) 2024-11-06T03:12:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:12:24Z- TerrainNav:: SubcloudNIS is 0.69, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:12:24Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:12:24Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:12:24Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:12:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:12:24Z- variances([0], [2], and psi_berg) = 0.165916 0.169057 0.000000 2024-11-06T03:12:24Z- publishEstimatesDecoder() - at 1730862744.68 sec 2024-11-06T03:12:24Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:12:24Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061757.91 594905.46 58.17 2024-11-06T03:12:24Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061755.83 594904.72 57.37 lat/lon: 36.70 -121.94 2024-11-06T03:12:24Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.17 1.24 0.00 2024-11-06T03:12:25Z- AHRS_M2 msg: 1730862745.08 epoch sec; seqNo:3217 2024-11-06T03:12:25Z- AHRS_M2 msg: 0.40 phi; 0.04 theta; 3.41 psi 2024-11-06T03:12:25Z- handleDepth()- Depth_Keller msg: 1730862745.09 epoch sec; seqNo:3429; depth 51.93 2024-11-06T03:12:25Z- handleDVL() - DVL msg epochMillisec: 1730862745.11 2024-11-06T03:12:25Z- handleDvl() - RDI_Pathfinder msg: 1730862745.11 sec; seqNo:1131; #beams:4 2024-11-06T03:12:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.72 , 37.99 , 48.81 , 45.05 2024-11-06T03:12:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.14 , 0.03 2024-11-06T03:12:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862745.12 epoch sec; seqNo:3429; 4061724.88 north; 594922.04 east; 36.70 lat; -121.94 lon; 51.93 depth; utmZone: 10 2024-11-06T03:12:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862744.68 2024-11-06T03:12:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:25Z- waiting for 1730862748.68, time is 1730862745.12 2024-11-06T03:12:25Z- AHRS_M2 msg: 1730862745.47 epoch sec; seqNo:3218 2024-11-06T03:12:25Z- AHRS_M2 msg: 0.40 phi; 0.05 theta; 3.44 psi 2024-11-06T03:12:25Z- handleDepth()- Depth_Keller msg: 1730862745.49 epoch sec; seqNo:3430; depth 51.95 2024-11-06T03:12:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862745.50 epoch sec; seqNo:3430; 4061724.48 north; 594920.82 east; 36.70 lat; -121.94 lon; 51.95 depth; utmZone: 10 2024-11-06T03:12:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862744.68 2024-11-06T03:12:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:25Z- waiting for 1730862748.68, time is 1730862745.50 2024-11-06T03:12:25Z- AHRS_M2 msg: 1730862745.88 epoch sec; seqNo:3219 2024-11-06T03:12:25Z- AHRS_M2 msg: 0.40 phi; 0.05 theta; 3.48 psi 2024-11-06T03:12:25Z- handleDepth()- Depth_Keller msg: 1730862745.89 epoch sec; seqNo:3431; depth 51.96 2024-11-06T03:12:25Z- handleDVL() - DVL msg epochMillisec: 1730862745.89 2024-11-06T03:12:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:25Z- handleDvl() - RDI_Pathfinder msg: 1730862745.89 sec; seqNo:1132; #beams:0 2024-11-06T03:12:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , 0.14 , 0.03 2024-11-06T03:12:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862745.90 epoch sec; seqNo:3431; 4061724.10 north; 594920.83 east; 36.70 lat; -121.94 lon; 51.96 depth; utmZone: 10 2024-11-06T03:12:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862744.68 2024-11-06T03:12:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:26Z- waiting for 1730862748.68, time is 1730862745.90 2024-11-06T03:12:26Z- AHRS_M2 msg: 1730862746.28 epoch sec; seqNo:3220 2024-11-06T03:12:26Z- AHRS_M2 msg: 0.39 phi; 0.05 theta; 3.51 psi 2024-11-06T03:12:26Z- handleDepth()- Depth_Keller msg: 1730862746.29 epoch sec; seqNo:3432; depth 51.97 2024-11-06T03:12:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862746.30 epoch sec; seqNo:3432; 4061724.10 north; 594920.83 east; 36.70 lat; -121.94 lon; 51.97 depth; utmZone: 10 2024-11-06T03:12:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862744.68 2024-11-06T03:12:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:26Z- waiting for 1730862748.68, time is 1730862746.30 2024-11-06T03:12:26Z- AHRS_M2 msg: 1730862746.69 epoch sec; seqNo:3221 2024-11-06T03:12:26Z- AHRS_M2 msg: 0.39 phi; 0.05 theta; 3.55 psi 2024-11-06T03:12:26Z- handleDepth()- Depth_Keller msg: 1730862746.73 epoch sec; seqNo:3433; depth 51.99 2024-11-06T03:12:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862746.77 epoch sec; seqNo:3433; 4061723.73 north; 594920.83 east; 36.70 lat; -121.94 lon; 51.99 depth; utmZone: 10 2024-11-06T03:12:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862744.68 2024-11-06T03:12:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:26Z- waiting for 1730862748.68, time is 1730862746.77 2024-11-06T03:12:27Z- AHRS_M2 msg: 1730862747.09 epoch sec; seqNo:3222 2024-11-06T03:12:27Z- AHRS_M2 msg: 0.38 phi; 0.06 theta; 3.58 psi 2024-11-06T03:12:27Z- handleDepth()- Depth_Keller msg: 1730862747.10 epoch sec; seqNo:3434; depth 52.00 2024-11-06T03:12:27Z- handleDVL() - DVL msg epochMillisec: 1730862747.13 2024-11-06T03:12:27Z- handleDvl() - RDI_Pathfinder msg: 1730862747.13 sec; seqNo:1133; #beams:4 2024-11-06T03:12:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.07 , 38.21 , 49.26 , 45.06 2024-11-06T03:12:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.07 , 0.07 2024-11-06T03:12:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862747.14 epoch sec; seqNo:3434; 4061723.35 north; 594920.83 east; 36.70 lat; -121.94 lon; 52.00 depth; utmZone: 10 2024-11-06T03:12:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862744.68 2024-11-06T03:12:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:27Z- waiting for 1730862748.68, time is 1730862747.14 2024-11-06T03:12:27Z- AHRS_M2 msg: 1730862747.50 epoch sec; seqNo:3223 2024-11-06T03:12:27Z- AHRS_M2 msg: 0.38 phi; 0.06 theta; 3.62 psi 2024-11-06T03:12:27Z- handleDepth()- Depth_Keller msg: 1730862747.50 epoch sec; seqNo:3435; depth 52.02 2024-11-06T03:12:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862747.52 epoch sec; seqNo:3435; 4061722.97 north; 594920.84 east; 36.70 lat; -121.94 lon; 52.02 depth; utmZone: 10 2024-11-06T03:12:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862744.68 2024-11-06T03:12:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:27Z- waiting for 1730862748.68, time is 1730862747.52 2024-11-06T03:12:27Z- AHRS_M2 msg: 1730862747.91 epoch sec; seqNo:3224 2024-11-06T03:12:27Z- AHRS_M2 msg: 0.39 phi; 0.06 theta; 3.66 psi 2024-11-06T03:12:27Z- handleDepth()- Depth_Keller msg: 1730862747.93 epoch sec; seqNo:3436; depth 52.03 2024-11-06T03:12:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862747.94 epoch sec; seqNo:3436; 4061722.97 north; 594920.84 east; 36.70 lat; -121.94 lon; 52.03 depth; utmZone: 10 2024-11-06T03:12:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862744.68 2024-11-06T03:12:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:28Z- waiting for 1730862748.68, time is 1730862747.94 2024-11-06T03:12:28Z- AHRS_M2 msg: 1730862748.30 epoch sec; seqNo:3225 2024-11-06T03:12:28Z- AHRS_M2 msg: 0.39 phi; 0.06 theta; 3.69 psi 2024-11-06T03:12:28Z- handleDepth()- Depth_Keller msg: 1730862748.31 epoch sec; seqNo:3437; depth 52.02 2024-11-06T03:12:28Z- handleDVL() - DVL msg epochMillisec: 1730862748.31 2024-11-06T03:12:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:28Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:28Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:28Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:28Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:28Z- handleDvl() - RDI_Pathfinder msg: 1730862748.31 sec; seqNo:1134; #beams:0 2024-11-06T03:12:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.07 , 0.07 2024-11-06T03:12:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862748.33 epoch sec; seqNo:3437; 4061722.58 north; 594919.62 east; 36.70 lat; -121.94 lon; 52.02 depth; utmZone: 10 2024-11-06T03:12:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862744.68 2024-11-06T03:12:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:28Z- waiting for 1730862748.68, time is 1730862748.33 2024-11-06T03:12:28Z- AHRS_M2 msg: 1730862748.76 epoch sec; seqNo:3226 2024-11-06T03:12:28Z- AHRS_M2 msg: 0.39 phi; 0.06 theta; 3.73 psi 2024-11-06T03:12:28Z- handleDepth()- Depth_Keller msg: 1730862748.77 epoch sec; seqNo:3438; depth 52.03 2024-11-06T03:12:28Z- handleDVL() - DVL msg epochMillisec: 1730862748.79 2024-11-06T03:12:28Z- handleDvl() - RDI_Pathfinder msg: 1730862748.79 sec; seqNo:1135; #beams:4 2024-11-06T03:12:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.62 , 38.65 , 49.48 , 45.06 2024-11-06T03:12:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.03 , 0.04 2024-11-06T03:12:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862748.81 epoch sec; seqNo:3438; 4061722.20 north; 594919.63 east; 36.70 lat; -121.94 lon; 52.03 depth; utmZone: 10 2024-11-06T03:12:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862744.68 2024-11-06T03:12:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:28Z- waiting for 1730862748.68, time is 1730862748.81 2024-11-06T03:12:28Z- LcmTrn::updateTrn() >>>> pose time 1730862748.81, meas time 1730862748.79 <<<< 2024-11-06T03:12:28Z- LcmTrn::updateTrn() equating pose 1730862748.79 and meas times 1730862748.79 2024-11-06T03:12:28Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862748.79 2024-11-06T03:12:28Z- TerrainNav::motionUpdate() - currEstimate = (4061722, 594919.63, 52.03) 2024-11-06T03:12:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:12:28Z- TerrainNav:: SubcloudNIS is 0.69, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:12:28Z- TNavPF::Projecting Measurements 2024-11-06T03:12:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:12:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:12:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:12:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:12:28Z- TNavFilter::4 good beams. 2024-11-06T03:12:28Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:12:28Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:12:28Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:12:28Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:12:28Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:12:28Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:12:28Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:12:28Z- TerrainMapOctree: (vehN, vehE ) = (4061722.20, 594919.63). 2024-11-06T03:12:28Z- TerrainMapOctree: Min Distance = 171.89. 2024-11-06T03:12:28Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:12:29Z- Forcing Subcloud Comparison 2024-11-06T03:12:29Z- Weighting particles with subcloud comparison 2024-11-06T03:12:29Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:29Z- meanExpectedMeasDiff: -2.537432 delta_rms_squared: 3.913118 alpha: 0.994915 2024-11-06T03:12:29Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:29Z- meanExpectedMeasDiff: 1.056935 delta_rms_squared: 1.458506 alpha: 0.986473 2024-11-06T03:12:29Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:29Z- meanExpectedMeasDiff: 1.046609 delta_rms_squared: 2.471917 alpha: 0.991974 2024-11-06T03:12:29Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:29Z- meanExpectedMeasDiff: -0.854749 delta_rms_squared: 2.105471 alpha: 0.990590 2024-11-06T03:12:29Z- Subcloud NIS: 0.662354 2024-11-06T03:12:29Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:12:29Z- TNavPF:: sumWeights = 0.304557 2024-11-06T03:12:29Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:12:29Z- TNavPF::First Term of Map Covariance Matrix is 1.24 2024-11-06T03:12:29Z- TNavPF::Calculated NIS Value : 0.14 numBeams normalized NIS: 0.04 2024-11-06T03:12:29Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862748.79 sec, ping # 0. 2024-11-06T03:12:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:12:29Z- variances([0], [2], and psi_berg) = 0.164000 0.172584 0.000000 2024-11-06T03:12:29Z- publishEstimatesDecoder() - at 1730862748.79 sec 2024-11-06T03:12:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:12:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061752.55 594902.22 57.70 2024-11-06T03:12:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061752.77 594902.31 57.61 lat/lon: 36.70 -121.94 2024-11-06T03:12:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.17 0.53 0.00 2024-11-06T03:12:29Z- AHRS_M2 msg: 1730862749.14 epoch sec; seqNo:3227 2024-11-06T03:12:29Z- AHRS_M2 msg: 0.39 phi; 0.07 theta; 3.77 psi 2024-11-06T03:12:29Z- handleDepth()- Depth_Keller msg: 1730862749.15 epoch sec; seqNo:3439; depth 52.04 2024-11-06T03:12:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862749.19 epoch sec; seqNo:3439; 4061721.82 north; 594919.63 east; 36.70 lat; -121.94 lon; 52.04 depth; utmZone: 10 2024-11-06T03:12:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862748.79 2024-11-06T03:12:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:29Z- waiting for 1730862752.79, time is 1730862749.19 2024-11-06T03:12:29Z- AHRS_M2 msg: 1730862749.52 epoch sec; seqNo:3228 2024-11-06T03:12:29Z- AHRS_M2 msg: 0.39 phi; 0.07 theta; 3.81 psi 2024-11-06T03:12:29Z- handleDepth()- Depth_Keller msg: 1730862749.53 epoch sec; seqNo:3440; depth 52.03 2024-11-06T03:12:29Z- handleDVL() - DVL msg epochMillisec: 1730862749.54 2024-11-06T03:12:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:29Z- handleDvl() - RDI_Pathfinder msg: 1730862749.54 sec; seqNo:1136; #beams:0 2024-11-06T03:12:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.03 , 0.04 2024-11-06T03:12:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862749.54 epoch sec; seqNo:3440; 4061721.82 north; 594919.63 east; 36.70 lat; -121.94 lon; 52.03 depth; utmZone: 10 2024-11-06T03:12:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862748.79 2024-11-06T03:12:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:29Z- waiting for 1730862752.79, time is 1730862749.54 2024-11-06T03:12:29Z- AHRS_M2 msg: 1730862749.92 epoch sec; seqNo:3229 2024-11-06T03:12:29Z- AHRS_M2 msg: 0.39 phi; 0.07 theta; 3.85 psi 2024-11-06T03:12:29Z- handleDepth()- Depth_Keller msg: 1730862749.93 epoch sec; seqNo:3441; depth 52.04 2024-11-06T03:12:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862749.94 epoch sec; seqNo:3441; 4061721.44 north; 594919.64 east; 36.70 lat; -121.94 lon; 52.04 depth; utmZone: 10 2024-11-06T03:12:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862748.79 2024-11-06T03:12:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:30Z- waiting for 1730862752.79, time is 1730862749.94 2024-11-06T03:12:30Z- AHRS_M2 msg: 1730862750.33 epoch sec; seqNo:3230 2024-11-06T03:12:30Z- AHRS_M2 msg: 0.39 phi; 0.07 theta; 3.89 psi 2024-11-06T03:12:30Z- handleDepth()- Depth_Keller msg: 1730862750.34 epoch sec; seqNo:3442; depth 52.05 2024-11-06T03:12:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862750.35 epoch sec; seqNo:3442; 4061721.06 north; 594919.64 east; 36.70 lat; -121.94 lon; 52.05 depth; utmZone: 10 2024-11-06T03:12:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862748.79 2024-11-06T03:12:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:30Z- waiting for 1730862752.79, time is 1730862750.35 2024-11-06T03:12:30Z- AHRS_M2 msg: 1730862750.76 epoch sec; seqNo:3231 2024-11-06T03:12:30Z- AHRS_M2 msg: 0.38 phi; 0.07 theta; 3.92 psi 2024-11-06T03:12:30Z- handleDepth()- Depth_Keller msg: 1730862750.77 epoch sec; seqNo:3443; depth 52.04 2024-11-06T03:12:30Z- handleDVL() - DVL msg epochMillisec: 1730862750.80 2024-11-06T03:12:30Z- handleDvl() - RDI_Pathfinder msg: 1730862750.80 sec; seqNo:1137; #beams:4 2024-11-06T03:12:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.73 , 38.43 , 50.14 , 45.28 2024-11-06T03:12:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.08 , -0.05 2024-11-06T03:12:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862750.81 epoch sec; seqNo:3443; 4061721.06 north; 594919.64 east; 36.70 lat; -121.94 lon; 52.04 depth; utmZone: 10 2024-11-06T03:12:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862748.79 2024-11-06T03:12:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:30Z- waiting for 1730862752.79, time is 1730862750.81 2024-11-06T03:12:31Z- AHRS_M2 msg: 1730862751.20 epoch sec; seqNo:3232 2024-11-06T03:12:31Z- AHRS_M2 msg: 0.38 phi; 0.07 theta; 3.96 psi 2024-11-06T03:12:31Z- handleDepth()- Depth_Keller msg: 1730862751.20 epoch sec; seqNo:3444; depth 52.02 2024-11-06T03:12:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862751.23 epoch sec; seqNo:3444; 4061720.67 north; 594918.42 east; 36.70 lat; -121.94 lon; 52.02 depth; utmZone: 10 2024-11-06T03:12:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862748.79 2024-11-06T03:12:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:31Z- waiting for 1730862752.79, time is 1730862751.23 2024-11-06T03:12:31Z- AHRS_M2 msg: 1730862751.60 epoch sec; seqNo:3233 2024-11-06T03:12:31Z- AHRS_M2 msg: 0.37 phi; 0.08 theta; 3.99 psi 2024-11-06T03:12:31Z- handleDepth()- Depth_Keller msg: 1730862751.61 epoch sec; seqNo:3445; depth 52.01 2024-11-06T03:12:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862751.63 epoch sec; seqNo:3445; 4061720.67 north; 594918.42 east; 36.70 lat; -121.94 lon; 52.01 depth; utmZone: 10 2024-11-06T03:12:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862748.79 2024-11-06T03:12:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:31Z- waiting for 1730862752.79, time is 1730862751.63 2024-11-06T03:12:32Z- AHRS_M2 msg: 1730862752.00 epoch sec; seqNo:3234 2024-11-06T03:12:32Z- AHRS_M2 msg: 0.36 phi; 0.08 theta; 4.03 psi 2024-11-06T03:12:32Z- handleDepth()- Depth_Keller msg: 1730862752.00 epoch sec; seqNo:3446; depth 51.98 2024-11-06T03:12:32Z- handleDVL() - DVL msg epochMillisec: 1730862752.01 2024-11-06T03:12:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:32Z- handleDvl() - RDI_Pathfinder msg: 1730862752.01 sec; seqNo:1138; #beams:0 2024-11-06T03:12:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.08 , -0.05 2024-11-06T03:12:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862752.01 epoch sec; seqNo:3446; 4061720.29 north; 594918.43 east; 36.70 lat; -121.94 lon; 51.98 depth; utmZone: 10 2024-11-06T03:12:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862748.79 2024-11-06T03:12:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:32Z- waiting for 1730862752.79, time is 1730862752.01 2024-11-06T03:12:32Z- AHRS_M2 msg: 1730862752.40 epoch sec; seqNo:3235 2024-11-06T03:12:32Z- AHRS_M2 msg: 0.36 phi; 0.09 theta; 4.06 psi 2024-11-06T03:12:32Z- handleDepth()- Depth_Keller msg: 1730862752.41 epoch sec; seqNo:3447; depth 51.97 2024-11-06T03:12:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862752.42 epoch sec; seqNo:3447; 4061720.29 north; 594918.43 east; 36.70 lat; -121.94 lon; 51.97 depth; utmZone: 10 2024-11-06T03:12:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862748.79 2024-11-06T03:12:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:32Z- waiting for 1730862752.79, time is 1730862752.42 2024-11-06T03:12:32Z- AHRS_M2 msg: 1730862752.81 epoch sec; seqNo:3236 2024-11-06T03:12:32Z- AHRS_M2 msg: 0.35 phi; 0.09 theta; 4.08 psi 2024-11-06T03:12:32Z- handleDepth()- Depth_Keller msg: 1730862752.82 epoch sec; seqNo:3448; depth 51.95 2024-11-06T03:12:32Z- handleDVL() - DVL msg epochMillisec: 1730862752.85 2024-11-06T03:12:32Z- handleDvl() - RDI_Pathfinder msg: 1730862752.85 sec; seqNo:1139; #beams:4 2024-11-06T03:12:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.18 , 39.01 , 49.71 , 44.96 2024-11-06T03:12:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.04 , -0.11 2024-11-06T03:12:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862752.88 epoch sec; seqNo:3448; 4061719.91 north; 594918.43 east; 36.70 lat; -121.94 lon; 51.95 depth; utmZone: 10 2024-11-06T03:12:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862748.79 2024-11-06T03:12:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:32Z- waiting for 1730862752.79, time is 1730862752.88 2024-11-06T03:12:32Z- LcmTrn::updateTrn() >>>> pose time 1730862752.88, meas time 1730862752.85 <<<< 2024-11-06T03:12:32Z- LcmTrn::updateTrn() equating pose 1730862752.85 and meas times 1730862752.85 2024-11-06T03:12:32Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862752.85 2024-11-06T03:12:32Z- TerrainNav::motionUpdate() - currEstimate = (4061720, 594918.43, 51.95) 2024-11-06T03:12:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:12:32Z- TerrainNav:: SubcloudNIS is 0.66, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:12:32Z- TNavPF::Projecting Measurements 2024-11-06T03:12:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:12:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:12:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:12:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:12:32Z- TNavFilter::4 good beams. 2024-11-06T03:12:32Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:12:32Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:12:32Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:12:32Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:12:32Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:12:32Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:12:32Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:12:32Z- TerrainMapOctree: (vehN, vehE ) = (4061719.91, 594918.43). 2024-11-06T03:12:32Z- TerrainMapOctree: Min Distance = 171.10. 2024-11-06T03:12:32Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:12:33Z- Forcing Subcloud Comparison 2024-11-06T03:12:33Z- Weighting particles with subcloud comparison 2024-11-06T03:12:33Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:33Z- meanExpectedMeasDiff: 0.740482 delta_rms_squared: 1.862500 alpha: 0.989376 2024-11-06T03:12:33Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:33Z- meanExpectedMeasDiff: 2.152555 delta_rms_squared: 0.753763 alpha: 0.974152 2024-11-06T03:12:33Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:33Z- meanExpectedMeasDiff: 2.379616 delta_rms_squared: 1.234695 alpha: 0.984060 2024-11-06T03:12:33Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:33Z- meanExpectedMeasDiff: 0.471769 delta_rms_squared: 1.038790 alpha: 0.981111 2024-11-06T03:12:33Z- Subcloud NIS: 0.636467 2024-11-06T03:12:33Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:12:33Z- TNavPF:: sumWeights = 0.348599 2024-11-06T03:12:33Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:12:33Z- TNavPF::First Term of Map Covariance Matrix is 0.35 2024-11-06T03:12:33Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T03:12:33Z- TerrainNav::Resampling particle filter... 2024-11-06T03:12:33Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862752.85 sec, ping # 0. 2024-11-06T03:12:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:12:33Z- variances([0], [2], and psi_berg) = 0.167830 0.171081 0.000000 2024-11-06T03:12:33Z- publishEstimatesDecoder() - at 1730862752.85 sec 2024-11-06T03:12:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:12:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061752.60 594901.85 57.99 2024-11-06T03:12:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061750.49 594901.12 57.00 lat/lon: 36.70 -121.94 2024-11-06T03:12:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.17 0.41 0.00 2024-11-06T03:12:33Z- AHRS_M2 msg: 1730862753.21 epoch sec; seqNo:3237 2024-11-06T03:12:33Z- AHRS_M2 msg: 0.34 phi; 0.10 theta; 4.11 psi 2024-11-06T03:12:33Z- handleDepth()- Depth_Keller msg: 1730862753.22 epoch sec; seqNo:3449; depth 51.92 2024-11-06T03:12:33Z- handleDVL() - DVL msg epochMillisec: 1730862753.23 2024-11-06T03:12:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:33Z- handleDvl() - RDI_Pathfinder msg: 1730862753.23 sec; seqNo:1140; #beams:0 2024-11-06T03:12:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.04 , -0.11 2024-11-06T03:12:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862753.29 epoch sec; seqNo:3449; 4061719.90 north; 594917.21 east; 36.70 lat; -121.94 lon; 51.92 depth; utmZone: 10 2024-11-06T03:12:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862752.85 2024-11-06T03:12:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:33Z- waiting for 1730862756.85, time is 1730862753.29 2024-11-06T03:12:33Z- AHRS_M2 msg: 1730862753.62 epoch sec; seqNo:3238 2024-11-06T03:12:33Z- AHRS_M2 msg: 0.33 phi; 0.11 theta; 4.14 psi 2024-11-06T03:12:33Z- handleDepth()- Depth_Keller msg: 1730862753.62 epoch sec; seqNo:3450; depth 51.90 2024-11-06T03:12:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862753.64 epoch sec; seqNo:3450; 4061719.52 north; 594917.22 east; 36.70 lat; -121.94 lon; 51.90 depth; utmZone: 10 2024-11-06T03:12:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862752.85 2024-11-06T03:12:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:33Z- waiting for 1730862756.85, time is 1730862753.64 2024-11-06T03:12:34Z- AHRS_M2 msg: 1730862754.02 epoch sec; seqNo:3239 2024-11-06T03:12:34Z- AHRS_M2 msg: 0.32 phi; 0.12 theta; 4.16 psi 2024-11-06T03:12:34Z- handleDepth()- Depth_Keller msg: 1730862754.03 epoch sec; seqNo:3451; depth 51.89 2024-11-06T03:12:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862754.04 epoch sec; seqNo:3451; 4061719.52 north; 594917.22 east; 36.70 lat; -121.94 lon; 51.89 depth; utmZone: 10 2024-11-06T03:12:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862752.85 2024-11-06T03:12:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:34Z- waiting for 1730862756.85, time is 1730862754.04 2024-11-06T03:12:34Z- AHRS_M2 msg: 1730862754.42 epoch sec; seqNo:3240 2024-11-06T03:12:34Z- AHRS_M2 msg: 0.31 phi; 0.13 theta; 4.18 psi 2024-11-06T03:12:34Z- handleDepth()- Depth_Keller msg: 1730862754.43 epoch sec; seqNo:3452; depth 51.85 2024-11-06T03:12:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862754.44 epoch sec; seqNo:3452; 4061719.14 north; 594917.22 east; 36.70 lat; -121.94 lon; 51.85 depth; utmZone: 10 2024-11-06T03:12:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862752.85 2024-11-06T03:12:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:34Z- waiting for 1730862756.85, time is 1730862754.44 2024-11-06T03:12:34Z- AHRS_M2 msg: 1730862754.83 epoch sec; seqNo:3241 2024-11-06T03:12:34Z- AHRS_M2 msg: 0.30 phi; 0.14 theta; 4.20 psi 2024-11-06T03:12:34Z- handleDepth()- Depth_Keller msg: 1730862754.83 epoch sec; seqNo:3453; depth 51.81 2024-11-06T03:12:34Z- handleDVL() - DVL msg epochMillisec: 1730862754.86 2024-11-06T03:12:34Z- handleDvl() - RDI_Pathfinder msg: 1730862754.86 sec; seqNo:1141; #beams:4 2024-11-06T03:12:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 57.42 , 38.87 , 49.91 , 43.51 2024-11-06T03:12:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.04 , -0.13 2024-11-06T03:12:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862754.88 epoch sec; seqNo:3453; 4061719.14 north; 594917.22 east; 36.70 lat; -121.94 lon; 51.81 depth; utmZone: 10 2024-11-06T03:12:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862752.85 2024-11-06T03:12:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:34Z- waiting for 1730862756.85, time is 1730862754.88 2024-11-06T03:12:35Z- AHRS_M2 msg: 1730862755.23 epoch sec; seqNo:3242 2024-11-06T03:12:35Z- AHRS_M2 msg: 0.29 phi; 0.15 theta; 4.21 psi 2024-11-06T03:12:35Z- handleDepth()- Depth_Keller msg: 1730862755.23 epoch sec; seqNo:3454; depth 51.77 2024-11-06T03:12:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862755.29 epoch sec; seqNo:3454; 4061719.13 north; 594916.00 east; 36.70 lat; -121.94 lon; 51.77 depth; utmZone: 10 2024-11-06T03:12:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862752.85 2024-11-06T03:12:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:35Z- waiting for 1730862756.85, time is 1730862755.29 2024-11-06T03:12:35Z- AHRS_M2 msg: 1730862755.68 epoch sec; seqNo:3243 2024-11-06T03:12:35Z- AHRS_M2 msg: 0.26 phi; 0.17 theta; 4.23 psi 2024-11-06T03:12:35Z- handleDepth()- Depth_Keller msg: 1730862755.69 epoch sec; seqNo:3455; depth 51.75 2024-11-06T03:12:35Z- handleDVL() - DVL msg epochMillisec: 1730862755.69 2024-11-06T03:12:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:35Z- handleDvl() - RDI_Pathfinder msg: 1730862755.69 sec; seqNo:1142; #beams:0 2024-11-06T03:12:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.04 , -0.13 2024-11-06T03:12:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862755.70 epoch sec; seqNo:3455; 4061718.75 north; 594916.00 east; 36.70 lat; -121.94 lon; 51.75 depth; utmZone: 10 2024-11-06T03:12:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862752.85 2024-11-06T03:12:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:35Z- waiting for 1730862756.85, time is 1730862755.70 2024-11-06T03:12:36Z- AHRS_M2 msg: 1730862756.09 epoch sec; seqNo:3244 2024-11-06T03:12:36Z- AHRS_M2 msg: 0.25 phi; 0.18 theta; 4.24 psi 2024-11-06T03:12:36Z- handleDepth()- Depth_Keller msg: 1730862756.10 epoch sec; seqNo:3456; depth 51.69 2024-11-06T03:12:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862756.11 epoch sec; seqNo:3456; 4061718.75 north; 594916.00 east; 36.70 lat; -121.94 lon; 51.69 depth; utmZone: 10 2024-11-06T03:12:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862752.85 2024-11-06T03:12:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:36Z- waiting for 1730862756.85, time is 1730862756.11 2024-11-06T03:12:36Z- AHRS_M2 msg: 1730862756.47 epoch sec; seqNo:3245 2024-11-06T03:12:36Z- AHRS_M2 msg: 0.23 phi; 0.19 theta; 4.25 psi 2024-11-06T03:12:36Z- handleDepth()- Depth_Keller msg: 1730862756.48 epoch sec; seqNo:3457; depth 51.65 2024-11-06T03:12:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862756.49 epoch sec; seqNo:3457; 4061718.75 north; 594916.00 east; 36.70 lat; -121.94 lon; 51.65 depth; utmZone: 10 2024-11-06T03:12:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862752.85 2024-11-06T03:12:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:36Z- waiting for 1730862756.85, time is 1730862756.49 2024-11-06T03:12:36Z- AHRS_M2 msg: 1730862756.88 epoch sec; seqNo:3246 2024-11-06T03:12:36Z- AHRS_M2 msg: 0.22 phi; 0.20 theta; 4.25 psi 2024-11-06T03:12:36Z- handleDepth()- Depth_Keller msg: 1730862756.90 epoch sec; seqNo:3458; depth 51.62 2024-11-06T03:12:36Z- handleDVL() - DVL msg epochMillisec: 1730862756.93 2024-11-06T03:12:36Z- handleDvl() - RDI_Pathfinder msg: 1730862756.93 sec; seqNo:1143; #beams:4 2024-11-06T03:12:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 54.12 , 39.32 , 50.36 , 41.75 2024-11-06T03:12:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.08 , -0.11 2024-11-06T03:12:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862756.94 epoch sec; seqNo:3458; 4061718.35 north; 594914.79 east; 36.70 lat; -121.94 lon; 51.62 depth; utmZone: 10 2024-11-06T03:12:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862752.85 2024-11-06T03:12:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:37Z- waiting for 1730862756.85, time is 1730862756.94 2024-11-06T03:12:37Z- LcmTrn::updateTrn() >>>> pose time 1730862756.94, meas time 1730862756.93 <<<< 2024-11-06T03:12:37Z- LcmTrn::updateTrn() equating pose 1730862756.93 and meas times 1730862756.93 2024-11-06T03:12:37Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862756.93 2024-11-06T03:12:37Z- TerrainNav::motionUpdate() - currEstimate = (4061718, 594914.79, 51.62) 2024-11-06T03:12:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:12:37Z- TerrainNav:: SubcloudNIS is 0.64, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:12:37Z- TNavPF::Projecting Measurements 2024-11-06T03:12:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:12:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:12:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:12:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:12:37Z- TNavFilter::4 good beams. 2024-11-06T03:12:37Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:12:37Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:12:37Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:12:37Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:12:37Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:12:37Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:12:37Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:12:37Z- TerrainMapOctree: (vehN, vehE ) = (4061718.35, 594914.79). 2024-11-06T03:12:37Z- TerrainMapOctree: Min Distance = 167.80. 2024-11-06T03:12:37Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:12:37Z- Forcing Subcloud Comparison 2024-11-06T03:12:37Z- Weighting particles with subcloud comparison 2024-11-06T03:12:37Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:37Z- meanExpectedMeasDiff: 1.557194 delta_rms_squared: 1.263645 alpha: 0.984419 2024-11-06T03:12:37Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:37Z- meanExpectedMeasDiff: 1.816681 delta_rms_squared: 0.748125 alpha: 0.973963 2024-11-06T03:12:37Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:37Z- meanExpectedMeasDiff: -0.467807 delta_rms_squared: 1.236894 alpha: 0.984088 2024-11-06T03:12:37Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:37Z- meanExpectedMeasDiff: -0.257992 delta_rms_squared: 0.756486 alpha: 0.974243 2024-11-06T03:12:37Z- Subcloud NIS: 0.594347 2024-11-06T03:12:37Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:12:37Z- TNavPF:: sumWeights = 0.449250 2024-11-06T03:12:37Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:12:37Z- TNavPF::First Term of Map Covariance Matrix is 0.53 2024-11-06T03:12:37Z- TNavPF::Calculated NIS Value : 0.25 numBeams normalized NIS: 0.06 2024-11-06T03:12:37Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862756.93 sec, ping # 0. 2024-11-06T03:12:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:12:37Z- variances([0], [2], and psi_berg) = 0.172923 0.168812 0.000000 2024-11-06T03:12:37Z- publishEstimatesDecoder() - at 1730862756.93 sec 2024-11-06T03:12:37Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:12:37Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061751.11 594898.63 56.77 2024-11-06T03:12:37Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061748.94 594897.47 56.43 lat/lon: 36.70 -121.94 2024-11-06T03:12:37Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.17 0.37 0.00 2024-11-06T03:12:37Z- AHRS_M2 msg: 1730862757.32 epoch sec; seqNo:3247 2024-11-06T03:12:37Z- AHRS_M2 msg: 0.20 phi; 0.21 theta; 4.26 psi 2024-11-06T03:12:37Z- handleDepth()- Depth_Keller msg: 1730862757.33 epoch sec; seqNo:3459; depth 51.57 2024-11-06T03:12:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862757.38 epoch sec; seqNo:3459; 4061718.35 north; 594914.79 east; 36.70 lat; -121.94 lon; 51.57 depth; utmZone: 10 2024-11-06T03:12:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862756.93 2024-11-06T03:12:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:37Z- waiting for 1730862760.93, time is 1730862757.38 2024-11-06T03:12:37Z- AHRS_M2 msg: 1730862757.71 epoch sec; seqNo:3248 2024-11-06T03:12:37Z- AHRS_M2 msg: 0.19 phi; 0.22 theta; 4.26 psi 2024-11-06T03:12:37Z- handleDepth()- Depth_Keller msg: 1730862757.72 epoch sec; seqNo:3460; depth 51.53 2024-11-06T03:12:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862757.74 epoch sec; seqNo:3460; 4061718.35 north; 594914.79 east; 36.70 lat; -121.94 lon; 51.53 depth; utmZone: 10 2024-11-06T03:12:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862756.93 2024-11-06T03:12:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:37Z- waiting for 1730862760.93, time is 1730862757.74 2024-11-06T03:12:38Z- AHRS_M2 msg: 1730862758.10 epoch sec; seqNo:3249 2024-11-06T03:12:38Z- AHRS_M2 msg: 0.17 phi; 0.23 theta; 4.26 psi 2024-11-06T03:12:38Z- handleDepth()- Depth_Keller msg: 1730862758.11 epoch sec; seqNo:3461; depth 51.48 2024-11-06T03:12:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862758.12 epoch sec; seqNo:3461; 4061718.35 north; 594914.79 east; 36.70 lat; -121.94 lon; 51.48 depth; utmZone: 10 2024-11-06T03:12:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862756.93 2024-11-06T03:12:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:38Z- waiting for 1730862760.93, time is 1730862758.12 2024-11-06T03:12:38Z- AHRS_M2 msg: 1730862758.51 epoch sec; seqNo:3250 2024-11-06T03:12:38Z- AHRS_M2 msg: 0.16 phi; 0.24 theta; 4.25 psi 2024-11-06T03:12:38Z- handleDepth()- Depth_Keller msg: 1730862758.53 epoch sec; seqNo:3462; depth 51.44 2024-11-06T03:12:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862758.54 epoch sec; seqNo:3462; 4061717.96 north; 594913.57 east; 36.70 lat; -121.94 lon; 51.44 depth; utmZone: 10 2024-11-06T03:12:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862756.93 2024-11-06T03:12:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:38Z- waiting for 1730862760.93, time is 1730862758.54 2024-11-06T03:12:38Z- AHRS_M2 msg: 1730862758.91 epoch sec; seqNo:3251 2024-11-06T03:12:38Z- AHRS_M2 msg: 0.15 phi; 0.24 theta; 4.25 psi 2024-11-06T03:12:38Z- handleDepth()- Depth_Keller msg: 1730862758.91 epoch sec; seqNo:3463; depth 51.39 2024-11-06T03:12:38Z- handleDVL() - DVL msg epochMillisec: 1730862758.94 2024-11-06T03:12:38Z- handleDvl() - RDI_Pathfinder msg: 1730862758.94 sec; seqNo:1144; #beams:4 2024-11-06T03:12:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 51.69 , 40.86 , 51.91 , 40.64 2024-11-06T03:12:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.11 , -0.06 2024-11-06T03:12:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862758.95 epoch sec; seqNo:3463; 4061717.96 north; 594913.57 east; 36.70 lat; -121.94 lon; 51.39 depth; utmZone: 10 2024-11-06T03:12:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862756.93 2024-11-06T03:12:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:39Z- waiting for 1730862760.93, time is 1730862758.95 2024-11-06T03:12:39Z- AHRS_M2 msg: 1730862759.32 epoch sec; seqNo:3252 2024-11-06T03:12:39Z- AHRS_M2 msg: 0.14 phi; 0.25 theta; 4.24 psi 2024-11-06T03:12:39Z- handleDepth()- Depth_Keller msg: 1730862759.35 epoch sec; seqNo:3464; depth 51.34 2024-11-06T03:12:39Z- handleDVL() - DVL msg epochMillisec: 1730862759.35 2024-11-06T03:12:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:39Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:39Z- handleDvl() - RDI_Pathfinder msg: 1730862759.35 sec; seqNo:1145; #beams:0 2024-11-06T03:12:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.11 , -0.06 2024-11-06T03:12:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862759.36 epoch sec; seqNo:3464; 4061717.96 north; 594913.57 east; 36.70 lat; -121.94 lon; 51.34 depth; utmZone: 10 2024-11-06T03:12:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862756.93 2024-11-06T03:12:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:39Z- waiting for 1730862760.93, time is 1730862759.36 2024-11-06T03:12:39Z- AHRS_M2 msg: 1730862759.72 epoch sec; seqNo:3253 2024-11-06T03:12:39Z- AHRS_M2 msg: 0.13 phi; 0.25 theta; 4.24 psi 2024-11-06T03:12:39Z- handleDepth()- Depth_Keller msg: 1730862759.73 epoch sec; seqNo:3465; depth 51.30 2024-11-06T03:12:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862759.74 epoch sec; seqNo:3465; 4061717.57 north; 594912.36 east; 36.70 lat; -121.94 lon; 51.30 depth; utmZone: 10 2024-11-06T03:12:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862756.93 2024-11-06T03:12:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:39Z- waiting for 1730862760.93, time is 1730862759.74 2024-11-06T03:12:40Z- AHRS_M2 msg: 1730862760.12 epoch sec; seqNo:3254 2024-11-06T03:12:40Z- AHRS_M2 msg: 0.12 phi; 0.26 theta; 4.23 psi 2024-11-06T03:12:40Z- handleDepth()- Depth_Keller msg: 1730862760.13 epoch sec; seqNo:3466; depth 51.25 2024-11-06T03:12:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862760.14 epoch sec; seqNo:3466; 4061717.57 north; 594912.36 east; 36.70 lat; -121.94 lon; 51.25 depth; utmZone: 10 2024-11-06T03:12:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862756.93 2024-11-06T03:12:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:40Z- waiting for 1730862760.93, time is 1730862760.14 2024-11-06T03:12:40Z- AHRS_M2 msg: 1730862760.53 epoch sec; seqNo:3255 2024-11-06T03:12:40Z- AHRS_M2 msg: 0.11 phi; 0.26 theta; 4.22 psi 2024-11-06T03:12:40Z- handleDepth()- Depth_Keller msg: 1730862760.53 epoch sec; seqNo:3467; depth 51.19 2024-11-06T03:12:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862760.54 epoch sec; seqNo:3467; 4061717.57 north; 594912.36 east; 36.70 lat; -121.94 lon; 51.19 depth; utmZone: 10 2024-11-06T03:12:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862756.93 2024-11-06T03:12:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:40Z- waiting for 1730862760.93, time is 1730862760.54 2024-11-06T03:12:40Z- AHRS_M2 msg: 1730862760.93 epoch sec; seqNo:3256 2024-11-06T03:12:40Z- AHRS_M2 msg: 0.10 phi; 0.27 theta; 4.21 psi 2024-11-06T03:12:40Z- handleDepth()- Depth_Keller msg: 1730862760.94 epoch sec; seqNo:3468; depth 51.13 2024-11-06T03:12:40Z- handleDVL() - DVL msg epochMillisec: 1730862760.96 2024-11-06T03:12:40Z- handleDvl() - RDI_Pathfinder msg: 1730862760.96 sec; seqNo:1146; #beams:4 2024-11-06T03:12:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.69 , 42.40 , 53.01 , 40.20 2024-11-06T03:12:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.09 , 0.01 2024-11-06T03:12:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862760.98 epoch sec; seqNo:3468; 4061717.18 north; 594911.14 east; 36.70 lat; -121.94 lon; 51.13 depth; utmZone: 10 2024-11-06T03:12:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862756.93 2024-11-06T03:12:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:41Z- waiting for 1730862760.93, time is 1730862760.98 2024-11-06T03:12:41Z- LcmTrn::updateTrn() >>>> pose time 1730862760.98, meas time 1730862760.96 <<<< 2024-11-06T03:12:41Z- LcmTrn::updateTrn() equating pose 1730862760.96 and meas times 1730862760.96 2024-11-06T03:12:41Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862760.96 2024-11-06T03:12:41Z- TerrainNav::motionUpdate() - currEstimate = (4061717, 594911.14, 51.13) 2024-11-06T03:12:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:12:41Z- TerrainNav:: SubcloudNIS is 0.59, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:12:41Z- TNavPF::Projecting Measurements 2024-11-06T03:12:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:12:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:12:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:12:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:12:41Z- TNavFilter::4 good beams. 2024-11-06T03:12:41Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:12:41Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:12:41Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:12:41Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:12:41Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:12:41Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:12:41Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:12:41Z- TerrainMapOctree: (vehN, vehE ) = (4061717.18, 594911.14). 2024-11-06T03:12:41Z- TerrainMapOctree: Min Distance = 164.45. 2024-11-06T03:12:41Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:12:41Z- Forcing Subcloud Comparison 2024-11-06T03:12:41Z- Weighting particles with subcloud comparison 2024-11-06T03:12:41Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:41Z- meanExpectedMeasDiff: 0.278432 delta_rms_squared: 0.982167 alpha: 0.980043 2024-11-06T03:12:41Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:41Z- meanExpectedMeasDiff: 1.705190 delta_rms_squared: 0.780810 alpha: 0.975025 2024-11-06T03:12:41Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:41Z- meanExpectedMeasDiff: -1.031525 delta_rms_squared: 1.239478 alpha: 0.984120 2024-11-06T03:12:41Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:41Z- meanExpectedMeasDiff: -0.006546 delta_rms_squared: 0.660849 alpha: 0.970625 2024-11-06T03:12:41Z- Subcloud NIS: 0.537924 2024-11-06T03:12:41Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:12:41Z- TNavPF:: sumWeights = 0.522786 2024-11-06T03:12:41Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:12:41Z- TNavPF::First Term of Map Covariance Matrix is 0.31 2024-11-06T03:12:41Z- TNavPF::Calculated NIS Value : 0.29 numBeams normalized NIS: 0.07 2024-11-06T03:12:41Z- TerrainNav::Resampling particle filter... 2024-11-06T03:12:41Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862760.96 sec, ping # 0. 2024-11-06T03:12:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:12:41Z- variances([0], [2], and psi_berg) = 0.175092 0.167852 0.000000 2024-11-06T03:12:41Z- publishEstimatesDecoder() - at 1730862760.96 sec 2024-11-06T03:12:41Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:12:41Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061749.89 594894.59 55.98 2024-11-06T03:12:41Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061747.76 594893.82 55.85 lat/lon: 36.70 -121.94 2024-11-06T03:12:41Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.17 0.36 0.00 2024-11-06T03:12:41Z- AHRS_M2 msg: 1730862761.34 epoch sec; seqNo:3257 2024-11-06T03:12:41Z- AHRS_M2 msg: 0.09 phi; 0.27 theta; 4.21 psi 2024-11-06T03:12:41Z- handleDepth()- Depth_Keller msg: 1730862761.35 epoch sec; seqNo:3469; depth 51.07 2024-11-06T03:12:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862761.39 epoch sec; seqNo:3469; 4061717.18 north; 594911.14 east; 36.70 lat; -121.94 lon; 51.07 depth; utmZone: 10 2024-11-06T03:12:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862760.96 2024-11-06T03:12:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:41Z- waiting for 1730862764.96, time is 1730862761.39 2024-11-06T03:12:41Z- AHRS_M2 msg: 1730862761.75 epoch sec; seqNo:3258 2024-11-06T03:12:41Z- AHRS_M2 msg: 0.09 phi; 0.27 theta; 4.20 psi 2024-11-06T03:12:41Z- handleDepth()- Depth_Keller msg: 1730862761.75 epoch sec; seqNo:3470; depth 51.01 2024-11-06T03:12:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862761.76 epoch sec; seqNo:3470; 4061716.80 north; 594911.14 east; 36.70 lat; -121.94 lon; 51.01 depth; utmZone: 10 2024-11-06T03:12:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862760.96 2024-11-06T03:12:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:41Z- waiting for 1730862764.96, time is 1730862761.76 2024-11-06T03:12:42Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:12:42Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:12:42Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:12:42Z- AHRS_M2 msg: 1730862762.14 epoch sec; seqNo:3259 2024-11-06T03:12:42Z- AHRS_M2 msg: 0.09 phi; 0.27 theta; 4.20 psi 2024-11-06T03:12:42Z- handleDepth()- Depth_Keller msg: 1730862762.17 epoch sec; seqNo:3471; depth 50.96 2024-11-06T03:12:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862762.18 epoch sec; seqNo:3471; 4061716.80 north; 594911.14 east; 36.70 lat; -121.94 lon; 50.96 depth; utmZone: 10 2024-11-06T03:12:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862760.96 2024-11-06T03:12:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:42Z- waiting for 1730862764.96, time is 1730862762.18 2024-11-06T03:12:42Z- AHRS_M2 msg: 1730862762.55 epoch sec; seqNo:3260 2024-11-06T03:12:42Z- AHRS_M2 msg: 0.08 phi; 0.28 theta; 4.19 psi 2024-11-06T03:12:42Z- handleDepth()- Depth_Keller msg: 1730862762.58 epoch sec; seqNo:3472; depth 50.91 2024-11-06T03:12:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862762.58 epoch sec; seqNo:3472; 4061716.78 north; 594909.92 east; 36.70 lat; -121.94 lon; 50.91 depth; utmZone: 10 2024-11-06T03:12:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862760.96 2024-11-06T03:12:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:42Z- waiting for 1730862764.96, time is 1730862762.58 2024-11-06T03:12:42Z- AHRS_M2 msg: 1730862762.97 epoch sec; seqNo:3261 2024-11-06T03:12:42Z- AHRS_M2 msg: 0.07 phi; 0.28 theta; 4.18 psi 2024-11-06T03:12:42Z- handleDepth()- Depth_Keller msg: 1730862762.98 epoch sec; seqNo:3473; depth 50.82 2024-11-06T03:12:43Z- handleDVL() - DVL msg epochMillisec: 1730862763.01 2024-11-06T03:12:43Z- handleDvl() - RDI_Pathfinder msg: 1730862763.01 sec; seqNo:1147; #beams:4 2024-11-06T03:12:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.25 , 43.73 , 53.89 , 40.42 2024-11-06T03:12:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , 0.05 2024-11-06T03:12:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862763.03 epoch sec; seqNo:3473; 4061716.41 north; 594909.93 east; 36.70 lat; -121.94 lon; 50.82 depth; utmZone: 10 2024-11-06T03:12:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862760.96 2024-11-06T03:12:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:43Z- waiting for 1730862764.96, time is 1730862763.03 2024-11-06T03:12:43Z- AHRS_M2 msg: 1730862763.36 epoch sec; seqNo:3262 2024-11-06T03:12:43Z- AHRS_M2 msg: 0.06 phi; 0.28 theta; 4.17 psi 2024-11-06T03:12:43Z- handleDepth()- Depth_Keller msg: 1730862763.37 epoch sec; seqNo:3474; depth 50.75 2024-11-06T03:12:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862763.40 epoch sec; seqNo:3474; 4061716.41 north; 594909.93 east; 36.70 lat; -121.94 lon; 50.75 depth; utmZone: 10 2024-11-06T03:12:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862760.96 2024-11-06T03:12:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:43Z- waiting for 1730862764.96, time is 1730862763.40 2024-11-06T03:12:43Z- AHRS_M2 msg: 1730862763.78 epoch sec; seqNo:3263 2024-11-06T03:12:43Z- AHRS_M2 msg: 0.05 phi; 0.28 theta; 4.16 psi 2024-11-06T03:12:43Z- handleDepth()- Depth_Keller msg: 1730862763.79 epoch sec; seqNo:3475; depth 50.69 2024-11-06T03:12:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862763.80 epoch sec; seqNo:3475; 4061716.03 north; 594909.93 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:12:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862760.96 2024-11-06T03:12:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:43Z- waiting for 1730862764.96, time is 1730862763.80 2024-11-06T03:12:44Z- AHRS_M2 msg: 1730862764.18 epoch sec; seqNo:3264 2024-11-06T03:12:44Z- AHRS_M2 msg: 0.04 phi; 0.28 theta; 4.15 psi 2024-11-06T03:12:44Z- handleDepth()- Depth_Keller msg: 1730862764.19 epoch sec; seqNo:3476; depth 50.61 2024-11-06T03:12:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862764.20 epoch sec; seqNo:3476; 4061716.01 north; 594908.71 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:12:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862760.96 2024-11-06T03:12:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:44Z- waiting for 1730862764.96, time is 1730862764.20 2024-11-06T03:12:44Z- AHRS_M2 msg: 1730862764.58 epoch sec; seqNo:3265 2024-11-06T03:12:44Z- AHRS_M2 msg: 0.04 phi; 0.28 theta; 4.14 psi 2024-11-06T03:12:44Z- handleDepth()- Depth_Keller msg: 1730862764.59 epoch sec; seqNo:3477; depth 50.54 2024-11-06T03:12:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862764.61 epoch sec; seqNo:3477; 4061715.63 north; 594908.72 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:12:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862760.96 2024-11-06T03:12:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:44Z- waiting for 1730862764.96, time is 1730862764.61 2024-11-06T03:12:44Z- AHRS_M2 msg: 1730862764.99 epoch sec; seqNo:3266 2024-11-06T03:12:45Z- AHRS_M2 msg: 0.03 phi; 0.28 theta; 4.14 psi 2024-11-06T03:12:45Z- handleDepth()- Depth_Keller msg: 1730862765.00 epoch sec; seqNo:3478; depth 50.45 2024-11-06T03:12:45Z- handleDVL() - DVL msg epochMillisec: 1730862765.03 2024-11-06T03:12:45Z- handleDvl() - RDI_Pathfinder msg: 1730862765.03 sec; seqNo:1148; #beams:4 2024-11-06T03:12:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.81 , 45.06 , 54.12 , 40.64 2024-11-06T03:12:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.01 2024-11-06T03:12:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862765.04 epoch sec; seqNo:3478; 4061715.63 north; 594908.72 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:12:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862760.96 2024-11-06T03:12:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:45Z- waiting for 1730862764.96, time is 1730862765.04 2024-11-06T03:12:45Z- LcmTrn::updateTrn() >>>> pose time 1730862765.04, meas time 1730862765.03 <<<< 2024-11-06T03:12:45Z- LcmTrn::updateTrn() equating pose 1730862765.03 and meas times 1730862765.03 2024-11-06T03:12:45Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862765.03 2024-11-06T03:12:45Z- TerrainNav::motionUpdate() - currEstimate = (4061716, 594908.72, 50.45) 2024-11-06T03:12:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:12:45Z- TerrainNav:: SubcloudNIS is 0.54, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:12:45Z- TNavPF::Projecting Measurements 2024-11-06T03:12:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:12:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:12:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:12:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:12:45Z- TNavFilter::4 good beams. 2024-11-06T03:12:45Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:12:45Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:12:45Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:12:45Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:12:45Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:12:45Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:12:45Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:12:45Z- TerrainMapOctree: (vehN, vehE ) = (4061715.63, 594908.72). 2024-11-06T03:12:45Z- TerrainMapOctree: Min Distance = 162.39. 2024-11-06T03:12:45Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:12:45Z- Forcing Subcloud Comparison 2024-11-06T03:12:45Z- Weighting particles with subcloud comparison 2024-11-06T03:12:45Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:45Z- meanExpectedMeasDiff: 0.440146 delta_rms_squared: 0.890809 alpha: 0.978041 2024-11-06T03:12:45Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:45Z- meanExpectedMeasDiff: 1.213188 delta_rms_squared: 1.302399 alpha: 0.984876 2024-11-06T03:12:45Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:45Z- meanExpectedMeasDiff: -1.108338 delta_rms_squared: 1.217517 alpha: 0.983839 2024-11-06T03:12:45Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:45Z- meanExpectedMeasDiff: -0.139928 delta_rms_squared: 0.630215 alpha: 0.969241 2024-11-06T03:12:45Z- Subcloud NIS: 0.491006 2024-11-06T03:12:45Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:12:45Z- TNavPF:: sumWeights = 0.587057 2024-11-06T03:12:45Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:12:45Z- TNavPF::First Term of Map Covariance Matrix is 0.33 2024-11-06T03:12:45Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.06 2024-11-06T03:12:45Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862765.03 sec, ping # 0. 2024-11-06T03:12:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:12:45Z- variances([0], [2], and psi_berg) = 0.169846 0.155803 0.000000 2024-11-06T03:12:45Z- publishEstimatesDecoder() - at 1730862765.03 sec 2024-11-06T03:12:45Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:12:45Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061747.16 594890.54 54.91 2024-11-06T03:12:45Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061746.20 594891.37 55.12 lat/lon: 36.70 -121.94 2024-11-06T03:12:45Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.16 0.35 0.00 2024-11-06T03:12:45Z- AHRS_M2 msg: 1730862765.41 epoch sec; seqNo:3267 2024-11-06T03:12:45Z- AHRS_M2 msg: 0.03 phi; 0.27 theta; 4.13 psi 2024-11-06T03:12:45Z- handleDepth()- Depth_Keller msg: 1730862765.42 epoch sec; seqNo:3479; depth 50.37 2024-11-06T03:12:45Z- handleDVL() - DVL msg epochMillisec: 1730862765.42 2024-11-06T03:12:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:45Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:45Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:45Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:45Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:45Z- handleDvl() - RDI_Pathfinder msg: 1730862765.42 sec; seqNo:1149; #beams:0 2024-11-06T03:12:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.01 2024-11-06T03:12:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862765.44 epoch sec; seqNo:3479; 4061715.26 north; 594908.72 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:12:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862765.03 2024-11-06T03:12:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:45Z- waiting for 1730862769.03, time is 1730862765.44 2024-11-06T03:12:45Z- AHRS_M2 msg: 1730862765.80 epoch sec; seqNo:3268 2024-11-06T03:12:45Z- AHRS_M2 msg: 0.03 phi; 0.27 theta; 4.12 psi 2024-11-06T03:12:45Z- handleDepth()- Depth_Keller msg: 1730862765.80 epoch sec; seqNo:3480; depth 50.27 2024-11-06T03:12:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862765.81 epoch sec; seqNo:3480; 4061715.26 north; 594908.72 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:12:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862765.03 2024-11-06T03:12:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:45Z- waiting for 1730862769.03, time is 1730862765.81 2024-11-06T03:12:46Z- AHRS_M2 msg: 1730862766.20 epoch sec; seqNo:3269 2024-11-06T03:12:46Z- AHRS_M2 msg: 0.02 phi; 0.27 theta; 4.11 psi 2024-11-06T03:12:46Z- handleDepth()- Depth_Keller msg: 1730862766.21 epoch sec; seqNo:3481; depth 50.19 2024-11-06T03:12:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862766.21 epoch sec; seqNo:3481; 4061714.86 north; 594907.50 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:12:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862765.03 2024-11-06T03:12:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:46Z- waiting for 1730862769.03, time is 1730862766.21 2024-11-06T03:12:46Z- AHRS_M2 msg: 1730862766.61 epoch sec; seqNo:3270 2024-11-06T03:12:46Z- AHRS_M2 msg: 0.02 phi; 0.26 theta; 4.10 psi 2024-11-06T03:12:46Z- handleDepth()- Depth_Keller msg: 1730862766.61 epoch sec; seqNo:3482; depth 50.10 2024-11-06T03:12:46Z- handleDVL() - DVL msg epochMillisec: 1730862766.62 2024-11-06T03:12:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:46Z- handleDvl() - RDI_Pathfinder msg: 1730862766.62 sec; seqNo:1150; #beams:0 2024-11-06T03:12:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.01 2024-11-06T03:12:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862766.63 epoch sec; seqNo:3482; 4061714.86 north; 594907.50 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:12:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862765.03 2024-11-06T03:12:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:46Z- waiting for 1730862769.03, time is 1730862766.63 2024-11-06T03:12:47Z- handleDepth()- Depth_Keller msg: 1730862767.03 epoch sec; seqNo:3483; depth 50.01 2024-11-06T03:12:47Z- handleDVL() - DVL msg epochMillisec: 1730862767.05 2024-11-06T03:12:47Z- handleDvl() - RDI_Pathfinder msg: 1730862767.05 sec; seqNo:1151; #beams:4 2024-11-06T03:12:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.37 , 45.94 , 53.67 , 41.08 2024-11-06T03:12:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.07 , -0.09 2024-11-06T03:12:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862767.06 epoch sec; seqNo:3483; 4061714.48 north; 594907.51 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:12:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862765.03 2024-11-06T03:12:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:47Z- waiting for 1730862769.03, time is 1730862767.06 2024-11-06T03:12:47Z- AHRS_M2 msg: 1730862767.45 epoch sec; seqNo:3271 2024-11-06T03:12:47Z- AHRS_M2 msg: 0.02 phi; 0.26 theta; 4.08 psi 2024-11-06T03:12:47Z- handleDepth()- Depth_Keller msg: 1730862767.46 epoch sec; seqNo:3484; depth 49.90 2024-11-06T03:12:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862767.48 epoch sec; seqNo:3484; 4061714.11 north; 594907.51 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:12:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862765.03 2024-11-06T03:12:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:47Z- waiting for 1730862769.03, time is 1730862767.48 2024-11-06T03:12:47Z- AHRS_M2 msg: 1730862767.85 epoch sec; seqNo:3272 2024-11-06T03:12:47Z- AHRS_M2 msg: 0.02 phi; 0.25 theta; 4.07 psi 2024-11-06T03:12:47Z- handleDepth()- Depth_Keller msg: 1730862767.86 epoch sec; seqNo:3485; depth 49.83 2024-11-06T03:12:47Z- handleDVL() - DVL msg epochMillisec: 1730862767.87 2024-11-06T03:12:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:47Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:47Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:47Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:47Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:47Z- handleDvl() - RDI_Pathfinder msg: 1730862767.87 sec; seqNo:1152; #beams:0 2024-11-06T03:12:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.07 , -0.09 2024-11-06T03:12:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862767.88 epoch sec; seqNo:3485; 4061714.09 north; 594906.29 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:12:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862765.03 2024-11-06T03:12:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:48Z- waiting for 1730862769.03, time is 1730862767.88 2024-11-06T03:12:48Z- AHRS_M2 msg: 1730862768.26 epoch sec; seqNo:3273 2024-11-06T03:12:48Z- AHRS_M2 msg: 0.02 phi; 0.24 theta; 4.06 psi 2024-11-06T03:12:48Z- handleDepth()- Depth_Keller msg: 1730862768.26 epoch sec; seqNo:3486; depth 49.73 2024-11-06T03:12:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862768.27 epoch sec; seqNo:3486; 4061713.71 north; 594906.30 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:12:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862765.03 2024-11-06T03:12:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:48Z- waiting for 1730862769.03, time is 1730862768.27 2024-11-06T03:12:48Z- AHRS_M2 msg: 1730862768.66 epoch sec; seqNo:3274 2024-11-06T03:12:48Z- AHRS_M2 msg: 0.02 phi; 0.24 theta; 4.05 psi 2024-11-06T03:12:48Z- handleDepth()- Depth_Keller msg: 1730862768.66 epoch sec; seqNo:3487; depth 49.63 2024-11-06T03:12:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862768.68 epoch sec; seqNo:3487; 4061713.71 north; 594906.30 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:12:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862765.03 2024-11-06T03:12:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:48Z- waiting for 1730862769.03, time is 1730862768.68 2024-11-06T03:12:49Z- AHRS_M2 msg: 1730862769.06 epoch sec; seqNo:3275 2024-11-06T03:12:49Z- AHRS_M2 msg: 0.02 phi; 0.23 theta; 4.04 psi 2024-11-06T03:12:49Z- handleDepth()- Depth_Keller msg: 1730862769.07 epoch sec; seqNo:3488; depth 49.55 2024-11-06T03:12:49Z- handleDVL() - DVL msg epochMillisec: 1730862769.11 2024-11-06T03:12:49Z- handleDvl() - RDI_Pathfinder msg: 1730862769.11 sec; seqNo:1153; #beams:4 2024-11-06T03:12:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.92 , 45.50 , 52.78 , 41.30 2024-11-06T03:12:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , -0.14 2024-11-06T03:12:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862769.12 epoch sec; seqNo:3488; 4061713.33 north; 594906.30 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:12:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862765.03 2024-11-06T03:12:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:49Z- waiting for 1730862769.03, time is 1730862769.12 2024-11-06T03:12:49Z- LcmTrn::updateTrn() >>>> pose time 1730862769.12, meas time 1730862769.11 <<<< 2024-11-06T03:12:49Z- LcmTrn::updateTrn() equating pose 1730862769.11 and meas times 1730862769.11 2024-11-06T03:12:49Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862769.11 2024-11-06T03:12:49Z- TerrainNav::motionUpdate() - currEstimate = (4061713, 594906.30, 49.55) 2024-11-06T03:12:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:12:49Z- TerrainNav:: SubcloudNIS is 0.49, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:12:49Z- TNavPF::Projecting Measurements 2024-11-06T03:12:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:12:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:12:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:12:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:12:49Z- TNavFilter::4 good beams. 2024-11-06T03:12:49Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:12:49Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:12:49Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:12:49Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:12:49Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:12:49Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:12:49Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:12:49Z- TerrainMapOctree: (vehN, vehE ) = (4061713.33, 594906.30). 2024-11-06T03:12:49Z- TerrainMapOctree: Min Distance = 160.54. 2024-11-06T03:12:49Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:12:49Z- Forcing Subcloud Comparison 2024-11-06T03:12:49Z- Weighting particles with subcloud comparison 2024-11-06T03:12:49Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:49Z- meanExpectedMeasDiff: -0.720752 delta_rms_squared: 0.862936 alpha: 0.977348 2024-11-06T03:12:49Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:49Z- meanExpectedMeasDiff: 0.424760 delta_rms_squared: 0.819330 alpha: 0.976171 2024-11-06T03:12:49Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:49Z- meanExpectedMeasDiff: -1.480035 delta_rms_squared: 1.131972 alpha: 0.982638 2024-11-06T03:12:49Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:49Z- meanExpectedMeasDiff: -0.940157 delta_rms_squared: 0.648179 alpha: 0.970068 2024-11-06T03:12:49Z- Subcloud NIS: 0.444997 2024-11-06T03:12:49Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:12:49Z- TNavPF:: sumWeights = 0.595419 2024-11-06T03:12:49Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:12:49Z- TNavPF::First Term of Map Covariance Matrix is 0.34 2024-11-06T03:12:49Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.05 2024-11-06T03:12:49Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862769.11 sec, ping # 0. 2024-11-06T03:12:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:12:49Z- variances([0], [2], and psi_berg) = 0.168245 0.158880 0.000000 2024-11-06T03:12:49Z- publishEstimatesDecoder() - at 1730862769.11 sec 2024-11-06T03:12:49Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:12:49Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061744.62 594888.28 55.16 2024-11-06T03:12:49Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061743.89 594888.95 54.44 lat/lon: 36.70 -121.94 2024-11-06T03:12:49Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.16 0.37 0.00 2024-11-06T03:12:49Z- AHRS_M2 msg: 1730862769.53 epoch sec; seqNo:3276 2024-11-06T03:12:49Z- AHRS_M2 msg: 0.03 phi; 0.22 theta; 4.02 psi 2024-11-06T03:12:49Z- handleDepth()- Depth_Keller msg: 1730862769.54 epoch sec; seqNo:3489; depth 49.45 2024-11-06T03:12:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862769.56 epoch sec; seqNo:3489; 4061713.33 north; 594906.30 east; 36.70 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:12:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862769.11 2024-11-06T03:12:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:49Z- waiting for 1730862773.11, time is 1730862769.56 2024-11-06T03:12:49Z- AHRS_M2 msg: 1730862769.90 epoch sec; seqNo:3277 2024-11-06T03:12:49Z- AHRS_M2 msg: 0.03 phi; 0.21 theta; 4.01 psi 2024-11-06T03:12:49Z- handleDepth()- Depth_Keller msg: 1730862769.91 epoch sec; seqNo:3490; depth 49.35 2024-11-06T03:12:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862769.93 epoch sec; seqNo:3490; 4061712.94 north; 594905.08 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:12:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862769.11 2024-11-06T03:12:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:50Z- waiting for 1730862773.11, time is 1730862769.93 2024-11-06T03:12:50Z- AHRS_M2 msg: 1730862770.31 epoch sec; seqNo:3278 2024-11-06T03:12:50Z- AHRS_M2 msg: 0.03 phi; 0.21 theta; 3.99 psi 2024-11-06T03:12:50Z- handleDepth()- Depth_Keller msg: 1730862770.31 epoch sec; seqNo:3491; depth 49.28 2024-11-06T03:12:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862770.32 epoch sec; seqNo:3491; 4061712.56 north; 594905.09 east; 36.70 lat; -121.94 lon; 49.28 depth; utmZone: 10 2024-11-06T03:12:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862769.11 2024-11-06T03:12:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:50Z- waiting for 1730862773.11, time is 1730862770.32 2024-11-06T03:12:50Z- AHRS_M2 msg: 1730862770.73 epoch sec; seqNo:3279 2024-11-06T03:12:50Z- AHRS_M2 msg: 0.03 phi; 0.20 theta; 3.98 psi 2024-11-06T03:12:50Z- handleDepth()- Depth_Keller msg: 1730862770.76 epoch sec; seqNo:3492; depth 49.20 2024-11-06T03:12:50Z- handleDVL() - DVL msg epochMillisec: 1730862770.78 2024-11-06T03:12:50Z- handleDvl() - RDI_Pathfinder msg: 1730862770.78 sec; seqNo:1154; #beams:4 2024-11-06T03:12:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.15 , 45.05 , 52.34 , 42.18 2024-11-06T03:12:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.08 , -0.10 2024-11-06T03:12:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862770.81 epoch sec; seqNo:3492; 4061712.56 north; 594905.09 east; 36.70 lat; -121.94 lon; 49.20 depth; utmZone: 10 2024-11-06T03:12:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862769.11 2024-11-06T03:12:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:50Z- waiting for 1730862773.11, time is 1730862770.81 2024-11-06T03:12:51Z- AHRS_M2 msg: 1730862771.11 epoch sec; seqNo:3280 2024-11-06T03:12:51Z- AHRS_M2 msg: 0.03 phi; 0.20 theta; 3.97 psi 2024-11-06T03:12:51Z- handleDepth()- Depth_Keller msg: 1730862771.12 epoch sec; seqNo:3493; depth 49.10 2024-11-06T03:12:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862771.13 epoch sec; seqNo:3493; 4061712.18 north; 594905.09 east; 36.70 lat; -121.94 lon; 49.10 depth; utmZone: 10 2024-11-06T03:12:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862769.11 2024-11-06T03:12:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:51Z- waiting for 1730862773.11, time is 1730862771.13 2024-11-06T03:12:51Z- AHRS_M2 msg: 1730862771.51 epoch sec; seqNo:3281 2024-11-06T03:12:51Z- AHRS_M2 msg: 0.04 phi; 0.19 theta; 3.96 psi 2024-11-06T03:12:51Z- handleDepth()- Depth_Keller msg: 1730862771.52 epoch sec; seqNo:3494; depth 49.05 2024-11-06T03:12:51Z- handleDVL() - DVL msg epochMillisec: 1730862771.52 2024-11-06T03:12:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:51Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:51Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:51Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:51Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:51Z- handleDvl() - RDI_Pathfinder msg: 1730862771.52 sec; seqNo:1155; #beams:0 2024-11-06T03:12:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.08 , -0.10 2024-11-06T03:12:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862771.55 epoch sec; seqNo:3494; 4061712.17 north; 594903.87 east; 36.70 lat; -121.94 lon; 49.05 depth; utmZone: 10 2024-11-06T03:12:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862769.11 2024-11-06T03:12:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:51Z- waiting for 1730862773.11, time is 1730862771.55 2024-11-06T03:12:51Z- AHRS_M2 msg: 1730862771.92 epoch sec; seqNo:3282 2024-11-06T03:12:51Z- AHRS_M2 msg: 0.04 phi; 0.18 theta; 3.95 psi 2024-11-06T03:12:51Z- handleDepth()- Depth_Keller msg: 1730862771.93 epoch sec; seqNo:3495; depth 48.97 2024-11-06T03:12:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862771.94 epoch sec; seqNo:3495; 4061711.79 north; 594903.88 east; 36.70 lat; -121.94 lon; 48.97 depth; utmZone: 10 2024-11-06T03:12:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862769.11 2024-11-06T03:12:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:52Z- waiting for 1730862773.11, time is 1730862771.94 2024-11-06T03:12:52Z- AHRS_M2 msg: 1730862772.33 epoch sec; seqNo:3283 2024-11-06T03:12:52Z- AHRS_M2 msg: 0.04 phi; 0.18 theta; 3.94 psi 2024-11-06T03:12:52Z- handleDepth()- Depth_Keller msg: 1730862772.34 epoch sec; seqNo:3496; depth 48.89 2024-11-06T03:12:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862772.35 epoch sec; seqNo:3496; 4061711.41 north; 594903.88 east; 36.70 lat; -121.94 lon; 48.89 depth; utmZone: 10 2024-11-06T03:12:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862769.11 2024-11-06T03:12:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:52Z- waiting for 1730862773.11, time is 1730862772.35 2024-11-06T03:12:52Z- handleDepth()- Depth_Keller msg: 1730862772.78 epoch sec; seqNo:3497; depth 48.83 2024-11-06T03:12:52Z- handleDVL() - DVL msg epochMillisec: 1730862772.81 2024-11-06T03:12:52Z- handleDvl() - RDI_Pathfinder msg: 1730862772.81 sec; seqNo:1156; #beams:4 2024-11-06T03:12:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.15 , 45.05 , 51.24 , 42.62 2024-11-06T03:12:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.13 , -0.03 2024-11-06T03:12:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862772.82 epoch sec; seqNo:3497; 4061711.41 north; 594903.88 east; 36.70 lat; -121.94 lon; 48.83 depth; utmZone: 10 2024-11-06T03:12:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862769.11 2024-11-06T03:12:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:52Z- waiting for 1730862773.11, time is 1730862772.82 2024-11-06T03:12:53Z- AHRS_M2 msg: 1730862773.13 epoch sec; seqNo:3284 2024-11-06T03:12:53Z- AHRS_M2 msg: 0.04 phi; 0.17 theta; 3.91 psi 2024-11-06T03:12:53Z- handleDepth()- Depth_Keller msg: 1730862773.14 epoch sec; seqNo:3498; depth 48.76 2024-11-06T03:12:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862773.16 epoch sec; seqNo:3498; 4061711.02 north; 594902.66 east; 36.70 lat; -121.94 lon; 48.76 depth; utmZone: 10 2024-11-06T03:12:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862769.11 2024-11-06T03:12:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:53Z- waiting for 1730862773.11, time is 1730862773.16 2024-11-06T03:12:53Z- LcmTrn::updateTrn() >>>> pose time 1730862773.16, meas time 1730862772.81 <<<< 2024-11-06T03:12:53Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862773.16 2024-11-06T03:12:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862772.81 sec, ping # 0; waiting for INS data more recent than 1730862769.11... 2024-11-06T03:12:53Z- TerrainNav::motionUpdate() - currEstimate = (4061711, 594902.66, 48.76) 2024-11-06T03:12:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:12:53Z- TerrainNav:: SubcloudNIS is 0.44, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:12:53Z- TNavPF::Projecting Measurements 2024-11-06T03:12:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:12:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:12:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:12:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:12:53Z- TNavFilter::4 good beams. 2024-11-06T03:12:53Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:12:53Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:12:53Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:12:53Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:12:53Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:12:53Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:12:53Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:12:53Z- TerrainMapOctree: (vehN, vehE ) = (4061711.22, 594902.98). 2024-11-06T03:12:53Z- TerrainMapOctree: Min Distance = 157.82. 2024-11-06T03:12:53Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:12:53Z- Forcing Subcloud Comparison 2024-11-06T03:12:53Z- Weighting particles with subcloud comparison 2024-11-06T03:12:53Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:53Z- meanExpectedMeasDiff: -0.950354 delta_rms_squared: 0.869237 alpha: 0.977509 2024-11-06T03:12:53Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:53Z- meanExpectedMeasDiff: 0.306726 delta_rms_squared: 0.797257 alpha: 0.975528 2024-11-06T03:12:53Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:53Z- meanExpectedMeasDiff: -0.861583 delta_rms_squared: 1.030254 alpha: 0.980957 2024-11-06T03:12:53Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:53Z- meanExpectedMeasDiff: -1.040549 delta_rms_squared: 0.699316 alpha: 0.972196 2024-11-06T03:12:53Z- Subcloud NIS: 0.396325 2024-11-06T03:12:53Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:12:53Z- TNavPF:: sumWeights = 0.635664 2024-11-06T03:12:53Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:12:53Z- TNavPF::First Term of Map Covariance Matrix is 0.38 2024-11-06T03:12:53Z- TNavPF::Calculated NIS Value : 0.19 numBeams normalized NIS: 0.05 2024-11-06T03:12:53Z- TerrainNav::Resampling particle filter... 2024-11-06T03:12:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862772.81 sec. 2024-11-06T03:12:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:12:53Z- variances([0], [2], and psi_berg) = 0.171542 0.161962 0.000000 2024-11-06T03:12:53Z- publishEstimatesDecoder() - at 1730862773.16 sec 2024-11-06T03:12:53Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:12:53Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061743.65 594886.12 54.53 2024-11-06T03:12:53Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061741.57 594885.32 53.86 lat/lon: 36.70 -121.94 2024-11-06T03:12:53Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.16 0.63 0.00 2024-11-06T03:12:53Z- AHRS_M2 msg: 1730862773.54 epoch sec; seqNo:3285 2024-11-06T03:12:53Z- AHRS_M2 msg: 0.04 phi; 0.16 theta; 3.90 psi 2024-11-06T03:12:53Z- handleDepth()- Depth_Keller msg: 1730862773.55 epoch sec; seqNo:3499; depth 48.72 2024-11-06T03:12:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862773.57 epoch sec; seqNo:3499; 4061711.02 north; 594902.66 east; 36.70 lat; -121.94 lon; 48.72 depth; utmZone: 10 2024-11-06T03:12:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862773.16 2024-11-06T03:12:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:53Z- waiting for 1730862777.16, time is 1730862773.57 2024-11-06T03:12:53Z- AHRS_M2 msg: 1730862773.94 epoch sec; seqNo:3286 2024-11-06T03:12:53Z- AHRS_M2 msg: 0.04 phi; 0.16 theta; 3.89 psi 2024-11-06T03:12:53Z- handleDepth()- Depth_Keller msg: 1730862773.96 epoch sec; seqNo:3500; depth 48.66 2024-11-06T03:12:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862773.97 epoch sec; seqNo:3500; 4061710.64 north; 594902.67 east; 36.70 lat; -121.94 lon; 48.66 depth; utmZone: 10 2024-11-06T03:12:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862773.16 2024-11-06T03:12:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:54Z- waiting for 1730862777.16, time is 1730862773.97 2024-11-06T03:12:54Z- AHRS_M2 msg: 1730862774.34 epoch sec; seqNo:3287 2024-11-06T03:12:54Z- AHRS_M2 msg: 0.04 phi; 0.15 theta; 3.88 psi 2024-11-06T03:12:54Z- handleDepth()- Depth_Keller msg: 1730862774.35 epoch sec; seqNo:3501; depth 48.61 2024-11-06T03:12:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862774.36 epoch sec; seqNo:3501; 4061710.26 north; 594902.67 east; 36.70 lat; -121.94 lon; 48.61 depth; utmZone: 10 2024-11-06T03:12:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862773.16 2024-11-06T03:12:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:54Z- waiting for 1730862777.16, time is 1730862774.36 2024-11-06T03:12:54Z- AHRS_M2 msg: 1730862774.77 epoch sec; seqNo:3288 2024-11-06T03:12:54Z- AHRS_M2 msg: 0.04 phi; 0.14 theta; 3.86 psi 2024-11-06T03:12:54Z- handleDepth()- Depth_Keller msg: 1730862774.77 epoch sec; seqNo:3502; depth 48.56 2024-11-06T03:12:54Z- handleDVL() - DVL msg epochMillisec: 1730862774.79 2024-11-06T03:12:54Z- handleDvl() - RDI_Pathfinder msg: 1730862774.79 sec; seqNo:1157; #beams:4 2024-11-06T03:12:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.59 , 44.83 , 50.58 , 43.29 2024-11-06T03:12:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.12 , 0.02 2024-11-06T03:12:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862774.80 epoch sec; seqNo:3502; 4061709.87 north; 594901.46 east; 36.70 lat; -121.94 lon; 48.56 depth; utmZone: 10 2024-11-06T03:12:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862773.16 2024-11-06T03:12:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:54Z- waiting for 1730862777.16, time is 1730862774.80 2024-11-06T03:12:55Z- AHRS_M2 msg: 1730862775.15 epoch sec; seqNo:3289 2024-11-06T03:12:55Z- AHRS_M2 msg: 0.04 phi; 0.14 theta; 3.85 psi 2024-11-06T03:12:55Z- handleDepth()- Depth_Keller msg: 1730862775.16 epoch sec; seqNo:3503; depth 48.53 2024-11-06T03:12:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862775.19 epoch sec; seqNo:3503; 4061709.87 north; 594901.46 east; 36.70 lat; -121.94 lon; 48.53 depth; utmZone: 10 2024-11-06T03:12:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862773.16 2024-11-06T03:12:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:55Z- waiting for 1730862777.16, time is 1730862775.19 2024-11-06T03:12:55Z- AHRS_M2 msg: 1730862775.56 epoch sec; seqNo:3290 2024-11-06T03:12:55Z- AHRS_M2 msg: 0.04 phi; 0.14 theta; 3.84 psi 2024-11-06T03:12:55Z- handleDepth()- Depth_Keller msg: 1730862775.57 epoch sec; seqNo:3504; depth 48.48 2024-11-06T03:12:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862775.58 epoch sec; seqNo:3504; 4061709.49 north; 594901.46 east; 36.70 lat; -121.94 lon; 48.48 depth; utmZone: 10 2024-11-06T03:12:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862773.16 2024-11-06T03:12:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:55Z- waiting for 1730862777.16, time is 1730862775.58 2024-11-06T03:12:55Z- AHRS_M2 msg: 1730862775.96 epoch sec; seqNo:3291 2024-11-06T03:12:55Z- AHRS_M2 msg: 0.04 phi; 0.13 theta; 3.83 psi 2024-11-06T03:12:55Z- handleDepth()- Depth_Keller msg: 1730862775.97 epoch sec; seqNo:3505; depth 48.43 2024-11-06T03:12:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862775.97 epoch sec; seqNo:3505; 4061709.49 north; 594901.46 east; 36.70 lat; -121.94 lon; 48.43 depth; utmZone: 10 2024-11-06T03:12:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862773.16 2024-11-06T03:12:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:56Z- waiting for 1730862777.16, time is 1730862775.97 2024-11-06T03:12:56Z- AHRS_M2 msg: 1730862776.36 epoch sec; seqNo:3292 2024-11-06T03:12:56Z- AHRS_M2 msg: 0.04 phi; 0.13 theta; 3.82 psi 2024-11-06T03:12:56Z- handleDepth()- Depth_Keller msg: 1730862776.37 epoch sec; seqNo:3506; depth 48.41 2024-11-06T03:12:56Z- handleDVL() - DVL msg epochMillisec: 1730862776.37 2024-11-06T03:12:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:56Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:56Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:56Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:56Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:56Z- handleDvl() - RDI_Pathfinder msg: 1730862776.37 sec; seqNo:1158; #beams:0 2024-11-06T03:12:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.12 , 0.02 2024-11-06T03:12:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862776.39 epoch sec; seqNo:3506; 4061709.11 north; 594901.46 east; 36.70 lat; -121.94 lon; 48.41 depth; utmZone: 10 2024-11-06T03:12:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862773.16 2024-11-06T03:12:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:56Z- waiting for 1730862777.16, time is 1730862776.39 2024-11-06T03:12:56Z- AHRS_M2 msg: 1730862776.77 epoch sec; seqNo:3293 2024-11-06T03:12:56Z- AHRS_M2 msg: 0.04 phi; 0.12 theta; 3.81 psi 2024-11-06T03:12:56Z- handleDepth()- Depth_Keller msg: 1730862776.78 epoch sec; seqNo:3507; depth 48.36 2024-11-06T03:12:56Z- handleDVL() - DVL msg epochMillisec: 1730862776.80 2024-11-06T03:12:56Z- handleDvl() - RDI_Pathfinder msg: 1730862776.80 sec; seqNo:1159; #beams:4 2024-11-06T03:12:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.81 , 44.83 , 50.13 , 43.95 2024-11-06T03:12:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , 0.04 2024-11-06T03:12:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862776.82 epoch sec; seqNo:3507; 4061708.72 north; 594900.25 east; 36.70 lat; -121.94 lon; 48.36 depth; utmZone: 10 2024-11-06T03:12:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862773.16 2024-11-06T03:12:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:56Z- waiting for 1730862777.16, time is 1730862776.82 2024-11-06T03:12:57Z- AHRS_M2 msg: 1730862777.17 epoch sec; seqNo:3294 2024-11-06T03:12:57Z- AHRS_M2 msg: 0.04 phi; 0.12 theta; 3.80 psi 2024-11-06T03:12:57Z- handleDepth()- Depth_Keller msg: 1730862777.18 epoch sec; seqNo:3508; depth 48.33 2024-11-06T03:12:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862777.19 epoch sec; seqNo:3508; 4061708.34 north; 594900.25 east; 36.70 lat; -121.94 lon; 48.33 depth; utmZone: 10 2024-11-06T03:12:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862773.16 2024-11-06T03:12:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:57Z- waiting for 1730862777.16, time is 1730862777.19 2024-11-06T03:12:57Z- LcmTrn::updateTrn() >>>> pose time 1730862777.19, meas time 1730862776.80 <<<< 2024-11-06T03:12:57Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862777.19 2024-11-06T03:12:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862776.80 sec, ping # 0; waiting for INS data more recent than 1730862773.16... 2024-11-06T03:12:57Z- TerrainNav::motionUpdate() - currEstimate = (4061708, 594900.25, 48.33) 2024-11-06T03:12:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:12:57Z- TerrainNav:: SubcloudNIS is 0.40, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:12:57Z- TNavPF::Projecting Measurements 2024-11-06T03:12:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:12:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:12:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:12:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:12:57Z- TNavFilter::4 good beams. 2024-11-06T03:12:57Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:12:57Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:12:57Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:12:57Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:12:57Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:12:57Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:12:57Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:12:57Z- TerrainMapOctree: (vehN, vehE ) = (4061708.60, 594900.49). 2024-11-06T03:12:57Z- TerrainMapOctree: Min Distance = 156.08. 2024-11-06T03:12:57Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:12:57Z- Forcing Subcloud Comparison 2024-11-06T03:12:57Z- Weighting particles with subcloud comparison 2024-11-06T03:12:57Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:57Z- meanExpectedMeasDiff: -0.291200 delta_rms_squared: 1.201677 alpha: 0.983629 2024-11-06T03:12:57Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:57Z- meanExpectedMeasDiff: 0.184794 delta_rms_squared: 1.096772 alpha: 0.982091 2024-11-06T03:12:57Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:57Z- meanExpectedMeasDiff: -0.310361 delta_rms_squared: 1.333328 alpha: 0.985222 2024-11-06T03:12:57Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:12:57Z- meanExpectedMeasDiff: 0.265783 delta_rms_squared: 1.159743 alpha: 0.983047 2024-11-06T03:12:57Z- Subcloud NIS: 0.358089 2024-11-06T03:12:57Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:12:57Z- TNavPF:: sumWeights = 0.704542 2024-11-06T03:12:57Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:12:57Z- TNavPF::First Term of Map Covariance Matrix is 0.46 2024-11-06T03:12:57Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T03:12:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862776.80 sec. 2024-11-06T03:12:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:12:57Z- variances([0], [2], and psi_berg) = 0.170484 0.158383 0.000000 2024-11-06T03:12:57Z- publishEstimatesDecoder() - at 1730862777.19 sec 2024-11-06T03:12:57Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:12:57Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061739.08 594882.81 52.88 2024-11-06T03:12:57Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061738.89 594882.91 53.44 lat/lon: 36.70 -121.94 2024-11-06T03:12:57Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.16 0.70 0.00 2024-11-06T03:12:57Z- AHRS_M2 msg: 1730862777.58 epoch sec; seqNo:3295 2024-11-06T03:12:57Z- AHRS_M2 msg: 0.04 phi; 0.11 theta; 3.78 psi 2024-11-06T03:12:57Z- handleDepth()- Depth_Keller msg: 1730862777.59 epoch sec; seqNo:3509; depth 48.29 2024-11-06T03:12:57Z- handleDVL() - DVL msg epochMillisec: 1730862777.59 2024-11-06T03:12:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:12:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:12:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:12:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:12:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:12:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:12:57Z- handleDvl() - RDI_Pathfinder msg: 1730862777.59 sec; seqNo:1160; #beams:0 2024-11-06T03:12:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:12:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , 0.04 2024-11-06T03:12:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862777.60 epoch sec; seqNo:3509; 4061708.34 north; 594900.25 east; 36.70 lat; -121.94 lon; 48.29 depth; utmZone: 10 2024-11-06T03:12:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862777.19 2024-11-06T03:12:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:57Z- waiting for 1730862781.19, time is 1730862777.60 2024-11-06T03:12:57Z- AHRS_M2 msg: 1730862777.99 epoch sec; seqNo:3296 2024-11-06T03:12:57Z- AHRS_M2 msg: 0.04 phi; 0.11 theta; 3.77 psi 2024-11-06T03:12:58Z- handleDepth()- Depth_Keller msg: 1730862778.00 epoch sec; seqNo:3510; depth 48.26 2024-11-06T03:12:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862778.01 epoch sec; seqNo:3510; 4061707.96 north; 594900.26 east; 36.70 lat; -121.94 lon; 48.26 depth; utmZone: 10 2024-11-06T03:12:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862777.19 2024-11-06T03:12:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:58Z- waiting for 1730862781.19, time is 1730862778.01 2024-11-06T03:12:58Z- AHRS_M2 msg: 1730862778.38 epoch sec; seqNo:3297 2024-11-06T03:12:58Z- AHRS_M2 msg: 0.04 phi; 0.10 theta; 3.76 psi 2024-11-06T03:12:58Z- handleDepth()- Depth_Keller msg: 1730862778.39 epoch sec; seqNo:3511; depth 48.21 2024-11-06T03:12:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862778.40 epoch sec; seqNo:3511; 4061707.58 north; 594900.26 east; 36.70 lat; -121.94 lon; 48.21 depth; utmZone: 10 2024-11-06T03:12:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862777.19 2024-11-06T03:12:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:58Z- waiting for 1730862781.19, time is 1730862778.40 2024-11-06T03:12:58Z- AHRS_M2 msg: 1730862778.79 epoch sec; seqNo:3298 2024-11-06T03:12:58Z- AHRS_M2 msg: 0.04 phi; 0.09 theta; 3.75 psi 2024-11-06T03:12:58Z- handleDepth()- Depth_Keller msg: 1730862778.79 epoch sec; seqNo:3512; depth 48.17 2024-11-06T03:12:58Z- handleDVL() - DVL msg epochMillisec: 1730862778.81 2024-11-06T03:12:58Z- handleDvl() - RDI_Pathfinder msg: 1730862778.81 sec; seqNo:1161; #beams:4 2024-11-06T03:12:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.81 , 44.83 , 49.69 , 44.61 2024-11-06T03:12:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.03 2024-11-06T03:12:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862778.82 epoch sec; seqNo:3512; 4061707.19 north; 594899.05 east; 36.70 lat; -121.94 lon; 48.17 depth; utmZone: 10 2024-11-06T03:12:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:12:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862777.19 2024-11-06T03:12:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:58Z- waiting for 1730862781.19, time is 1730862778.82 2024-11-06T03:12:59Z- AHRS_M2 msg: 1730862779.20 epoch sec; seqNo:3299 2024-11-06T03:12:59Z- AHRS_M2 msg: 0.04 phi; 0.09 theta; 3.74 psi 2024-11-06T03:12:59Z- handleDepth()- Depth_Keller msg: 1730862779.20 epoch sec; seqNo:3513; depth 48.14 2024-11-06T03:12:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862779.22 epoch sec; seqNo:3513; 4061707.19 north; 594899.05 east; 36.70 lat; -121.94 lon; 48.14 depth; utmZone: 10 2024-11-06T03:12:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862777.19 2024-11-06T03:12:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:59Z- waiting for 1730862781.19, time is 1730862779.22 2024-11-06T03:12:59Z- AHRS_M2 msg: 1730862779.60 epoch sec; seqNo:3300 2024-11-06T03:12:59Z- AHRS_M2 msg: 0.04 phi; 0.08 theta; 3.73 psi 2024-11-06T03:12:59Z- handleDepth()- Depth_Keller msg: 1730862779.60 epoch sec; seqNo:3514; depth 48.12 2024-11-06T03:12:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862779.63 epoch sec; seqNo:3514; 4061706.81 north; 594899.05 east; 36.70 lat; -121.94 lon; 48.12 depth; utmZone: 10 2024-11-06T03:12:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:12:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862777.19 2024-11-06T03:12:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:12:59Z- waiting for 1730862781.19, time is 1730862779.63 2024-11-06T03:13:00Z- AHRS_M2 msg: 1730862780.00 epoch sec; seqNo:3301 2024-11-06T03:13:00Z- AHRS_M2 msg: 0.04 phi; 0.08 theta; 3.72 psi 2024-11-06T03:13:00Z- handleDepth()- Depth_Keller msg: 1730862780.01 epoch sec; seqNo:3515; depth 48.08 2024-11-06T03:13:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862780.02 epoch sec; seqNo:3515; 4061706.43 north; 594899.05 east; 36.70 lat; -121.94 lon; 48.08 depth; utmZone: 10 2024-11-06T03:13:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862777.19 2024-11-06T03:13:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:00Z- waiting for 1730862781.19, time is 1730862780.02 2024-11-06T03:13:00Z- handleDepth()- Depth_Keller msg: 1730862780.43 epoch sec; seqNo:3516; depth 48.06 2024-11-06T03:13:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862780.45 epoch sec; seqNo:3516; 4061706.06 north; 594899.06 east; 36.70 lat; -121.94 lon; 48.06 depth; utmZone: 10 2024-11-06T03:13:00Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:13:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862777.19 2024-11-06T03:13:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:00Z- waiting for 1730862781.19, time is 1730862780.45 2024-11-06T03:13:00Z- AHRS_M2 msg: 1730862780.81 epoch sec; seqNo:3302 2024-11-06T03:13:00Z- AHRS_M2 msg: 0.04 phi; 0.07 theta; 3.70 psi 2024-11-06T03:13:00Z- handleDepth()- Depth_Keller msg: 1730862780.82 epoch sec; seqNo:3517; depth 48.02 2024-11-06T03:13:00Z- handleDVL() - DVL msg epochMillisec: 1730862780.84 2024-11-06T03:13:00Z- handleDvl() - RDI_Pathfinder msg: 1730862780.84 sec; seqNo:1162; #beams:4 2024-11-06T03:13:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.03 , 43.73 , 48.81 , 45.28 2024-11-06T03:13:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , -0.01 2024-11-06T03:13:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862780.85 epoch sec; seqNo:3517; 4061705.68 north; 594899.06 east; 36.70 lat; -121.94 lon; 48.02 depth; utmZone: 10 2024-11-06T03:13:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862777.19 2024-11-06T03:13:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:00Z- waiting for 1730862781.19, time is 1730862780.85 2024-11-06T03:13:01Z- AHRS_M2 msg: 1730862781.21 epoch sec; seqNo:3303 2024-11-06T03:13:01Z- AHRS_M2 msg: 0.05 phi; 0.06 theta; 3.70 psi 2024-11-06T03:13:01Z- handleDepth()- Depth_Keller msg: 1730862781.22 epoch sec; seqNo:3518; depth 47.95 2024-11-06T03:13:01Z- handleDVL() - DVL msg epochMillisec: 1730862781.23 2024-11-06T03:13:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:01Z- handleDvl() - RDI_Pathfinder msg: 1730862781.23 sec; seqNo:1163; #beams:0 2024-11-06T03:13:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , -0.01 2024-11-06T03:13:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862781.24 epoch sec; seqNo:3518; 4061705.66 north; 594897.84 east; 36.70 lat; -121.94 lon; 47.95 depth; utmZone: 10 2024-11-06T03:13:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862777.19 2024-11-06T03:13:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:01Z- waiting for 1730862781.19, time is 1730862781.24 2024-11-06T03:13:01Z- LcmTrn::updateTrn() >>>> pose time 1730862781.24, meas time 1730862781.23 <<<< 2024-11-06T03:13:01Z- LcmTrn::updateTrn() equating pose 1730862781.23 and meas times 1730862781.23 2024-11-06T03:13:01Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862781.23 2024-11-06T03:13:01Z- TerrainNav::motionUpdate() - currEstimate = (4061706, 594897.84, 47.95) 2024-11-06T03:13:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:13:01Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:13:01Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:13:01Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:13:01Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:13:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:13:01Z- variances([0], [2], and psi_berg) = 0.172811 0.159699 0.000000 2024-11-06T03:13:01Z- publishEstimatesDecoder() - at 1730862781.23 sec 2024-11-06T03:13:01Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:13:01Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061736.44 594880.48 52.94 2024-11-06T03:13:01Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061736.21 594880.50 53.07 lat/lon: 36.70 -121.94 2024-11-06T03:13:01Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.16 0.96 0.00 2024-11-06T03:13:01Z- AHRS_M2 msg: 1730862781.62 epoch sec; seqNo:3304 2024-11-06T03:13:01Z- AHRS_M2 msg: 0.05 phi; 0.06 theta; 3.69 psi 2024-11-06T03:13:01Z- handleDepth()- Depth_Keller msg: 1730862781.63 epoch sec; seqNo:3519; depth 47.94 2024-11-06T03:13:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862781.66 epoch sec; seqNo:3519; 4061705.28 north; 594897.85 east; 36.70 lat; -121.94 lon; 47.94 depth; utmZone: 10 2024-11-06T03:13:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862781.23 2024-11-06T03:13:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:01Z- waiting for 1730862785.23, time is 1730862781.66 2024-11-06T03:13:02Z- AHRS_M2 msg: 1730862782.02 epoch sec; seqNo:3305 2024-11-06T03:13:02Z- AHRS_M2 msg: 0.05 phi; 0.05 theta; 3.68 psi 2024-11-06T03:13:02Z- handleDepth()- Depth_Keller msg: 1730862782.03 epoch sec; seqNo:3520; depth 47.92 2024-11-06T03:13:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862782.04 epoch sec; seqNo:3520; 4061704.91 north; 594897.85 east; 36.70 lat; -121.94 lon; 47.92 depth; utmZone: 10 2024-11-06T03:13:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862781.23 2024-11-06T03:13:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:02Z- waiting for 1730862785.23, time is 1730862782.04 2024-11-06T03:13:02Z- AHRS_M2 msg: 1730862782.43 epoch sec; seqNo:3306 2024-11-06T03:13:02Z- AHRS_M2 msg: 0.05 phi; 0.05 theta; 3.67 psi 2024-11-06T03:13:02Z- handleDepth()- Depth_Keller msg: 1730862782.43 epoch sec; seqNo:3521; depth 47.89 2024-11-06T03:13:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862782.44 epoch sec; seqNo:3521; 4061704.53 north; 594897.85 east; 36.70 lat; -121.94 lon; 47.89 depth; utmZone: 10 2024-11-06T03:13:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862781.23 2024-11-06T03:13:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:02Z- waiting for 1730862785.23, time is 1730862782.44 2024-11-06T03:13:02Z- AHRS_M2 msg: 1730862782.83 epoch sec; seqNo:3307 2024-11-06T03:13:02Z- AHRS_M2 msg: 0.05 phi; 0.04 theta; 3.66 psi 2024-11-06T03:13:02Z- handleDepth()- Depth_Keller msg: 1730862782.83 epoch sec; seqNo:3522; depth 47.86 2024-11-06T03:13:02Z- handleDVL() - DVL msg epochMillisec: 1730862782.87 2024-11-06T03:13:02Z- handleDvl() - RDI_Pathfinder msg: 1730862782.87 sec; seqNo:1164; #beams:4 2024-11-06T03:13:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.25 , 44.39 , 48.15 , 46.16 2024-11-06T03:13:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , -0.06 2024-11-06T03:13:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862782.88 epoch sec; seqNo:3522; 4061704.15 north; 594897.86 east; 36.70 lat; -121.94 lon; 47.86 depth; utmZone: 10 2024-11-06T03:13:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862781.23 2024-11-06T03:13:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:02Z- waiting for 1730862785.23, time is 1730862782.88 2024-11-06T03:13:03Z- handleDepth()- Depth_Keller msg: 1730862783.25 epoch sec; seqNo:3523; depth 47.83 2024-11-06T03:13:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862783.27 epoch sec; seqNo:3523; 4061703.77 north; 594897.86 east; 36.70 lat; -121.94 lon; 47.83 depth; utmZone: 10 2024-11-06T03:13:03Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:13:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862781.23 2024-11-06T03:13:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:03Z- waiting for 1730862785.23, time is 1730862783.27 2024-11-06T03:13:03Z- AHRS_M2 msg: 1730862783.64 epoch sec; seqNo:3308 2024-11-06T03:13:03Z- AHRS_M2 msg: 0.05 phi; 0.03 theta; 3.64 psi 2024-11-06T03:13:03Z- handleDepth()- Depth_Keller msg: 1730862783.65 epoch sec; seqNo:3524; depth 47.81 2024-11-06T03:13:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862783.66 epoch sec; seqNo:3524; 4061703.38 north; 594896.65 east; 36.70 lat; -121.94 lon; 47.81 depth; utmZone: 10 2024-11-06T03:13:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862781.23 2024-11-06T03:13:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:03Z- waiting for 1730862785.23, time is 1730862783.66 2024-11-06T03:13:04Z- handleDepth()- Depth_Keller msg: 1730862784.06 epoch sec; seqNo:3525; depth 47.78 2024-11-06T03:13:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862784.07 epoch sec; seqNo:3525; 4061703.38 north; 594896.65 east; 36.70 lat; -121.94 lon; 47.78 depth; utmZone: 10 2024-11-06T03:13:04Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:13:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862781.23 2024-11-06T03:13:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:04Z- waiting for 1730862785.23, time is 1730862784.07 2024-11-06T03:13:04Z- AHRS_M2 msg: 1730862784.44 epoch sec; seqNo:3309 2024-11-06T03:13:04Z- AHRS_M2 msg: 0.05 phi; 0.02 theta; 3.63 psi 2024-11-06T03:13:04Z- handleDepth()- Depth_Keller msg: 1730862784.45 epoch sec; seqNo:3526; depth 47.77 2024-11-06T03:13:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862784.47 epoch sec; seqNo:3526; 4061703.00 north; 594896.65 east; 36.70 lat; -121.94 lon; 47.77 depth; utmZone: 10 2024-11-06T03:13:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862781.23 2024-11-06T03:13:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:04Z- waiting for 1730862785.23, time is 1730862784.47 2024-11-06T03:13:04Z- AHRS_M2 msg: 1730862784.85 epoch sec; seqNo:3310 2024-11-06T03:13:04Z- AHRS_M2 msg: 0.05 phi; 0.02 theta; 3.62 psi 2024-11-06T03:13:04Z- handleDepth()- Depth_Keller msg: 1730862784.86 epoch sec; seqNo:3527; depth 47.74 2024-11-06T03:13:04Z- handleDVL() - DVL msg epochMillisec: 1730862784.90 2024-11-06T03:13:04Z- handleDvl() - RDI_Pathfinder msg: 1730862784.90 sec; seqNo:1165; #beams:4 2024-11-06T03:13:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.03 , 43.95 , 47.48 , 46.38 2024-11-06T03:13:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.08 , -0.06 2024-11-06T03:13:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862784.91 epoch sec; seqNo:3527; 4061702.62 north; 594896.65 east; 36.70 lat; -121.94 lon; 47.74 depth; utmZone: 10 2024-11-06T03:13:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862781.23 2024-11-06T03:13:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:05Z- waiting for 1730862785.23, time is 1730862784.91 2024-11-06T03:13:05Z- AHRS_M2 msg: 1730862785.26 epoch sec; seqNo:3311 2024-11-06T03:13:05Z- AHRS_M2 msg: 0.06 phi; 0.01 theta; 3.61 psi 2024-11-06T03:13:05Z- handleDepth()- Depth_Keller msg: 1730862785.27 epoch sec; seqNo:3528; depth 47.72 2024-11-06T03:13:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862785.29 epoch sec; seqNo:3528; 4061702.24 north; 594896.66 east; 36.70 lat; -121.94 lon; 47.72 depth; utmZone: 10 2024-11-06T03:13:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862781.23 2024-11-06T03:13:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:05Z- waiting for 1730862785.23, time is 1730862785.29 2024-11-06T03:13:05Z- LcmTrn::updateTrn() >>>> pose time 1730862785.29, meas time 1730862784.90 <<<< 2024-11-06T03:13:05Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862785.29 2024-11-06T03:13:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862784.90 sec, ping # 0; waiting for INS data more recent than 1730862781.23... 2024-11-06T03:13:05Z- TerrainNav::motionUpdate() - currEstimate = (4061702, 594896.66, 47.72) 2024-11-06T03:13:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:13:05Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:13:05Z- TNavPF::Projecting Measurements 2024-11-06T03:13:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:13:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:13:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:13:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:13:05Z- TNavFilter::4 good beams. 2024-11-06T03:13:05Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:13:05Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:13:05Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:13:05Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:13:05Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:13:05Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:13:05Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:13:05Z- TerrainMapOctree: (vehN, vehE ) = (4061702.57, 594896.77). 2024-11-06T03:13:05Z- TerrainMapOctree: Min Distance = 154.25. 2024-11-06T03:13:05Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:13:05Z- Forcing Subcloud Comparison 2024-11-06T03:13:05Z- Weighting particles with subcloud comparison 2024-11-06T03:13:05Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:05Z- meanExpectedMeasDiff: -0.908788 delta_rms_squared: 1.710083 alpha: 0.988440 2024-11-06T03:13:05Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:05Z- meanExpectedMeasDiff: -1.830818 delta_rms_squared: 1.519434 alpha: 0.987008 2024-11-06T03:13:05Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:05Z- meanExpectedMeasDiff: -0.195640 delta_rms_squared: 1.672312 alpha: 0.988182 2024-11-06T03:13:05Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:05Z- meanExpectedMeasDiff: 0.755309 delta_rms_squared: 1.576084 alpha: 0.987469 2024-11-06T03:13:05Z- Subcloud NIS: 0.344017 2024-11-06T03:13:05Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:13:05Z- TNavPF:: sumWeights = 0.445936 2024-11-06T03:13:05Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:13:05Z- TNavPF::First Term of Map Covariance Matrix is 0.39 2024-11-06T03:13:05Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T03:13:05Z- TerrainNav::Resampling particle filter... 2024-11-06T03:13:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862784.90 sec. 2024-11-06T03:13:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:13:05Z- variances([0], [2], and psi_berg) = 0.174180 0.160077 0.000000 2024-11-06T03:13:05Z- publishEstimatesDecoder() - at 1730862785.29 sec 2024-11-06T03:13:05Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:13:05Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061734.82 594880.12 54.35 2024-11-06T03:13:05Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061732.79 594879.32 53.10 lat/lon: 36.70 -121.94 2024-11-06T03:13:05Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.16 0.78 0.00 2024-11-06T03:13:05Z- handleDepth()- Depth_Keller msg: 1730862785.70 epoch sec; seqNo:3529; depth 47.69 2024-11-06T03:13:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862785.72 epoch sec; seqNo:3529; 4061701.86 north; 594896.66 east; 36.70 lat; -121.94 lon; 47.69 depth; utmZone: 10 2024-11-06T03:13:05Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:13:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862785.29 2024-11-06T03:13:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:05Z- waiting for 1730862789.29, time is 1730862785.72 2024-11-06T03:13:06Z- AHRS_M2 msg: 1730862786.06 epoch sec; seqNo:3312 2024-11-06T03:13:06Z- AHRS_M2 msg: 0.06 phi; -0.00 theta; 3.58 psi 2024-11-06T03:13:06Z- handleDepth()- Depth_Keller msg: 1730862786.07 epoch sec; seqNo:3530; depth 47.70 2024-11-06T03:13:06Z- handleDVL() - DVL msg epochMillisec: 1730862786.07 2024-11-06T03:13:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:06Z- handleDvl() - RDI_Pathfinder msg: 1730862786.07 sec; seqNo:1166; #beams:0 2024-11-06T03:13:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.08 , -0.06 2024-11-06T03:13:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862786.09 epoch sec; seqNo:3530; 4061701.47 north; 594895.45 east; 36.70 lat; -121.94 lon; 47.70 depth; utmZone: 10 2024-11-06T03:13:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862785.29 2024-11-06T03:13:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:06Z- waiting for 1730862789.29, time is 1730862786.09 2024-11-06T03:13:06Z- AHRS_M2 msg: 1730862786.47 epoch sec; seqNo:3313 2024-11-06T03:13:06Z- AHRS_M2 msg: 0.06 phi; -0.01 theta; 3.57 psi 2024-11-06T03:13:06Z- handleDepth()- Depth_Keller msg: 1730862786.47 epoch sec; seqNo:3531; depth 47.68 2024-11-06T03:13:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862786.48 epoch sec; seqNo:3531; 4061701.09 north; 594895.45 east; 36.70 lat; -121.94 lon; 47.68 depth; utmZone: 10 2024-11-06T03:13:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862785.29 2024-11-06T03:13:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:06Z- waiting for 1730862789.29, time is 1730862786.48 2024-11-06T03:13:06Z- AHRS_M2 msg: 1730862786.87 epoch sec; seqNo:3314 2024-11-06T03:13:06Z- AHRS_M2 msg: 0.06 phi; -0.01 theta; 3.56 psi 2024-11-06T03:13:06Z- handleDepth()- Depth_Keller msg: 1730862786.88 epoch sec; seqNo:3532; depth 47.66 2024-11-06T03:13:06Z- handleDVL() - DVL msg epochMillisec: 1730862786.90 2024-11-06T03:13:06Z- handleDvl() - RDI_Pathfinder msg: 1730862786.90 sec; seqNo:1167; #beams:4 2024-11-06T03:13:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.47 , 43.95 , 46.82 , 47.04 2024-11-06T03:13:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.14 , -0.02 2024-11-06T03:13:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862786.94 epoch sec; seqNo:3532; 4061700.71 north; 594895.46 east; 36.70 lat; -121.94 lon; 47.66 depth; utmZone: 10 2024-11-06T03:13:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862785.29 2024-11-06T03:13:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:07Z- waiting for 1730862789.29, time is 1730862786.94 2024-11-06T03:13:07Z- AHRS_M2 msg: 1730862787.27 epoch sec; seqNo:3315 2024-11-06T03:13:07Z- AHRS_M2 msg: 0.07 phi; -0.01 theta; 3.56 psi 2024-11-06T03:13:07Z- handleDepth()- Depth_Keller msg: 1730862787.28 epoch sec; seqNo:3533; depth 47.65 2024-11-06T03:13:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862787.30 epoch sec; seqNo:3533; 4061700.71 north; 594895.46 east; 36.70 lat; -121.94 lon; 47.65 depth; utmZone: 10 2024-11-06T03:13:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862785.29 2024-11-06T03:13:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:07Z- waiting for 1730862789.29, time is 1730862787.30 2024-11-06T03:13:07Z- AHRS_M2 msg: 1730862787.68 epoch sec; seqNo:3316 2024-11-06T03:13:07Z- AHRS_M2 msg: 0.08 phi; -0.02 theta; 3.55 psi 2024-11-06T03:13:07Z- handleDepth()- Depth_Keller msg: 1730862787.68 epoch sec; seqNo:3534; depth 47.65 2024-11-06T03:13:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862787.69 epoch sec; seqNo:3534; 4061700.33 north; 594895.46 east; 36.70 lat; -121.94 lon; 47.65 depth; utmZone: 10 2024-11-06T03:13:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862785.29 2024-11-06T03:13:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:07Z- waiting for 1730862789.29, time is 1730862787.69 2024-11-06T03:13:08Z- AHRS_M2 msg: 1730862788.08 epoch sec; seqNo:3317 2024-11-06T03:13:08Z- AHRS_M2 msg: 0.08 phi; -0.02 theta; 3.55 psi 2024-11-06T03:13:08Z- handleDepth()- Depth_Keller msg: 1730862788.09 epoch sec; seqNo:3535; depth 47.64 2024-11-06T03:13:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862788.10 epoch sec; seqNo:3535; 4061699.95 north; 594895.46 east; 36.70 lat; -121.94 lon; 47.64 depth; utmZone: 10 2024-11-06T03:13:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862785.29 2024-11-06T03:13:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:08Z- waiting for 1730862789.29, time is 1730862788.10 2024-11-06T03:13:08Z- AHRS_M2 msg: 1730862788.48 epoch sec; seqNo:3318 2024-11-06T03:13:08Z- AHRS_M2 msg: 0.09 phi; -0.03 theta; 3.54 psi 2024-11-06T03:13:08Z- handleDepth()- Depth_Keller msg: 1730862788.49 epoch sec; seqNo:3536; depth 47.64 2024-11-06T03:13:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862788.51 epoch sec; seqNo:3536; 4061699.56 north; 594894.25 east; 36.70 lat; -121.94 lon; 47.64 depth; utmZone: 10 2024-11-06T03:13:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862785.29 2024-11-06T03:13:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:08Z- waiting for 1730862789.29, time is 1730862788.51 2024-11-06T03:13:08Z- AHRS_M2 msg: 1730862788.88 epoch sec; seqNo:3319 2024-11-06T03:13:08Z- AHRS_M2 msg: 0.09 phi; -0.03 theta; 3.54 psi 2024-11-06T03:13:08Z- handleDepth()- Depth_Keller msg: 1730862788.89 epoch sec; seqNo:3537; depth 47.64 2024-11-06T03:13:08Z- handleDVL() - DVL msg epochMillisec: 1730862788.92 2024-11-06T03:13:08Z- handleDvl() - RDI_Pathfinder msg: 1730862788.92 sec; seqNo:1168; #beams:4 2024-11-06T03:13:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.13 , 43.51 , 46.38 , 47.71 2024-11-06T03:13:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.13 , 0.03 2024-11-06T03:13:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862788.94 epoch sec; seqNo:3537; 4061699.18 north; 594894.25 east; 36.70 lat; -121.94 lon; 47.64 depth; utmZone: 10 2024-11-06T03:13:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862785.29 2024-11-06T03:13:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:09Z- waiting for 1730862789.29, time is 1730862788.94 2024-11-06T03:13:09Z- AHRS_M2 msg: 1730862789.29 epoch sec; seqNo:3320 2024-11-06T03:13:09Z- AHRS_M2 msg: 0.10 phi; -0.04 theta; 3.53 psi 2024-11-06T03:13:09Z- handleDepth()- Depth_Keller msg: 1730862789.30 epoch sec; seqNo:3538; depth 47.66 2024-11-06T03:13:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862789.33 epoch sec; seqNo:3538; 4061698.80 north; 594894.26 east; 36.70 lat; -121.94 lon; 47.66 depth; utmZone: 10 2024-11-06T03:13:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862785.29 2024-11-06T03:13:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:09Z- waiting for 1730862789.29, time is 1730862789.33 2024-11-06T03:13:09Z- LcmTrn::updateTrn() >>>> pose time 1730862789.33, meas time 1730862788.92 <<<< 2024-11-06T03:13:09Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862789.33 2024-11-06T03:13:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862788.92 sec, ping # 0; waiting for INS data more recent than 1730862785.29... 2024-11-06T03:13:09Z- TerrainNav::motionUpdate() - currEstimate = (4061699, 594894.26, 47.66) 2024-11-06T03:13:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:13:09Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:13:09Z- TNavPF::Projecting Measurements 2024-11-06T03:13:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:13:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:13:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:13:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:13:09Z- TNavFilter::4 good beams. 2024-11-06T03:13:09Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:13:09Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:13:09Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:13:09Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:13:09Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:13:09Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:13:09Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:13:09Z- TerrainMapOctree: (vehN, vehE ) = (4061699.14, 594894.50). 2024-11-06T03:13:09Z- TerrainMapOctree: Min Distance = 153.18. 2024-11-06T03:13:09Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:13:09Z- Forcing Subcloud Comparison 2024-11-06T03:13:09Z- Weighting particles with subcloud comparison 2024-11-06T03:13:09Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:09Z- meanExpectedMeasDiff: -0.983732 delta_rms_squared: 1.569741 alpha: 0.987419 2024-11-06T03:13:09Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:09Z- meanExpectedMeasDiff: -1.101514 delta_rms_squared: 1.536162 alpha: 0.987148 2024-11-06T03:13:09Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:09Z- meanExpectedMeasDiff: -0.664549 delta_rms_squared: 2.346241 alpha: 0.991548 2024-11-06T03:13:09Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:09Z- meanExpectedMeasDiff: 0.908296 delta_rms_squared: 1.762690 alpha: 0.988781 2024-11-06T03:13:09Z- Subcloud NIS: 0.322282 2024-11-06T03:13:09Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:13:09Z- TNavPF:: sumWeights = 0.481377 2024-11-06T03:13:09Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:13:09Z- TNavPF::First Term of Map Covariance Matrix is 0.39 2024-11-06T03:13:09Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T03:13:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862788.92 sec. 2024-11-06T03:13:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:13:09Z- variances([0], [2], and psi_berg) = 0.181182 0.164581 0.000000 2024-11-06T03:13:09Z- publishEstimatesDecoder() - at 1730862789.33 sec 2024-11-06T03:13:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:13:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061728.78 594877.81 52.92 2024-11-06T03:13:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061729.35 594876.92 53.23 lat/lon: 36.70 -121.94 2024-11-06T03:13:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.16 0.70 0.00 2024-11-06T03:13:09Z- AHRS_M2 msg: 1730862789.71 epoch sec; seqNo:3321 2024-11-06T03:13:09Z- AHRS_M2 msg: 0.10 phi; -0.04 theta; 3.53 psi 2024-11-06T03:13:09Z- handleDepth()- Depth_Keller msg: 1730862789.71 epoch sec; seqNo:3539; depth 47.66 2024-11-06T03:13:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862789.72 epoch sec; seqNo:3539; 4061698.80 north; 594894.26 east; 36.70 lat; -121.94 lon; 47.66 depth; utmZone: 10 2024-11-06T03:13:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862789.33 2024-11-06T03:13:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:09Z- waiting for 1730862793.33, time is 1730862789.72 2024-11-06T03:13:10Z- AHRS_M2 msg: 1730862790.10 epoch sec; seqNo:3322 2024-11-06T03:13:10Z- AHRS_M2 msg: 0.10 phi; -0.04 theta; 3.52 psi 2024-11-06T03:13:10Z- handleDepth()- Depth_Keller msg: 1730862790.11 epoch sec; seqNo:3540; depth 47.67 2024-11-06T03:13:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862790.13 epoch sec; seqNo:3540; 4061698.42 north; 594894.26 east; 36.70 lat; -121.94 lon; 47.67 depth; utmZone: 10 2024-11-06T03:13:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862789.33 2024-11-06T03:13:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:10Z- waiting for 1730862793.33, time is 1730862790.13 2024-11-06T03:13:10Z- AHRS_M2 msg: 1730862790.50 epoch sec; seqNo:3323 2024-11-06T03:13:10Z- AHRS_M2 msg: 0.10 phi; -0.05 theta; 3.52 psi 2024-11-06T03:13:10Z- handleDepth()- Depth_Keller msg: 1730862790.51 epoch sec; seqNo:3541; depth 47.69 2024-11-06T03:13:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862790.52 epoch sec; seqNo:3541; 4061698.05 north; 594894.26 east; 36.70 lat; -121.94 lon; 47.69 depth; utmZone: 10 2024-11-06T03:13:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862789.33 2024-11-06T03:13:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:10Z- waiting for 1730862793.33, time is 1730862790.52 2024-11-06T03:13:10Z- AHRS_M2 msg: 1730862790.91 epoch sec; seqNo:3324 2024-11-06T03:13:10Z- AHRS_M2 msg: 0.10 phi; -0.05 theta; 3.52 psi 2024-11-06T03:13:10Z- handleDepth()- Depth_Keller msg: 1730862790.92 epoch sec; seqNo:3542; depth 47.70 2024-11-06T03:13:10Z- handleDVL() - DVL msg epochMillisec: 1730862790.95 2024-11-06T03:13:10Z- handleDvl() - RDI_Pathfinder msg: 1730862790.95 sec; seqNo:1169; #beams:4 2024-11-06T03:13:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.80 , 42.85 , 45.94 , 48.37 2024-11-06T03:13:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.09 , 0.05 2024-11-06T03:13:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862790.97 epoch sec; seqNo:3542; 4061697.67 north; 594894.27 east; 36.70 lat; -121.94 lon; 47.70 depth; utmZone: 10 2024-11-06T03:13:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862789.33 2024-11-06T03:13:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:11Z- waiting for 1730862793.33, time is 1730862790.97 2024-11-06T03:13:11Z- AHRS_M2 msg: 1730862791.31 epoch sec; seqNo:3325 2024-11-06T03:13:11Z- AHRS_M2 msg: 0.10 phi; -0.05 theta; 3.51 psi 2024-11-06T03:13:11Z- handleDepth()- Depth_Keller msg: 1730862791.32 epoch sec; seqNo:3543; depth 47.71 2024-11-06T03:13:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862791.34 epoch sec; seqNo:3543; 4061697.27 north; 594893.05 east; 36.70 lat; -121.94 lon; 47.71 depth; utmZone: 10 2024-11-06T03:13:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862789.33 2024-11-06T03:13:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:11Z- waiting for 1730862793.33, time is 1730862791.34 2024-11-06T03:13:11Z- AHRS_M2 msg: 1730862791.72 epoch sec; seqNo:3326 2024-11-06T03:13:11Z- AHRS_M2 msg: 0.10 phi; -0.06 theta; 3.51 psi 2024-11-06T03:13:11Z- handleDepth()- Depth_Keller msg: 1730862791.72 epoch sec; seqNo:3544; depth 47.73 2024-11-06T03:13:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862791.73 epoch sec; seqNo:3544; 4061696.90 north; 594893.06 east; 36.70 lat; -121.94 lon; 47.73 depth; utmZone: 10 2024-11-06T03:13:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862789.33 2024-11-06T03:13:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:11Z- waiting for 1730862793.33, time is 1730862791.73 2024-11-06T03:13:12Z- AHRS_M2 msg: 1730862792.12 epoch sec; seqNo:3327 2024-11-06T03:13:12Z- AHRS_M2 msg: 0.10 phi; -0.06 theta; 3.50 psi 2024-11-06T03:13:12Z- handleDepth()- Depth_Keller msg: 1730862792.13 epoch sec; seqNo:3545; depth 47.74 2024-11-06T03:13:12Z- handleDVL() - DVL msg epochMillisec: 1730862792.13 2024-11-06T03:13:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:12Z- handleDvl() - RDI_Pathfinder msg: 1730862792.13 sec; seqNo:1170; #beams:0 2024-11-06T03:13:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.09 , 0.05 2024-11-06T03:13:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862792.14 epoch sec; seqNo:3545; 4061696.90 north; 594893.06 east; 36.70 lat; -121.94 lon; 47.74 depth; utmZone: 10 2024-11-06T03:13:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862789.33 2024-11-06T03:13:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:12Z- waiting for 1730862793.33, time is 1730862792.14 2024-11-06T03:13:12Z- AHRS_M2 msg: 1730862792.52 epoch sec; seqNo:3328 2024-11-06T03:13:12Z- AHRS_M2 msg: 0.10 phi; -0.06 theta; 3.50 psi 2024-11-06T03:13:12Z- handleDepth()- Depth_Keller msg: 1730862792.54 epoch sec; seqNo:3546; depth 47.77 2024-11-06T03:13:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862792.55 epoch sec; seqNo:3546; 4061696.52 north; 594893.06 east; 36.70 lat; -121.94 lon; 47.77 depth; utmZone: 10 2024-11-06T03:13:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862789.33 2024-11-06T03:13:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:12Z- waiting for 1730862793.33, time is 1730862792.55 2024-11-06T03:13:12Z- AHRS_M2 msg: 1730862792.93 epoch sec; seqNo:3329 2024-11-06T03:13:12Z- AHRS_M2 msg: 0.10 phi; -0.07 theta; 3.49 psi 2024-11-06T03:13:12Z- handleDepth()- Depth_Keller msg: 1730862792.94 epoch sec; seqNo:3547; depth 47.78 2024-11-06T03:13:13Z- handleDVL() - DVL msg epochMillisec: 1730862792.99 2024-11-06T03:13:13Z- handleDvl() - RDI_Pathfinder msg: 1730862792.99 sec; seqNo:1171; #beams:4 2024-11-06T03:13:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.80 , 43.07 , 45.72 , 49.03 2024-11-06T03:13:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , 0.04 2024-11-06T03:13:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862793.02 epoch sec; seqNo:3547; 4061696.14 north; 594893.07 east; 36.70 lat; -121.94 lon; 47.78 depth; utmZone: 10 2024-11-06T03:13:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862789.33 2024-11-06T03:13:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:13Z- waiting for 1730862793.33, time is 1730862793.02 2024-11-06T03:13:13Z- AHRS_M2 msg: 1730862793.33 epoch sec; seqNo:3330 2024-11-06T03:13:13Z- AHRS_M2 msg: 0.10 phi; -0.07 theta; 3.49 psi 2024-11-06T03:13:13Z- handleDepth()- Depth_Keller msg: 1730862793.34 epoch sec; seqNo:3548; depth 47.80 2024-11-06T03:13:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862793.35 epoch sec; seqNo:3548; 4061695.76 north; 594893.07 east; 36.70 lat; -121.94 lon; 47.80 depth; utmZone: 10 2024-11-06T03:13:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862789.33 2024-11-06T03:13:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:13Z- waiting for 1730862793.33, time is 1730862793.35 2024-11-06T03:13:13Z- LcmTrn::updateTrn() >>>> pose time 1730862793.35, meas time 1730862792.99 <<<< 2024-11-06T03:13:13Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862793.35 2024-11-06T03:13:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862792.99 sec, ping # 0; waiting for INS data more recent than 1730862789.33... 2024-11-06T03:13:13Z- TerrainNav::motionUpdate() - currEstimate = (4061696, 594893.07, 47.80) 2024-11-06T03:13:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:13:13Z- TerrainNav:: SubcloudNIS is 0.32, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:13:13Z- TNavPF::Projecting Measurements 2024-11-06T03:13:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:13:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:13:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:13:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:13:13Z- TNavFilter::4 good beams. 2024-11-06T03:13:13Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:13:13Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:13:13Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:13:13Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:13:13Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:13:13Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:13:13Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:13:13Z- TerrainMapOctree: (vehN, vehE ) = (4061696.03, 594893.18). 2024-11-06T03:13:13Z- TerrainMapOctree: Min Distance = 153.01. 2024-11-06T03:13:13Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:13:13Z- Forcing Subcloud Comparison 2024-11-06T03:13:13Z- Weighting particles with subcloud comparison 2024-11-06T03:13:13Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:13Z- meanExpectedMeasDiff: -0.279776 delta_rms_squared: 1.460928 alpha: 0.986495 2024-11-06T03:13:13Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:13Z- meanExpectedMeasDiff: -0.220139 delta_rms_squared: 1.108405 alpha: 0.982276 2024-11-06T03:13:13Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:13Z- meanExpectedMeasDiff: -0.517166 delta_rms_squared: 1.327330 alpha: 0.985156 2024-11-06T03:13:13Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:13Z- meanExpectedMeasDiff: 2.392855 delta_rms_squared: 2.151968 alpha: 0.990792 2024-11-06T03:13:13Z- Subcloud NIS: 0.304687 2024-11-06T03:13:13Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:13:13Z- TNavPF:: sumWeights = 0.400842 2024-11-06T03:13:13Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:13:13Z- TNavPF::First Term of Map Covariance Matrix is 0.33 2024-11-06T03:13:13Z- TNavPF::Calculated NIS Value : 0.19 numBeams normalized NIS: 0.05 2024-11-06T03:13:13Z- TerrainNav::Resampling particle filter... 2024-11-06T03:13:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862792.99 sec. 2024-11-06T03:13:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:13:13Z- variances([0], [2], and psi_berg) = 0.182150 0.176602 0.000000 2024-11-06T03:13:13Z- publishEstimatesDecoder() - at 1730862793.35 sec 2024-11-06T03:13:13Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:13:13Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061728.48 594876.58 54.40 2024-11-06T03:13:13Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061726.31 594875.76 53.19 lat/lon: 36.70 -121.94 2024-11-06T03:13:13Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.18 0.73 0.00 2024-11-06T03:13:13Z- AHRS_M2 msg: 1730862793.74 epoch sec; seqNo:3331 2024-11-06T03:13:13Z- AHRS_M2 msg: 0.10 phi; -0.07 theta; 3.49 psi 2024-11-06T03:13:13Z- handleDepth()- Depth_Keller msg: 1730862793.74 epoch sec; seqNo:3549; depth 47.81 2024-11-06T03:13:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862793.75 epoch sec; seqNo:3549; 4061695.38 north; 594893.07 east; 36.70 lat; -121.94 lon; 47.81 depth; utmZone: 10 2024-11-06T03:13:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862793.35 2024-11-06T03:13:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:13Z- waiting for 1730862797.35, time is 1730862793.75 2024-11-06T03:13:14Z- AHRS_M2 msg: 1730862794.14 epoch sec; seqNo:3332 2024-11-06T03:13:14Z- AHRS_M2 msg: 0.10 phi; -0.08 theta; 3.48 psi 2024-11-06T03:13:14Z- handleDepth()- Depth_Keller msg: 1730862794.15 epoch sec; seqNo:3550; depth 47.84 2024-11-06T03:13:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862794.16 epoch sec; seqNo:3550; 4061695.00 north; 594893.08 east; 36.70 lat; -121.94 lon; 47.84 depth; utmZone: 10 2024-11-06T03:13:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862793.35 2024-11-06T03:13:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:14Z- waiting for 1730862797.35, time is 1730862794.16 2024-11-06T03:13:14Z- AHRS_M2 msg: 1730862794.54 epoch sec; seqNo:3333 2024-11-06T03:13:14Z- AHRS_M2 msg: 0.10 phi; -0.08 theta; 3.48 psi 2024-11-06T03:13:14Z- handleDepth()- Depth_Keller msg: 1730862794.55 epoch sec; seqNo:3551; depth 47.87 2024-11-06T03:13:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862794.56 epoch sec; seqNo:3551; 4061694.61 north; 594891.86 east; 36.70 lat; -121.94 lon; 47.87 depth; utmZone: 10 2024-11-06T03:13:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862793.35 2024-11-06T03:13:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:14Z- waiting for 1730862797.35, time is 1730862794.56 2024-11-06T03:13:14Z- AHRS_M2 msg: 1730862794.95 epoch sec; seqNo:3334 2024-11-06T03:13:14Z- AHRS_M2 msg: 0.10 phi; -0.08 theta; 3.47 psi 2024-11-06T03:13:14Z- handleDepth()- Depth_Keller msg: 1730862794.96 epoch sec; seqNo:3552; depth 47.89 2024-11-06T03:13:15Z- handleDVL() - DVL msg epochMillisec: 1730862795.00 2024-11-06T03:13:15Z- handleDvl() - RDI_Pathfinder msg: 1730862795.00 sec; seqNo:1172; #beams:4 2024-11-06T03:13:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 51.02 , 42.41 , 45.28 , 49.26 2024-11-06T03:13:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , 0.02 2024-11-06T03:13:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862795.01 epoch sec; seqNo:3552; 4061694.23 north; 594891.87 east; 36.70 lat; -121.94 lon; 47.89 depth; utmZone: 10 2024-11-06T03:13:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862793.35 2024-11-06T03:13:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:15Z- waiting for 1730862797.35, time is 1730862795.01 2024-11-06T03:13:15Z- AHRS_M2 msg: 1730862795.35 epoch sec; seqNo:3335 2024-11-06T03:13:15Z- AHRS_M2 msg: 0.10 phi; -0.08 theta; 3.47 psi 2024-11-06T03:13:15Z- handleDepth()- Depth_Keller msg: 1730862795.36 epoch sec; seqNo:3553; depth 47.92 2024-11-06T03:13:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862795.37 epoch sec; seqNo:3553; 4061694.23 north; 594891.87 east; 36.70 lat; -121.94 lon; 47.92 depth; utmZone: 10 2024-11-06T03:13:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862793.35 2024-11-06T03:13:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:15Z- waiting for 1730862797.35, time is 1730862795.37 2024-11-06T03:13:15Z- AHRS_M2 msg: 1730862795.75 epoch sec; seqNo:3336 2024-11-06T03:13:15Z- AHRS_M2 msg: 0.10 phi; -0.09 theta; 3.47 psi 2024-11-06T03:13:15Z- handleDepth()- Depth_Keller msg: 1730862795.77 epoch sec; seqNo:3554; depth 47.94 2024-11-06T03:13:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862795.78 epoch sec; seqNo:3554; 4061693.85 north; 594891.87 east; 36.70 lat; -121.94 lon; 47.94 depth; utmZone: 10 2024-11-06T03:13:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862793.35 2024-11-06T03:13:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:15Z- waiting for 1730862797.35, time is 1730862795.78 2024-11-06T03:13:16Z- handleDepth()- Depth_Keller msg: 1730862796.18 epoch sec; seqNo:3555; depth 47.98 2024-11-06T03:13:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862796.19 epoch sec; seqNo:3555; 4061693.47 north; 594891.87 east; 36.70 lat; -121.94 lon; 47.98 depth; utmZone: 10 2024-11-06T03:13:16Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:13:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862793.35 2024-11-06T03:13:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:16Z- waiting for 1730862797.35, time is 1730862796.19 2024-11-06T03:13:16Z- handleDepth()- Depth_Keller msg: 1730862796.58 epoch sec; seqNo:3556; depth 47.99 2024-11-06T03:13:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862796.59 epoch sec; seqNo:3556; 4061693.09 north; 594891.88 east; 36.70 lat; -121.94 lon; 47.99 depth; utmZone: 10 2024-11-06T03:13:16Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:13:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862793.35 2024-11-06T03:13:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:16Z- waiting for 1730862797.35, time is 1730862796.59 2024-11-06T03:13:16Z- AHRS_M2 msg: 1730862796.97 epoch sec; seqNo:3337 2024-11-06T03:13:16Z- AHRS_M2 msg: 0.10 phi; -0.09 theta; 3.45 psi 2024-11-06T03:13:16Z- handleDepth()- Depth_Keller msg: 1730862796.98 epoch sec; seqNo:3557; depth 48.02 2024-11-06T03:13:17Z- handleDVL() - DVL msg epochMillisec: 1730862797.01 2024-11-06T03:13:17Z- handleDvl() - RDI_Pathfinder msg: 1730862797.01 sec; seqNo:1173; #beams:4 2024-11-06T03:13:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.80 , 42.63 , 44.84 , 49.48 2024-11-06T03:13:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.05 , 0.00 2024-11-06T03:13:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862797.04 epoch sec; seqNo:3557; 4061692.71 north; 594891.88 east; 36.70 lat; -121.94 lon; 48.02 depth; utmZone: 10 2024-11-06T03:13:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862793.35 2024-11-06T03:13:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:17Z- waiting for 1730862797.35, time is 1730862797.04 2024-11-06T03:13:17Z- AHRS_M2 msg: 1730862797.37 epoch sec; seqNo:3338 2024-11-06T03:13:17Z- AHRS_M2 msg: 0.10 phi; -0.10 theta; 3.45 psi 2024-11-06T03:13:17Z- handleDepth()- Depth_Keller msg: 1730862797.38 epoch sec; seqNo:3558; depth 48.05 2024-11-06T03:13:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862797.40 epoch sec; seqNo:3558; 4061692.34 north; 594891.89 east; 36.70 lat; -121.94 lon; 48.05 depth; utmZone: 10 2024-11-06T03:13:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862793.35 2024-11-06T03:13:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:17Z- waiting for 1730862797.35, time is 1730862797.40 2024-11-06T03:13:17Z- LcmTrn::updateTrn() >>>> pose time 1730862797.40, meas time 1730862797.01 <<<< 2024-11-06T03:13:17Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862797.40 2024-11-06T03:13:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862797.01 sec, ping # 0; waiting for INS data more recent than 1730862793.35... 2024-11-06T03:13:17Z- TerrainNav::motionUpdate() - currEstimate = (4061692, 594891.89, 48.05) 2024-11-06T03:13:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:13:17Z- TerrainNav:: SubcloudNIS is 0.30, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:13:17Z- TNavPF::Projecting Measurements 2024-11-06T03:13:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:13:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:13:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:13:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:13:17Z- TNavFilter::4 good beams. 2024-11-06T03:13:17Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:13:17Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:13:17Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:13:17Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:13:17Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:13:17Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:13:17Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:13:17Z- TerrainMapOctree: (vehN, vehE ) = (4061692.66, 594892.00). 2024-11-06T03:13:17Z- TerrainMapOctree: Min Distance = 153.14. 2024-11-06T03:13:17Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:13:17Z- Forcing Subcloud Comparison 2024-11-06T03:13:17Z- Weighting particles with subcloud comparison 2024-11-06T03:13:17Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:17Z- meanExpectedMeasDiff: -0.301161 delta_rms_squared: 1.467535 alpha: 0.986555 2024-11-06T03:13:17Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:17Z- meanExpectedMeasDiff: -0.596810 delta_rms_squared: 1.157583 alpha: 0.983016 2024-11-06T03:13:17Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:17Z- meanExpectedMeasDiff: -0.775194 delta_rms_squared: 1.167127 alpha: 0.983153 2024-11-06T03:13:17Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:17Z- meanExpectedMeasDiff: 2.303829 delta_rms_squared: 1.455993 alpha: 0.986450 2024-11-06T03:13:17Z- Subcloud NIS: 0.294854 2024-11-06T03:13:17Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:13:17Z- TNavPF:: sumWeights = 0.403221 2024-11-06T03:13:17Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:13:17Z- TNavPF::First Term of Map Covariance Matrix is 0.32 2024-11-06T03:13:17Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.05 2024-11-06T03:13:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862797.01 sec. 2024-11-06T03:13:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:13:17Z- variances([0], [2], and psi_berg) = 0.185921 0.180091 0.000000 2024-11-06T03:13:17Z- publishEstimatesDecoder() - at 1730862797.40 sec 2024-11-06T03:13:17Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:13:17Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061722.83 594873.84 53.26 2024-11-06T03:13:17Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061722.89 594874.58 53.39 lat/lon: 36.70 -121.94 2024-11-06T03:13:17Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.19 0.18 0.73 0.00 2024-11-06T03:13:17Z- AHRS_M2 msg: 1730862797.77 epoch sec; seqNo:3339 2024-11-06T03:13:17Z- AHRS_M2 msg: 0.10 phi; -0.10 theta; 3.44 psi 2024-11-06T03:13:17Z- handleDepth()- Depth_Keller msg: 1730862797.80 epoch sec; seqNo:3559; depth 48.08 2024-11-06T03:13:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862797.82 epoch sec; seqNo:3559; 4061691.96 north; 594891.89 east; 36.70 lat; -121.94 lon; 48.08 depth; utmZone: 10 2024-11-06T03:13:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862797.40 2024-11-06T03:13:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:17Z- waiting for 1730862801.40, time is 1730862797.82 2024-11-06T03:13:18Z- AHRS_M2 msg: 1730862798.18 epoch sec; seqNo:3340 2024-11-06T03:13:18Z- AHRS_M2 msg: 0.10 phi; -0.10 theta; 3.44 psi 2024-11-06T03:13:18Z- handleDepth()- Depth_Keller msg: 1730862798.18 epoch sec; seqNo:3560; depth 48.10 2024-11-06T03:13:18Z- handleDVL() - DVL msg epochMillisec: 1730862798.19 2024-11-06T03:13:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:18Z- handleDvl() - RDI_Pathfinder msg: 1730862798.19 sec; seqNo:1174; #beams:0 2024-11-06T03:13:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.05 , 0.00 2024-11-06T03:13:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862798.19 epoch sec; seqNo:3560; 4061691.56 north; 594890.68 east; 36.70 lat; -121.94 lon; 48.10 depth; utmZone: 10 2024-11-06T03:13:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862797.40 2024-11-06T03:13:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:18Z- waiting for 1730862801.40, time is 1730862798.19 2024-11-06T03:13:18Z- AHRS_M2 msg: 1730862798.58 epoch sec; seqNo:3341 2024-11-06T03:13:18Z- AHRS_M2 msg: 0.10 phi; -0.10 theta; 3.43 psi 2024-11-06T03:13:18Z- handleDepth()- Depth_Keller msg: 1730862798.60 epoch sec; seqNo:3561; depth 48.13 2024-11-06T03:13:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862798.61 epoch sec; seqNo:3561; 4061691.19 north; 594890.68 east; 36.70 lat; -121.94 lon; 48.13 depth; utmZone: 10 2024-11-06T03:13:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862797.40 2024-11-06T03:13:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:18Z- waiting for 1730862801.40, time is 1730862798.61 2024-11-06T03:13:18Z- AHRS_M2 msg: 1730862798.98 epoch sec; seqNo:3342 2024-11-06T03:13:18Z- AHRS_M2 msg: 0.10 phi; -0.10 theta; 3.43 psi 2024-11-06T03:13:18Z- handleDepth()- Depth_Keller msg: 1730862798.99 epoch sec; seqNo:3562; depth 48.15 2024-11-06T03:13:19Z- handleDVL() - DVL msg epochMillisec: 1730862799.02 2024-11-06T03:13:19Z- handleDvl() - RDI_Pathfinder msg: 1730862799.02 sec; seqNo:1175; #beams:4 2024-11-06T03:13:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 51.02 , 41.75 , 44.62 , 50.14 2024-11-06T03:13:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.06 , 0.01 2024-11-06T03:13:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862799.04 epoch sec; seqNo:3562; 4061690.81 north; 594890.68 east; 36.70 lat; -121.94 lon; 48.15 depth; utmZone: 10 2024-11-06T03:13:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862797.40 2024-11-06T03:13:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:19Z- waiting for 1730862801.40, time is 1730862799.04 2024-11-06T03:13:19Z- handleDepth()- Depth_Keller msg: 1730862799.41 epoch sec; seqNo:3563; depth 48.17 2024-11-06T03:13:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862799.44 epoch sec; seqNo:3563; 4061690.43 north; 594890.69 east; 36.70 lat; -121.94 lon; 48.17 depth; utmZone: 10 2024-11-06T03:13:19Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:13:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862797.40 2024-11-06T03:13:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:19Z- waiting for 1730862801.40, time is 1730862799.44 2024-11-06T03:13:19Z- AHRS_M2 msg: 1730862799.80 epoch sec; seqNo:3343 2024-11-06T03:13:19Z- AHRS_M2 msg: 0.10 phi; -0.10 theta; 3.42 psi 2024-11-06T03:13:19Z- handleDepth()- Depth_Keller msg: 1730862799.80 epoch sec; seqNo:3564; depth 48.20 2024-11-06T03:13:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862799.81 epoch sec; seqNo:3564; 4061690.05 north; 594890.69 east; 36.70 lat; -121.94 lon; 48.20 depth; utmZone: 10 2024-11-06T03:13:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862797.40 2024-11-06T03:13:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:19Z- waiting for 1730862801.40, time is 1730862799.81 2024-11-06T03:13:20Z- AHRS_M2 msg: 1730862800.20 epoch sec; seqNo:3344 2024-11-06T03:13:20Z- AHRS_M2 msg: 0.10 phi; -0.10 theta; 3.42 psi 2024-11-06T03:13:20Z- handleDepth()- Depth_Keller msg: 1730862800.20 epoch sec; seqNo:3565; depth 48.24 2024-11-06T03:13:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862800.22 epoch sec; seqNo:3565; 4061689.67 north; 594890.70 east; 36.70 lat; -121.94 lon; 48.24 depth; utmZone: 10 2024-11-06T03:13:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862797.40 2024-11-06T03:13:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:20Z- waiting for 1730862801.40, time is 1730862800.22 2024-11-06T03:13:20Z- AHRS_M2 msg: 1730862800.61 epoch sec; seqNo:3345 2024-11-06T03:13:20Z- AHRS_M2 msg: 0.10 phi; -0.10 theta; 3.41 psi 2024-11-06T03:13:20Z- handleDepth()- Depth_Keller msg: 1730862800.62 epoch sec; seqNo:3566; depth 48.26 2024-11-06T03:13:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862800.64 epoch sec; seqNo:3566; 4061689.29 north; 594890.70 east; 36.70 lat; -121.94 lon; 48.26 depth; utmZone: 10 2024-11-06T03:13:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862797.40 2024-11-06T03:13:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:20Z- waiting for 1730862801.40, time is 1730862800.64 2024-11-06T03:13:21Z- AHRS_M2 msg: 1730862801.01 epoch sec; seqNo:3346 2024-11-06T03:13:21Z- AHRS_M2 msg: 0.11 phi; -0.11 theta; 3.41 psi 2024-11-06T03:13:21Z- handleDepth()- Depth_Keller msg: 1730862801.02 epoch sec; seqNo:3567; depth 48.28 2024-11-06T03:13:21Z- handleDVL() - DVL msg epochMillisec: 1730862801.04 2024-11-06T03:13:21Z- handleDvl() - RDI_Pathfinder msg: 1730862801.04 sec; seqNo:1176; #beams:4 2024-11-06T03:13:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.80 , 41.97 , 44.17 , 49.92 2024-11-06T03:13:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , 0.01 2024-11-06T03:13:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862801.06 epoch sec; seqNo:3567; 4061688.91 north; 594890.70 east; 36.70 lat; -121.94 lon; 48.28 depth; utmZone: 10 2024-11-06T03:13:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862797.40 2024-11-06T03:13:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:21Z- waiting for 1730862801.40, time is 1730862801.06 2024-11-06T03:13:21Z- AHRS_M2 msg: 1730862801.41 epoch sec; seqNo:3347 2024-11-06T03:13:21Z- AHRS_M2 msg: 0.11 phi; -0.11 theta; 3.41 psi 2024-11-06T03:13:21Z- handleDepth()- Depth_Keller msg: 1730862801.42 epoch sec; seqNo:3568; depth 48.31 2024-11-06T03:13:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862801.45 epoch sec; seqNo:3568; 4061688.91 north; 594890.70 east; 36.70 lat; -121.94 lon; 48.31 depth; utmZone: 10 2024-11-06T03:13:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862797.40 2024-11-06T03:13:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:21Z- waiting for 1730862801.40, time is 1730862801.45 2024-11-06T03:13:21Z- LcmTrn::updateTrn() >>>> pose time 1730862801.45, meas time 1730862801.04 <<<< 2024-11-06T03:13:21Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862801.45 2024-11-06T03:13:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862801.04 sec, ping # 0; waiting for INS data more recent than 1730862797.40... 2024-11-06T03:13:21Z- TerrainNav::motionUpdate() - currEstimate = (4061689, 594890.70, 48.31) 2024-11-06T03:13:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:13:21Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:13:21Z- TNavPF::Projecting Measurements 2024-11-06T03:13:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:13:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:13:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:13:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:13:21Z- TNavFilter::4 good beams. 2024-11-06T03:13:21Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:13:21Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:13:21Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:13:21Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:13:21Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:13:21Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:13:21Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:13:21Z- TerrainMapOctree: (vehN, vehE ) = (4061689.26, 594890.83). 2024-11-06T03:13:21Z- TerrainMapOctree: Min Distance = 153.36. 2024-11-06T03:13:21Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:13:21Z- Forcing Subcloud Comparison 2024-11-06T03:13:21Z- Weighting particles with subcloud comparison 2024-11-06T03:13:21Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:21Z- meanExpectedMeasDiff: -0.319818 delta_rms_squared: 1.506002 alpha: 0.986894 2024-11-06T03:13:21Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:21Z- meanExpectedMeasDiff: -0.963037 delta_rms_squared: 1.171317 alpha: 0.983212 2024-11-06T03:13:21Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:21Z- meanExpectedMeasDiff: -0.979889 delta_rms_squared: 1.172761 alpha: 0.983232 2024-11-06T03:13:21Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:21Z- meanExpectedMeasDiff: 2.532941 delta_rms_squared: 1.383021 alpha: 0.985745 2024-11-06T03:13:21Z- Subcloud NIS: 0.286042 2024-11-06T03:13:21Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:13:21Z- TNavPF:: sumWeights = 0.327557 2024-11-06T03:13:21Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:13:21Z- TNavPF::First Term of Map Covariance Matrix is 0.27 2024-11-06T03:13:21Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.05 2024-11-06T03:13:21Z- TerrainNav::Resampling particle filter... 2024-11-06T03:13:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862801.04 sec. 2024-11-06T03:13:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:13:21Z- variances([0], [2], and psi_berg) = 0.189850 0.182143 0.000000 2024-11-06T03:13:21Z- publishEstimatesDecoder() - at 1730862801.45 sec 2024-11-06T03:13:21Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:13:21Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061721.61 594874.22 53.18 2024-11-06T03:13:21Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061719.47 594873.40 53.67 lat/lon: 36.70 -121.94 2024-11-06T03:13:21Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.19 0.18 0.76 0.00 2024-11-06T03:13:21Z- AHRS_M2 msg: 1730862801.81 epoch sec; seqNo:3348 2024-11-06T03:13:21Z- AHRS_M2 msg: 0.11 phi; -0.11 theta; 3.41 psi 2024-11-06T03:13:21Z- handleDepth()- Depth_Keller msg: 1730862801.82 epoch sec; seqNo:3569; depth 48.35 2024-11-06T03:13:21Z- handleDVL() - DVL msg epochMillisec: 1730862801.82 2024-11-06T03:13:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:21Z- handleDvl() - RDI_Pathfinder msg: 1730862801.82 sec; seqNo:1177; #beams:0 2024-11-06T03:13:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , 0.01 2024-11-06T03:13:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862801.83 epoch sec; seqNo:3569; 4061688.53 north; 594890.71 east; 36.70 lat; -121.94 lon; 48.35 depth; utmZone: 10 2024-11-06T03:13:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862801.45 2024-11-06T03:13:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:21Z- waiting for 1730862805.45, time is 1730862801.83 2024-11-06T03:13:22Z- AHRS_M2 msg: 1730862802.22 epoch sec; seqNo:3349 2024-11-06T03:13:22Z- AHRS_M2 msg: 0.11 phi; -0.11 theta; 3.40 psi 2024-11-06T03:13:22Z- handleDepth()- Depth_Keller msg: 1730862802.23 epoch sec; seqNo:3570; depth 48.37 2024-11-06T03:13:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862802.24 epoch sec; seqNo:3570; 4061688.14 north; 594889.49 east; 36.70 lat; -121.94 lon; 48.37 depth; utmZone: 10 2024-11-06T03:13:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862801.45 2024-11-06T03:13:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:22Z- waiting for 1730862805.45, time is 1730862802.24 2024-11-06T03:13:22Z- AHRS_M2 msg: 1730862802.63 epoch sec; seqNo:3350 2024-11-06T03:13:22Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 3.40 psi 2024-11-06T03:13:22Z- handleDepth()- Depth_Keller msg: 1730862802.64 epoch sec; seqNo:3571; depth 48.40 2024-11-06T03:13:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862802.66 epoch sec; seqNo:3571; 4061687.76 north; 594889.50 east; 36.70 lat; -121.94 lon; 48.40 depth; utmZone: 10 2024-11-06T03:13:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862801.45 2024-11-06T03:13:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:22Z- waiting for 1730862805.45, time is 1730862802.66 2024-11-06T03:13:23Z- AHRS_M2 msg: 1730862803.03 epoch sec; seqNo:3351 2024-11-06T03:13:23Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 3.40 psi 2024-11-06T03:13:23Z- handleDepth()- Depth_Keller msg: 1730862803.04 epoch sec; seqNo:3572; depth 48.43 2024-11-06T03:13:23Z- handleDVL() - DVL msg epochMillisec: 1730862803.07 2024-11-06T03:13:23Z- handleDvl() - RDI_Pathfinder msg: 1730862803.07 sec; seqNo:1178; #beams:4 2024-11-06T03:13:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.80 , 42.19 , 43.96 , 50.14 2024-11-06T03:13:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.06 , -0.01 2024-11-06T03:13:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862803.09 epoch sec; seqNo:3572; 4061687.38 north; 594889.50 east; 36.70 lat; -121.94 lon; 48.43 depth; utmZone: 10 2024-11-06T03:13:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862801.45 2024-11-06T03:13:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:23Z- waiting for 1730862805.45, time is 1730862803.09 2024-11-06T03:13:23Z- AHRS_M2 msg: 1730862803.45 epoch sec; seqNo:3352 2024-11-06T03:13:23Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 3.40 psi 2024-11-06T03:13:23Z- handleDepth()- Depth_Keller msg: 1730862803.45 epoch sec; seqNo:3573; depth 48.47 2024-11-06T03:13:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862803.47 epoch sec; seqNo:3573; 4061687.00 north; 594889.51 east; 36.70 lat; -121.94 lon; 48.47 depth; utmZone: 10 2024-11-06T03:13:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862801.45 2024-11-06T03:13:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:23Z- waiting for 1730862805.45, time is 1730862803.47 2024-11-06T03:13:23Z- AHRS_M2 msg: 1730862803.84 epoch sec; seqNo:3353 2024-11-06T03:13:23Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 3.40 psi 2024-11-06T03:13:23Z- handleDepth()- Depth_Keller msg: 1730862803.85 epoch sec; seqNo:3574; depth 48.49 2024-11-06T03:13:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862803.86 epoch sec; seqNo:3574; 4061686.63 north; 594889.51 east; 36.70 lat; -121.94 lon; 48.49 depth; utmZone: 10 2024-11-06T03:13:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862801.45 2024-11-06T03:13:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:24Z- waiting for 1730862805.45, time is 1730862803.86 2024-11-06T03:13:24Z- AHRS_M2 msg: 1730862804.24 epoch sec; seqNo:3354 2024-11-06T03:13:24Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 3.40 psi 2024-11-06T03:13:24Z- handleDepth()- Depth_Keller msg: 1730862804.24 epoch sec; seqNo:3575; depth 48.53 2024-11-06T03:13:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862804.27 epoch sec; seqNo:3575; 4061686.25 north; 594889.51 east; 36.70 lat; -121.94 lon; 48.53 depth; utmZone: 10 2024-11-06T03:13:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862801.45 2024-11-06T03:13:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:24Z- waiting for 1730862805.45, time is 1730862804.27 2024-11-06T03:13:24Z- AHRS_M2 msg: 1730862804.65 epoch sec; seqNo:3355 2024-11-06T03:13:24Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 3.39 psi 2024-11-06T03:13:24Z- handleDepth()- Depth_Keller msg: 1730862804.65 epoch sec; seqNo:3576; depth 48.57 2024-11-06T03:13:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862804.66 epoch sec; seqNo:3576; 4061685.87 north; 594889.52 east; 36.70 lat; -121.94 lon; 48.57 depth; utmZone: 10 2024-11-06T03:13:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862801.45 2024-11-06T03:13:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:24Z- waiting for 1730862805.45, time is 1730862804.66 2024-11-06T03:13:25Z- AHRS_M2 msg: 1730862805.05 epoch sec; seqNo:3356 2024-11-06T03:13:25Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 3.39 psi 2024-11-06T03:13:25Z- handleDepth()- Depth_Keller msg: 1730862805.06 epoch sec; seqNo:3577; depth 48.61 2024-11-06T03:13:25Z- handleDVL() - DVL msg epochMillisec: 1730862805.08 2024-11-06T03:13:25Z- handleDvl() - RDI_Pathfinder msg: 1730862805.08 sec; seqNo:1179; #beams:4 2024-11-06T03:13:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.80 , 41.75 , 43.96 , 49.92 2024-11-06T03:13:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.12 , -0.03 2024-11-06T03:13:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862805.09 epoch sec; seqNo:3577; 4061685.49 north; 594889.52 east; 36.70 lat; -121.94 lon; 48.61 depth; utmZone: 10 2024-11-06T03:13:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862801.45 2024-11-06T03:13:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:25Z- waiting for 1730862805.45, time is 1730862805.09 2024-11-06T03:13:25Z- AHRS_M2 msg: 1730862805.46 epoch sec; seqNo:3357 2024-11-06T03:13:25Z- AHRS_M2 msg: 0.13 phi; -0.11 theta; 3.39 psi 2024-11-06T03:13:25Z- handleDepth()- Depth_Keller msg: 1730862805.47 epoch sec; seqNo:3578; depth 48.64 2024-11-06T03:13:25Z- handleDVL() - DVL msg epochMillisec: 1730862805.47 2024-11-06T03:13:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:25Z- handleDvl() - RDI_Pathfinder msg: 1730862805.47 sec; seqNo:1180; #beams:0 2024-11-06T03:13:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.12 , -0.03 2024-11-06T03:13:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862805.48 epoch sec; seqNo:3578; 4061685.11 north; 594889.53 east; 36.70 lat; -121.94 lon; 48.64 depth; utmZone: 10 2024-11-06T03:13:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862801.45 2024-11-06T03:13:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:25Z- waiting for 1730862805.45, time is 1730862805.48 2024-11-06T03:13:25Z- LcmTrn::updateTrn() >>>> pose time 1730862805.48, meas time 1730862805.47 <<<< 2024-11-06T03:13:25Z- LcmTrn::updateTrn() equating pose 1730862805.47 and meas times 1730862805.47 2024-11-06T03:13:25Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862805.47 2024-11-06T03:13:25Z- TerrainNav::motionUpdate() - currEstimate = (4061685, 594889.53, 48.64) 2024-11-06T03:13:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:13:25Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:13:25Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:13:25Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:13:25Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:13:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:13:25Z- variances([0], [2], and psi_berg) = 0.192406 0.184808 0.000000 2024-11-06T03:13:25Z- publishEstimatesDecoder() - at 1730862805.47 sec 2024-11-06T03:13:25Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:13:25Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061717.79 594872.99 53.73 2024-11-06T03:13:25Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061715.66 594872.22 54.00 lat/lon: 36.70 -121.94 2024-11-06T03:13:25Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.19 0.18 1.01 0.00 2024-11-06T03:13:25Z- AHRS_M2 msg: 1730862805.85 epoch sec; seqNo:3358 2024-11-06T03:13:25Z- AHRS_M2 msg: 0.13 phi; -0.11 theta; 3.39 psi 2024-11-06T03:13:25Z- handleDepth()- Depth_Keller msg: 1730862805.86 epoch sec; seqNo:3579; depth 48.68 2024-11-06T03:13:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862805.88 epoch sec; seqNo:3579; 4061685.11 north; 594889.53 east; 36.70 lat; -121.94 lon; 48.68 depth; utmZone: 10 2024-11-06T03:13:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862805.47 2024-11-06T03:13:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:26Z- waiting for 1730862809.47, time is 1730862805.88 2024-11-06T03:13:26Z- AHRS_M2 msg: 1730862806.26 epoch sec; seqNo:3359 2024-11-06T03:13:26Z- AHRS_M2 msg: 0.13 phi; -0.11 theta; 3.38 psi 2024-11-06T03:13:26Z- handleDepth()- Depth_Keller msg: 1730862806.27 epoch sec; seqNo:3580; depth 48.72 2024-11-06T03:13:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862806.28 epoch sec; seqNo:3580; 4061684.72 north; 594888.31 east; 36.70 lat; -121.94 lon; 48.72 depth; utmZone: 10 2024-11-06T03:13:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862805.47 2024-11-06T03:13:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:26Z- waiting for 1730862809.47, time is 1730862806.28 2024-11-06T03:13:26Z- AHRS_M2 msg: 1730862806.67 epoch sec; seqNo:3360 2024-11-06T03:13:26Z- AHRS_M2 msg: 0.13 phi; -0.11 theta; 3.38 psi 2024-11-06T03:13:26Z- handleDepth()- Depth_Keller msg: 1730862806.67 epoch sec; seqNo:3581; depth 48.76 2024-11-06T03:13:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862806.69 epoch sec; seqNo:3581; 4061684.34 north; 594888.31 east; 36.70 lat; -121.94 lon; 48.76 depth; utmZone: 10 2024-11-06T03:13:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862805.47 2024-11-06T03:13:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:26Z- waiting for 1730862809.47, time is 1730862806.69 2024-11-06T03:13:27Z- AHRS_M2 msg: 1730862807.07 epoch sec; seqNo:3361 2024-11-06T03:13:27Z- AHRS_M2 msg: 0.13 phi; -0.11 theta; 3.38 psi 2024-11-06T03:13:27Z- handleDepth()- Depth_Keller msg: 1730862807.08 epoch sec; seqNo:3582; depth 48.80 2024-11-06T03:13:27Z- handleDVL() - DVL msg epochMillisec: 1730862807.10 2024-11-06T03:13:27Z- handleDvl() - RDI_Pathfinder msg: 1730862807.10 sec; seqNo:1181; #beams:4 2024-11-06T03:13:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 51.02 , 41.53 , 43.51 , 49.70 2024-11-06T03:13:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.14 , 0.02 2024-11-06T03:13:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862807.12 epoch sec; seqNo:3582; 4061683.96 north; 594888.32 east; 36.70 lat; -121.94 lon; 48.80 depth; utmZone: 10 2024-11-06T03:13:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862805.47 2024-11-06T03:13:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:27Z- waiting for 1730862809.47, time is 1730862807.12 2024-11-06T03:13:27Z- AHRS_M2 msg: 1730862807.47 epoch sec; seqNo:3362 2024-11-06T03:13:27Z- AHRS_M2 msg: 0.13 phi; -0.11 theta; 3.37 psi 2024-11-06T03:13:27Z- handleDepth()- Depth_Keller msg: 1730862807.48 epoch sec; seqNo:3583; depth 48.84 2024-11-06T03:13:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862807.49 epoch sec; seqNo:3583; 4061683.58 north; 594888.32 east; 36.70 lat; -121.94 lon; 48.84 depth; utmZone: 10 2024-11-06T03:13:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862805.47 2024-11-06T03:13:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:27Z- waiting for 1730862809.47, time is 1730862807.49 2024-11-06T03:13:27Z- AHRS_M2 msg: 1730862807.88 epoch sec; seqNo:3363 2024-11-06T03:13:27Z- AHRS_M2 msg: 0.14 phi; -0.11 theta; 3.37 psi 2024-11-06T03:13:27Z- handleDepth()- Depth_Keller msg: 1730862807.88 epoch sec; seqNo:3584; depth 48.88 2024-11-06T03:13:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862807.89 epoch sec; seqNo:3584; 4061683.20 north; 594888.33 east; 36.70 lat; -121.94 lon; 48.88 depth; utmZone: 10 2024-11-06T03:13:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862805.47 2024-11-06T03:13:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:28Z- waiting for 1730862809.47, time is 1730862807.89 2024-11-06T03:13:28Z- AHRS_M2 msg: 1730862808.29 epoch sec; seqNo:3364 2024-11-06T03:13:28Z- AHRS_M2 msg: 0.14 phi; -0.11 theta; 3.37 psi 2024-11-06T03:13:28Z- handleDepth()- Depth_Keller msg: 1730862808.29 epoch sec; seqNo:3585; depth 48.91 2024-11-06T03:13:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862808.30 epoch sec; seqNo:3585; 4061682.82 north; 594888.33 east; 36.70 lat; -121.94 lon; 48.91 depth; utmZone: 10 2024-11-06T03:13:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862805.47 2024-11-06T03:13:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:28Z- waiting for 1730862809.47, time is 1730862808.30 2024-11-06T03:13:28Z- AHRS_M2 msg: 1730862808.69 epoch sec; seqNo:3365 2024-11-06T03:13:28Z- AHRS_M2 msg: 0.14 phi; -0.11 theta; 3.36 psi 2024-11-06T03:13:28Z- handleDepth()- Depth_Keller msg: 1730862808.70 epoch sec; seqNo:3586; depth 48.96 2024-11-06T03:13:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862808.76 epoch sec; seqNo:3586; 4061682.44 north; 594888.34 east; 36.70 lat; -121.94 lon; 48.96 depth; utmZone: 10 2024-11-06T03:13:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862805.47 2024-11-06T03:13:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:28Z- waiting for 1730862809.47, time is 1730862808.76 2024-11-06T03:13:29Z- AHRS_M2 msg: 1730862809.09 epoch sec; seqNo:3366 2024-11-06T03:13:29Z- AHRS_M2 msg: 0.14 phi; -0.11 theta; 3.36 psi 2024-11-06T03:13:29Z- handleDepth()- Depth_Keller msg: 1730862809.09 epoch sec; seqNo:3587; depth 49.00 2024-11-06T03:13:29Z- handleDVL() - DVL msg epochMillisec: 1730862809.12 2024-11-06T03:13:29Z- handleDvl() - RDI_Pathfinder msg: 1730862809.12 sec; seqNo:1182; #beams:4 2024-11-06T03:13:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 51.02 , 41.08 , 43.07 , 49.70 2024-11-06T03:13:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.09 , 0.08 2024-11-06T03:13:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862809.14 epoch sec; seqNo:3587; 4061682.44 north; 594888.34 east; 36.70 lat; -121.94 lon; 49.00 depth; utmZone: 10 2024-11-06T03:13:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862805.47 2024-11-06T03:13:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:29Z- waiting for 1730862809.47, time is 1730862809.14 2024-11-06T03:13:29Z- AHRS_M2 msg: 1730862809.50 epoch sec; seqNo:3367 2024-11-06T03:13:29Z- AHRS_M2 msg: 0.14 phi; -0.11 theta; 3.36 psi 2024-11-06T03:13:29Z- handleDepth()- Depth_Keller msg: 1730862809.50 epoch sec; seqNo:3588; depth 49.05 2024-11-06T03:13:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862809.52 epoch sec; seqNo:3588; 4061682.07 north; 594888.34 east; 36.70 lat; -121.94 lon; 49.05 depth; utmZone: 10 2024-11-06T03:13:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862805.47 2024-11-06T03:13:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:29Z- waiting for 1730862809.47, time is 1730862809.52 2024-11-06T03:13:29Z- LcmTrn::updateTrn() >>>> pose time 1730862809.52, meas time 1730862809.12 <<<< 2024-11-06T03:13:29Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862809.52 2024-11-06T03:13:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862809.12 sec, ping # 0; waiting for INS data more recent than 1730862805.47... 2024-11-06T03:13:29Z- TerrainNav::motionUpdate() - currEstimate = (4061682, 594888.34, 49.05) 2024-11-06T03:13:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:13:29Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:13:29Z- TNavPF::Projecting Measurements 2024-11-06T03:13:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:13:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:13:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:13:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:13:29Z- TNavFilter::4 good beams. 2024-11-06T03:13:29Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:13:29Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:13:29Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:13:29Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:13:29Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:13:29Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:13:29Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:13:29Z- TerrainMapOctree: (vehN, vehE ) = (4061682.37, 594888.46). 2024-11-06T03:13:29Z- TerrainMapOctree: Min Distance = 154.09. 2024-11-06T03:13:29Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:13:29Z- Forcing Subcloud Comparison 2024-11-06T03:13:29Z- Weighting particles with subcloud comparison 2024-11-06T03:13:29Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:29Z- meanExpectedMeasDiff: -1.258553 delta_rms_squared: 3.453661 alpha: 0.994242 2024-11-06T03:13:29Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:29Z- meanExpectedMeasDiff: -1.441386 delta_rms_squared: 1.187774 alpha: 0.983441 2024-11-06T03:13:29Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:29Z- meanExpectedMeasDiff: -2.385653 delta_rms_squared: 1.555022 alpha: 0.987302 2024-11-06T03:13:29Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:29Z- meanExpectedMeasDiff: 1.062109 delta_rms_squared: 1.112416 alpha: 0.982339 2024-11-06T03:13:29Z- Subcloud NIS: 0.283808 2024-11-06T03:13:29Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:13:29Z- TNavPF:: sumWeights = 0.295090 2024-11-06T03:13:29Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:13:29Z- TNavPF::First Term of Map Covariance Matrix is 0.55 2024-11-06T03:13:29Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T03:13:29Z- TerrainNav::Resampling particle filter... 2024-11-06T03:13:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862809.12 sec. 2024-11-06T03:13:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:13:29Z- variances([0], [2], and psi_berg) = 0.199648 0.188662 0.000000 2024-11-06T03:13:29Z- publishEstimatesDecoder() - at 1730862809.52 sec 2024-11-06T03:13:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:13:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061714.76 594871.84 54.21 2024-11-06T03:13:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061712.61 594871.01 55.00 lat/lon: 36.70 -121.94 2024-11-06T03:13:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.20 0.19 0.80 0.00 2024-11-06T03:13:29Z- AHRS_M2 msg: 1730862809.90 epoch sec; seqNo:3368 2024-11-06T03:13:29Z- AHRS_M2 msg: 0.14 phi; -0.11 theta; 3.36 psi 2024-11-06T03:13:29Z- handleDepth()- Depth_Keller msg: 1730862809.93 epoch sec; seqNo:3589; depth 49.08 2024-11-06T03:13:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862809.94 epoch sec; seqNo:3589; 4061681.69 north; 594888.34 east; 36.70 lat; -121.94 lon; 49.08 depth; utmZone: 10 2024-11-06T03:13:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862809.52 2024-11-06T03:13:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:30Z- waiting for 1730862813.52, time is 1730862809.94 2024-11-06T03:13:30Z- AHRS_M2 msg: 1730862810.30 epoch sec; seqNo:3369 2024-11-06T03:13:30Z- AHRS_M2 msg: 0.14 phi; -0.11 theta; 3.36 psi 2024-11-06T03:13:30Z- handleDepth()- Depth_Keller msg: 1730862810.31 epoch sec; seqNo:3590; depth 49.14 2024-11-06T03:13:30Z- handleDVL() - DVL msg epochMillisec: 1730862810.31 2024-11-06T03:13:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:30Z- handleDvl() - RDI_Pathfinder msg: 1730862810.31 sec; seqNo:1183; #beams:0 2024-11-06T03:13:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.09 , 0.08 2024-11-06T03:13:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862810.33 epoch sec; seqNo:3590; 4061681.29 north; 594887.13 east; 36.70 lat; -121.94 lon; 49.14 depth; utmZone: 10 2024-11-06T03:13:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862809.52 2024-11-06T03:13:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:30Z- waiting for 1730862813.52, time is 1730862810.33 2024-11-06T03:13:30Z- AHRS_M2 msg: 1730862810.74 epoch sec; seqNo:3370 2024-11-06T03:13:30Z- AHRS_M2 msg: 0.14 phi; -0.11 theta; 3.36 psi 2024-11-06T03:13:30Z- handleDepth()- Depth_Keller msg: 1730862810.76 epoch sec; seqNo:3591; depth 49.17 2024-11-06T03:13:30Z- handleDVL() - DVL msg epochMillisec: 1730862810.78 2024-11-06T03:13:30Z- handleDvl() - RDI_Pathfinder msg: 1730862810.78 sec; seqNo:1184; #beams:4 2024-11-06T03:13:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 51.02 , 40.20 , 43.51 , 49.70 2024-11-06T03:13:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , 0.08 2024-11-06T03:13:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862810.81 epoch sec; seqNo:3591; 4061680.92 north; 594887.13 east; 36.70 lat; -121.94 lon; 49.17 depth; utmZone: 10 2024-11-06T03:13:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862809.52 2024-11-06T03:13:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:30Z- waiting for 1730862813.52, time is 1730862810.81 2024-11-06T03:13:31Z- AHRS_M2 msg: 1730862811.11 epoch sec; seqNo:3371 2024-11-06T03:13:31Z- AHRS_M2 msg: 0.14 phi; -0.11 theta; 3.36 psi 2024-11-06T03:13:31Z- handleDepth()- Depth_Keller msg: 1730862811.12 epoch sec; seqNo:3592; depth 49.21 2024-11-06T03:13:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862811.13 epoch sec; seqNo:3592; 4061680.54 north; 594887.14 east; 36.70 lat; -121.94 lon; 49.21 depth; utmZone: 10 2024-11-06T03:13:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862809.52 2024-11-06T03:13:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:31Z- waiting for 1730862813.52, time is 1730862811.13 2024-11-06T03:13:31Z- AHRS_M2 msg: 1730862811.51 epoch sec; seqNo:3372 2024-11-06T03:13:31Z- AHRS_M2 msg: 0.14 phi; -0.11 theta; 3.36 psi 2024-11-06T03:13:31Z- handleDepth()- Depth_Keller msg: 1730862811.51 epoch sec; seqNo:3593; depth 49.25 2024-11-06T03:13:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862811.53 epoch sec; seqNo:3593; 4061680.16 north; 594887.14 east; 36.70 lat; -121.94 lon; 49.25 depth; utmZone: 10 2024-11-06T03:13:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862809.52 2024-11-06T03:13:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:31Z- waiting for 1730862813.52, time is 1730862811.53 2024-11-06T03:13:31Z- AHRS_M2 msg: 1730862811.91 epoch sec; seqNo:3373 2024-11-06T03:13:31Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 3.36 psi 2024-11-06T03:13:31Z- handleDepth()- Depth_Keller msg: 1730862811.93 epoch sec; seqNo:3594; depth 49.28 2024-11-06T03:13:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862811.93 epoch sec; seqNo:3594; 4061679.78 north; 594887.14 east; 36.70 lat; -121.94 lon; 49.28 depth; utmZone: 10 2024-11-06T03:13:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862809.52 2024-11-06T03:13:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:32Z- waiting for 1730862813.52, time is 1730862811.93 2024-11-06T03:13:32Z- AHRS_M2 msg: 1730862812.32 epoch sec; seqNo:3374 2024-11-06T03:13:32Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 3.36 psi 2024-11-06T03:13:32Z- handleDepth()- Depth_Keller msg: 1730862812.33 epoch sec; seqNo:3595; depth 49.32 2024-11-06T03:13:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862812.35 epoch sec; seqNo:3595; 4061679.40 north; 594887.15 east; 36.70 lat; -121.94 lon; 49.32 depth; utmZone: 10 2024-11-06T03:13:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862809.52 2024-11-06T03:13:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:32Z- waiting for 1730862813.52, time is 1730862812.35 2024-11-06T03:13:32Z- AHRS_M2 msg: 1730862812.76 epoch sec; seqNo:3375 2024-11-06T03:13:32Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 3.36 psi 2024-11-06T03:13:32Z- handleDepth()- Depth_Keller msg: 1730862812.77 epoch sec; seqNo:3596; depth 49.34 2024-11-06T03:13:32Z- handleDVL() - DVL msg epochMillisec: 1730862812.79 2024-11-06T03:13:32Z- handleDvl() - RDI_Pathfinder msg: 1730862812.79 sec; seqNo:1185; #beams:4 2024-11-06T03:13:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.80 , 40.86 , 43.51 , 49.26 2024-11-06T03:13:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.01 , 0.02 2024-11-06T03:13:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862812.80 epoch sec; seqNo:3596; 4061679.02 north; 594887.15 east; 36.70 lat; -121.94 lon; 49.34 depth; utmZone: 10 2024-11-06T03:13:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862809.52 2024-11-06T03:13:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:32Z- waiting for 1730862813.52, time is 1730862812.80 2024-11-06T03:13:33Z- AHRS_M2 msg: 1730862813.13 epoch sec; seqNo:3376 2024-11-06T03:13:33Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 3.36 psi 2024-11-06T03:13:33Z- handleDepth()- Depth_Keller msg: 1730862813.13 epoch sec; seqNo:3597; depth 49.39 2024-11-06T03:13:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862813.15 epoch sec; seqNo:3597; 4061678.64 north; 594887.16 east; 36.70 lat; -121.94 lon; 49.39 depth; utmZone: 10 2024-11-06T03:13:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862809.52 2024-11-06T03:13:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:33Z- waiting for 1730862813.52, time is 1730862813.15 2024-11-06T03:13:33Z- AHRS_M2 msg: 1730862813.53 epoch sec; seqNo:3377 2024-11-06T03:13:33Z- AHRS_M2 msg: 0.14 phi; -0.09 theta; 3.36 psi 2024-11-06T03:13:33Z- handleDepth()- Depth_Keller msg: 1730862813.54 epoch sec; seqNo:3598; depth 49.41 2024-11-06T03:13:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862813.56 epoch sec; seqNo:3598; 4061678.26 north; 594887.16 east; 36.70 lat; -121.94 lon; 49.41 depth; utmZone: 10 2024-11-06T03:13:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862809.52 2024-11-06T03:13:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:33Z- waiting for 1730862813.52, time is 1730862813.56 2024-11-06T03:13:33Z- LcmTrn::updateTrn() >>>> pose time 1730862813.56, meas time 1730862812.79 <<<< 2024-11-06T03:13:33Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862813.56 2024-11-06T03:13:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862812.79 sec, ping # 0; waiting for INS data more recent than 1730862809.52... 2024-11-06T03:13:33Z- TerrainNav::motionUpdate() - currEstimate = (4061678, 594887.16, 49.41) 2024-11-06T03:13:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:13:33Z- TerrainNav:: SubcloudNIS is 0.28, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:13:33Z- TNavPF::Projecting Measurements 2024-11-06T03:13:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:13:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:13:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:13:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:13:33Z- TNavFilter::4 good beams. 2024-11-06T03:13:33Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:13:33Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:13:33Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:13:33Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:13:33Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:13:33Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:13:33Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:13:33Z- TerrainMapOctree: (vehN, vehE ) = (4061678.99, 594887.39). 2024-11-06T03:13:33Z- TerrainMapOctree: Min Distance = 154.65. 2024-11-06T03:13:33Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:13:33Z- Forcing Subcloud Comparison 2024-11-06T03:13:33Z- Weighting particles with subcloud comparison 2024-11-06T03:13:33Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:33Z- meanExpectedMeasDiff: -0.129853 delta_rms_squared: 3.040191 alpha: 0.993464 2024-11-06T03:13:33Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:33Z- meanExpectedMeasDiff: -0.751572 delta_rms_squared: 1.206529 alpha: 0.983694 2024-11-06T03:13:33Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:33Z- meanExpectedMeasDiff: -1.244825 delta_rms_squared: 1.262611 alpha: 0.984407 2024-11-06T03:13:33Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:33Z- meanExpectedMeasDiff: 1.599259 delta_rms_squared: 1.587093 alpha: 0.987555 2024-11-06T03:13:33Z- Subcloud NIS: 0.268641 2024-11-06T03:13:33Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:13:33Z- TNavPF:: sumWeights = 0.410922 2024-11-06T03:13:33Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:13:33Z- TNavPF::First Term of Map Covariance Matrix is 0.66 2024-11-06T03:13:33Z- TNavPF::Calculated NIS Value : 0.19 numBeams normalized NIS: 0.05 2024-11-06T03:13:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862812.79 sec. 2024-11-06T03:13:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:13:33Z- variances([0], [2], and psi_berg) = 0.203800 0.192087 0.000000 2024-11-06T03:13:33Z- publishEstimatesDecoder() - at 1730862813.56 sec 2024-11-06T03:13:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:13:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061708.35 594870.25 55.69 2024-11-06T03:13:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061708.80 594869.83 55.45 lat/lon: 36.70 -121.94 2024-11-06T03:13:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.20 0.19 0.70 0.00 2024-11-06T03:13:33Z- AHRS_M2 msg: 1730862813.94 epoch sec; seqNo:3378 2024-11-06T03:13:33Z- AHRS_M2 msg: 0.14 phi; -0.09 theta; 3.36 psi 2024-11-06T03:13:33Z- handleDepth()- Depth_Keller msg: 1730862813.95 epoch sec; seqNo:3599; depth 49.42 2024-11-06T03:13:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862813.98 epoch sec; seqNo:3599; 4061677.89 north; 594887.17 east; 36.70 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T03:13:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862813.56 2024-11-06T03:13:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:34Z- waiting for 1730862817.56, time is 1730862813.98 2024-11-06T03:13:34Z- AHRS_M2 msg: 1730862814.34 epoch sec; seqNo:3379 2024-11-06T03:13:34Z- AHRS_M2 msg: 0.14 phi; -0.09 theta; 3.36 psi 2024-11-06T03:13:34Z- handleDepth()- Depth_Keller msg: 1730862814.35 epoch sec; seqNo:3600; depth 49.46 2024-11-06T03:13:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862814.36 epoch sec; seqNo:3600; 4061677.51 north; 594887.17 east; 36.70 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T03:13:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862813.56 2024-11-06T03:13:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:34Z- waiting for 1730862817.56, time is 1730862814.36 2024-11-06T03:13:34Z- AHRS_M2 msg: 1730862814.78 epoch sec; seqNo:3380 2024-11-06T03:13:34Z- AHRS_M2 msg: 0.14 phi; -0.09 theta; 3.36 psi 2024-11-06T03:13:34Z- handleDepth()- Depth_Keller msg: 1730862814.79 epoch sec; seqNo:3601; depth 49.46 2024-11-06T03:13:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862814.82 epoch sec; seqNo:3601; 4061677.13 north; 594887.17 east; 36.70 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T03:13:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862813.56 2024-11-06T03:13:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:34Z- waiting for 1730862817.56, time is 1730862814.82 2024-11-06T03:13:35Z- AHRS_M2 msg: 1730862815.15 epoch sec; seqNo:3381 2024-11-06T03:13:35Z- AHRS_M2 msg: 0.14 phi; -0.08 theta; 3.36 psi 2024-11-06T03:13:35Z- handleDepth()- Depth_Keller msg: 1730862815.16 epoch sec; seqNo:3602; depth 49.51 2024-11-06T03:13:35Z- handleDVL() - DVL msg epochMillisec: 1730862815.16 2024-11-06T03:13:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:35Z- handleDvl() - RDI_Pathfinder msg: 1730862815.16 sec; seqNo:1186; #beams:0 2024-11-06T03:13:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.01 , 0.02 2024-11-06T03:13:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862815.18 epoch sec; seqNo:3602; 4061676.75 north; 594887.18 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T03:13:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862813.56 2024-11-06T03:13:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:35Z- waiting for 1730862817.56, time is 1730862815.18 2024-11-06T03:13:35Z- AHRS_M2 msg: 1730862815.55 epoch sec; seqNo:3382 2024-11-06T03:13:35Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 3.36 psi 2024-11-06T03:13:35Z- handleDepth()- Depth_Keller msg: 1730862815.56 epoch sec; seqNo:3603; depth 49.52 2024-11-06T03:13:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862815.58 epoch sec; seqNo:3603; 4061676.37 north; 594887.18 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T03:13:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862813.56 2024-11-06T03:13:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:35Z- waiting for 1730862817.56, time is 1730862815.58 2024-11-06T03:13:35Z- AHRS_M2 msg: 1730862815.96 epoch sec; seqNo:3383 2024-11-06T03:13:35Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 3.36 psi 2024-11-06T03:13:35Z- handleDepth()- Depth_Keller msg: 1730862815.96 epoch sec; seqNo:3604; depth 49.54 2024-11-06T03:13:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862815.97 epoch sec; seqNo:3604; 4061675.99 north; 594887.19 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T03:13:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862813.56 2024-11-06T03:13:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:36Z- waiting for 1730862817.56, time is 1730862815.97 2024-11-06T03:13:36Z- AHRS_M2 msg: 1730862816.36 epoch sec; seqNo:3384 2024-11-06T03:13:36Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 3.36 psi 2024-11-06T03:13:36Z- handleDepth()- Depth_Keller msg: 1730862816.38 epoch sec; seqNo:3605; depth 49.55 2024-11-06T03:13:36Z- handleDVL() - DVL msg epochMillisec: 1730862816.38 2024-11-06T03:13:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:36Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:36Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:36Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:36Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:36Z- handleDvl() - RDI_Pathfinder msg: 1730862816.38 sec; seqNo:1187; #beams:0 2024-11-06T03:13:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.01 , 0.02 2024-11-06T03:13:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862816.40 epoch sec; seqNo:3605; 4061675.60 north; 594885.97 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:13:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862813.56 2024-11-06T03:13:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:36Z- waiting for 1730862817.56, time is 1730862816.40 2024-11-06T03:13:36Z- AHRS_M2 msg: 1730862816.77 epoch sec; seqNo:3385 2024-11-06T03:13:36Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 3.36 psi 2024-11-06T03:13:36Z- handleDepth()- Depth_Keller msg: 1730862816.78 epoch sec; seqNo:3606; depth 49.59 2024-11-06T03:13:36Z- handleDVL() - DVL msg epochMillisec: 1730862816.81 2024-11-06T03:13:36Z- handleDvl() - RDI_Pathfinder msg: 1730862816.81 sec; seqNo:1188; #beams:4 2024-11-06T03:13:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.59 , 40.42 , 43.52 , 48.16 2024-11-06T03:13:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , -0.03 2024-11-06T03:13:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862816.82 epoch sec; seqNo:3606; 4061675.22 north; 594885.97 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:13:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862813.56 2024-11-06T03:13:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:36Z- waiting for 1730862817.56, time is 1730862816.82 2024-11-06T03:13:37Z- AHRS_M2 msg: 1730862817.17 epoch sec; seqNo:3386 2024-11-06T03:13:37Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 3.36 psi 2024-11-06T03:13:37Z- handleDepth()- Depth_Keller msg: 1730862817.17 epoch sec; seqNo:3607; depth 49.60 2024-11-06T03:13:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862817.19 epoch sec; seqNo:3607; 4061674.84 north; 594885.98 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:13:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862813.56 2024-11-06T03:13:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:37Z- waiting for 1730862817.56, time is 1730862817.19 2024-11-06T03:13:37Z- AHRS_M2 msg: 1730862817.57 epoch sec; seqNo:3387 2024-11-06T03:13:37Z- AHRS_M2 msg: 0.14 phi; -0.08 theta; 3.35 psi 2024-11-06T03:13:37Z- handleDepth()- Depth_Keller msg: 1730862817.58 epoch sec; seqNo:3608; depth 49.63 2024-11-06T03:13:37Z- handleDVL() - DVL msg epochMillisec: 1730862817.58 2024-11-06T03:13:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:37Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:37Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:37Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:37Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:37Z- handleDvl() - RDI_Pathfinder msg: 1730862817.58 sec; seqNo:1189; #beams:0 2024-11-06T03:13:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , -0.03 2024-11-06T03:13:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862817.59 epoch sec; seqNo:3608; 4061674.46 north; 594885.98 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:13:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862813.56 2024-11-06T03:13:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:37Z- waiting for 1730862817.56, time is 1730862817.59 2024-11-06T03:13:37Z- LcmTrn::updateTrn() >>>> pose time 1730862817.59, meas time 1730862817.58 <<<< 2024-11-06T03:13:37Z- LcmTrn::updateTrn() equating pose 1730862817.58 and meas times 1730862817.58 2024-11-06T03:13:37Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862817.58 2024-11-06T03:13:37Z- TerrainNav::motionUpdate() - currEstimate = (4061674, 594885.98, 49.63) 2024-11-06T03:13:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:13:37Z- TerrainNav:: SubcloudNIS is 0.27, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:13:37Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:13:37Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:13:37Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:13:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:13:37Z- variances([0], [2], and psi_berg) = 0.206298 0.194372 0.000000 2024-11-06T03:13:37Z- publishEstimatesDecoder() - at 1730862817.58 sec 2024-11-06T03:13:37Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:13:37Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061704.59 594869.04 55.39 2024-11-06T03:13:37Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061705.00 594868.66 55.66 lat/lon: 36.70 -121.94 2024-11-06T03:13:37Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.19 0.98 0.00 2024-11-06T03:13:38Z- AHRS_M2 msg: 1730862818.00 epoch sec; seqNo:3388 2024-11-06T03:13:38Z- AHRS_M2 msg: 0.14 phi; -0.08 theta; 3.35 psi 2024-11-06T03:13:38Z- handleDepth()- Depth_Keller msg: 1730862818.02 epoch sec; seqNo:3609; depth 49.64 2024-11-06T03:13:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862818.04 epoch sec; seqNo:3609; 4061674.08 north; 594885.99 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:13:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862817.58 2024-11-06T03:13:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:38Z- waiting for 1730862821.58, time is 1730862818.04 2024-11-06T03:13:38Z- AHRS_M2 msg: 1730862818.38 epoch sec; seqNo:3389 2024-11-06T03:13:38Z- AHRS_M2 msg: 0.14 phi; -0.08 theta; 3.35 psi 2024-11-06T03:13:38Z- handleDepth()- Depth_Keller msg: 1730862818.39 epoch sec; seqNo:3610; depth 49.66 2024-11-06T03:13:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862818.40 epoch sec; seqNo:3610; 4061673.70 north; 594885.99 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:13:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862817.58 2024-11-06T03:13:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:38Z- waiting for 1730862821.58, time is 1730862818.40 2024-11-06T03:13:38Z- AHRS_M2 msg: 1730862818.78 epoch sec; seqNo:3390 2024-11-06T03:13:38Z- AHRS_M2 msg: 0.14 phi; -0.08 theta; 3.35 psi 2024-11-06T03:13:38Z- handleDepth()- Depth_Keller msg: 1730862818.79 epoch sec; seqNo:3611; depth 49.69 2024-11-06T03:13:38Z- handleDVL() - DVL msg epochMillisec: 1730862818.81 2024-11-06T03:13:38Z- handleDvl() - RDI_Pathfinder msg: 1730862818.81 sec; seqNo:1190; #beams:4 2024-11-06T03:13:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.59 , 40.20 , 43.30 , 47.94 2024-11-06T03:13:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.12 , 0.01 2024-11-06T03:13:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862818.84 epoch sec; seqNo:3611; 4061673.70 north; 594885.99 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:13:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862817.58 2024-11-06T03:13:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:38Z- waiting for 1730862821.58, time is 1730862818.84 2024-11-06T03:13:39Z- AHRS_M2 msg: 1730862819.19 epoch sec; seqNo:3391 2024-11-06T03:13:39Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.35 psi 2024-11-06T03:13:39Z- handleDepth()- Depth_Keller msg: 1730862819.19 epoch sec; seqNo:3612; depth 49.71 2024-11-06T03:13:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862819.21 epoch sec; seqNo:3612; 4061673.33 north; 594886.00 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:13:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862817.58 2024-11-06T03:13:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:39Z- waiting for 1730862821.58, time is 1730862819.21 2024-11-06T03:13:39Z- AHRS_M2 msg: 1730862819.59 epoch sec; seqNo:3392 2024-11-06T03:13:39Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.35 psi 2024-11-06T03:13:39Z- handleDepth()- Depth_Keller msg: 1730862819.60 epoch sec; seqNo:3613; depth 49.74 2024-11-06T03:13:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862819.62 epoch sec; seqNo:3613; 4061672.95 north; 594886.00 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:13:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862817.58 2024-11-06T03:13:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:39Z- waiting for 1730862821.58, time is 1730862819.62 2024-11-06T03:13:40Z- AHRS_M2 msg: 1730862820.01 epoch sec; seqNo:3393 2024-11-06T03:13:40Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.35 psi 2024-11-06T03:13:40Z- handleDepth()- Depth_Keller msg: 1730862820.01 epoch sec; seqNo:3614; depth 49.75 2024-11-06T03:13:40Z- handleDVL() - DVL msg epochMillisec: 1730862820.01 2024-11-06T03:13:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:40Z- handleDvl() - RDI_Pathfinder msg: 1730862820.01 sec; seqNo:1191; #beams:0 2024-11-06T03:13:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.12 , 0.01 2024-11-06T03:13:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862820.02 epoch sec; seqNo:3614; 4061672.57 north; 594886.00 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:13:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862817.58 2024-11-06T03:13:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:40Z- waiting for 1730862821.58, time is 1730862820.02 2024-11-06T03:13:40Z- AHRS_M2 msg: 1730862820.40 epoch sec; seqNo:3394 2024-11-06T03:13:40Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.35 psi 2024-11-06T03:13:40Z- handleDepth()- Depth_Keller msg: 1730862820.41 epoch sec; seqNo:3615; depth 49.79 2024-11-06T03:13:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862820.42 epoch sec; seqNo:3615; 4061672.18 north; 594884.79 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:13:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862817.58 2024-11-06T03:13:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:40Z- waiting for 1730862821.58, time is 1730862820.42 2024-11-06T03:13:40Z- AHRS_M2 msg: 1730862820.81 epoch sec; seqNo:3395 2024-11-06T03:13:40Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.35 psi 2024-11-06T03:13:40Z- handleDepth()- Depth_Keller msg: 1730862820.82 epoch sec; seqNo:3616; depth 49.81 2024-11-06T03:13:40Z- handleDVL() - DVL msg epochMillisec: 1730862820.85 2024-11-06T03:13:40Z- handleDvl() - RDI_Pathfinder msg: 1730862820.85 sec; seqNo:1192; #beams:4 2024-11-06T03:13:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 51.03 , 39.76 , 43.30 , 47.71 2024-11-06T03:13:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.12 , 0.06 2024-11-06T03:13:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862820.86 epoch sec; seqNo:3616; 4061671.80 north; 594884.79 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:13:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862817.58 2024-11-06T03:13:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:40Z- waiting for 1730862821.58, time is 1730862820.86 2024-11-06T03:13:41Z- AHRS_M2 msg: 1730862821.21 epoch sec; seqNo:3396 2024-11-06T03:13:41Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.34 psi 2024-11-06T03:13:41Z- handleDepth()- Depth_Keller msg: 1730862821.21 epoch sec; seqNo:3617; depth 49.83 2024-11-06T03:13:41Z- handleDVL() - DVL msg epochMillisec: 1730862821.21 2024-11-06T03:13:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:41Z- handleDvl() - RDI_Pathfinder msg: 1730862821.21 sec; seqNo:1193; #beams:0 2024-11-06T03:13:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.12 , 0.06 2024-11-06T03:13:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862821.23 epoch sec; seqNo:3617; 4061671.42 north; 594884.80 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:13:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862817.58 2024-11-06T03:13:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:41Z- waiting for 1730862821.58, time is 1730862821.23 2024-11-06T03:13:41Z- AHRS_M2 msg: 1730862821.61 epoch sec; seqNo:3397 2024-11-06T03:13:41Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.34 psi 2024-11-06T03:13:41Z- handleDepth()- Depth_Keller msg: 1730862821.62 epoch sec; seqNo:3618; depth 49.87 2024-11-06T03:13:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862821.63 epoch sec; seqNo:3618; 4061671.04 north; 594884.80 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:13:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862817.58 2024-11-06T03:13:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:41Z- waiting for 1730862821.58, time is 1730862821.63 2024-11-06T03:13:41Z- LcmTrn::updateTrn() >>>> pose time 1730862821.63, meas time 1730862821.21 <<<< 2024-11-06T03:13:41Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862821.63 2024-11-06T03:13:41Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:13:41Z- TerrainNav::motionUpdate() - currEstimate = (4061671, 594884.80, 49.87) 2024-11-06T03:13:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:13:41Z- TerrainNav:: SubcloudNIS is 0.27, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:13:41Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:13:41Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:13:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:13:41Z- variances([0], [2], and psi_berg) = 0.208114 0.195970 0.000000 2024-11-06T03:13:41Z- publishEstimatesDecoder() - at 1730862821.63 sec 2024-11-06T03:13:41Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:13:41Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061701.23 594867.91 55.29 2024-11-06T03:13:41Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061701.58 594867.47 55.90 lat/lon: 36.70 -121.94 2024-11-06T03:13:41Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.20 1.23 0.00 2024-11-06T03:13:42Z- AHRS_M2 msg: 1730862822.03 epoch sec; seqNo:3398 2024-11-06T03:13:42Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.34 psi 2024-11-06T03:13:42Z- handleDepth()- Depth_Keller msg: 1730862822.04 epoch sec; seqNo:3619; depth 49.90 2024-11-06T03:13:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862822.05 epoch sec; seqNo:3619; 4061670.66 north; 594884.80 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:13:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862821.63 2024-11-06T03:13:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:42Z- waiting for 1730862825.63, time is 1730862822.05 2024-11-06T03:13:42Z- AHRS_M2 msg: 1730862822.42 epoch sec; seqNo:3399 2024-11-06T03:13:42Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.34 psi 2024-11-06T03:13:42Z- handleDepth()- Depth_Keller msg: 1730862822.43 epoch sec; seqNo:3620; depth 49.93 2024-11-06T03:13:42Z- handleDVL() - DVL msg epochMillisec: 1730862822.43 2024-11-06T03:13:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:42Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:42Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:42Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:42Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:42Z- handleDvl() - RDI_Pathfinder msg: 1730862822.43 sec; seqNo:1194; #beams:0 2024-11-06T03:13:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.12 , 0.06 2024-11-06T03:13:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862822.44 epoch sec; seqNo:3620; 4061670.66 north; 594884.80 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:13:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862821.63 2024-11-06T03:13:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:42Z- waiting for 1730862825.63, time is 1730862822.44 2024-11-06T03:13:42Z- AHRS_M2 msg: 1730862822.82 epoch sec; seqNo:3400 2024-11-06T03:13:42Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.34 psi 2024-11-06T03:13:42Z- handleDepth()- Depth_Keller msg: 1730862822.83 epoch sec; seqNo:3621; depth 49.95 2024-11-06T03:13:42Z- handleDVL() - DVL msg epochMillisec: 1730862822.85 2024-11-06T03:13:42Z- handleDvl() - RDI_Pathfinder msg: 1730862822.85 sec; seqNo:1195; #beams:4 2024-11-06T03:13:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.81 , 39.76 , 43.30 , 47.71 2024-11-06T03:13:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.08 , 0.08 2024-11-06T03:13:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862822.87 epoch sec; seqNo:3621; 4061670.28 north; 594884.81 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:13:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862821.63 2024-11-06T03:13:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:42Z- waiting for 1730862825.63, time is 1730862822.87 2024-11-06T03:13:43Z- AHRS_M2 msg: 1730862823.23 epoch sec; seqNo:3401 2024-11-06T03:13:43Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.34 psi 2024-11-06T03:13:43Z- handleDepth()- Depth_Keller msg: 1730862823.23 epoch sec; seqNo:3622; depth 49.97 2024-11-06T03:13:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862823.25 epoch sec; seqNo:3622; 4061669.90 north; 594884.81 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:13:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862821.63 2024-11-06T03:13:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:43Z- waiting for 1730862825.63, time is 1730862823.25 2024-11-06T03:13:43Z- AHRS_M2 msg: 1730862823.63 epoch sec; seqNo:3402 2024-11-06T03:13:43Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.34 psi 2024-11-06T03:13:43Z- handleDepth()- Depth_Keller msg: 1730862823.65 epoch sec; seqNo:3623; depth 50.02 2024-11-06T03:13:43Z- handleDVL() - DVL msg epochMillisec: 1730862823.65 2024-11-06T03:13:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:43Z- handleDvl() - RDI_Pathfinder msg: 1730862823.65 sec; seqNo:1196; #beams:0 2024-11-06T03:13:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.08 , 0.08 2024-11-06T03:13:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862823.66 epoch sec; seqNo:3623; 4061669.52 north; 594884.82 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:13:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862821.63 2024-11-06T03:13:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:43Z- waiting for 1730862825.63, time is 1730862823.66 2024-11-06T03:13:44Z- AHRS_M2 msg: 1730862824.04 epoch sec; seqNo:3403 2024-11-06T03:13:44Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.34 psi 2024-11-06T03:13:44Z- handleDepth()- Depth_Keller msg: 1730862824.04 epoch sec; seqNo:3624; depth 50.03 2024-11-06T03:13:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862824.05 epoch sec; seqNo:3624; 4061669.14 north; 594884.82 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:13:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862821.63 2024-11-06T03:13:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:44Z- waiting for 1730862825.63, time is 1730862824.05 2024-11-06T03:13:44Z- AHRS_M2 msg: 1730862824.44 epoch sec; seqNo:3404 2024-11-06T03:13:44Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.34 psi 2024-11-06T03:13:44Z- handleDepth()- Depth_Keller msg: 1730862824.45 epoch sec; seqNo:3625; depth 50.05 2024-11-06T03:13:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862824.46 epoch sec; seqNo:3625; 4061668.77 north; 594884.83 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:13:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862821.63 2024-11-06T03:13:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:44Z- waiting for 1730862825.63, time is 1730862824.46 2024-11-06T03:13:44Z- AHRS_M2 msg: 1730862824.84 epoch sec; seqNo:3405 2024-11-06T03:13:44Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.34 psi 2024-11-06T03:13:44Z- handleDepth()- Depth_Keller msg: 1730862824.85 epoch sec; seqNo:3626; depth 50.08 2024-11-06T03:13:44Z- handleDVL() - DVL msg epochMillisec: 1730862824.87 2024-11-06T03:13:44Z- handleDvl() - RDI_Pathfinder msg: 1730862824.87 sec; seqNo:1197; #beams:4 2024-11-06T03:13:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.81 , 39.76 , 43.30 , 47.49 2024-11-06T03:13:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , 0.05 2024-11-06T03:13:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862824.89 epoch sec; seqNo:3626; 4061668.39 north; 594884.83 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:13:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862821.63 2024-11-06T03:13:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:45Z- waiting for 1730862825.63, time is 1730862824.89 2024-11-06T03:13:45Z- AHRS_M2 msg: 1730862825.25 epoch sec; seqNo:3406 2024-11-06T03:13:45Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.35 psi 2024-11-06T03:13:45Z- handleDepth()- Depth_Keller msg: 1730862825.25 epoch sec; seqNo:3627; depth 50.11 2024-11-06T03:13:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862825.27 epoch sec; seqNo:3627; 4061667.99 north; 594883.61 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:13:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862821.63 2024-11-06T03:13:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:45Z- waiting for 1730862825.63, time is 1730862825.27 2024-11-06T03:13:45Z- AHRS_M2 msg: 1730862825.65 epoch sec; seqNo:3407 2024-11-06T03:13:45Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.35 psi 2024-11-06T03:13:45Z- handleDepth()- Depth_Keller msg: 1730862825.66 epoch sec; seqNo:3628; depth 50.12 2024-11-06T03:13:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862825.67 epoch sec; seqNo:3628; 4061667.62 north; 594883.62 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:13:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862821.63 2024-11-06T03:13:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:45Z- waiting for 1730862825.63, time is 1730862825.67 2024-11-06T03:13:45Z- LcmTrn::updateTrn() >>>> pose time 1730862825.67, meas time 1730862824.87 <<<< 2024-11-06T03:13:45Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862825.67 2024-11-06T03:13:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862824.87 sec, ping # 0; waiting for INS data more recent than 1730862821.63... 2024-11-06T03:13:45Z- TerrainNav::motionUpdate() - currEstimate = (4061668, 594883.62, 50.12) 2024-11-06T03:13:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:13:45Z- TerrainNav:: SubcloudNIS is 0.27, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:13:45Z- TNavPF::Projecting Measurements 2024-11-06T03:13:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:13:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:13:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:13:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:13:45Z- TNavFilter::4 good beams. 2024-11-06T03:13:45Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:13:45Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:13:45Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:13:45Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:13:45Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:13:45Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:13:45Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:13:45Z- TerrainMapOctree: (vehN, vehE ) = (4061668.29, 594883.85). 2024-11-06T03:13:45Z- TerrainMapOctree: Min Distance = 156.82. 2024-11-06T03:13:45Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:13:45Z- Forcing Subcloud Comparison 2024-11-06T03:13:45Z- Weighting particles with subcloud comparison 2024-11-06T03:13:45Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:45Z- meanExpectedMeasDiff: -0.246022 delta_rms_squared: 2.452228 alpha: 0.991910 2024-11-06T03:13:45Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:45Z- meanExpectedMeasDiff: -0.597238 delta_rms_squared: 1.667584 alpha: 0.988149 2024-11-06T03:13:45Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:45Z- meanExpectedMeasDiff: -1.226921 delta_rms_squared: 1.862633 alpha: 0.989377 2024-11-06T03:13:45Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:45Z- meanExpectedMeasDiff: 1.516484 delta_rms_squared: 2.205106 alpha: 0.991012 2024-11-06T03:13:45Z- Subcloud NIS: 0.264517 2024-11-06T03:13:45Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:13:45Z- TNavPF:: sumWeights = 0.410764 2024-11-06T03:13:45Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:13:46Z- TNavPF::First Term of Map Covariance Matrix is 0.39 2024-11-06T03:13:46Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.05 2024-11-06T03:13:46Z- TerrainNav::Resampling particle filter... 2024-11-06T03:13:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862824.87 sec. 2024-11-06T03:13:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:13:46Z- variances([0], [2], and psi_berg) = 0.208893 0.196877 0.000000 2024-11-06T03:13:46Z- publishEstimatesDecoder() - at 1730862825.67 sec 2024-11-06T03:13:46Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:13:46Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061700.29 594867.18 56.11 2024-11-06T03:13:46Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061698.15 594866.29 56.24 lat/lon: 36.70 -121.94 2024-11-06T03:13:46Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.20 0.82 0.00 2024-11-06T03:13:46Z- AHRS_M2 msg: 1730862826.05 epoch sec; seqNo:3408 2024-11-06T03:13:46Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.35 psi 2024-11-06T03:13:46Z- handleDepth()- Depth_Keller msg: 1730862826.06 epoch sec; seqNo:3629; depth 50.15 2024-11-06T03:13:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862826.09 epoch sec; seqNo:3629; 4061667.24 north; 594883.62 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:13:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862825.67 2024-11-06T03:13:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:46Z- waiting for 1730862829.67, time is 1730862826.09 2024-11-06T03:13:46Z- AHRS_M2 msg: 1730862826.46 epoch sec; seqNo:3409 2024-11-06T03:13:46Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.35 psi 2024-11-06T03:13:46Z- handleDepth()- Depth_Keller msg: 1730862826.47 epoch sec; seqNo:3630; depth 50.17 2024-11-06T03:13:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862826.48 epoch sec; seqNo:3630; 4061666.86 north; 594883.63 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:13:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862825.67 2024-11-06T03:13:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:46Z- waiting for 1730862829.67, time is 1730862826.48 2024-11-06T03:13:46Z- AHRS_M2 msg: 1730862826.86 epoch sec; seqNo:3410 2024-11-06T03:13:46Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.35 psi 2024-11-06T03:13:46Z- handleDepth()- Depth_Keller msg: 1730862826.87 epoch sec; seqNo:3631; depth 50.19 2024-11-06T03:13:46Z- handleDVL() - DVL msg epochMillisec: 1730862826.89 2024-11-06T03:13:46Z- handleDvl() - RDI_Pathfinder msg: 1730862826.89 sec; seqNo:1198; #beams:4 2024-11-06T03:13:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.59 , 39.54 , 43.30 , 47.49 2024-11-06T03:13:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , 0.00 2024-11-06T03:13:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862826.90 epoch sec; seqNo:3631; 4061666.48 north; 594883.63 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:13:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862825.67 2024-11-06T03:13:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:47Z- waiting for 1730862829.67, time is 1730862826.90 2024-11-06T03:13:47Z- AHRS_M2 msg: 1730862827.27 epoch sec; seqNo:3411 2024-11-06T03:13:47Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.35 psi 2024-11-06T03:13:47Z- handleDepth()- Depth_Keller msg: 1730862827.27 epoch sec; seqNo:3632; depth 50.22 2024-11-06T03:13:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862827.29 epoch sec; seqNo:3632; 4061666.10 north; 594883.63 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:13:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862825.67 2024-11-06T03:13:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:47Z- waiting for 1730862829.67, time is 1730862827.29 2024-11-06T03:13:47Z- handleDepth()- Depth_Keller msg: 1730862827.70 epoch sec; seqNo:3633; depth 50.23 2024-11-06T03:13:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862827.71 epoch sec; seqNo:3633; 4061665.72 north; 594883.64 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:13:47Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:13:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862825.67 2024-11-06T03:13:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:47Z- waiting for 1730862829.67, time is 1730862827.71 2024-11-06T03:13:48Z- AHRS_M2 msg: 1730862828.09 epoch sec; seqNo:3412 2024-11-06T03:13:48Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.35 psi 2024-11-06T03:13:48Z- handleDepth()- Depth_Keller msg: 1730862828.09 epoch sec; seqNo:3634; depth 50.25 2024-11-06T03:13:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862828.10 epoch sec; seqNo:3634; 4061665.34 north; 594883.64 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:13:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862825.67 2024-11-06T03:13:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:48Z- waiting for 1730862829.67, time is 1730862828.10 2024-11-06T03:13:48Z- AHRS_M2 msg: 1730862828.48 epoch sec; seqNo:3413 2024-11-06T03:13:48Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.35 psi 2024-11-06T03:13:48Z- handleDepth()- Depth_Keller msg: 1730862828.49 epoch sec; seqNo:3635; depth 50.28 2024-11-06T03:13:48Z- handleDVL() - DVL msg epochMillisec: 1730862828.50 2024-11-06T03:13:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:48Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:48Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:48Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:48Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:48Z- handleDvl() - RDI_Pathfinder msg: 1730862828.50 sec; seqNo:1199; #beams:0 2024-11-06T03:13:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , 0.00 2024-11-06T03:13:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862828.50 epoch sec; seqNo:3635; 4061664.96 north; 594883.65 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:13:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862825.67 2024-11-06T03:13:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:48Z- waiting for 1730862829.67, time is 1730862828.50 2024-11-06T03:13:48Z- AHRS_M2 msg: 1730862828.89 epoch sec; seqNo:3414 2024-11-06T03:13:48Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.36 psi 2024-11-06T03:13:48Z- handleDepth()- Depth_Keller msg: 1730862828.89 epoch sec; seqNo:3636; depth 50.28 2024-11-06T03:13:48Z- handleDVL() - DVL msg epochMillisec: 1730862828.92 2024-11-06T03:13:48Z- handleDvl() - RDI_Pathfinder msg: 1730862828.92 sec; seqNo:1200; #beams:4 2024-11-06T03:13:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.16 , 39.56 , 43.02 , 47.10 2024-11-06T03:13:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.03 , -0.02 2024-11-06T03:13:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862828.93 epoch sec; seqNo:3636; 4061664.58 north; 594883.65 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:13:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862825.67 2024-11-06T03:13:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:49Z- waiting for 1730862829.67, time is 1730862828.93 2024-11-06T03:13:49Z- AHRS_M2 msg: 1730862829.29 epoch sec; seqNo:3415 2024-11-06T03:13:49Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.36 psi 2024-11-06T03:13:49Z- handleDepth()- Depth_Keller msg: 1730862829.30 epoch sec; seqNo:3637; depth 50.31 2024-11-06T03:13:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862829.31 epoch sec; seqNo:3637; 4061664.21 north; 594883.66 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:13:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862825.67 2024-11-06T03:13:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:49Z- waiting for 1730862829.67, time is 1730862829.31 2024-11-06T03:13:49Z- AHRS_M2 msg: 1730862829.69 epoch sec; seqNo:3416 2024-11-06T03:13:49Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.36 psi 2024-11-06T03:13:49Z- handleDepth()- Depth_Keller msg: 1730862829.70 epoch sec; seqNo:3638; depth 50.32 2024-11-06T03:13:49Z- handleDVL() - DVL msg epochMillisec: 1730862829.70 2024-11-06T03:13:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:49Z- handleDvl() - RDI_Pathfinder msg: 1730862829.70 sec; seqNo:1201; #beams:0 2024-11-06T03:13:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.03 , -0.02 2024-11-06T03:13:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862829.71 epoch sec; seqNo:3638; 4061663.83 north; 594883.66 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:13:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862825.67 2024-11-06T03:13:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:49Z- waiting for 1730862829.67, time is 1730862829.71 2024-11-06T03:13:49Z- LcmTrn::updateTrn() >>>> pose time 1730862829.71, meas time 1730862829.70 <<<< 2024-11-06T03:13:49Z- LcmTrn::updateTrn() equating pose 1730862829.70 and meas times 1730862829.70 2024-11-06T03:13:49Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862829.70 2024-11-06T03:13:49Z- TerrainNav::motionUpdate() - currEstimate = (4061664, 594883.66, 50.32) 2024-11-06T03:13:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:13:49Z- TerrainNav:: SubcloudNIS is 0.26, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:13:49Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:13:49Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:13:49Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:13:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:13:49Z- variances([0], [2], and psi_berg) = 0.211229 0.199752 0.000000 2024-11-06T03:13:49Z- publishEstimatesDecoder() - at 1730862829.70 sec 2024-11-06T03:13:49Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:13:49Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061696.61 594867.25 56.34 2024-11-06T03:13:49Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061694.36 594866.33 56.43 lat/lon: 36.70 -121.94 2024-11-06T03:13:49Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.20 1.07 0.00 2024-11-06T03:13:50Z- AHRS_M2 msg: 1730862830.10 epoch sec; seqNo:3417 2024-11-06T03:13:50Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.36 psi 2024-11-06T03:13:50Z- handleDepth()- Depth_Keller msg: 1730862830.12 epoch sec; seqNo:3639; depth 50.32 2024-11-06T03:13:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862830.13 epoch sec; seqNo:3639; 4061663.45 north; 594883.66 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:13:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862829.70 2024-11-06T03:13:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:50Z- waiting for 1730862833.70, time is 1730862830.13 2024-11-06T03:13:50Z- AHRS_M2 msg: 1730862830.50 epoch sec; seqNo:3418 2024-11-06T03:13:50Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.36 psi 2024-11-06T03:13:50Z- handleDepth()- Depth_Keller msg: 1730862830.51 epoch sec; seqNo:3640; depth 50.35 2024-11-06T03:13:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862830.51 epoch sec; seqNo:3640; 4061663.07 north; 594883.67 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:13:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862829.70 2024-11-06T03:13:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:50Z- waiting for 1730862833.70, time is 1730862830.51 2024-11-06T03:13:50Z- AHRS_M2 msg: 1730862830.91 epoch sec; seqNo:3419 2024-11-06T03:13:50Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.36 psi 2024-11-06T03:13:50Z- handleDepth()- Depth_Keller msg: 1730862830.91 epoch sec; seqNo:3641; depth 50.36 2024-11-06T03:13:50Z- handleDVL() - DVL msg epochMillisec: 1730862830.93 2024-11-06T03:13:50Z- handleDvl() - RDI_Pathfinder msg: 1730862830.93 sec; seqNo:1202; #beams:4 2024-11-06T03:13:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.16 , 39.35 , 43.02 , 46.69 2024-11-06T03:13:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.06 , -0.00 2024-11-06T03:13:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862830.96 epoch sec; seqNo:3641; 4061662.68 north; 594882.45 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:13:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862829.70 2024-11-06T03:13:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:51Z- waiting for 1730862833.70, time is 1730862830.96 2024-11-06T03:13:51Z- AHRS_M2 msg: 1730862831.31 epoch sec; seqNo:3420 2024-11-06T03:13:51Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.36 psi 2024-11-06T03:13:51Z- handleDepth()- Depth_Keller msg: 1730862831.32 epoch sec; seqNo:3642; depth 50.37 2024-11-06T03:13:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862831.34 epoch sec; seqNo:3642; 4061662.30 north; 594882.46 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:13:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862829.70 2024-11-06T03:13:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:51Z- waiting for 1730862833.70, time is 1730862831.34 2024-11-06T03:13:51Z- AHRS_M2 msg: 1730862831.71 epoch sec; seqNo:3421 2024-11-06T03:13:51Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.36 psi 2024-11-06T03:13:51Z- handleDepth()- Depth_Keller msg: 1730862831.72 epoch sec; seqNo:3643; depth 50.38 2024-11-06T03:13:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862831.73 epoch sec; seqNo:3643; 4061661.92 north; 594882.46 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:13:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862829.70 2024-11-06T03:13:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:51Z- waiting for 1730862833.70, time is 1730862831.73 2024-11-06T03:13:52Z- AHRS_M2 msg: 1730862832.12 epoch sec; seqNo:3422 2024-11-06T03:13:52Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.36 psi 2024-11-06T03:13:52Z- handleDepth()- Depth_Keller msg: 1730862832.13 epoch sec; seqNo:3644; depth 50.39 2024-11-06T03:13:52Z- handleDVL() - DVL msg epochMillisec: 1730862832.13 2024-11-06T03:13:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:52Z- handleDvl() - RDI_Pathfinder msg: 1730862832.13 sec; seqNo:1203; #beams:0 2024-11-06T03:13:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.06 , -0.00 2024-11-06T03:13:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862832.14 epoch sec; seqNo:3644; 4061661.54 north; 594882.46 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:13:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862829.70 2024-11-06T03:13:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:52Z- waiting for 1730862833.70, time is 1730862832.14 2024-11-06T03:13:52Z- AHRS_M2 msg: 1730862832.52 epoch sec; seqNo:3423 2024-11-06T03:13:52Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.36 psi 2024-11-06T03:13:52Z- handleDepth()- Depth_Keller msg: 1730862832.53 epoch sec; seqNo:3645; depth 50.41 2024-11-06T03:13:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862832.54 epoch sec; seqNo:3645; 4061661.16 north; 594882.47 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:13:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862829.70 2024-11-06T03:13:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:52Z- waiting for 1730862833.70, time is 1730862832.54 2024-11-06T03:13:52Z- AHRS_M2 msg: 1730862832.93 epoch sec; seqNo:3424 2024-11-06T03:13:52Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.36 psi 2024-11-06T03:13:52Z- handleDepth()- Depth_Keller msg: 1730862832.93 epoch sec; seqNo:3646; depth 50.42 2024-11-06T03:13:52Z- handleDVL() - DVL msg epochMillisec: 1730862832.96 2024-11-06T03:13:52Z- handleDvl() - RDI_Pathfinder msg: 1730862832.96 sec; seqNo:1204; #beams:4 2024-11-06T03:13:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.96 , 38.95 , 43.02 , 46.29 2024-11-06T03:13:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , 0.01 2024-11-06T03:13:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862832.97 epoch sec; seqNo:3646; 4061660.78 north; 594882.47 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:13:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862829.70 2024-11-06T03:13:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:53Z- waiting for 1730862833.70, time is 1730862832.97 2024-11-06T03:13:53Z- handleDepth()- Depth_Keller msg: 1730862833.35 epoch sec; seqNo:3647; depth 50.44 2024-11-06T03:13:53Z- handleDVL() - DVL msg epochMillisec: 1730862833.36 2024-11-06T03:13:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:53Z- handleDvl() - RDI_Pathfinder msg: 1730862833.36 sec; seqNo:1205; #beams:0 2024-11-06T03:13:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , 0.01 2024-11-06T03:13:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862833.38 epoch sec; seqNo:3647; 4061660.40 north; 594882.48 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:13:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862829.70 2024-11-06T03:13:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:53Z- waiting for 1730862833.70, time is 1730862833.38 2024-11-06T03:13:53Z- AHRS_M2 msg: 1730862833.73 epoch sec; seqNo:3425 2024-11-06T03:13:53Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.37 psi 2024-11-06T03:13:53Z- handleDepth()- Depth_Keller msg: 1730862833.74 epoch sec; seqNo:3648; depth 50.46 2024-11-06T03:13:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862833.76 epoch sec; seqNo:3648; 4061660.03 north; 594882.48 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:13:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862829.70 2024-11-06T03:13:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:53Z- waiting for 1730862833.70, time is 1730862833.76 2024-11-06T03:13:53Z- LcmTrn::updateTrn() >>>> pose time 1730862833.76, meas time 1730862833.36 <<<< 2024-11-06T03:13:53Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862833.76 2024-11-06T03:13:53Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:13:53Z- TerrainNav::motionUpdate() - currEstimate = (4061660, 594882.48, 50.46) 2024-11-06T03:13:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:13:53Z- TerrainNav:: SubcloudNIS is 0.26, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:13:53Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:13:53Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:13:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:13:53Z- variances([0], [2], and psi_berg) = 0.213887 0.202172 0.000000 2024-11-06T03:13:53Z- publishEstimatesDecoder() - at 1730862833.76 sec 2024-11-06T03:13:53Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:13:53Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061692.85 594866.13 56.62 2024-11-06T03:13:53Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061690.56 594865.15 56.57 lat/lon: 36.70 -121.94 2024-11-06T03:13:53Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.20 1.30 0.00 2024-11-06T03:13:54Z- AHRS_M2 msg: 1730862834.14 epoch sec; seqNo:3426 2024-11-06T03:13:54Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.37 psi 2024-11-06T03:13:54Z- handleDepth()- Depth_Keller msg: 1730862834.16 epoch sec; seqNo:3649; depth 50.46 2024-11-06T03:13:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862834.18 epoch sec; seqNo:3649; 4061659.65 north; 594882.49 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:13:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862833.76 2024-11-06T03:13:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:54Z- waiting for 1730862837.76, time is 1730862834.18 2024-11-06T03:13:54Z- AHRS_M2 msg: 1730862834.55 epoch sec; seqNo:3427 2024-11-06T03:13:54Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.37 psi 2024-11-06T03:13:54Z- handleDepth()- Depth_Keller msg: 1730862834.55 epoch sec; seqNo:3650; depth 50.46 2024-11-06T03:13:54Z- handleDVL() - DVL msg epochMillisec: 1730862834.55 2024-11-06T03:13:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:54Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:54Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:54Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:54Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:54Z- handleDvl() - RDI_Pathfinder msg: 1730862834.55 sec; seqNo:1206; #beams:0 2024-11-06T03:13:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , 0.01 2024-11-06T03:13:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862834.56 epoch sec; seqNo:3650; 4061659.65 north; 594882.49 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:13:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862833.76 2024-11-06T03:13:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:54Z- waiting for 1730862837.76, time is 1730862834.56 2024-11-06T03:13:54Z- AHRS_M2 msg: 1730862834.95 epoch sec; seqNo:3428 2024-11-06T03:13:54Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.37 psi 2024-11-06T03:13:54Z- handleDepth()- Depth_Keller msg: 1730862834.95 epoch sec; seqNo:3651; depth 50.48 2024-11-06T03:13:54Z- handleDVL() - DVL msg epochMillisec: 1730862834.97 2024-11-06T03:13:54Z- handleDvl() - RDI_Pathfinder msg: 1730862834.97 sec; seqNo:1207; #beams:4 2024-11-06T03:13:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.54 , 38.33 , 43.02 , 45.67 2024-11-06T03:13:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.08 , 0.01 2024-11-06T03:13:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862835.01 epoch sec; seqNo:3651; 4061659.27 north; 594882.49 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:13:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862833.76 2024-11-06T03:13:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:55Z- waiting for 1730862837.76, time is 1730862835.01 2024-11-06T03:13:55Z- AHRS_M2 msg: 1730862835.35 epoch sec; seqNo:3429 2024-11-06T03:13:55Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.37 psi 2024-11-06T03:13:55Z- handleDepth()- Depth_Keller msg: 1730862835.36 epoch sec; seqNo:3652; depth 50.49 2024-11-06T03:13:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862835.37 epoch sec; seqNo:3652; 4061658.88 north; 594881.27 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:13:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862833.76 2024-11-06T03:13:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:55Z- waiting for 1730862837.76, time is 1730862835.37 2024-11-06T03:13:55Z- handleDepth()- Depth_Keller msg: 1730862835.78 epoch sec; seqNo:3653; depth 50.50 2024-11-06T03:13:55Z- handleDVL() - DVL msg epochMillisec: 1730862835.78 2024-11-06T03:13:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:55Z- handleDvl() - RDI_Pathfinder msg: 1730862835.78 sec; seqNo:1208; #beams:0 2024-11-06T03:13:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.08 , 0.01 2024-11-06T03:13:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862835.79 epoch sec; seqNo:3653; 4061658.50 north; 594881.28 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:13:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862833.76 2024-11-06T03:13:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:55Z- waiting for 1730862837.76, time is 1730862835.79 2024-11-06T03:13:56Z- AHRS_M2 msg: 1730862836.16 epoch sec; seqNo:3430 2024-11-06T03:13:56Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.37 psi 2024-11-06T03:13:56Z- handleDepth()- Depth_Keller msg: 1730862836.16 epoch sec; seqNo:3654; depth 50.51 2024-11-06T03:13:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862836.17 epoch sec; seqNo:3654; 4061658.12 north; 594881.28 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:13:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862833.76 2024-11-06T03:13:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:56Z- waiting for 1730862837.76, time is 1730862836.17 2024-11-06T03:13:56Z- handleDepth()- Depth_Keller msg: 1730862836.59 epoch sec; seqNo:3655; depth 50.53 2024-11-06T03:13:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862836.62 epoch sec; seqNo:3655; 4061657.74 north; 594881.29 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:13:56Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:13:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862833.76 2024-11-06T03:13:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:56Z- waiting for 1730862837.76, time is 1730862836.62 2024-11-06T03:13:56Z- handleDepth()- Depth_Keller msg: 1730862836.99 epoch sec; seqNo:3656; depth 50.54 2024-11-06T03:13:57Z- handleDVL() - DVL msg epochMillisec: 1730862837.01 2024-11-06T03:13:57Z- handleDvl() - RDI_Pathfinder msg: 1730862837.01 sec; seqNo:1209; #beams:4 2024-11-06T03:13:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.55 , 38.54 , 42.82 , 45.27 2024-11-06T03:13:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , 0.01 2024-11-06T03:13:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862837.03 epoch sec; seqNo:3656; 4061657.36 north; 594881.29 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:13:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862833.76 2024-11-06T03:13:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:57Z- waiting for 1730862837.76, time is 1730862837.03 2024-11-06T03:13:57Z- AHRS_M2 msg: 1730862837.38 epoch sec; seqNo:3431 2024-11-06T03:13:57Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.37 psi 2024-11-06T03:13:57Z- handleDepth()- Depth_Keller msg: 1730862837.38 epoch sec; seqNo:3657; depth 50.55 2024-11-06T03:13:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862837.40 epoch sec; seqNo:3657; 4061656.98 north; 594881.29 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:13:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862833.76 2024-11-06T03:13:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:57Z- waiting for 1730862837.76, time is 1730862837.40 2024-11-06T03:13:57Z- AHRS_M2 msg: 1730862837.78 epoch sec; seqNo:3432 2024-11-06T03:13:57Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.37 psi 2024-11-06T03:13:57Z- handleDepth()- Depth_Keller msg: 1730862837.79 epoch sec; seqNo:3658; depth 50.56 2024-11-06T03:13:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862837.82 epoch sec; seqNo:3658; 4061656.60 north; 594881.30 east; 36.70 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T03:13:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862833.76 2024-11-06T03:13:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:57Z- waiting for 1730862837.76, time is 1730862837.82 2024-11-06T03:13:57Z- LcmTrn::updateTrn() >>>> pose time 1730862837.82, meas time 1730862837.01 <<<< 2024-11-06T03:13:57Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862837.82 2024-11-06T03:13:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862837.01 sec, ping # 0; waiting for INS data more recent than 1730862833.76... 2024-11-06T03:13:57Z- TerrainNav::motionUpdate() - currEstimate = (4061657, 594881.30, 50.56) 2024-11-06T03:13:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:13:57Z- TerrainNav:: SubcloudNIS is 0.26, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:13:57Z- TNavPF::Projecting Measurements 2024-11-06T03:13:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:13:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:13:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:13:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:13:57Z- TNavFilter::4 good beams. 2024-11-06T03:13:57Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:13:57Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:13:57Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:13:57Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:13:57Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:13:57Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:13:57Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:13:57Z- TerrainMapOctree: (vehN, vehE ) = (4061657.28, 594881.53). 2024-11-06T03:13:57Z- TerrainMapOctree: Min Distance = 160.93. 2024-11-06T03:13:57Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:13:58Z- Forcing Subcloud Comparison 2024-11-06T03:13:58Z- Weighting particles with subcloud comparison 2024-11-06T03:13:58Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:58Z- meanExpectedMeasDiff: -1.413898 delta_rms_squared: 1.896733 alpha: 0.989566 2024-11-06T03:13:58Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:58Z- meanExpectedMeasDiff: -2.238454 delta_rms_squared: 1.774247 alpha: 0.988853 2024-11-06T03:13:58Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:58Z- meanExpectedMeasDiff: -3.134536 delta_rms_squared: 2.037471 alpha: 0.990279 2024-11-06T03:13:58Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:13:58Z- meanExpectedMeasDiff: 1.185384 delta_rms_squared: 2.118804 alpha: 0.990649 2024-11-06T03:13:58Z- Subcloud NIS: 0.276175 2024-11-06T03:13:58Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:13:58Z- TNavPF:: sumWeights = 0.144964 2024-11-06T03:13:58Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:13:58Z- TNavPF::First Term of Map Covariance Matrix is 0.16 2024-11-06T03:13:58Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.01 2024-11-06T03:13:58Z- TerrainNav::Resampling particle filter... 2024-11-06T03:13:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862837.01 sec. 2024-11-06T03:13:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:13:58Z- variances([0], [2], and psi_berg) = 0.216974 0.203013 0.000000 2024-11-06T03:13:58Z- publishEstimatesDecoder() - at 1730862837.82 sec 2024-11-06T03:13:58Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:13:58Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061689.45 594864.65 56.64 2024-11-06T03:13:58Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061687.14 594863.99 57.47 lat/lon: 36.70 -121.94 2024-11-06T03:13:58Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.20 0.86 0.00 2024-11-06T03:13:58Z- AHRS_M2 msg: 1730862838.18 epoch sec; seqNo:3433 2024-11-06T03:13:58Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.37 psi 2024-11-06T03:13:58Z- handleDepth()- Depth_Keller msg: 1730862838.19 epoch sec; seqNo:3659; depth 50.58 2024-11-06T03:13:58Z- handleDVL() - DVL msg epochMillisec: 1730862838.20 2024-11-06T03:13:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:58Z- handleDvl() - RDI_Pathfinder msg: 1730862838.20 sec; seqNo:1210; #beams:0 2024-11-06T03:13:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , 0.01 2024-11-06T03:13:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862838.22 epoch sec; seqNo:3659; 4061656.22 north; 594881.30 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T03:13:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862837.82 2024-11-06T03:13:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:58Z- waiting for 1730862841.82, time is 1730862838.22 2024-11-06T03:13:58Z- AHRS_M2 msg: 1730862838.59 epoch sec; seqNo:3434 2024-11-06T03:13:58Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.37 psi 2024-11-06T03:13:58Z- handleDepth()- Depth_Keller msg: 1730862838.60 epoch sec; seqNo:3660; depth 50.58 2024-11-06T03:13:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862838.61 epoch sec; seqNo:3660; 4061655.84 north; 594881.31 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T03:13:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862837.82 2024-11-06T03:13:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:58Z- waiting for 1730862841.82, time is 1730862838.61 2024-11-06T03:13:58Z- AHRS_M2 msg: 1730862838.99 epoch sec; seqNo:3435 2024-11-06T03:13:58Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.37 psi 2024-11-06T03:13:59Z- handleDepth()- Depth_Keller msg: 1730862839.00 epoch sec; seqNo:3661; depth 50.59 2024-11-06T03:13:59Z- handleDVL() - DVL msg epochMillisec: 1730862839.02 2024-11-06T03:13:59Z- handleDvl() - RDI_Pathfinder msg: 1730862839.02 sec; seqNo:1211; #beams:4 2024-11-06T03:13:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.54 , 38.74 , 43.02 , 45.06 2024-11-06T03:13:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , 0.04 2024-11-06T03:13:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862839.03 epoch sec; seqNo:3661; 4061655.84 north; 594881.31 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:13:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862837.82 2024-11-06T03:13:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:59Z- waiting for 1730862841.82, time is 1730862839.03 2024-11-06T03:13:59Z- AHRS_M2 msg: 1730862839.39 epoch sec; seqNo:3436 2024-11-06T03:13:59Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.37 psi 2024-11-06T03:13:59Z- handleDepth()- Depth_Keller msg: 1730862839.40 epoch sec; seqNo:3662; depth 50.61 2024-11-06T03:13:59Z- handleDVL() - DVL msg epochMillisec: 1730862839.40 2024-11-06T03:13:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:13:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:13:59Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:13:59Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:13:59Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:13:59Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:13:59Z- handleDvl() - RDI_Pathfinder msg: 1730862839.40 sec; seqNo:1212; #beams:0 2024-11-06T03:13:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:13:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , 0.04 2024-11-06T03:13:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862839.42 epoch sec; seqNo:3662; 4061655.47 north; 594881.31 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:13:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:13:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862837.82 2024-11-06T03:13:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:59Z- waiting for 1730862841.82, time is 1730862839.42 2024-11-06T03:13:59Z- AHRS_M2 msg: 1730862839.79 epoch sec; seqNo:3437 2024-11-06T03:13:59Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.37 psi 2024-11-06T03:13:59Z- handleDepth()- Depth_Keller msg: 1730862839.81 epoch sec; seqNo:3663; depth 50.63 2024-11-06T03:13:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862839.82 epoch sec; seqNo:3663; 4061655.07 north; 594880.10 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:13:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:13:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862837.82 2024-11-06T03:13:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:13:59Z- waiting for 1730862841.82, time is 1730862839.82 2024-11-06T03:14:00Z- AHRS_M2 msg: 1730862840.20 epoch sec; seqNo:3438 2024-11-06T03:14:00Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.37 psi 2024-11-06T03:14:00Z- handleDepth()- Depth_Keller msg: 1730862840.21 epoch sec; seqNo:3664; depth 50.64 2024-11-06T03:14:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862840.22 epoch sec; seqNo:3664; 4061654.69 north; 594880.10 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:14:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862837.82 2024-11-06T03:14:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:00Z- waiting for 1730862841.82, time is 1730862840.22 2024-11-06T03:14:00Z- AHRS_M2 msg: 1730862840.61 epoch sec; seqNo:3439 2024-11-06T03:14:00Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.37 psi 2024-11-06T03:14:00Z- handleDepth()- Depth_Keller msg: 1730862840.62 epoch sec; seqNo:3665; depth 50.64 2024-11-06T03:14:00Z- handleDVL() - DVL msg epochMillisec: 1730862840.62 2024-11-06T03:14:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:00Z- handleDvl() - RDI_Pathfinder msg: 1730862840.62 sec; seqNo:1213; #beams:0 2024-11-06T03:14:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , 0.04 2024-11-06T03:14:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862840.65 epoch sec; seqNo:3665; 4061654.32 north; 594880.10 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:14:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862837.82 2024-11-06T03:14:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:00Z- waiting for 1730862841.82, time is 1730862840.65 2024-11-06T03:14:01Z- AHRS_M2 msg: 1730862841.01 epoch sec; seqNo:3440 2024-11-06T03:14:01Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.37 psi 2024-11-06T03:14:01Z- handleDepth()- Depth_Keller msg: 1730862841.02 epoch sec; seqNo:3666; depth 50.66 2024-11-06T03:14:01Z- handleDVL() - DVL msg epochMillisec: 1730862841.04 2024-11-06T03:14:01Z- handleDvl() - RDI_Pathfinder msg: 1730862841.04 sec; seqNo:1214; #beams:4 2024-11-06T03:14:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.35 , 38.54 , 42.82 , 45.06 2024-11-06T03:14:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.04 , 0.04 2024-11-06T03:14:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862841.06 epoch sec; seqNo:3666; 4061653.94 north; 594880.11 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T03:14:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862837.82 2024-11-06T03:14:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:01Z- waiting for 1730862841.82, time is 1730862841.06 2024-11-06T03:14:01Z- AHRS_M2 msg: 1730862841.41 epoch sec; seqNo:3441 2024-11-06T03:14:01Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.37 psi 2024-11-06T03:14:01Z- handleDepth()- Depth_Keller msg: 1730862841.42 epoch sec; seqNo:3667; depth 50.66 2024-11-06T03:14:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862841.43 epoch sec; seqNo:3667; 4061653.56 north; 594880.11 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T03:14:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862837.82 2024-11-06T03:14:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:01Z- waiting for 1730862841.82, time is 1730862841.43 2024-11-06T03:14:01Z- handleDepth()- Depth_Keller msg: 1730862841.84 epoch sec; seqNo:3668; depth 50.68 2024-11-06T03:14:01Z- handleDVL() - DVL msg epochMillisec: 1730862841.86 2024-11-06T03:14:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:01Z- handleDvl() - RDI_Pathfinder msg: 1730862841.86 sec; seqNo:1215; #beams:0 2024-11-06T03:14:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.04 , 0.04 2024-11-06T03:14:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862841.89 epoch sec; seqNo:3668; 4061653.18 north; 594880.12 east; 36.70 lat; -121.94 lon; 50.68 depth; utmZone: 10 2024-11-06T03:14:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862837.82 2024-11-06T03:14:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:01Z- waiting for 1730862841.82, time is 1730862841.89 2024-11-06T03:14:01Z- LcmTrn::updateTrn() >>>> pose time 1730862841.89, meas time 1730862841.86 <<<< 2024-11-06T03:14:01Z- LcmTrn::updateTrn() equating pose 1730862841.86 and meas times 1730862841.86 2024-11-06T03:14:01Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862841.86 2024-11-06T03:14:01Z- TerrainNav::motionUpdate() - currEstimate = (4061653, 594880.12, 50.68) 2024-11-06T03:14:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:14:01Z- TerrainNav:: SubcloudNIS is 0.28, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:14:01Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:14:01Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:14:02Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:14:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:14:02Z- variances([0], [2], and psi_berg) = 0.219015 0.205162 0.000000 2024-11-06T03:14:02Z- publishEstimatesDecoder() - at 1730862841.86 sec 2024-11-06T03:14:02Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:14:02Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061686.01 594863.55 55.70 2024-11-06T03:14:02Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061683.72 594862.80 57.59 lat/lon: 36.70 -121.94 2024-11-06T03:14:02Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.21 1.12 0.00 2024-11-06T03:14:02Z- AHRS_M2 msg: 1730862842.22 epoch sec; seqNo:3442 2024-11-06T03:14:02Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.38 psi 2024-11-06T03:14:02Z- handleDepth()- Depth_Keller msg: 1730862842.23 epoch sec; seqNo:3669; depth 50.69 2024-11-06T03:14:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862842.25 epoch sec; seqNo:3669; 4061652.80 north; 594880.12 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:14:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862841.86 2024-11-06T03:14:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:02Z- waiting for 1730862845.86, time is 1730862842.25 2024-11-06T03:14:02Z- AHRS_M2 msg: 1730862842.63 epoch sec; seqNo:3443 2024-11-06T03:14:02Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.38 psi 2024-11-06T03:14:02Z- handleDepth()- Depth_Keller msg: 1730862842.64 epoch sec; seqNo:3670; depth 50.70 2024-11-06T03:14:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862842.65 epoch sec; seqNo:3670; 4061652.42 north; 594880.12 east; 36.70 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T03:14:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862841.86 2024-11-06T03:14:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:02Z- waiting for 1730862845.86, time is 1730862842.65 2024-11-06T03:14:03Z- AHRS_M2 msg: 1730862843.05 epoch sec; seqNo:3444 2024-11-06T03:14:03Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.38 psi 2024-11-06T03:14:03Z- handleDepth()- Depth_Keller msg: 1730862843.05 epoch sec; seqNo:3671; depth 50.70 2024-11-06T03:14:03Z- handleDVL() - DVL msg epochMillisec: 1730862843.08 2024-11-06T03:14:03Z- handleDvl() - RDI_Pathfinder msg: 1730862843.08 sec; seqNo:1216; #beams:4 2024-11-06T03:14:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.14 , 38.74 , 43.02 , 44.85 2024-11-06T03:14:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , 0.01 2024-11-06T03:14:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862843.11 epoch sec; seqNo:3671; 4061652.04 north; 594880.13 east; 36.70 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T03:14:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862841.86 2024-11-06T03:14:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:03Z- waiting for 1730862845.86, time is 1730862843.11 2024-11-06T03:14:03Z- AHRS_M2 msg: 1730862843.44 epoch sec; seqNo:3445 2024-11-06T03:14:03Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.38 psi 2024-11-06T03:14:03Z- handleDepth()- Depth_Keller msg: 1730862843.46 epoch sec; seqNo:3672; depth 50.69 2024-11-06T03:14:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862843.48 epoch sec; seqNo:3672; 4061651.66 north; 594880.13 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:14:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862841.86 2024-11-06T03:14:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:03Z- waiting for 1730862845.86, time is 1730862843.48 2024-11-06T03:14:03Z- AHRS_M2 msg: 1730862843.84 epoch sec; seqNo:3446 2024-11-06T03:14:03Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.38 psi 2024-11-06T03:14:03Z- handleDepth()- Depth_Keller msg: 1730862843.85 epoch sec; seqNo:3673; depth 50.70 2024-11-06T03:14:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862843.86 epoch sec; seqNo:3673; 4061651.28 north; 594880.14 east; 36.70 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T03:14:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862841.86 2024-11-06T03:14:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:04Z- waiting for 1730862845.86, time is 1730862843.86 2024-11-06T03:14:04Z- AHRS_M2 msg: 1730862844.24 epoch sec; seqNo:3447 2024-11-06T03:14:04Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 3.38 psi 2024-11-06T03:14:04Z- handleDepth()- Depth_Keller msg: 1730862844.25 epoch sec; seqNo:3674; depth 50.71 2024-11-06T03:14:04Z- handleDVL() - DVL msg epochMillisec: 1730862844.25 2024-11-06T03:14:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:04Z- handleDvl() - RDI_Pathfinder msg: 1730862844.25 sec; seqNo:1217; #beams:0 2024-11-06T03:14:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , 0.01 2024-11-06T03:14:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862844.26 epoch sec; seqNo:3674; 4061650.91 north; 594880.14 east; 36.70 lat; -121.94 lon; 50.71 depth; utmZone: 10 2024-11-06T03:14:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862841.86 2024-11-06T03:14:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:04Z- waiting for 1730862845.86, time is 1730862844.26 2024-11-06T03:14:04Z- AHRS_M2 msg: 1730862844.66 epoch sec; seqNo:3448 2024-11-06T03:14:04Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 3.39 psi 2024-11-06T03:14:04Z- handleDepth()- Depth_Keller msg: 1730862844.66 epoch sec; seqNo:3675; depth 50.71 2024-11-06T03:14:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862844.68 epoch sec; seqNo:3675; 4061650.51 north; 594878.93 east; 36.70 lat; -121.94 lon; 50.71 depth; utmZone: 10 2024-11-06T03:14:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862841.86 2024-11-06T03:14:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:04Z- waiting for 1730862845.86, time is 1730862844.68 2024-11-06T03:14:05Z- AHRS_M2 msg: 1730862845.05 epoch sec; seqNo:3449 2024-11-06T03:14:05Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T03:14:05Z- handleDepth()- Depth_Keller msg: 1730862845.06 epoch sec; seqNo:3676; depth 50.70 2024-11-06T03:14:05Z- handleDVL() - DVL msg epochMillisec: 1730862845.09 2024-11-06T03:14:05Z- handleDvl() - RDI_Pathfinder msg: 1730862845.09 sec; seqNo:1218; #beams:4 2024-11-06T03:14:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.73 , 38.13 , 42.82 , 44.45 2024-11-06T03:14:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , -0.02 2024-11-06T03:14:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862845.10 epoch sec; seqNo:3676; 4061650.13 north; 594878.93 east; 36.70 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T03:14:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862841.86 2024-11-06T03:14:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:05Z- waiting for 1730862845.86, time is 1730862845.10 2024-11-06T03:14:05Z- AHRS_M2 msg: 1730862845.45 epoch sec; seqNo:3450 2024-11-06T03:14:05Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T03:14:05Z- handleDepth()- Depth_Keller msg: 1730862845.46 epoch sec; seqNo:3677; depth 50.70 2024-11-06T03:14:05Z- handleDVL() - DVL msg epochMillisec: 1730862845.46 2024-11-06T03:14:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:05Z- handleDvl() - RDI_Pathfinder msg: 1730862845.46 sec; seqNo:1219; #beams:0 2024-11-06T03:14:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , -0.02 2024-11-06T03:14:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862845.48 epoch sec; seqNo:3677; 4061649.76 north; 594878.93 east; 36.70 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T03:14:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862841.86 2024-11-06T03:14:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:05Z- waiting for 1730862845.86, time is 1730862845.48 2024-11-06T03:14:05Z- AHRS_M2 msg: 1730862845.86 epoch sec; seqNo:3451 2024-11-06T03:14:05Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T03:14:05Z- handleDepth()- Depth_Keller msg: 1730862845.87 epoch sec; seqNo:3678; depth 50.69 2024-11-06T03:14:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862845.88 epoch sec; seqNo:3678; 4061649.38 north; 594878.94 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:14:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862841.86 2024-11-06T03:14:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:06Z- waiting for 1730862845.86, time is 1730862845.88 2024-11-06T03:14:06Z- LcmTrn::updateTrn() >>>> pose time 1730862845.88, meas time 1730862845.46 <<<< 2024-11-06T03:14:06Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862845.88 2024-11-06T03:14:06Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:14:06Z- TerrainNav::motionUpdate() - currEstimate = (4061649, 594878.94, 50.69) 2024-11-06T03:14:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:14:06Z- TerrainNav:: SubcloudNIS is 0.28, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:14:06Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:14:06Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:14:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:14:06Z- variances([0], [2], and psi_berg) = 0.221145 0.208325 0.000000 2024-11-06T03:14:06Z- publishEstimatesDecoder() - at 1730862845.88 sec 2024-11-06T03:14:06Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:14:06Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061682.17 594862.31 55.89 2024-11-06T03:14:06Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061679.92 594861.63 57.60 lat/lon: 36.70 -121.94 2024-11-06T03:14:06Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.21 1.38 0.00 2024-11-06T03:14:06Z- AHRS_M2 msg: 1730862846.26 epoch sec; seqNo:3452 2024-11-06T03:14:06Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T03:14:06Z- handleDepth()- Depth_Keller msg: 1730862846.27 epoch sec; seqNo:3679; depth 50.70 2024-11-06T03:14:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862846.29 epoch sec; seqNo:3679; 4061649.00 north; 594878.94 east; 36.70 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T03:14:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862845.88 2024-11-06T03:14:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:06Z- waiting for 1730862849.88, time is 1730862846.29 2024-11-06T03:14:06Z- AHRS_M2 msg: 1730862846.67 epoch sec; seqNo:3453 2024-11-06T03:14:06Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 3.39 psi 2024-11-06T03:14:06Z- handleDepth()- Depth_Keller msg: 1730862846.68 epoch sec; seqNo:3680; depth 50.70 2024-11-06T03:14:06Z- handleDVL() - DVL msg epochMillisec: 1730862846.68 2024-11-06T03:14:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:06Z- handleDvl() - RDI_Pathfinder msg: 1730862846.68 sec; seqNo:1220; #beams:0 2024-11-06T03:14:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , -0.02 2024-11-06T03:14:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862846.71 epoch sec; seqNo:3680; 4061648.62 north; 594878.95 east; 36.70 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T03:14:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862845.88 2024-11-06T03:14:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:06Z- waiting for 1730862849.88, time is 1730862846.71 2024-11-06T03:14:07Z- AHRS_M2 msg: 1730862847.07 epoch sec; seqNo:3454 2024-11-06T03:14:07Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 3.39 psi 2024-11-06T03:14:07Z- handleDepth()- Depth_Keller msg: 1730862847.08 epoch sec; seqNo:3681; depth 50.67 2024-11-06T03:14:07Z- handleDVL() - DVL msg epochMillisec: 1730862847.10 2024-11-06T03:14:07Z- handleDvl() - RDI_Pathfinder msg: 1730862847.10 sec; seqNo:1221; #beams:4 2024-11-06T03:14:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.73 , 38.13 , 42.82 , 44.24 2024-11-06T03:14:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , -0.03 2024-11-06T03:14:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862847.11 epoch sec; seqNo:3681; 4061648.24 north; 594878.95 east; 36.70 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T03:14:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862845.88 2024-11-06T03:14:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:07Z- waiting for 1730862849.88, time is 1730862847.11 2024-11-06T03:14:07Z- AHRS_M2 msg: 1730862847.47 epoch sec; seqNo:3455 2024-11-06T03:14:07Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 3.39 psi 2024-11-06T03:14:07Z- handleDepth()- Depth_Keller msg: 1730862847.48 epoch sec; seqNo:3682; depth 50.69 2024-11-06T03:14:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862847.50 epoch sec; seqNo:3682; 4061648.24 north; 594878.95 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:14:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862845.88 2024-11-06T03:14:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:07Z- waiting for 1730862849.88, time is 1730862847.50 2024-11-06T03:14:07Z- AHRS_M2 msg: 1730862847.88 epoch sec; seqNo:3456 2024-11-06T03:14:07Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 3.39 psi 2024-11-06T03:14:07Z- handleDepth()- Depth_Keller msg: 1730862847.89 epoch sec; seqNo:3683; depth 50.67 2024-11-06T03:14:07Z- handleDVL() - DVL msg epochMillisec: 1730862847.89 2024-11-06T03:14:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:07Z- handleDvl() - RDI_Pathfinder msg: 1730862847.89 sec; seqNo:1222; #beams:0 2024-11-06T03:14:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , -0.03 2024-11-06T03:14:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862847.90 epoch sec; seqNo:3683; 4061647.86 north; 594878.95 east; 36.70 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T03:14:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862845.88 2024-11-06T03:14:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:08Z- waiting for 1730862849.88, time is 1730862847.90 2024-11-06T03:14:08Z- AHRS_M2 msg: 1730862848.28 epoch sec; seqNo:3457 2024-11-06T03:14:08Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 3.39 psi 2024-11-06T03:14:08Z- handleDepth()- Depth_Keller msg: 1730862848.29 epoch sec; seqNo:3684; depth 50.67 2024-11-06T03:14:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862848.30 epoch sec; seqNo:3684; 4061647.48 north; 594878.96 east; 36.70 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T03:14:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862845.88 2024-11-06T03:14:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:08Z- waiting for 1730862849.88, time is 1730862848.30 2024-11-06T03:14:08Z- AHRS_M2 msg: 1730862848.73 epoch sec; seqNo:3458 2024-11-06T03:14:08Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 3.39 psi 2024-11-06T03:14:08Z- handleDepth()- Depth_Keller msg: 1730862848.77 epoch sec; seqNo:3685; depth 50.65 2024-11-06T03:14:08Z- handleDVL() - DVL msg epochMillisec: 1730862848.81 2024-11-06T03:14:08Z- handleDvl() - RDI_Pathfinder msg: 1730862848.81 sec; seqNo:1223; #beams:4 2024-11-06T03:14:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.53 , 38.33 , 42.61 , 43.84 2024-11-06T03:14:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.09 , -0.01 2024-11-06T03:14:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862848.83 epoch sec; seqNo:3685; 4061647.10 north; 594878.96 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:14:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862845.88 2024-11-06T03:14:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:08Z- waiting for 1730862849.88, time is 1730862848.83 2024-11-06T03:14:09Z- AHRS_M2 msg: 1730862849.10 epoch sec; seqNo:3459 2024-11-06T03:14:09Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 3.38 psi 2024-11-06T03:14:09Z- handleDepth()- Depth_Keller msg: 1730862849.10 epoch sec; seqNo:3686; depth 50.64 2024-11-06T03:14:09Z- handleDVL() - DVL msg epochMillisec: 1730862849.11 2024-11-06T03:14:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:09Z- handleDvl() - RDI_Pathfinder msg: 1730862849.11 sec; seqNo:1224; #beams:0 2024-11-06T03:14:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.09 , -0.01 2024-11-06T03:14:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862849.12 epoch sec; seqNo:3686; 4061646.71 north; 594877.75 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:14:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862845.88 2024-11-06T03:14:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:09Z- waiting for 1730862849.88, time is 1730862849.12 2024-11-06T03:14:09Z- AHRS_M2 msg: 1730862849.49 epoch sec; seqNo:3460 2024-11-06T03:14:09Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 3.38 psi 2024-11-06T03:14:09Z- handleDepth()- Depth_Keller msg: 1730862849.50 epoch sec; seqNo:3687; depth 50.65 2024-11-06T03:14:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862849.51 epoch sec; seqNo:3687; 4061646.33 north; 594877.75 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:14:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862845.88 2024-11-06T03:14:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:09Z- waiting for 1730862849.88, time is 1730862849.51 2024-11-06T03:14:09Z- AHRS_M2 msg: 1730862849.90 epoch sec; seqNo:3461 2024-11-06T03:14:09Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 3.38 psi 2024-11-06T03:14:09Z- handleDepth()- Depth_Keller msg: 1730862849.90 epoch sec; seqNo:3688; depth 50.64 2024-11-06T03:14:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862849.91 epoch sec; seqNo:3688; 4061645.95 north; 594877.76 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:14:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862845.88 2024-11-06T03:14:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:10Z- waiting for 1730862849.88, time is 1730862849.91 2024-11-06T03:14:10Z- LcmTrn::updateTrn() >>>> pose time 1730862849.91, meas time 1730862849.11 <<<< 2024-11-06T03:14:10Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862849.91 2024-11-06T03:14:10Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:14:10Z- TerrainNav::motionUpdate() - currEstimate = (4061646, 594877.76, 50.64) 2024-11-06T03:14:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:14:10Z- TerrainNav:: SubcloudNIS is 0.28, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:14:10Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:14:10Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:14:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:14:10Z- variances([0], [2], and psi_berg) = 0.222676 0.210735 0.000000 2024-11-06T03:14:10Z- publishEstimatesDecoder() - at 1730862849.91 sec 2024-11-06T03:14:10Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:14:10Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061678.76 594861.15 55.87 2024-11-06T03:14:10Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061676.50 594860.44 57.55 lat/lon: 36.70 -121.94 2024-11-06T03:14:10Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.21 1.64 0.00 2024-11-06T03:14:10Z- AHRS_M2 msg: 1730862850.30 epoch sec; seqNo:3462 2024-11-06T03:14:10Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 3.38 psi 2024-11-06T03:14:10Z- handleDepth()- Depth_Keller msg: 1730862850.32 epoch sec; seqNo:3689; depth 50.64 2024-11-06T03:14:10Z- handleDVL() - DVL msg epochMillisec: 1730862850.32 2024-11-06T03:14:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:10Z- handleDvl() - RDI_Pathfinder msg: 1730862850.32 sec; seqNo:1225; #beams:0 2024-11-06T03:14:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.09 , -0.01 2024-11-06T03:14:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862850.33 epoch sec; seqNo:3689; 4061645.58 north; 594877.76 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:14:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862849.91 2024-11-06T03:14:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:10Z- waiting for 1730862853.91, time is 1730862850.33 2024-11-06T03:14:10Z- AHRS_M2 msg: 1730862850.75 epoch sec; seqNo:3463 2024-11-06T03:14:10Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 3.38 psi 2024-11-06T03:14:10Z- handleDepth()- Depth_Keller msg: 1730862850.76 epoch sec; seqNo:3690; depth 50.62 2024-11-06T03:14:10Z- handleDVL() - DVL msg epochMillisec: 1730862850.79 2024-11-06T03:14:10Z- handleDvl() - RDI_Pathfinder msg: 1730862850.79 sec; seqNo:1226; #beams:4 2024-11-06T03:14:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.73 , 38.33 , 42.82 , 43.63 2024-11-06T03:14:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.11 , 0.01 2024-11-06T03:14:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862850.81 epoch sec; seqNo:3690; 4061645.20 north; 594877.76 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:14:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862849.91 2024-11-06T03:14:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:10Z- waiting for 1730862853.91, time is 1730862850.81 2024-11-06T03:14:11Z- AHRS_M2 msg: 1730862851.11 epoch sec; seqNo:3464 2024-11-06T03:14:11Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 3.38 psi 2024-11-06T03:14:11Z- handleDepth()- Depth_Keller msg: 1730862851.12 epoch sec; seqNo:3691; depth 50.62 2024-11-06T03:14:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862851.13 epoch sec; seqNo:3691; 4061644.82 north; 594877.77 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:14:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862849.91 2024-11-06T03:14:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:11Z- waiting for 1730862853.91, time is 1730862851.13 2024-11-06T03:14:11Z- AHRS_M2 msg: 1730862851.52 epoch sec; seqNo:3465 2024-11-06T03:14:11Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.38 psi 2024-11-06T03:14:11Z- handleDepth()- Depth_Keller msg: 1730862851.52 epoch sec; seqNo:3692; depth 50.62 2024-11-06T03:14:11Z- handleDVL() - DVL msg epochMillisec: 1730862851.52 2024-11-06T03:14:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:11Z- handleDvl() - RDI_Pathfinder msg: 1730862851.52 sec; seqNo:1227; #beams:0 2024-11-06T03:14:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.11 , 0.01 2024-11-06T03:14:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862851.53 epoch sec; seqNo:3692; 4061644.44 north; 594877.77 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:14:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862849.91 2024-11-06T03:14:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:11Z- waiting for 1730862853.91, time is 1730862851.53 2024-11-06T03:14:11Z- AHRS_M2 msg: 1730862851.92 epoch sec; seqNo:3466 2024-11-06T03:14:11Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.38 psi 2024-11-06T03:14:11Z- handleDepth()- Depth_Keller msg: 1730862851.94 epoch sec; seqNo:3693; depth 50.62 2024-11-06T03:14:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862851.95 epoch sec; seqNo:3693; 4061644.44 north; 594877.77 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:14:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862849.91 2024-11-06T03:14:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:12Z- waiting for 1730862853.91, time is 1730862851.95 2024-11-06T03:14:12Z- AHRS_M2 msg: 1730862852.32 epoch sec; seqNo:3467 2024-11-06T03:14:12Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.38 psi 2024-11-06T03:14:12Z- handleDepth()- Depth_Keller msg: 1730862852.33 epoch sec; seqNo:3694; depth 50.62 2024-11-06T03:14:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862852.34 epoch sec; seqNo:3694; 4061644.06 north; 594877.78 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:14:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862849.91 2024-11-06T03:14:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:12Z- waiting for 1730862853.91, time is 1730862852.34 2024-11-06T03:14:12Z- AHRS_M2 msg: 1730862852.77 epoch sec; seqNo:3468 2024-11-06T03:14:12Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.38 psi 2024-11-06T03:14:12Z- handleDepth()- Depth_Keller msg: 1730862852.78 epoch sec; seqNo:3695; depth 50.61 2024-11-06T03:14:12Z- handleDVL() - DVL msg epochMillisec: 1730862852.80 2024-11-06T03:14:12Z- handleDvl() - RDI_Pathfinder msg: 1730862852.80 sec; seqNo:1228; #beams:4 2024-11-06T03:14:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.53 , 38.53 , 42.82 , 43.63 2024-11-06T03:14:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.11 , 0.03 2024-11-06T03:14:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862852.81 epoch sec; seqNo:3695; 4061643.68 north; 594877.78 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:14:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862849.91 2024-11-06T03:14:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:12Z- waiting for 1730862853.91, time is 1730862852.81 2024-11-06T03:14:13Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:14:13Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:14:13Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:14:13Z- AHRS_M2 msg: 1730862853.12 epoch sec; seqNo:3469 2024-11-06T03:14:13Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.38 psi 2024-11-06T03:14:13Z- handleDepth()- Depth_Keller msg: 1730862853.15 epoch sec; seqNo:3696; depth 50.62 2024-11-06T03:14:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862853.17 epoch sec; seqNo:3696; 4061643.29 north; 594876.56 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:14:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862849.91 2024-11-06T03:14:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:13Z- waiting for 1730862853.91, time is 1730862853.17 2024-11-06T03:14:13Z- AHRS_M2 msg: 1730862853.53 epoch sec; seqNo:3470 2024-11-06T03:14:13Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:13Z- handleDepth()- Depth_Keller msg: 1730862853.54 epoch sec; seqNo:3697; depth 50.63 2024-11-06T03:14:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862853.56 epoch sec; seqNo:3697; 4061642.91 north; 594876.57 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:14:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862849.91 2024-11-06T03:14:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:13Z- waiting for 1730862853.91, time is 1730862853.56 2024-11-06T03:14:13Z- AHRS_M2 msg: 1730862853.94 epoch sec; seqNo:3471 2024-11-06T03:14:13Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:13Z- handleDepth()- Depth_Keller msg: 1730862853.95 epoch sec; seqNo:3698; depth 50.62 2024-11-06T03:14:13Z- handleDVL() - DVL msg epochMillisec: 1730862853.95 2024-11-06T03:14:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:13Z- handleDvl() - RDI_Pathfinder msg: 1730862853.95 sec; seqNo:1229; #beams:0 2024-11-06T03:14:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.11 , 0.03 2024-11-06T03:14:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862853.98 epoch sec; seqNo:3698; 4061642.53 north; 594876.57 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:14:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862849.91 2024-11-06T03:14:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:14Z- waiting for 1730862853.91, time is 1730862853.98 2024-11-06T03:14:14Z- LcmTrn::updateTrn() >>>> pose time 1730862853.98, meas time 1730862853.95 <<<< 2024-11-06T03:14:14Z- LcmTrn::updateTrn() equating pose 1730862853.95 and meas times 1730862853.95 2024-11-06T03:14:14Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862853.95 2024-11-06T03:14:14Z- TerrainNav::motionUpdate() - currEstimate = (4061643, 594876.57, 50.62) 2024-11-06T03:14:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:14:14Z- TerrainNav:: SubcloudNIS is 0.28, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:14:14Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:14:14Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:14:14Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:14:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:14:14Z- variances([0], [2], and psi_berg) = 0.224121 0.212372 0.000000 2024-11-06T03:14:14Z- publishEstimatesDecoder() - at 1730862853.95 sec 2024-11-06T03:14:14Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:14:14Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061675.40 594859.99 56.49 2024-11-06T03:14:14Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061673.07 594859.26 57.53 lat/lon: 36.70 -121.94 2024-11-06T03:14:14Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.21 1.89 0.00 2024-11-06T03:14:14Z- AHRS_M2 msg: 1730862854.35 epoch sec; seqNo:3472 2024-11-06T03:14:14Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:14Z- handleDepth()- Depth_Keller msg: 1730862854.36 epoch sec; seqNo:3699; depth 50.62 2024-11-06T03:14:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862854.37 epoch sec; seqNo:3699; 4061642.15 north; 594876.58 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:14:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862853.95 2024-11-06T03:14:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:14Z- waiting for 1730862857.95, time is 1730862854.37 2024-11-06T03:14:14Z- AHRS_M2 msg: 1730862854.77 epoch sec; seqNo:3473 2024-11-06T03:14:14Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:14Z- handleDepth()- Depth_Keller msg: 1730862854.78 epoch sec; seqNo:3700; depth 50.62 2024-11-06T03:14:14Z- handleDVL() - DVL msg epochMillisec: 1730862854.80 2024-11-06T03:14:14Z- handleDvl() - RDI_Pathfinder msg: 1730862854.80 sec; seqNo:1230; #beams:4 2024-11-06T03:14:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.73 , 38.33 , 43.02 , 43.63 2024-11-06T03:14:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.08 , 0.04 2024-11-06T03:14:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862854.84 epoch sec; seqNo:3700; 4061641.77 north; 594876.58 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:14:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862853.95 2024-11-06T03:14:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:14Z- waiting for 1730862857.95, time is 1730862854.84 2024-11-06T03:14:15Z- AHRS_M2 msg: 1730862855.15 epoch sec; seqNo:3474 2024-11-06T03:14:15Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:15Z- handleDepth()- Depth_Keller msg: 1730862855.15 epoch sec; seqNo:3701; depth 50.61 2024-11-06T03:14:15Z- handleDVL() - DVL msg epochMillisec: 1730862855.16 2024-11-06T03:14:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:15Z- handleDvl() - RDI_Pathfinder msg: 1730862855.16 sec; seqNo:1231; #beams:0 2024-11-06T03:14:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.08 , 0.04 2024-11-06T03:14:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862855.17 epoch sec; seqNo:3701; 4061641.39 north; 594876.59 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:14:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862853.95 2024-11-06T03:14:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:15Z- waiting for 1730862857.95, time is 1730862855.17 2024-11-06T03:14:15Z- AHRS_M2 msg: 1730862855.55 epoch sec; seqNo:3475 2024-11-06T03:14:15Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:15Z- handleDepth()- Depth_Keller msg: 1730862855.57 epoch sec; seqNo:3702; depth 50.62 2024-11-06T03:14:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862855.58 epoch sec; seqNo:3702; 4061641.02 north; 594876.59 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:14:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862853.95 2024-11-06T03:14:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:15Z- waiting for 1730862857.95, time is 1730862855.58 2024-11-06T03:14:15Z- AHRS_M2 msg: 1730862855.96 epoch sec; seqNo:3476 2024-11-06T03:14:15Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:15Z- handleDepth()- Depth_Keller msg: 1730862855.97 epoch sec; seqNo:3703; depth 50.60 2024-11-06T03:14:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862855.98 epoch sec; seqNo:3703; 4061640.64 north; 594876.59 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:14:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862853.95 2024-11-06T03:14:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:16Z- waiting for 1730862857.95, time is 1730862855.98 2024-11-06T03:14:16Z- AHRS_M2 msg: 1730862856.36 epoch sec; seqNo:3477 2024-11-06T03:14:16Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:16Z- handleDepth()- Depth_Keller msg: 1730862856.37 epoch sec; seqNo:3704; depth 50.61 2024-11-06T03:14:16Z- handleDVL() - DVL msg epochMillisec: 1730862856.38 2024-11-06T03:14:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:16Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:16Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:16Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:16Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:16Z- handleDvl() - RDI_Pathfinder msg: 1730862856.38 sec; seqNo:1232; #beams:0 2024-11-06T03:14:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.08 , 0.04 2024-11-06T03:14:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862856.38 epoch sec; seqNo:3704; 4061640.26 north; 594876.60 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:14:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862853.95 2024-11-06T03:14:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:16Z- waiting for 1730862857.95, time is 1730862856.38 2024-11-06T03:14:16Z- AHRS_M2 msg: 1730862856.77 epoch sec; seqNo:3478 2024-11-06T03:14:16Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:16Z- handleDepth()- Depth_Keller msg: 1730862856.78 epoch sec; seqNo:3705; depth 50.60 2024-11-06T03:14:16Z- handleDVL() - DVL msg epochMillisec: 1730862856.80 2024-11-06T03:14:16Z- handleDvl() - RDI_Pathfinder msg: 1730862856.80 sec; seqNo:1233; #beams:4 2024-11-06T03:14:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.73 , 38.13 , 42.82 , 43.63 2024-11-06T03:14:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , 0.04 2024-11-06T03:14:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862856.82 epoch sec; seqNo:3705; 4061639.88 north; 594876.60 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:14:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862853.95 2024-11-06T03:14:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:16Z- waiting for 1730862857.95, time is 1730862856.82 2024-11-06T03:14:17Z- AHRS_M2 msg: 1730862857.17 epoch sec; seqNo:3479 2024-11-06T03:14:17Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:17Z- handleDepth()- Depth_Keller msg: 1730862857.17 epoch sec; seqNo:3706; depth 50.59 2024-11-06T03:14:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862857.19 epoch sec; seqNo:3706; 4061639.88 north; 594876.60 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:14:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862853.95 2024-11-06T03:14:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:17Z- waiting for 1730862857.95, time is 1730862857.19 2024-11-06T03:14:17Z- AHRS_M2 msg: 1730862857.58 epoch sec; seqNo:3480 2024-11-06T03:14:17Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:17Z- handleDepth()- Depth_Keller msg: 1730862857.58 epoch sec; seqNo:3707; depth 50.59 2024-11-06T03:14:17Z- handleDVL() - DVL msg epochMillisec: 1730862857.58 2024-11-06T03:14:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:17Z- handleDvl() - RDI_Pathfinder msg: 1730862857.58 sec; seqNo:1234; #beams:0 2024-11-06T03:14:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , 0.04 2024-11-06T03:14:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862857.59 epoch sec; seqNo:3707; 4061639.49 north; 594875.39 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:14:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862853.95 2024-11-06T03:14:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:17Z- waiting for 1730862857.95, time is 1730862857.59 2024-11-06T03:14:18Z- AHRS_M2 msg: 1730862858.01 epoch sec; seqNo:3481 2024-11-06T03:14:18Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:18Z- handleDepth()- Depth_Keller msg: 1730862858.04 epoch sec; seqNo:3708; depth 50.59 2024-11-06T03:14:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862858.06 epoch sec; seqNo:3708; 4061638.73 north; 594875.39 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:14:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862853.95 2024-11-06T03:14:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:18Z- waiting for 1730862857.95, time is 1730862858.06 2024-11-06T03:14:18Z- LcmTrn::updateTrn() >>>> pose time 1730862858.06, meas time 1730862857.58 <<<< 2024-11-06T03:14:18Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862858.06 2024-11-06T03:14:18Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:14:18Z- TerrainNav::motionUpdate() - currEstimate = (4061639, 594875.39, 50.59) 2024-11-06T03:14:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:14:18Z- TerrainNav:: SubcloudNIS is 0.28, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:14:18Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:14:18Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:14:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:14:18Z- variances([0], [2], and psi_berg) = 0.227344 0.214814 0.000000 2024-11-06T03:14:18Z- publishEstimatesDecoder() - at 1730862858.06 sec 2024-11-06T03:14:18Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:14:18Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061671.62 594858.75 56.03 2024-11-06T03:14:18Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061669.27 594858.08 57.50 lat/lon: 36.70 -121.94 2024-11-06T03:14:18Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.21 2.17 0.00 2024-11-06T03:14:18Z- AHRS_M2 msg: 1730862858.38 epoch sec; seqNo:3482 2024-11-06T03:14:18Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:18Z- handleDepth()- Depth_Keller msg: 1730862858.39 epoch sec; seqNo:3709; depth 50.57 2024-11-06T03:14:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862858.40 epoch sec; seqNo:3709; 4061638.73 north; 594875.39 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T03:14:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862858.06 2024-11-06T03:14:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:18Z- waiting for 1730862862.06, time is 1730862858.40 2024-11-06T03:14:18Z- AHRS_M2 msg: 1730862858.79 epoch sec; seqNo:3483 2024-11-06T03:14:18Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:18Z- handleDepth()- Depth_Keller msg: 1730862858.80 epoch sec; seqNo:3710; depth 50.55 2024-11-06T03:14:18Z- handleDVL() - DVL msg epochMillisec: 1730862858.82 2024-11-06T03:14:18Z- handleDvl() - RDI_Pathfinder msg: 1730862858.82 sec; seqNo:1235; #beams:4 2024-11-06T03:14:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.53 , 38.33 , 43.02 , 43.63 2024-11-06T03:14:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.02 , 0.01 2024-11-06T03:14:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862858.84 epoch sec; seqNo:3710; 4061638.35 north; 594875.40 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:14:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862858.06 2024-11-06T03:14:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:18Z- waiting for 1730862862.06, time is 1730862858.84 2024-11-06T03:14:19Z- AHRS_M2 msg: 1730862859.19 epoch sec; seqNo:3484 2024-11-06T03:14:19Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:19Z- handleDepth()- Depth_Keller msg: 1730862859.20 epoch sec; seqNo:3711; depth 50.54 2024-11-06T03:14:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862859.22 epoch sec; seqNo:3711; 4061637.97 north; 594875.40 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:14:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862858.06 2024-11-06T03:14:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:19Z- waiting for 1730862862.06, time is 1730862859.22 2024-11-06T03:14:19Z- AHRS_M2 msg: 1730862859.60 epoch sec; seqNo:3485 2024-11-06T03:14:19Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:19Z- handleDepth()- Depth_Keller msg: 1730862859.60 epoch sec; seqNo:3712; depth 50.53 2024-11-06T03:14:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862859.62 epoch sec; seqNo:3712; 4061637.59 north; 594875.41 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:14:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862858.06 2024-11-06T03:14:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:19Z- waiting for 1730862862.06, time is 1730862859.62 2024-11-06T03:14:20Z- AHRS_M2 msg: 1730862860.00 epoch sec; seqNo:3486 2024-11-06T03:14:20Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:20Z- handleDepth()- Depth_Keller msg: 1730862860.00 epoch sec; seqNo:3713; depth 50.53 2024-11-06T03:14:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862860.01 epoch sec; seqNo:3713; 4061637.21 north; 594875.41 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:14:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862858.06 2024-11-06T03:14:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:20Z- waiting for 1730862862.06, time is 1730862860.01 2024-11-06T03:14:20Z- AHRS_M2 msg: 1730862860.40 epoch sec; seqNo:3487 2024-11-06T03:14:20Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:20Z- handleDepth()- Depth_Keller msg: 1730862860.41 epoch sec; seqNo:3714; depth 50.51 2024-11-06T03:14:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862860.42 epoch sec; seqNo:3714; 4061636.83 north; 594875.42 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:14:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862858.06 2024-11-06T03:14:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:20Z- waiting for 1730862862.06, time is 1730862860.42 2024-11-06T03:14:20Z- AHRS_M2 msg: 1730862860.81 epoch sec; seqNo:3488 2024-11-06T03:14:20Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:20Z- handleDepth()- Depth_Keller msg: 1730862860.82 epoch sec; seqNo:3715; depth 50.50 2024-11-06T03:14:20Z- handleDVL() - DVL msg epochMillisec: 1730862860.85 2024-11-06T03:14:20Z- handleDvl() - RDI_Pathfinder msg: 1730862860.85 sec; seqNo:1236; #beams:4 2024-11-06T03:14:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.93 , 38.13 , 43.22 , 43.63 2024-11-06T03:14:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , -0.04 2024-11-06T03:14:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862860.88 epoch sec; seqNo:3715; 4061636.46 north; 594875.42 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:14:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862858.06 2024-11-06T03:14:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:20Z- waiting for 1730862862.06, time is 1730862860.88 2024-11-06T03:14:21Z- AHRS_M2 msg: 1730862861.21 epoch sec; seqNo:3489 2024-11-06T03:14:21Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:21Z- handleDepth()- Depth_Keller msg: 1730862861.21 epoch sec; seqNo:3716; depth 50.46 2024-11-06T03:14:21Z- handleDVL() - DVL msg epochMillisec: 1730862861.22 2024-11-06T03:14:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:21Z- handleDvl() - RDI_Pathfinder msg: 1730862861.22 sec; seqNo:1237; #beams:0 2024-11-06T03:14:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , -0.04 2024-11-06T03:14:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862861.24 epoch sec; seqNo:3716; 4061636.08 north; 594875.42 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:14:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862858.06 2024-11-06T03:14:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:21Z- waiting for 1730862862.06, time is 1730862861.24 2024-11-06T03:14:21Z- AHRS_M2 msg: 1730862861.62 epoch sec; seqNo:3490 2024-11-06T03:14:21Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:21Z- handleDepth()- Depth_Keller msg: 1730862861.63 epoch sec; seqNo:3717; depth 50.45 2024-11-06T03:14:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862861.65 epoch sec; seqNo:3717; 4061635.70 north; 594875.43 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:14:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862858.06 2024-11-06T03:14:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:21Z- waiting for 1730862862.06, time is 1730862861.65 2024-11-06T03:14:22Z- AHRS_M2 msg: 1730862862.02 epoch sec; seqNo:3491 2024-11-06T03:14:22Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:22Z- handleDepth()- Depth_Keller msg: 1730862862.02 epoch sec; seqNo:3718; depth 50.44 2024-11-06T03:14:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862862.03 epoch sec; seqNo:3718; 4061635.32 north; 594875.43 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:14:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862858.06 2024-11-06T03:14:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:22Z- waiting for 1730862862.06, time is 1730862862.03 2024-11-06T03:14:22Z- AHRS_M2 msg: 1730862862.43 epoch sec; seqNo:3492 2024-11-06T03:14:22Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:22Z- handleDepth()- Depth_Keller msg: 1730862862.43 epoch sec; seqNo:3719; depth 50.42 2024-11-06T03:14:22Z- handleDVL() - DVL msg epochMillisec: 1730862862.44 2024-11-06T03:14:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:22Z- handleDvl() - RDI_Pathfinder msg: 1730862862.44 sec; seqNo:1238; #beams:0 2024-11-06T03:14:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , -0.04 2024-11-06T03:14:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862862.45 epoch sec; seqNo:3719; 4061634.94 north; 594875.44 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:14:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862858.06 2024-11-06T03:14:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:22Z- waiting for 1730862862.06, time is 1730862862.45 2024-11-06T03:14:22Z- LcmTrn::updateTrn() >>>> pose time 1730862862.45, meas time 1730862862.44 <<<< 2024-11-06T03:14:22Z- LcmTrn::updateTrn() equating pose 1730862862.44 and meas times 1730862862.44 2024-11-06T03:14:22Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862862.44 2024-11-06T03:14:22Z- TerrainNav::motionUpdate() - currEstimate = (4061635, 594875.44, 50.42) 2024-11-06T03:14:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:14:22Z- TerrainNav:: SubcloudNIS is 0.28, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:14:22Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:14:22Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:14:22Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:14:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:14:22Z- variances([0], [2], and psi_berg) = 0.229275 0.216552 0.000000 2024-11-06T03:14:22Z- publishEstimatesDecoder() - at 1730862862.44 sec 2024-11-06T03:14:22Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:14:22Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061667.81 594858.80 54.94 2024-11-06T03:14:22Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061665.48 594858.12 57.33 lat/lon: 36.70 -121.94 2024-11-06T03:14:22Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.22 2.41 0.00 2024-11-06T03:14:22Z- AHRS_M2 msg: 1730862862.83 epoch sec; seqNo:3493 2024-11-06T03:14:22Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:22Z- handleDepth()- Depth_Keller msg: 1730862862.84 epoch sec; seqNo:3720; depth 50.42 2024-11-06T03:14:22Z- handleDVL() - DVL msg epochMillisec: 1730862862.87 2024-11-06T03:14:22Z- handleDvl() - RDI_Pathfinder msg: 1730862862.87 sec; seqNo:1239; #beams:4 2024-11-06T03:14:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.73 , 36.50 , 43.22 , 43.22 2024-11-06T03:14:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.09 , -0.05 2024-11-06T03:14:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862862.88 epoch sec; seqNo:3720; 4061634.55 north; 594874.22 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:14:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862862.44 2024-11-06T03:14:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:22Z- waiting for 1730862866.44, time is 1730862862.88 2024-11-06T03:14:23Z- handleDepth()- Depth_Keller msg: 1730862863.25 epoch sec; seqNo:3721; depth 50.39 2024-11-06T03:14:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862863.27 epoch sec; seqNo:3721; 4061634.17 north; 594874.22 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:14:23Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:14:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862862.44 2024-11-06T03:14:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:23Z- waiting for 1730862866.44, time is 1730862863.27 2024-11-06T03:14:23Z- AHRS_M2 msg: 1730862863.64 epoch sec; seqNo:3494 2024-11-06T03:14:23Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.37 psi 2024-11-06T03:14:23Z- handleDepth()- Depth_Keller msg: 1730862863.65 epoch sec; seqNo:3722; depth 50.38 2024-11-06T03:14:23Z- handleDVL() - DVL msg epochMillisec: 1730862863.65 2024-11-06T03:14:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:23Z- handleDvl() - RDI_Pathfinder msg: 1730862863.65 sec; seqNo:1240; #beams:0 2024-11-06T03:14:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.09 , -0.05 2024-11-06T03:14:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862863.66 epoch sec; seqNo:3722; 4061633.79 north; 594874.23 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:14:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862862.44 2024-11-06T03:14:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:23Z- waiting for 1730862866.44, time is 1730862863.66 2024-11-06T03:14:24Z- AHRS_M2 msg: 1730862864.04 epoch sec; seqNo:3495 2024-11-06T03:14:24Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.36 psi 2024-11-06T03:14:24Z- handleDepth()- Depth_Keller msg: 1730862864.05 epoch sec; seqNo:3723; depth 50.37 2024-11-06T03:14:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862864.06 epoch sec; seqNo:3723; 4061633.41 north; 594874.23 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:14:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862862.44 2024-11-06T03:14:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:24Z- waiting for 1730862866.44, time is 1730862864.06 2024-11-06T03:14:24Z- AHRS_M2 msg: 1730862864.44 epoch sec; seqNo:3496 2024-11-06T03:14:24Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.36 psi 2024-11-06T03:14:24Z- handleDepth()- Depth_Keller msg: 1730862864.45 epoch sec; seqNo:3724; depth 50.34 2024-11-06T03:14:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862864.47 epoch sec; seqNo:3724; 4061633.03 north; 594874.24 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:14:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862862.44 2024-11-06T03:14:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:24Z- waiting for 1730862866.44, time is 1730862864.47 2024-11-06T03:14:24Z- AHRS_M2 msg: 1730862864.85 epoch sec; seqNo:3497 2024-11-06T03:14:24Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.36 psi 2024-11-06T03:14:24Z- handleDepth()- Depth_Keller msg: 1730862864.86 epoch sec; seqNo:3725; depth 50.34 2024-11-06T03:14:24Z- handleDVL() - DVL msg epochMillisec: 1730862864.89 2024-11-06T03:14:24Z- handleDvl() - RDI_Pathfinder msg: 1730862864.89 sec; seqNo:1241; #beams:4 2024-11-06T03:14:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.73 , 36.90 , 43.02 , 43.43 2024-11-06T03:14:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.16 , -0.00 2024-11-06T03:14:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862864.91 epoch sec; seqNo:3725; 4061632.65 north; 594874.24 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:14:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862862.44 2024-11-06T03:14:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:25Z- waiting for 1730862866.44, time is 1730862864.91 2024-11-06T03:14:25Z- AHRS_M2 msg: 1730862865.26 epoch sec; seqNo:3498 2024-11-06T03:14:25Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 3.36 psi 2024-11-06T03:14:25Z- handleDepth()- Depth_Keller msg: 1730862865.26 epoch sec; seqNo:3726; depth 50.34 2024-11-06T03:14:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862865.28 epoch sec; seqNo:3726; 4061632.27 north; 594874.25 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:14:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862862.44 2024-11-06T03:14:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:25Z- waiting for 1730862866.44, time is 1730862865.28 2024-11-06T03:14:25Z- AHRS_M2 msg: 1730862865.66 epoch sec; seqNo:3499 2024-11-06T03:14:25Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 3.35 psi 2024-11-06T03:14:25Z- handleDepth()- Depth_Keller msg: 1730862865.66 epoch sec; seqNo:3727; depth 50.33 2024-11-06T03:14:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862865.68 epoch sec; seqNo:3727; 4061632.27 north; 594874.25 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:14:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862862.44 2024-11-06T03:14:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:25Z- waiting for 1730862866.44, time is 1730862865.68 2024-11-06T03:14:26Z- AHRS_M2 msg: 1730862866.06 epoch sec; seqNo:3500 2024-11-06T03:14:26Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 3.35 psi 2024-11-06T03:14:26Z- handleDepth()- Depth_Keller msg: 1730862866.06 epoch sec; seqNo:3728; depth 50.32 2024-11-06T03:14:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862866.07 epoch sec; seqNo:3728; 4061631.90 north; 594874.25 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:14:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862862.44 2024-11-06T03:14:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:26Z- waiting for 1730862866.44, time is 1730862866.07 2024-11-06T03:14:26Z- AHRS_M2 msg: 1730862866.46 epoch sec; seqNo:3501 2024-11-06T03:14:26Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 3.35 psi 2024-11-06T03:14:26Z- handleDepth()- Depth_Keller msg: 1730862866.47 epoch sec; seqNo:3729; depth 50.31 2024-11-06T03:14:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862866.48 epoch sec; seqNo:3729; 4061631.52 north; 594874.25 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:14:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862862.44 2024-11-06T03:14:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:26Z- waiting for 1730862866.44, time is 1730862866.48 2024-11-06T03:14:26Z- LcmTrn::updateTrn() >>>> pose time 1730862866.48, meas time 1730862864.89 <<<< 2024-11-06T03:14:26Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862866.48 2024-11-06T03:14:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862864.89 sec, ping # 0; waiting for INS data more recent than 1730862862.44... 2024-11-06T03:14:26Z- TerrainNav::motionUpdate() - currEstimate = (4061632, 594874.25, 50.31) 2024-11-06T03:14:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:14:26Z- TerrainNav:: SubcloudNIS is 0.28, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:14:26Z- TNavPF::Projecting Measurements 2024-11-06T03:14:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:14:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:14:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:14:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:14:26Z- TNavFilter::4 good beams. 2024-11-06T03:14:26Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:14:26Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:14:26Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:14:26Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:14:26Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:14:26Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:14:26Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:14:26Z- TerrainMapOctree: (vehN, vehE ) = (4061632.86, 594874.72). 2024-11-06T03:14:26Z- TerrainMapOctree: Min Distance = 171.10. 2024-11-06T03:14:26Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:14:26Z- Forcing Subcloud Comparison 2024-11-06T03:14:26Z- Weighting particles with subcloud comparison 2024-11-06T03:14:26Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:26Z- meanExpectedMeasDiff: -1.008624 delta_rms_squared: 4.207209 alpha: 0.995269 2024-11-06T03:14:26Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:26Z- meanExpectedMeasDiff: -3.278672 delta_rms_squared: 2.240197 alpha: 0.991151 2024-11-06T03:14:26Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:26Z- meanExpectedMeasDiff: -2.128684 delta_rms_squared: 3.648215 alpha: 0.994548 2024-11-06T03:14:26Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:26Z- meanExpectedMeasDiff: -0.928616 delta_rms_squared: 3.520985 alpha: 0.994352 2024-11-06T03:14:26Z- Subcloud NIS: 0.278181 2024-11-06T03:14:26Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:14:26Z- TNavPF:: sumWeights = 0.185880 2024-11-06T03:14:26Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:14:26Z- TNavPF::First Term of Map Covariance Matrix is 0.24 2024-11-06T03:14:26Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.02 2024-11-06T03:14:26Z- TerrainNav::Resampling particle filter... 2024-11-06T03:14:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862864.89 sec. 2024-11-06T03:14:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:14:26Z- variances([0], [2], and psi_berg) = 0.223475 0.224398 0.000000 2024-11-06T03:14:26Z- publishEstimatesDecoder() - at 1730862866.48 sec 2024-11-06T03:14:26Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:14:26Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061663.95 594857.76 54.51 2024-11-06T03:14:26Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061662.06 594856.92 58.47 lat/lon: 36.70 -121.94 2024-11-06T03:14:26Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.22 0.97 0.00 2024-11-06T03:14:26Z- AHRS_M2 msg: 1730862866.87 epoch sec; seqNo:3502 2024-11-06T03:14:26Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 3.35 psi 2024-11-06T03:14:26Z- handleDepth()- Depth_Keller msg: 1730862866.88 epoch sec; seqNo:3730; depth 50.31 2024-11-06T03:14:26Z- handleDVL() - DVL msg epochMillisec: 1730862866.89 2024-11-06T03:14:26Z- handleDvl() - RDI_Pathfinder msg: 1730862866.89 sec; seqNo:1242; #beams:4 2024-11-06T03:14:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.93 , 36.29 , 43.22 , 43.43 2024-11-06T03:14:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.15 , 0.07 2024-11-06T03:14:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862866.91 epoch sec; seqNo:3730; 4061631.12 north; 594873.04 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:14:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862866.48 2024-11-06T03:14:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:27Z- waiting for 1730862870.48, time is 1730862866.91 2024-11-06T03:14:27Z- AHRS_M2 msg: 1730862867.27 epoch sec; seqNo:3503 2024-11-06T03:14:27Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 3.35 psi 2024-11-06T03:14:27Z- handleDepth()- Depth_Keller msg: 1730862867.28 epoch sec; seqNo:3731; depth 50.30 2024-11-06T03:14:27Z- handleDVL() - DVL msg epochMillisec: 1730862867.28 2024-11-06T03:14:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:27Z- handleDvl() - RDI_Pathfinder msg: 1730862867.28 sec; seqNo:1243; #beams:0 2024-11-06T03:14:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.15 , 0.07 2024-11-06T03:14:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862867.31 epoch sec; seqNo:3731; 4061630.75 north; 594873.04 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:14:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862866.48 2024-11-06T03:14:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:27Z- waiting for 1730862870.48, time is 1730862867.31 2024-11-06T03:14:27Z- AHRS_M2 msg: 1730862867.67 epoch sec; seqNo:3504 2024-11-06T03:14:27Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 3.35 psi 2024-11-06T03:14:27Z- handleDepth()- Depth_Keller msg: 1730862867.68 epoch sec; seqNo:3732; depth 50.30 2024-11-06T03:14:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862867.69 epoch sec; seqNo:3732; 4061630.37 north; 594873.05 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:14:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862866.48 2024-11-06T03:14:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:27Z- waiting for 1730862870.48, time is 1730862867.69 2024-11-06T03:14:28Z- AHRS_M2 msg: 1730862868.08 epoch sec; seqNo:3505 2024-11-06T03:14:28Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 3.35 psi 2024-11-06T03:14:28Z- handleDepth()- Depth_Keller msg: 1730862868.09 epoch sec; seqNo:3733; depth 50.29 2024-11-06T03:14:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862868.10 epoch sec; seqNo:3733; 4061629.99 north; 594873.05 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:14:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862866.48 2024-11-06T03:14:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:28Z- waiting for 1730862870.48, time is 1730862868.10 2024-11-06T03:14:28Z- AHRS_M2 msg: 1730862868.48 epoch sec; seqNo:3506 2024-11-06T03:14:28Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 3.35 psi 2024-11-06T03:14:28Z- handleDepth()- Depth_Keller msg: 1730862868.50 epoch sec; seqNo:3734; depth 50.30 2024-11-06T03:14:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862868.51 epoch sec; seqNo:3734; 4061629.61 north; 594873.05 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:14:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862866.48 2024-11-06T03:14:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:28Z- waiting for 1730862870.48, time is 1730862868.51 2024-11-06T03:14:28Z- AHRS_M2 msg: 1730862868.89 epoch sec; seqNo:3507 2024-11-06T03:14:28Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 3.35 psi 2024-11-06T03:14:28Z- handleDepth()- Depth_Keller msg: 1730862868.90 epoch sec; seqNo:3735; depth 50.29 2024-11-06T03:14:28Z- handleDVL() - DVL msg epochMillisec: 1730862868.92 2024-11-06T03:14:28Z- handleDvl() - RDI_Pathfinder msg: 1730862868.92 sec; seqNo:1244; #beams:4 2024-11-06T03:14:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.53 , 36.70 , 43.43 , 43.43 2024-11-06T03:14:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , 0.09 2024-11-06T03:14:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862868.94 epoch sec; seqNo:3735; 4061629.23 north; 594873.06 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:14:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862866.48 2024-11-06T03:14:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:29Z- waiting for 1730862870.48, time is 1730862868.94 2024-11-06T03:14:29Z- AHRS_M2 msg: 1730862869.29 epoch sec; seqNo:3508 2024-11-06T03:14:29Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 3.34 psi 2024-11-06T03:14:29Z- handleDepth()- Depth_Keller msg: 1730862869.30 epoch sec; seqNo:3736; depth 50.28 2024-11-06T03:14:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862869.31 epoch sec; seqNo:3736; 4061628.85 north; 594873.06 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:14:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862866.48 2024-11-06T03:14:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:29Z- waiting for 1730862870.48, time is 1730862869.31 2024-11-06T03:14:29Z- AHRS_M2 msg: 1730862869.69 epoch sec; seqNo:3509 2024-11-06T03:14:29Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 3.34 psi 2024-11-06T03:14:29Z- handleDepth()- Depth_Keller msg: 1730862869.71 epoch sec; seqNo:3737; depth 50.29 2024-11-06T03:14:29Z- handleDVL() - DVL msg epochMillisec: 1730862869.71 2024-11-06T03:14:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:29Z- handleDvl() - RDI_Pathfinder msg: 1730862869.71 sec; seqNo:1245; #beams:0 2024-11-06T03:14:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , 0.09 2024-11-06T03:14:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862869.72 epoch sec; seqNo:3737; 4061628.47 north; 594873.07 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:14:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862866.48 2024-11-06T03:14:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:29Z- waiting for 1730862870.48, time is 1730862869.72 2024-11-06T03:14:30Z- AHRS_M2 msg: 1730862870.10 epoch sec; seqNo:3510 2024-11-06T03:14:30Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 3.34 psi 2024-11-06T03:14:30Z- handleDepth()- Depth_Keller msg: 1730862870.10 epoch sec; seqNo:3738; depth 50.27 2024-11-06T03:14:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862870.11 epoch sec; seqNo:3738; 4061628.09 north; 594873.07 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:14:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862866.48 2024-11-06T03:14:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:30Z- waiting for 1730862870.48, time is 1730862870.11 2024-11-06T03:14:30Z- AHRS_M2 msg: 1730862870.51 epoch sec; seqNo:3511 2024-11-06T03:14:30Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 3.34 psi 2024-11-06T03:14:30Z- handleDepth()- Depth_Keller msg: 1730862870.52 epoch sec; seqNo:3739; depth 50.28 2024-11-06T03:14:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862870.53 epoch sec; seqNo:3739; 4061627.72 north; 594873.08 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:14:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862866.48 2024-11-06T03:14:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:30Z- waiting for 1730862870.48, time is 1730862870.53 2024-11-06T03:14:30Z- LcmTrn::updateTrn() >>>> pose time 1730862870.53, meas time 1730862869.71 <<<< 2024-11-06T03:14:30Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862870.53 2024-11-06T03:14:30Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:14:30Z- TerrainNav::motionUpdate() - currEstimate = (4061628, 594873.08, 50.28) 2024-11-06T03:14:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:14:30Z- TerrainNav:: SubcloudNIS is 0.28, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:14:30Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:14:30Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:14:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:14:30Z- variances([0], [2], and psi_berg) = 0.225859 0.227059 0.000000 2024-11-06T03:14:30Z- publishEstimatesDecoder() - at 1730862870.53 sec 2024-11-06T03:14:30Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:14:30Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061660.09 594856.52 53.67 2024-11-06T03:14:30Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061658.26 594855.74 58.44 lat/lon: 36.70 -121.94 2024-11-06T03:14:30Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.23 1.22 0.00 2024-11-06T03:14:30Z- AHRS_M2 msg: 1730862870.91 epoch sec; seqNo:3512 2024-11-06T03:14:30Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 3.34 psi 2024-11-06T03:14:30Z- handleDepth()- Depth_Keller msg: 1730862870.91 epoch sec; seqNo:3740; depth 50.25 2024-11-06T03:14:30Z- handleDVL() - DVL msg epochMillisec: 1730862870.96 2024-11-06T03:14:30Z- handleDvl() - RDI_Pathfinder msg: 1730862870.96 sec; seqNo:1246; #beams:4 2024-11-06T03:14:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.14 , 36.90 , 43.63 , 43.43 2024-11-06T03:14:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.03 , 0.04 2024-11-06T03:14:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862870.97 epoch sec; seqNo:3740; 4061627.34 north; 594873.08 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:14:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862870.53 2024-11-06T03:14:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:31Z- waiting for 1730862874.53, time is 1730862870.97 2024-11-06T03:14:31Z- AHRS_M2 msg: 1730862871.31 epoch sec; seqNo:3513 2024-11-06T03:14:31Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 3.35 psi 2024-11-06T03:14:31Z- handleDepth()- Depth_Keller msg: 1730862871.32 epoch sec; seqNo:3741; depth 50.25 2024-11-06T03:14:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862871.34 epoch sec; seqNo:3741; 4061626.96 north; 594873.08 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:14:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862870.53 2024-11-06T03:14:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:31Z- waiting for 1730862874.53, time is 1730862871.34 2024-11-06T03:14:31Z- AHRS_M2 msg: 1730862871.72 epoch sec; seqNo:3514 2024-11-06T03:14:31Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 3.35 psi 2024-11-06T03:14:31Z- handleDepth()- Depth_Keller msg: 1730862871.73 epoch sec; seqNo:3742; depth 50.24 2024-11-06T03:14:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862871.74 epoch sec; seqNo:3742; 4061626.57 north; 594871.87 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:14:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862870.53 2024-11-06T03:14:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:31Z- waiting for 1730862874.53, time is 1730862871.74 2024-11-06T03:14:32Z- AHRS_M2 msg: 1730862872.12 epoch sec; seqNo:3515 2024-11-06T03:14:32Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.34 psi 2024-11-06T03:14:32Z- handleDepth()- Depth_Keller msg: 1730862872.12 epoch sec; seqNo:3743; depth 50.23 2024-11-06T03:14:32Z- handleDVL() - DVL msg epochMillisec: 1730862872.13 2024-11-06T03:14:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:32Z- handleDvl() - RDI_Pathfinder msg: 1730862872.13 sec; seqNo:1247; #beams:0 2024-11-06T03:14:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.03 , 0.04 2024-11-06T03:14:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862872.13 epoch sec; seqNo:3743; 4061626.19 north; 594871.87 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:14:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862870.53 2024-11-06T03:14:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:32Z- waiting for 1730862874.53, time is 1730862872.13 2024-11-06T03:14:32Z- AHRS_M2 msg: 1730862872.52 epoch sec; seqNo:3516 2024-11-06T03:14:32Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.34 psi 2024-11-06T03:14:32Z- handleDepth()- Depth_Keller msg: 1730862872.53 epoch sec; seqNo:3744; depth 50.23 2024-11-06T03:14:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862872.54 epoch sec; seqNo:3744; 4061625.81 north; 594871.88 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:14:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862870.53 2024-11-06T03:14:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:32Z- waiting for 1730862874.53, time is 1730862872.54 2024-11-06T03:14:32Z- AHRS_M2 msg: 1730862872.93 epoch sec; seqNo:3517 2024-11-06T03:14:32Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.34 psi 2024-11-06T03:14:32Z- handleDepth()- Depth_Keller msg: 1730862872.93 epoch sec; seqNo:3745; depth 50.19 2024-11-06T03:14:32Z- handleDVL() - DVL msg epochMillisec: 1730862872.96 2024-11-06T03:14:32Z- handleDvl() - RDI_Pathfinder msg: 1730862872.96 sec; seqNo:1248; #beams:4 2024-11-06T03:14:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.75 , 37.11 , 43.84 , 43.63 2024-11-06T03:14:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.01 , -0.02 2024-11-06T03:14:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862872.99 epoch sec; seqNo:3745; 4061625.43 north; 594871.88 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:14:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862870.53 2024-11-06T03:14:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:33Z- waiting for 1730862874.53, time is 1730862872.99 2024-11-06T03:14:33Z- AHRS_M2 msg: 1730862873.33 epoch sec; seqNo:3518 2024-11-06T03:14:33Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.34 psi 2024-11-06T03:14:33Z- handleDepth()- Depth_Keller msg: 1730862873.34 epoch sec; seqNo:3746; depth 50.18 2024-11-06T03:14:33Z- handleDVL() - DVL msg epochMillisec: 1730862873.34 2024-11-06T03:14:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:33Z- handleDvl() - RDI_Pathfinder msg: 1730862873.34 sec; seqNo:1249; #beams:0 2024-11-06T03:14:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.01 , -0.02 2024-11-06T03:14:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862873.36 epoch sec; seqNo:3746; 4061625.05 north; 594871.88 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:14:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862870.53 2024-11-06T03:14:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:33Z- waiting for 1730862874.53, time is 1730862873.36 2024-11-06T03:14:33Z- AHRS_M2 msg: 1730862873.73 epoch sec; seqNo:3519 2024-11-06T03:14:33Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.34 psi 2024-11-06T03:14:33Z- handleDepth()- Depth_Keller msg: 1730862873.74 epoch sec; seqNo:3747; depth 50.16 2024-11-06T03:14:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862873.75 epoch sec; seqNo:3747; 4061624.67 north; 594871.89 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:14:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862870.53 2024-11-06T03:14:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:33Z- waiting for 1730862874.53, time is 1730862873.75 2024-11-06T03:14:34Z- AHRS_M2 msg: 1730862874.14 epoch sec; seqNo:3520 2024-11-06T03:14:34Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.34 psi 2024-11-06T03:14:34Z- handleDepth()- Depth_Keller msg: 1730862874.14 epoch sec; seqNo:3748; depth 50.15 2024-11-06T03:14:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862874.15 epoch sec; seqNo:3748; 4061624.29 north; 594871.89 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:14:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862870.53 2024-11-06T03:14:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:34Z- waiting for 1730862874.53, time is 1730862874.15 2024-11-06T03:14:34Z- AHRS_M2 msg: 1730862874.54 epoch sec; seqNo:3521 2024-11-06T03:14:34Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.34 psi 2024-11-06T03:14:34Z- handleDepth()- Depth_Keller msg: 1730862874.55 epoch sec; seqNo:3749; depth 50.13 2024-11-06T03:14:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862874.56 epoch sec; seqNo:3749; 4061623.91 north; 594871.90 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:14:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862870.53 2024-11-06T03:14:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:34Z- waiting for 1730862874.53, time is 1730862874.56 2024-11-06T03:14:34Z- LcmTrn::updateTrn() >>>> pose time 1730862874.56, meas time 1730862873.34 <<<< 2024-11-06T03:14:34Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862874.56 2024-11-06T03:14:34Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:14:34Z- TerrainNav::motionUpdate() - currEstimate = (4061624, 594871.90, 50.13) 2024-11-06T03:14:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:14:34Z- TerrainNav:: SubcloudNIS is 0.28, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:14:34Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:14:34Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:14:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:14:34Z- variances([0], [2], and psi_berg) = 0.227551 0.229349 0.000000 2024-11-06T03:14:34Z- publishEstimatesDecoder() - at 1730862874.56 sec 2024-11-06T03:14:34Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:14:34Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061656.30 594855.37 53.81 2024-11-06T03:14:34Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061654.46 594854.56 58.30 lat/lon: 36.70 -121.94 2024-11-06T03:14:34Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.23 1.48 0.00 2024-11-06T03:14:34Z- AHRS_M2 msg: 1730862874.95 epoch sec; seqNo:3522 2024-11-06T03:14:34Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.34 psi 2024-11-06T03:14:34Z- handleDepth()- Depth_Keller msg: 1730862874.95 epoch sec; seqNo:3750; depth 50.10 2024-11-06T03:14:34Z- handleDVL() - DVL msg epochMillisec: 1730862874.98 2024-11-06T03:14:34Z- handleDvl() - RDI_Pathfinder msg: 1730862874.98 sec; seqNo:1250; #beams:4 2024-11-06T03:14:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.54 , 36.70 , 43.63 , 43.43 2024-11-06T03:14:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.05 , -0.03 2024-11-06T03:14:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862875.00 epoch sec; seqNo:3750; 4061623.53 north; 594871.90 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:14:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862874.56 2024-11-06T03:14:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:35Z- waiting for 1730862878.56, time is 1730862875.00 2024-11-06T03:14:35Z- AHRS_M2 msg: 1730862875.35 epoch sec; seqNo:3523 2024-11-06T03:14:35Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.34 psi 2024-11-06T03:14:35Z- handleDepth()- Depth_Keller msg: 1730862875.36 epoch sec; seqNo:3751; depth 50.08 2024-11-06T03:14:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862875.37 epoch sec; seqNo:3751; 4061623.16 north; 594871.91 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:14:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862874.56 2024-11-06T03:14:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:35Z- waiting for 1730862878.56, time is 1730862875.37 2024-11-06T03:14:35Z- AHRS_M2 msg: 1730862875.76 epoch sec; seqNo:3524 2024-11-06T03:14:35Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.34 psi 2024-11-06T03:14:35Z- handleDepth()- Depth_Keller msg: 1730862875.78 epoch sec; seqNo:3752; depth 50.06 2024-11-06T03:14:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862875.78 epoch sec; seqNo:3752; 4061622.78 north; 594871.91 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:14:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862874.56 2024-11-06T03:14:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:35Z- waiting for 1730862878.56, time is 1730862875.78 2024-11-06T03:14:36Z- AHRS_M2 msg: 1730862876.16 epoch sec; seqNo:3525 2024-11-06T03:14:36Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.33 psi 2024-11-06T03:14:36Z- handleDepth()- Depth_Keller msg: 1730862876.17 epoch sec; seqNo:3753; depth 50.03 2024-11-06T03:14:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862876.18 epoch sec; seqNo:3753; 4061622.40 north; 594871.91 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:14:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862874.56 2024-11-06T03:14:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:36Z- waiting for 1730862878.56, time is 1730862876.18 2024-11-06T03:14:36Z- AHRS_M2 msg: 1730862876.56 epoch sec; seqNo:3526 2024-11-06T03:14:36Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.33 psi 2024-11-06T03:14:36Z- handleDepth()- Depth_Keller msg: 1730862876.57 epoch sec; seqNo:3754; depth 50.03 2024-11-06T03:14:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862876.58 epoch sec; seqNo:3754; 4061622.02 north; 594871.92 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:14:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862874.56 2024-11-06T03:14:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:36Z- waiting for 1730862878.56, time is 1730862876.58 2024-11-06T03:14:36Z- AHRS_M2 msg: 1730862876.97 epoch sec; seqNo:3527 2024-11-06T03:14:36Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.33 psi 2024-11-06T03:14:36Z- handleDepth()- Depth_Keller msg: 1730862876.97 epoch sec; seqNo:3755; depth 50.02 2024-11-06T03:14:37Z- handleDVL() - DVL msg epochMillisec: 1730862877.00 2024-11-06T03:14:37Z- handleDvl() - RDI_Pathfinder msg: 1730862877.00 sec; seqNo:1251; #beams:4 2024-11-06T03:14:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.54 , 36.50 , 43.84 , 43.22 2024-11-06T03:14:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.11 , -0.01 2024-11-06T03:14:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862877.03 epoch sec; seqNo:3755; 4061621.64 north; 594871.92 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:14:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862874.56 2024-11-06T03:14:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:37Z- waiting for 1730862878.56, time is 1730862877.03 2024-11-06T03:14:37Z- AHRS_M2 msg: 1730862877.37 epoch sec; seqNo:3528 2024-11-06T03:14:37Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.33 psi 2024-11-06T03:14:37Z- handleDepth()- Depth_Keller msg: 1730862877.38 epoch sec; seqNo:3756; depth 49.99 2024-11-06T03:14:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862877.40 epoch sec; seqNo:3756; 4061621.63 north; 594870.70 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:14:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862874.56 2024-11-06T03:14:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:37Z- waiting for 1730862878.56, time is 1730862877.40 2024-11-06T03:14:37Z- AHRS_M2 msg: 1730862877.78 epoch sec; seqNo:3529 2024-11-06T03:14:37Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.33 psi 2024-11-06T03:14:37Z- handleDepth()- Depth_Keller msg: 1730862877.78 epoch sec; seqNo:3757; depth 49.98 2024-11-06T03:14:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862877.79 epoch sec; seqNo:3757; 4061621.25 north; 594870.71 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:14:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862874.56 2024-11-06T03:14:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:37Z- waiting for 1730862878.56, time is 1730862877.79 2024-11-06T03:14:38Z- AHRS_M2 msg: 1730862878.18 epoch sec; seqNo:3530 2024-11-06T03:14:38Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.33 psi 2024-11-06T03:14:38Z- handleDepth()- Depth_Keller msg: 1730862878.18 epoch sec; seqNo:3758; depth 49.96 2024-11-06T03:14:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862878.19 epoch sec; seqNo:3758; 4061620.87 north; 594870.71 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:14:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862874.56 2024-11-06T03:14:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:38Z- waiting for 1730862878.56, time is 1730862878.19 2024-11-06T03:14:38Z- AHRS_M2 msg: 1730862878.58 epoch sec; seqNo:3531 2024-11-06T03:14:38Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.33 psi 2024-11-06T03:14:38Z- handleDepth()- Depth_Keller msg: 1730862878.59 epoch sec; seqNo:3759; depth 49.94 2024-11-06T03:14:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862878.60 epoch sec; seqNo:3759; 4061620.49 north; 594870.71 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:14:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862874.56 2024-11-06T03:14:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:38Z- waiting for 1730862878.56, time is 1730862878.60 2024-11-06T03:14:38Z- LcmTrn::updateTrn() >>>> pose time 1730862878.60, meas time 1730862877.00 <<<< 2024-11-06T03:14:38Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862878.60 2024-11-06T03:14:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862877.00 sec, ping # 0; waiting for INS data more recent than 1730862874.56... 2024-11-06T03:14:38Z- TerrainNav::motionUpdate() - currEstimate = (4061620, 594870.71, 49.94) 2024-11-06T03:14:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:14:38Z- TerrainNav:: SubcloudNIS is 0.28, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:14:38Z- TNavPF::Projecting Measurements 2024-11-06T03:14:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:14:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:14:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:14:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:14:38Z- TNavFilter::4 good beams. 2024-11-06T03:14:38Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:14:38Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:14:38Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:14:38Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:14:38Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:14:38Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:14:38Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:14:38Z- TerrainMapOctree: (vehN, vehE ) = (4061621.85, 594871.18). 2024-11-06T03:14:38Z- TerrainMapOctree: Min Distance = 176.38. 2024-11-06T03:14:38Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:14:38Z- Forcing Subcloud Comparison 2024-11-06T03:14:38Z- Weighting particles with subcloud comparison 2024-11-06T03:14:38Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:38Z- meanExpectedMeasDiff: 0.573613 delta_rms_squared: 2.801147 alpha: 0.992911 2024-11-06T03:14:38Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:38Z- meanExpectedMeasDiff: -1.230853 delta_rms_squared: 1.324290 alpha: 0.985122 2024-11-06T03:14:38Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:38Z- meanExpectedMeasDiff: -0.425894 delta_rms_squared: 2.410673 alpha: 0.991772 2024-11-06T03:14:38Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:38Z- meanExpectedMeasDiff: -0.027717 delta_rms_squared: 2.301441 alpha: 0.991385 2024-11-06T03:14:38Z- Subcloud NIS: 0.255266 2024-11-06T03:14:38Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:14:38Z- TNavPF:: sumWeights = 0.478314 2024-11-06T03:14:38Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:14:38Z- TNavPF::First Term of Map Covariance Matrix is 0.56 2024-11-06T03:14:38Z- TNavPF::Calculated NIS Value : 0.14 numBeams normalized NIS: 0.04 2024-11-06T03:14:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862877.00 sec. 2024-11-06T03:14:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:14:38Z- variances([0], [2], and psi_berg) = 0.228470 0.219825 0.000000 2024-11-06T03:14:38Z- publishEstimatesDecoder() - at 1730862878.60 sec 2024-11-06T03:14:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:14:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061650.95 594852.68 58.04 2024-11-06T03:14:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061651.04 594853.34 58.24 lat/lon: 36.70 -121.94 2024-11-06T03:14:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.22 0.88 0.00 2024-11-06T03:14:38Z- AHRS_M2 msg: 1730862878.99 epoch sec; seqNo:3532 2024-11-06T03:14:38Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.32 psi 2024-11-06T03:14:39Z- handleDepth()- Depth_Keller msg: 1730862879.00 epoch sec; seqNo:3760; depth 49.93 2024-11-06T03:14:39Z- handleDVL() - DVL msg epochMillisec: 1730862879.03 2024-11-06T03:14:39Z- handleDvl() - RDI_Pathfinder msg: 1730862879.03 sec; seqNo:1252; #beams:4 2024-11-06T03:14:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.95 , 36.29 , 43.63 , 43.22 2024-11-06T03:14:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.13 , 0.01 2024-11-06T03:14:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862879.04 epoch sec; seqNo:3760; 4061620.11 north; 594870.72 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:14:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862878.60 2024-11-06T03:14:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:39Z- waiting for 1730862882.60, time is 1730862879.04 2024-11-06T03:14:39Z- AHRS_M2 msg: 1730862879.39 epoch sec; seqNo:3533 2024-11-06T03:14:39Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.32 psi 2024-11-06T03:14:39Z- handleDepth()- Depth_Keller msg: 1730862879.40 epoch sec; seqNo:3761; depth 49.91 2024-11-06T03:14:39Z- handleDVL() - DVL msg epochMillisec: 1730862879.40 2024-11-06T03:14:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:39Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:39Z- handleDvl() - RDI_Pathfinder msg: 1730862879.40 sec; seqNo:1253; #beams:0 2024-11-06T03:14:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.13 , 0.01 2024-11-06T03:14:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862879.43 epoch sec; seqNo:3761; 4061619.73 north; 594870.72 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:14:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862878.60 2024-11-06T03:14:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:39Z- waiting for 1730862882.60, time is 1730862879.43 2024-11-06T03:14:39Z- AHRS_M2 msg: 1730862879.80 epoch sec; seqNo:3534 2024-11-06T03:14:39Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.32 psi 2024-11-06T03:14:39Z- handleDepth()- Depth_Keller msg: 1730862879.80 epoch sec; seqNo:3762; depth 49.90 2024-11-06T03:14:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862879.81 epoch sec; seqNo:3762; 4061619.35 north; 594870.73 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:14:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862878.60 2024-11-06T03:14:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:39Z- waiting for 1730862882.60, time is 1730862879.81 2024-11-06T03:14:40Z- AHRS_M2 msg: 1730862880.20 epoch sec; seqNo:3535 2024-11-06T03:14:40Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.32 psi 2024-11-06T03:14:40Z- handleDepth()- Depth_Keller msg: 1730862880.21 epoch sec; seqNo:3763; depth 49.89 2024-11-06T03:14:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862880.21 epoch sec; seqNo:3763; 4061618.97 north; 594870.73 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:14:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862878.60 2024-11-06T03:14:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:40Z- waiting for 1730862882.60, time is 1730862880.21 2024-11-06T03:14:40Z- handleDepth()- Depth_Keller msg: 1730862880.62 epoch sec; seqNo:3764; depth 49.88 2024-11-06T03:14:40Z- handleDVL() - DVL msg epochMillisec: 1730862880.62 2024-11-06T03:14:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:40Z- handleDvl() - RDI_Pathfinder msg: 1730862880.62 sec; seqNo:1254; #beams:0 2024-11-06T03:14:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.13 , 0.01 2024-11-06T03:14:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862880.64 epoch sec; seqNo:3764; 4061618.60 north; 594870.74 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:14:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862878.60 2024-11-06T03:14:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:40Z- waiting for 1730862882.60, time is 1730862880.64 2024-11-06T03:14:41Z- AHRS_M2 msg: 1730862881.01 epoch sec; seqNo:3536 2024-11-06T03:14:41Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.32 psi 2024-11-06T03:14:41Z- handleDepth()- Depth_Keller msg: 1730862881.02 epoch sec; seqNo:3765; depth 49.88 2024-11-06T03:14:41Z- handleDVL() - DVL msg epochMillisec: 1730862881.05 2024-11-06T03:14:41Z- handleDvl() - RDI_Pathfinder msg: 1730862881.05 sec; seqNo:1255; #beams:4 2024-11-06T03:14:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.95 , 35.88 , 43.63 , 43.22 2024-11-06T03:14:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.13 , 0.04 2024-11-06T03:14:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862881.07 epoch sec; seqNo:3765; 4061618.22 north; 594870.74 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:14:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862878.60 2024-11-06T03:14:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:41Z- waiting for 1730862882.60, time is 1730862881.07 2024-11-06T03:14:41Z- AHRS_M2 msg: 1730862881.41 epoch sec; seqNo:3537 2024-11-06T03:14:41Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.32 psi 2024-11-06T03:14:41Z- handleDepth()- Depth_Keller msg: 1730862881.42 epoch sec; seqNo:3766; depth 49.86 2024-11-06T03:14:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862881.44 epoch sec; seqNo:3766; 4061617.84 north; 594870.74 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:14:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862878.60 2024-11-06T03:14:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:41Z- waiting for 1730862882.60, time is 1730862881.44 2024-11-06T03:14:41Z- AHRS_M2 msg: 1730862881.82 epoch sec; seqNo:3538 2024-11-06T03:14:41Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.32 psi 2024-11-06T03:14:41Z- handleDepth()- Depth_Keller msg: 1730862881.83 epoch sec; seqNo:3767; depth 49.86 2024-11-06T03:14:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862881.83 epoch sec; seqNo:3767; 4061617.45 north; 594869.53 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:14:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862878.60 2024-11-06T03:14:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:41Z- waiting for 1730862882.60, time is 1730862881.83 2024-11-06T03:14:42Z- AHRS_M2 msg: 1730862882.22 epoch sec; seqNo:3539 2024-11-06T03:14:42Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.31 psi 2024-11-06T03:14:42Z- handleDepth()- Depth_Keller msg: 1730862882.23 epoch sec; seqNo:3768; depth 49.84 2024-11-06T03:14:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862882.24 epoch sec; seqNo:3768; 4061617.07 north; 594869.53 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:14:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862878.60 2024-11-06T03:14:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:42Z- waiting for 1730862882.60, time is 1730862882.24 2024-11-06T03:14:42Z- AHRS_M2 msg: 1730862882.63 epoch sec; seqNo:3540 2024-11-06T03:14:42Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.31 psi 2024-11-06T03:14:42Z- handleDepth()- Depth_Keller msg: 1730862882.63 epoch sec; seqNo:3769; depth 49.85 2024-11-06T03:14:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862882.65 epoch sec; seqNo:3769; 4061617.07 north; 594869.53 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:14:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862878.60 2024-11-06T03:14:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:42Z- waiting for 1730862882.60, time is 1730862882.65 2024-11-06T03:14:42Z- LcmTrn::updateTrn() >>>> pose time 1730862882.65, meas time 1730862881.05 <<<< 2024-11-06T03:14:42Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862882.65 2024-11-06T03:14:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862881.05 sec, ping # 0; waiting for INS data more recent than 1730862878.60... 2024-11-06T03:14:42Z- TerrainNav::motionUpdate() - currEstimate = (4061617, 594869.53, 49.85) 2024-11-06T03:14:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:14:42Z- TerrainNav:: SubcloudNIS is 0.26, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:14:42Z- TNavPF::Projecting Measurements 2024-11-06T03:14:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:14:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:14:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:14:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:14:42Z- TNavFilter::4 good beams. 2024-11-06T03:14:42Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:14:42Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:14:42Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:14:42Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:14:42Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:14:42Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:14:42Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:14:42Z- TerrainMapOctree: (vehN, vehE ) = (4061618.42, 594870.00). 2024-11-06T03:14:42Z- TerrainMapOctree: Min Distance = 178.08. 2024-11-06T03:14:42Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:14:42Z- Forcing Subcloud Comparison 2024-11-06T03:14:42Z- Weighting particles with subcloud comparison 2024-11-06T03:14:42Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:42Z- meanExpectedMeasDiff: 0.816126 delta_rms_squared: 1.831304 alpha: 0.989197 2024-11-06T03:14:42Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:42Z- meanExpectedMeasDiff: -1.409152 delta_rms_squared: 1.069021 alpha: 0.981635 2024-11-06T03:14:42Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:42Z- meanExpectedMeasDiff: -0.705882 delta_rms_squared: 1.570281 alpha: 0.987424 2024-11-06T03:14:42Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:42Z- meanExpectedMeasDiff: -0.021361 delta_rms_squared: 1.493679 alpha: 0.986787 2024-11-06T03:14:42Z- Subcloud NIS: 0.248068 2024-11-06T03:14:42Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:14:42Z- TNavPF:: sumWeights = 0.476157 2024-11-06T03:14:42Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:14:42Z- TNavPF::First Term of Map Covariance Matrix is 0.53 2024-11-06T03:14:42Z- TNavPF::Calculated NIS Value : 0.21 numBeams normalized NIS: 0.05 2024-11-06T03:14:42Z- TerrainNav::Resampling particle filter... 2024-11-06T03:14:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862881.05 sec. 2024-11-06T03:14:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:14:42Z- variances([0], [2], and psi_berg) = 0.222300 0.207209 0.000000 2024-11-06T03:14:42Z- publishEstimatesDecoder() - at 1730862882.65 sec 2024-11-06T03:14:42Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:14:42Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061647.52 594853.33 59.68 2024-11-06T03:14:42Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061647.64 594852.13 58.30 lat/lon: 36.70 -121.94 2024-11-06T03:14:42Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.21 0.78 0.00 2024-11-06T03:14:43Z- AHRS_M2 msg: 1730862883.06 epoch sec; seqNo:3541 2024-11-06T03:14:43Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.31 psi 2024-11-06T03:14:43Z- handleDepth()- Depth_Keller msg: 1730862883.08 epoch sec; seqNo:3770; depth 49.82 2024-11-06T03:14:43Z- handleDVL() - DVL msg epochMillisec: 1730862883.12 2024-11-06T03:14:43Z- handleDvl() - RDI_Pathfinder msg: 1730862883.12 sec; seqNo:1256; #beams:4 2024-11-06T03:14:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.95 , 35.68 , 44.04 , 43.43 2024-11-06T03:14:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.08 , 0.08 2024-11-06T03:14:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862883.14 epoch sec; seqNo:3770; 4061616.31 north; 594869.54 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:14:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862882.65 2024-11-06T03:14:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:43Z- waiting for 1730862886.65, time is 1730862883.14 2024-11-06T03:14:43Z- AHRS_M2 msg: 1730862883.43 epoch sec; seqNo:3542 2024-11-06T03:14:43Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.31 psi 2024-11-06T03:14:43Z- handleDepth()- Depth_Keller msg: 1730862883.44 epoch sec; seqNo:3771; depth 49.82 2024-11-06T03:14:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862883.47 epoch sec; seqNo:3771; 4061616.31 north; 594869.54 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:14:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862882.65 2024-11-06T03:14:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:43Z- waiting for 1730862886.65, time is 1730862883.47 2024-11-06T03:14:43Z- AHRS_M2 msg: 1730862883.84 epoch sec; seqNo:3543 2024-11-06T03:14:43Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.31 psi 2024-11-06T03:14:43Z- handleDepth()- Depth_Keller msg: 1730862883.84 epoch sec; seqNo:3772; depth 49.81 2024-11-06T03:14:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862883.85 epoch sec; seqNo:3772; 4061615.93 north; 594869.54 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:14:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862882.65 2024-11-06T03:14:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:43Z- waiting for 1730862886.65, time is 1730862883.85 2024-11-06T03:14:44Z- AHRS_M2 msg: 1730862884.24 epoch sec; seqNo:3544 2024-11-06T03:14:44Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.31 psi 2024-11-06T03:14:44Z- handleDepth()- Depth_Keller msg: 1730862884.24 epoch sec; seqNo:3773; depth 49.78 2024-11-06T03:14:44Z- handleDVL() - DVL msg epochMillisec: 1730862884.25 2024-11-06T03:14:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:44Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:44Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:44Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:44Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:44Z- handleDvl() - RDI_Pathfinder msg: 1730862884.25 sec; seqNo:1257; #beams:0 2024-11-06T03:14:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.08 , 0.08 2024-11-06T03:14:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862884.26 epoch sec; seqNo:3773; 4061615.55 north; 594869.55 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:14:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862882.65 2024-11-06T03:14:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:44Z- waiting for 1730862886.65, time is 1730862884.26 2024-11-06T03:14:44Z- AHRS_M2 msg: 1730862884.65 epoch sec; seqNo:3545 2024-11-06T03:14:44Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.31 psi 2024-11-06T03:14:44Z- handleDepth()- Depth_Keller msg: 1730862884.65 epoch sec; seqNo:3774; depth 49.79 2024-11-06T03:14:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862884.67 epoch sec; seqNo:3774; 4061615.17 north; 594869.55 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:14:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862882.65 2024-11-06T03:14:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:44Z- waiting for 1730862886.65, time is 1730862884.67 2024-11-06T03:14:45Z- AHRS_M2 msg: 1730862885.05 epoch sec; seqNo:3546 2024-11-06T03:14:45Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.32 psi 2024-11-06T03:14:45Z- handleDepth()- Depth_Keller msg: 1730862885.05 epoch sec; seqNo:3775; depth 49.78 2024-11-06T03:14:45Z- handleDVL() - DVL msg epochMillisec: 1730862885.07 2024-11-06T03:14:45Z- handleDvl() - RDI_Pathfinder msg: 1730862885.07 sec; seqNo:1258; #beams:4 2024-11-06T03:14:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.95 , 35.88 , 44.04 , 43.63 2024-11-06T03:14:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.00 , 0.07 2024-11-06T03:14:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862885.10 epoch sec; seqNo:3775; 4061614.79 north; 594869.56 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:14:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862882.65 2024-11-06T03:14:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:45Z- waiting for 1730862886.65, time is 1730862885.10 2024-11-06T03:14:45Z- AHRS_M2 msg: 1730862885.46 epoch sec; seqNo:3547 2024-11-06T03:14:45Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.32 psi 2024-11-06T03:14:45Z- handleDepth()- Depth_Keller msg: 1730862885.46 epoch sec; seqNo:3776; depth 49.76 2024-11-06T03:14:45Z- handleDVL() - DVL msg epochMillisec: 1730862885.46 2024-11-06T03:14:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:45Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:45Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:45Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:45Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:45Z- handleDvl() - RDI_Pathfinder msg: 1730862885.46 sec; seqNo:1259; #beams:0 2024-11-06T03:14:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.00 , 0.07 2024-11-06T03:14:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862885.48 epoch sec; seqNo:3776; 4061614.41 north; 594869.56 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:14:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862882.65 2024-11-06T03:14:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:45Z- waiting for 1730862886.65, time is 1730862885.48 2024-11-06T03:14:45Z- AHRS_M2 msg: 1730862885.86 epoch sec; seqNo:3548 2024-11-06T03:14:45Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.32 psi 2024-11-06T03:14:45Z- handleDepth()- Depth_Keller msg: 1730862885.88 epoch sec; seqNo:3777; depth 49.75 2024-11-06T03:14:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862885.88 epoch sec; seqNo:3777; 4061614.04 north; 594869.57 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:14:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862882.65 2024-11-06T03:14:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:46Z- waiting for 1730862886.65, time is 1730862885.88 2024-11-06T03:14:46Z- AHRS_M2 msg: 1730862886.26 epoch sec; seqNo:3549 2024-11-06T03:14:46Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.32 psi 2024-11-06T03:14:46Z- handleDepth()- Depth_Keller msg: 1730862886.27 epoch sec; seqNo:3778; depth 49.74 2024-11-06T03:14:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862886.30 epoch sec; seqNo:3778; 4061613.66 north; 594869.57 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:14:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862882.65 2024-11-06T03:14:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:46Z- waiting for 1730862886.65, time is 1730862886.30 2024-11-06T03:14:46Z- AHRS_M2 msg: 1730862886.66 epoch sec; seqNo:3550 2024-11-06T03:14:46Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.32 psi 2024-11-06T03:14:46Z- handleDepth()- Depth_Keller msg: 1730862886.67 epoch sec; seqNo:3779; depth 49.71 2024-11-06T03:14:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862886.69 epoch sec; seqNo:3779; 4061613.28 north; 594869.57 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:14:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862882.65 2024-11-06T03:14:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:46Z- waiting for 1730862886.65, time is 1730862886.69 2024-11-06T03:14:46Z- LcmTrn::updateTrn() >>>> pose time 1730862886.69, meas time 1730862885.46 <<<< 2024-11-06T03:14:46Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862886.69 2024-11-06T03:14:46Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:14:46Z- TerrainNav::motionUpdate() - currEstimate = (4061613, 594869.57, 49.71) 2024-11-06T03:14:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:14:46Z- TerrainNav:: SubcloudNIS is 0.25, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:14:46Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:14:46Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:14:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:14:46Z- variances([0], [2], and psi_berg) = 0.224572 0.208695 0.000000 2024-11-06T03:14:46Z- publishEstimatesDecoder() - at 1730862886.69 sec 2024-11-06T03:14:46Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:14:46Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061643.76 594853.37 59.22 2024-11-06T03:14:46Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061643.86 594852.17 58.16 lat/lon: 36.70 -121.94 2024-11-06T03:14:46Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.21 1.04 0.00 2024-11-06T03:14:47Z- AHRS_M2 msg: 1730862887.07 epoch sec; seqNo:3551 2024-11-06T03:14:47Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.32 psi 2024-11-06T03:14:47Z- handleDepth()- Depth_Keller msg: 1730862887.08 epoch sec; seqNo:3780; depth 49.70 2024-11-06T03:14:47Z- handleDVL() - DVL msg epochMillisec: 1730862887.11 2024-11-06T03:14:47Z- handleDvl() - RDI_Pathfinder msg: 1730862887.11 sec; seqNo:1260; #beams:4 2024-11-06T03:14:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.16 , 35.68 , 44.04 , 43.63 2024-11-06T03:14:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.02 , 0.01 2024-11-06T03:14:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862887.12 epoch sec; seqNo:3780; 4061612.90 north; 594869.58 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:14:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862886.69 2024-11-06T03:14:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:47Z- waiting for 1730862890.69, time is 1730862887.12 2024-11-06T03:14:47Z- AHRS_M2 msg: 1730862887.47 epoch sec; seqNo:3552 2024-11-06T03:14:47Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.32 psi 2024-11-06T03:14:47Z- handleDepth()- Depth_Keller msg: 1730862887.47 epoch sec; seqNo:3781; depth 49.68 2024-11-06T03:14:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862887.50 epoch sec; seqNo:3781; 4061612.52 north; 594869.58 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:14:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862886.69 2024-11-06T03:14:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:47Z- waiting for 1730862890.69, time is 1730862887.50 2024-11-06T03:14:47Z- AHRS_M2 msg: 1730862887.87 epoch sec; seqNo:3553 2024-11-06T03:14:47Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.32 psi 2024-11-06T03:14:47Z- handleDepth()- Depth_Keller msg: 1730862887.88 epoch sec; seqNo:3782; depth 49.66 2024-11-06T03:14:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862887.90 epoch sec; seqNo:3782; 4061612.13 north; 594868.37 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:14:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862886.69 2024-11-06T03:14:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:48Z- waiting for 1730862890.69, time is 1730862887.90 2024-11-06T03:14:48Z- AHRS_M2 msg: 1730862888.28 epoch sec; seqNo:3554 2024-11-06T03:14:48Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.32 psi 2024-11-06T03:14:48Z- handleDepth()- Depth_Keller msg: 1730862888.29 epoch sec; seqNo:3783; depth 49.64 2024-11-06T03:14:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862888.30 epoch sec; seqNo:3783; 4061611.75 north; 594868.37 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:14:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862886.69 2024-11-06T03:14:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:48Z- waiting for 1730862890.69, time is 1730862888.30 2024-11-06T03:14:48Z- handleDepth()- Depth_Keller msg: 1730862888.74 epoch sec; seqNo:3784; depth 49.62 2024-11-06T03:14:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862888.76 epoch sec; seqNo:3784; 4061610.99 north; 594868.38 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:14:48Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:14:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862886.69 2024-11-06T03:14:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:48Z- waiting for 1730862890.69, time is 1730862888.76 2024-11-06T03:14:49Z- AHRS_M2 msg: 1730862889.09 epoch sec; seqNo:3555 2024-11-06T03:14:49Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.32 psi 2024-11-06T03:14:49Z- handleDepth()- Depth_Keller msg: 1730862889.10 epoch sec; seqNo:3785; depth 49.58 2024-11-06T03:14:49Z- handleDVL() - DVL msg epochMillisec: 1730862889.12 2024-11-06T03:14:49Z- handleDvl() - RDI_Pathfinder msg: 1730862889.12 sec; seqNo:1261; #beams:4 2024-11-06T03:14:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.36 , 35.27 , 44.45 , 43.63 2024-11-06T03:14:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.03 , -0.04 2024-11-06T03:14:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862889.13 epoch sec; seqNo:3785; 4061610.99 north; 594868.38 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:14:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862886.69 2024-11-06T03:14:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:49Z- waiting for 1730862890.69, time is 1730862889.13 2024-11-06T03:14:49Z- AHRS_M2 msg: 1730862889.49 epoch sec; seqNo:3556 2024-11-06T03:14:49Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.32 psi 2024-11-06T03:14:49Z- handleDepth()- Depth_Keller msg: 1730862889.50 epoch sec; seqNo:3786; depth 49.57 2024-11-06T03:14:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862889.53 epoch sec; seqNo:3786; 4061610.61 north; 594868.38 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:14:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862886.69 2024-11-06T03:14:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:49Z- waiting for 1730862890.69, time is 1730862889.53 2024-11-06T03:14:49Z- AHRS_M2 msg: 1730862889.89 epoch sec; seqNo:3557 2024-11-06T03:14:49Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.32 psi 2024-11-06T03:14:49Z- handleDepth()- Depth_Keller msg: 1730862889.90 epoch sec; seqNo:3787; depth 49.55 2024-11-06T03:14:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862889.91 epoch sec; seqNo:3787; 4061610.23 north; 594868.39 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:14:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862886.69 2024-11-06T03:14:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:50Z- waiting for 1730862890.69, time is 1730862889.91 2024-11-06T03:14:50Z- AHRS_M2 msg: 1730862890.30 epoch sec; seqNo:3558 2024-11-06T03:14:50Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.32 psi 2024-11-06T03:14:50Z- handleDepth()- Depth_Keller msg: 1730862890.31 epoch sec; seqNo:3788; depth 49.50 2024-11-06T03:14:50Z- handleDVL() - DVL msg epochMillisec: 1730862890.31 2024-11-06T03:14:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:50Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:50Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:50Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:50Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:50Z- handleDvl() - RDI_Pathfinder msg: 1730862890.31 sec; seqNo:1262; #beams:0 2024-11-06T03:14:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.03 , -0.04 2024-11-06T03:14:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862890.32 epoch sec; seqNo:3788; 4061609.86 north; 594868.39 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:14:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862886.69 2024-11-06T03:14:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:50Z- waiting for 1730862890.69, time is 1730862890.32 2024-11-06T03:14:50Z- AHRS_M2 msg: 1730862890.74 epoch sec; seqNo:3559 2024-11-06T03:14:50Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.32 psi 2024-11-06T03:14:50Z- handleDepth()- Depth_Keller msg: 1730862890.76 epoch sec; seqNo:3789; depth 49.50 2024-11-06T03:14:50Z- handleDVL() - DVL msg epochMillisec: 1730862890.79 2024-11-06T03:14:50Z- handleDvl() - RDI_Pathfinder msg: 1730862890.79 sec; seqNo:1263; #beams:4 2024-11-06T03:14:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.16 , 34.66 , 44.45 , 43.63 2024-11-06T03:14:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.12 , -0.03 2024-11-06T03:14:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862890.83 epoch sec; seqNo:3789; 4061609.10 north; 594868.40 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:14:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862886.69 2024-11-06T03:14:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:50Z- waiting for 1730862890.69, time is 1730862890.83 2024-11-06T03:14:50Z- LcmTrn::updateTrn() >>>> pose time 1730862890.83, meas time 1730862890.79 <<<< 2024-11-06T03:14:50Z- LcmTrn::updateTrn() equating pose 1730862890.79 and meas times 1730862890.79 2024-11-06T03:14:50Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862890.79 2024-11-06T03:14:50Z- TerrainNav::motionUpdate() - currEstimate = (4061609, 594868.40, 49.50) 2024-11-06T03:14:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:14:50Z- TerrainNav:: SubcloudNIS is 0.25, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:14:50Z- TNavPF::Projecting Measurements 2024-11-06T03:14:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:14:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:14:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:14:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:14:50Z- TNavFilter::4 good beams. 2024-11-06T03:14:50Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:14:50Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:14:50Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:14:50Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:14:50Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:14:50Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:14:50Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:14:50Z- TerrainMapOctree: (vehN, vehE ) = (4061609.10, 594868.40). 2024-11-06T03:14:50Z- TerrainMapOctree: Min Distance = 184.04. 2024-11-06T03:14:50Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:14:51Z- Forcing Subcloud Comparison 2024-11-06T03:14:51Z- Weighting particles with subcloud comparison 2024-11-06T03:14:51Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:51Z- meanExpectedMeasDiff: 0.733981 delta_rms_squared: 2.078464 alpha: 0.990469 2024-11-06T03:14:51Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:51Z- meanExpectedMeasDiff: -2.829946 delta_rms_squared: 1.195752 alpha: 0.983549 2024-11-06T03:14:51Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:51Z- meanExpectedMeasDiff: 0.244817 delta_rms_squared: 1.901756 alpha: 0.989593 2024-11-06T03:14:51Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:51Z- meanExpectedMeasDiff: -0.173421 delta_rms_squared: 1.854272 alpha: 0.989329 2024-11-06T03:14:51Z- Subcloud NIS: 0.248843 2024-11-06T03:14:51Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:14:51Z- TNavPF:: sumWeights = 0.230974 2024-11-06T03:14:51Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:14:51Z- TNavPF::First Term of Map Covariance Matrix is 0.35 2024-11-06T03:14:51Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T03:14:51Z- TerrainNav::Resampling particle filter... 2024-11-06T03:14:51Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862890.79 sec, ping # 0. 2024-11-06T03:14:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:14:51Z- variances([0], [2], and psi_berg) = 0.229229 0.164565 0.000000 2024-11-06T03:14:51Z- publishEstimatesDecoder() - at 1730862890.79 sec 2024-11-06T03:14:51Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:14:51Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061639.39 594852.02 58.42 2024-11-06T03:14:51Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061639.68 594850.96 58.24 lat/lon: 36.70 -121.94 2024-11-06T03:14:51Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.16 0.51 0.00 2024-11-06T03:14:51Z- AHRS_M2 msg: 1730862891.11 epoch sec; seqNo:3560 2024-11-06T03:14:51Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.32 psi 2024-11-06T03:14:51Z- handleDepth()- Depth_Keller msg: 1730862891.11 epoch sec; seqNo:3790; depth 49.46 2024-11-06T03:14:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862891.12 epoch sec; seqNo:3790; 4061609.10 north; 594868.40 east; 36.70 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T03:14:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862890.79 2024-11-06T03:14:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:51Z- waiting for 1730862894.79, time is 1730862891.12 2024-11-06T03:14:51Z- AHRS_M2 msg: 1730862891.51 epoch sec; seqNo:3561 2024-11-06T03:14:51Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.32 psi 2024-11-06T03:14:51Z- handleDepth()- Depth_Keller msg: 1730862891.52 epoch sec; seqNo:3791; depth 49.44 2024-11-06T03:14:51Z- handleDVL() - DVL msg epochMillisec: 1730862891.52 2024-11-06T03:14:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:51Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:51Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:51Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:51Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:51Z- handleDvl() - RDI_Pathfinder msg: 1730862891.52 sec; seqNo:1264; #beams:0 2024-11-06T03:14:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.12 , -0.03 2024-11-06T03:14:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862891.53 epoch sec; seqNo:3791; 4061608.72 north; 594868.40 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:14:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862890.79 2024-11-06T03:14:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:51Z- waiting for 1730862894.79, time is 1730862891.53 2024-11-06T03:14:51Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:14:51Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:14:51Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:14:51Z- AHRS_M2 msg: 1730862891.91 epoch sec; seqNo:3562 2024-11-06T03:14:51Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.32 psi 2024-11-06T03:14:51Z- handleDepth()- Depth_Keller msg: 1730862891.95 epoch sec; seqNo:3792; depth 49.43 2024-11-06T03:14:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862891.97 epoch sec; seqNo:3792; 4061608.34 north; 594868.41 east; 36.70 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T03:14:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862890.79 2024-11-06T03:14:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:52Z- waiting for 1730862894.79, time is 1730862891.97 2024-11-06T03:14:52Z- AHRS_M2 msg: 1730862892.32 epoch sec; seqNo:3563 2024-11-06T03:14:52Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.31 psi 2024-11-06T03:14:52Z- handleDepth()- Depth_Keller msg: 1730862892.33 epoch sec; seqNo:3793; depth 49.40 2024-11-06T03:14:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862892.34 epoch sec; seqNo:3793; 4061607.96 north; 594868.41 east; 36.70 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T03:14:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862890.79 2024-11-06T03:14:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:52Z- waiting for 1730862894.79, time is 1730862892.34 2024-11-06T03:14:52Z- handleDepth()- Depth_Keller msg: 1730862892.78 epoch sec; seqNo:3794; depth 49.38 2024-11-06T03:14:52Z- handleDVL() - DVL msg epochMillisec: 1730862892.80 2024-11-06T03:14:52Z- handleDvl() - RDI_Pathfinder msg: 1730862892.80 sec; seqNo:1265; #beams:4 2024-11-06T03:14:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.56 , 34.05 , 44.24 , 43.43 2024-11-06T03:14:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.15 , 0.04 2024-11-06T03:14:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862892.82 epoch sec; seqNo:3794; 4061607.58 north; 594868.42 east; 36.70 lat; -121.94 lon; 49.38 depth; utmZone: 10 2024-11-06T03:14:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862890.79 2024-11-06T03:14:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:52Z- waiting for 1730862894.79, time is 1730862892.82 2024-11-06T03:14:53Z- AHRS_M2 msg: 1730862893.13 epoch sec; seqNo:3564 2024-11-06T03:14:53Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.31 psi 2024-11-06T03:14:53Z- handleDepth()- Depth_Keller msg: 1730862893.13 epoch sec; seqNo:3795; depth 49.37 2024-11-06T03:14:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862893.15 epoch sec; seqNo:3795; 4061607.19 north; 594867.20 east; 36.70 lat; -121.94 lon; 49.37 depth; utmZone: 10 2024-11-06T03:14:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862890.79 2024-11-06T03:14:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:53Z- waiting for 1730862894.79, time is 1730862893.15 2024-11-06T03:14:53Z- AHRS_M2 msg: 1730862893.53 epoch sec; seqNo:3565 2024-11-06T03:14:53Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.31 psi 2024-11-06T03:14:53Z- handleDepth()- Depth_Keller msg: 1730862893.54 epoch sec; seqNo:3796; depth 49.35 2024-11-06T03:14:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862893.55 epoch sec; seqNo:3796; 4061606.81 north; 594867.20 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:14:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862890.79 2024-11-06T03:14:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:53Z- waiting for 1730862894.79, time is 1730862893.55 2024-11-06T03:14:53Z- AHRS_M2 msg: 1730862893.93 epoch sec; seqNo:3566 2024-11-06T03:14:53Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.31 psi 2024-11-06T03:14:53Z- handleDepth()- Depth_Keller msg: 1730862893.94 epoch sec; seqNo:3797; depth 49.35 2024-11-06T03:14:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862893.95 epoch sec; seqNo:3797; 4061606.43 north; 594867.21 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:14:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862890.79 2024-11-06T03:14:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:54Z- waiting for 1730862894.79, time is 1730862893.95 2024-11-06T03:14:54Z- AHRS_M2 msg: 1730862894.34 epoch sec; seqNo:3567 2024-11-06T03:14:54Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.31 psi 2024-11-06T03:14:54Z- handleDepth()- Depth_Keller msg: 1730862894.34 epoch sec; seqNo:3798; depth 49.34 2024-11-06T03:14:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862894.35 epoch sec; seqNo:3798; 4061606.05 north; 594867.21 east; 36.70 lat; -121.94 lon; 49.34 depth; utmZone: 10 2024-11-06T03:14:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862890.79 2024-11-06T03:14:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:54Z- waiting for 1730862894.79, time is 1730862894.35 2024-11-06T03:14:54Z- AHRS_M2 msg: 1730862894.76 epoch sec; seqNo:3568 2024-11-06T03:14:54Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.31 psi 2024-11-06T03:14:54Z- handleDepth()- Depth_Keller msg: 1730862894.78 epoch sec; seqNo:3799; depth 49.33 2024-11-06T03:14:54Z- handleDVL() - DVL msg epochMillisec: 1730862894.80 2024-11-06T03:14:54Z- handleDvl() - RDI_Pathfinder msg: 1730862894.80 sec; seqNo:1266; #beams:4 2024-11-06T03:14:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.16 , 34.46 , 44.45 , 44.24 2024-11-06T03:14:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.11 , 0.09 2024-11-06T03:14:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862894.82 epoch sec; seqNo:3799; 4061605.67 north; 594867.22 east; 36.70 lat; -121.94 lon; 49.33 depth; utmZone: 10 2024-11-06T03:14:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862890.79 2024-11-06T03:14:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:54Z- waiting for 1730862894.79, time is 1730862894.82 2024-11-06T03:14:54Z- LcmTrn::updateTrn() >>>> pose time 1730862894.82, meas time 1730862894.80 <<<< 2024-11-06T03:14:54Z- LcmTrn::updateTrn() equating pose 1730862894.80 and meas times 1730862894.80 2024-11-06T03:14:54Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862894.80 2024-11-06T03:14:54Z- TerrainNav::motionUpdate() - currEstimate = (4061606, 594867.22, 49.33) 2024-11-06T03:14:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:14:54Z- TerrainNav:: SubcloudNIS is 0.25, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:14:54Z- TNavPF::Projecting Measurements 2024-11-06T03:14:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:14:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:14:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:14:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:14:54Z- TNavFilter::4 good beams. 2024-11-06T03:14:54Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:14:54Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:14:54Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:14:54Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:14:54Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:14:54Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:14:54Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:14:54Z- TerrainMapOctree: (vehN, vehE ) = (4061605.67, 594867.22). 2024-11-06T03:14:54Z- TerrainMapOctree: Min Distance = 185.93. 2024-11-06T03:14:54Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:14:55Z- Forcing Subcloud Comparison 2024-11-06T03:14:55Z- Weighting particles with subcloud comparison 2024-11-06T03:14:55Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:55Z- meanExpectedMeasDiff: 0.685451 delta_rms_squared: 1.253972 alpha: 0.984301 2024-11-06T03:14:55Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:55Z- meanExpectedMeasDiff: -2.827102 delta_rms_squared: 0.730704 alpha: 0.973358 2024-11-06T03:14:55Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:55Z- meanExpectedMeasDiff: 1.480058 delta_rms_squared: 1.401772 alpha: 0.985933 2024-11-06T03:14:55Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:55Z- meanExpectedMeasDiff: 0.094777 delta_rms_squared: 1.170298 alpha: 0.983197 2024-11-06T03:14:55Z- Subcloud NIS: 0.252385 2024-11-06T03:14:55Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:14:55Z- TNavPF:: sumWeights = 0.193297 2024-11-06T03:14:55Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:14:55Z- TNavPF::First Term of Map Covariance Matrix is 0.20 2024-11-06T03:14:55Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T03:14:55Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862894.80 sec, ping # 0. 2024-11-06T03:14:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:14:55Z- variances([0], [2], and psi_berg) = 0.238311 0.149313 0.000000 2024-11-06T03:14:55Z- publishEstimatesDecoder() - at 1730862894.80 sec 2024-11-06T03:14:55Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:14:55Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061637.08 594850.96 58.68 2024-11-06T03:14:55Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061636.26 594849.77 58.13 lat/lon: 36.70 -121.94 2024-11-06T03:14:55Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.15 0.39 0.00 2024-11-06T03:14:55Z- AHRS_M2 msg: 1730862895.17 epoch sec; seqNo:3569 2024-11-06T03:14:55Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.31 psi 2024-11-06T03:14:55Z- handleDepth()- Depth_Keller msg: 1730862895.18 epoch sec; seqNo:3800; depth 49.32 2024-11-06T03:14:55Z- handleDVL() - DVL msg epochMillisec: 1730862895.18 2024-11-06T03:14:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:14:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:14:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:14:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:14:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:14:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:14:55Z- handleDvl() - RDI_Pathfinder msg: 1730862895.18 sec; seqNo:1267; #beams:0 2024-11-06T03:14:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:14:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.11 , 0.09 2024-11-06T03:14:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862895.21 epoch sec; seqNo:3800; 4061605.67 north; 594867.22 east; 36.70 lat; -121.94 lon; 49.32 depth; utmZone: 10 2024-11-06T03:14:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862894.80 2024-11-06T03:14:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:55Z- waiting for 1730862898.80, time is 1730862895.21 2024-11-06T03:14:55Z- AHRS_M2 msg: 1730862895.55 epoch sec; seqNo:3570 2024-11-06T03:14:55Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.31 psi 2024-11-06T03:14:55Z- handleDepth()- Depth_Keller msg: 1730862895.56 epoch sec; seqNo:3801; depth 49.33 2024-11-06T03:14:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862895.57 epoch sec; seqNo:3801; 4061605.30 north; 594867.22 east; 36.70 lat; -121.94 lon; 49.33 depth; utmZone: 10 2024-11-06T03:14:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862894.80 2024-11-06T03:14:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:55Z- waiting for 1730862898.80, time is 1730862895.57 2024-11-06T03:14:55Z- AHRS_M2 msg: 1730862895.95 epoch sec; seqNo:3571 2024-11-06T03:14:55Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.31 psi 2024-11-06T03:14:55Z- handleDepth()- Depth_Keller msg: 1730862895.96 epoch sec; seqNo:3802; depth 49.32 2024-11-06T03:14:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862895.97 epoch sec; seqNo:3802; 4061604.92 north; 594867.23 east; 36.70 lat; -121.94 lon; 49.32 depth; utmZone: 10 2024-11-06T03:14:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862894.80 2024-11-06T03:14:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:56Z- waiting for 1730862898.80, time is 1730862895.97 2024-11-06T03:14:56Z- handleDepth()- Depth_Keller msg: 1730862896.38 epoch sec; seqNo:3803; depth 49.31 2024-11-06T03:14:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862896.39 epoch sec; seqNo:3803; 4061604.54 north; 594867.23 east; 36.70 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T03:14:56Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:14:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862894.80 2024-11-06T03:14:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:56Z- waiting for 1730862898.80, time is 1730862896.39 2024-11-06T03:14:56Z- handleDepth()- Depth_Keller msg: 1730862896.79 epoch sec; seqNo:3804; depth 49.31 2024-11-06T03:14:56Z- handleDVL() - DVL msg epochMillisec: 1730862896.82 2024-11-06T03:14:56Z- handleDvl() - RDI_Pathfinder msg: 1730862896.82 sec; seqNo:1268; #beams:4 2024-11-06T03:14:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.77 , 34.66 , 44.04 , 44.24 2024-11-06T03:14:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , 0.08 2024-11-06T03:14:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862896.84 epoch sec; seqNo:3804; 4061604.16 north; 594867.23 east; 36.70 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T03:14:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862894.80 2024-11-06T03:14:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:56Z- waiting for 1730862898.80, time is 1730862896.84 2024-11-06T03:14:57Z- AHRS_M2 msg: 1730862897.17 epoch sec; seqNo:3572 2024-11-06T03:14:57Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.32 psi 2024-11-06T03:14:57Z- handleDepth()- Depth_Keller msg: 1730862897.17 epoch sec; seqNo:3805; depth 49.31 2024-11-06T03:14:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862897.19 epoch sec; seqNo:3805; 4061603.78 north; 594867.24 east; 36.70 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T03:14:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862894.80 2024-11-06T03:14:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:57Z- waiting for 1730862898.80, time is 1730862897.19 2024-11-06T03:14:57Z- AHRS_M2 msg: 1730862897.58 epoch sec; seqNo:3573 2024-11-06T03:14:57Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.32 psi 2024-11-06T03:14:57Z- handleDepth()- Depth_Keller msg: 1730862897.58 epoch sec; seqNo:3806; depth 49.31 2024-11-06T03:14:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862897.59 epoch sec; seqNo:3806; 4061603.40 north; 594867.24 east; 36.70 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T03:14:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862894.80 2024-11-06T03:14:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:57Z- waiting for 1730862898.80, time is 1730862897.59 2024-11-06T03:14:57Z- AHRS_M2 msg: 1730862897.97 epoch sec; seqNo:3574 2024-11-06T03:14:57Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.32 psi 2024-11-06T03:14:57Z- handleDepth()- Depth_Keller msg: 1730862897.99 epoch sec; seqNo:3807; depth 49.31 2024-11-06T03:14:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862898.00 epoch sec; seqNo:3807; 4061603.01 north; 594866.03 east; 36.70 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T03:14:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862894.80 2024-11-06T03:14:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:58Z- waiting for 1730862898.80, time is 1730862898.00 2024-11-06T03:14:58Z- AHRS_M2 msg: 1730862898.38 epoch sec; seqNo:3575 2024-11-06T03:14:58Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.33 psi 2024-11-06T03:14:58Z- handleDepth()- Depth_Keller msg: 1730862898.38 epoch sec; seqNo:3808; depth 49.31 2024-11-06T03:14:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862898.39 epoch sec; seqNo:3808; 4061602.63 north; 594866.03 east; 36.70 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T03:14:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862894.80 2024-11-06T03:14:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:58Z- waiting for 1730862898.80, time is 1730862898.39 2024-11-06T03:14:58Z- AHRS_M2 msg: 1730862898.79 epoch sec; seqNo:3576 2024-11-06T03:14:58Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.33 psi 2024-11-06T03:14:58Z- handleDepth()- Depth_Keller msg: 1730862898.79 epoch sec; seqNo:3809; depth 49.32 2024-11-06T03:14:58Z- handleDVL() - DVL msg epochMillisec: 1730862898.81 2024-11-06T03:14:58Z- handleDvl() - RDI_Pathfinder msg: 1730862898.81 sec; seqNo:1269; #beams:4 2024-11-06T03:14:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 51.18 , 34.05 , 43.84 , 45.06 2024-11-06T03:14:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , 0.05 2024-11-06T03:14:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862898.85 epoch sec; seqNo:3809; 4061602.25 north; 594866.03 east; 36.70 lat; -121.94 lon; 49.32 depth; utmZone: 10 2024-11-06T03:14:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:14:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862894.80 2024-11-06T03:14:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:58Z- waiting for 1730862898.80, time is 1730862898.85 2024-11-06T03:14:58Z- LcmTrn::updateTrn() >>>> pose time 1730862898.85, meas time 1730862898.81 <<<< 2024-11-06T03:14:58Z- LcmTrn::updateTrn() equating pose 1730862898.81 and meas times 1730862898.81 2024-11-06T03:14:58Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862898.81 2024-11-06T03:14:58Z- TerrainNav::motionUpdate() - currEstimate = (4061602, 594866.03, 49.32) 2024-11-06T03:14:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:14:58Z- TerrainNav:: SubcloudNIS is 0.25, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:14:58Z- TNavPF::Projecting Measurements 2024-11-06T03:14:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:14:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:14:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:14:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:14:58Z- TNavFilter::4 good beams. 2024-11-06T03:14:58Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:14:58Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:14:58Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:14:58Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:14:58Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:14:58Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:14:58Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:14:58Z- TerrainMapOctree: (vehN, vehE ) = (4061602.25, 594866.03). 2024-11-06T03:14:58Z- TerrainMapOctree: Min Distance = 187.87. 2024-11-06T03:14:58Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:14:59Z- Forcing Subcloud Comparison 2024-11-06T03:14:59Z- Weighting particles with subcloud comparison 2024-11-06T03:14:59Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:59Z- meanExpectedMeasDiff: 1.365513 delta_rms_squared: 1.073302 alpha: 0.981707 2024-11-06T03:14:59Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:59Z- meanExpectedMeasDiff: -2.958963 delta_rms_squared: 0.712226 alpha: 0.972686 2024-11-06T03:14:59Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:59Z- meanExpectedMeasDiff: 1.607219 delta_rms_squared: 1.156932 alpha: 0.983007 2024-11-06T03:14:59Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:14:59Z- meanExpectedMeasDiff: 0.286005 delta_rms_squared: 0.961706 alpha: 0.979627 2024-11-06T03:14:59Z- Subcloud NIS: 0.264112 2024-11-06T03:14:59Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:14:59Z- TNavPF:: sumWeights = 0.148864 2024-11-06T03:14:59Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:14:59Z- TNavPF::First Term of Map Covariance Matrix is 0.30 2024-11-06T03:14:59Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T03:14:59Z- TerrainNav::Resampling particle filter... 2024-11-06T03:14:59Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862898.81 sec, ping # 0. 2024-11-06T03:14:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:14:59Z- variances([0], [2], and psi_berg) = 0.239989 0.146329 0.000000 2024-11-06T03:14:59Z- publishEstimatesDecoder() - at 1730862898.81 sec 2024-11-06T03:14:59Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:14:59Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061632.55 594849.66 57.24 2024-11-06T03:14:59Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061632.83 594848.60 58.14 lat/lon: 36.70 -121.94 2024-11-06T03:14:59Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.15 0.36 0.00 2024-11-06T03:14:59Z- AHRS_M2 msg: 1730862899.19 epoch sec; seqNo:3577 2024-11-06T03:14:59Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.33 psi 2024-11-06T03:14:59Z- handleDepth()- Depth_Keller msg: 1730862899.21 epoch sec; seqNo:3810; depth 49.30 2024-11-06T03:14:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862899.23 epoch sec; seqNo:3810; 4061601.87 north; 594866.04 east; 36.70 lat; -121.94 lon; 49.30 depth; utmZone: 10 2024-11-06T03:14:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862898.81 2024-11-06T03:14:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:59Z- waiting for 1730862902.81, time is 1730862899.23 2024-11-06T03:14:59Z- AHRS_M2 msg: 1730862899.59 epoch sec; seqNo:3578 2024-11-06T03:14:59Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T03:14:59Z- handleDepth()- Depth_Keller msg: 1730862899.60 epoch sec; seqNo:3811; depth 49.30 2024-11-06T03:14:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862899.62 epoch sec; seqNo:3811; 4061601.49 north; 594866.04 east; 36.70 lat; -121.94 lon; 49.30 depth; utmZone: 10 2024-11-06T03:14:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:14:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862898.81 2024-11-06T03:14:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:14:59Z- waiting for 1730862902.81, time is 1730862899.62 2024-11-06T03:15:00Z- AHRS_M2 msg: 1730862899.99 epoch sec; seqNo:3579 2024-11-06T03:15:00Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T03:15:00Z- handleDepth()- Depth_Keller msg: 1730862900.01 epoch sec; seqNo:3812; depth 49.31 2024-11-06T03:15:00Z- handleDVL() - DVL msg epochMillisec: 1730862900.01 2024-11-06T03:15:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:00Z- handleDvl() - RDI_Pathfinder msg: 1730862900.01 sec; seqNo:1270; #beams:0 2024-11-06T03:15:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , 0.05 2024-11-06T03:15:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862900.02 epoch sec; seqNo:3812; 4061601.11 north; 594866.05 east; 36.70 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T03:15:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862898.81 2024-11-06T03:15:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:00Z- waiting for 1730862902.81, time is 1730862900.02 2024-11-06T03:15:00Z- AHRS_M2 msg: 1730862900.40 epoch sec; seqNo:3580 2024-11-06T03:15:00Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.34 psi 2024-11-06T03:15:00Z- handleDepth()- Depth_Keller msg: 1730862900.41 epoch sec; seqNo:3813; depth 49.30 2024-11-06T03:15:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862900.42 epoch sec; seqNo:3813; 4061600.74 north; 594866.05 east; 36.70 lat; -121.94 lon; 49.30 depth; utmZone: 10 2024-11-06T03:15:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862898.81 2024-11-06T03:15:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:00Z- waiting for 1730862902.81, time is 1730862900.42 2024-11-06T03:15:00Z- AHRS_M2 msg: 1730862900.80 epoch sec; seqNo:3581 2024-11-06T03:15:00Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.34 psi 2024-11-06T03:15:00Z- handleDepth()- Depth_Keller msg: 1730862900.81 epoch sec; seqNo:3814; depth 49.31 2024-11-06T03:15:00Z- handleDVL() - DVL msg epochMillisec: 1730862900.84 2024-11-06T03:15:00Z- handleDvl() - RDI_Pathfinder msg: 1730862900.84 sec; seqNo:1271; #beams:4 2024-11-06T03:15:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.97 , 34.87 , 43.43 , 45.26 2024-11-06T03:15:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , 0.03 2024-11-06T03:15:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862900.85 epoch sec; seqNo:3814; 4061600.36 north; 594866.06 east; 36.70 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T03:15:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862898.81 2024-11-06T03:15:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:00Z- waiting for 1730862902.81, time is 1730862900.85 2024-11-06T03:15:01Z- AHRS_M2 msg: 1730862901.21 epoch sec; seqNo:3582 2024-11-06T03:15:01Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.34 psi 2024-11-06T03:15:01Z- handleDepth()- Depth_Keller msg: 1730862901.22 epoch sec; seqNo:3815; depth 49.30 2024-11-06T03:15:01Z- handleDVL() - DVL msg epochMillisec: 1730862901.22 2024-11-06T03:15:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:01Z- handleDvl() - RDI_Pathfinder msg: 1730862901.22 sec; seqNo:1272; #beams:0 2024-11-06T03:15:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , 0.03 2024-11-06T03:15:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862901.25 epoch sec; seqNo:3815; 4061599.98 north; 594866.06 east; 36.70 lat; -121.94 lon; 49.30 depth; utmZone: 10 2024-11-06T03:15:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862898.81 2024-11-06T03:15:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:01Z- waiting for 1730862902.81, time is 1730862901.25 2024-11-06T03:15:01Z- AHRS_M2 msg: 1730862901.61 epoch sec; seqNo:3583 2024-11-06T03:15:01Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.34 psi 2024-11-06T03:15:01Z- handleDepth()- Depth_Keller msg: 1730862901.62 epoch sec; seqNo:3816; depth 49.29 2024-11-06T03:15:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862901.63 epoch sec; seqNo:3816; 4061599.60 north; 594866.06 east; 36.70 lat; -121.94 lon; 49.29 depth; utmZone: 10 2024-11-06T03:15:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862898.81 2024-11-06T03:15:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:01Z- waiting for 1730862902.81, time is 1730862901.63 2024-11-06T03:15:02Z- AHRS_M2 msg: 1730862902.02 epoch sec; seqNo:3584 2024-11-06T03:15:02Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.34 psi 2024-11-06T03:15:02Z- handleDepth()- Depth_Keller msg: 1730862902.03 epoch sec; seqNo:3817; depth 49.30 2024-11-06T03:15:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862902.03 epoch sec; seqNo:3817; 4061599.22 north; 594866.07 east; 36.70 lat; -121.94 lon; 49.30 depth; utmZone: 10 2024-11-06T03:15:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862898.81 2024-11-06T03:15:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:02Z- waiting for 1730862902.81, time is 1730862902.03 2024-11-06T03:15:02Z- AHRS_M2 msg: 1730862902.42 epoch sec; seqNo:3585 2024-11-06T03:15:02Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.35 psi 2024-11-06T03:15:02Z- handleDepth()- Depth_Keller msg: 1730862902.43 epoch sec; seqNo:3818; depth 49.29 2024-11-06T03:15:02Z- handleDVL() - DVL msg epochMillisec: 1730862902.43 2024-11-06T03:15:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:02Z- handleDvl() - RDI_Pathfinder msg: 1730862902.43 sec; seqNo:1273; #beams:0 2024-11-06T03:15:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , 0.03 2024-11-06T03:15:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862902.45 epoch sec; seqNo:3818; 4061598.84 north; 594866.07 east; 36.70 lat; -121.94 lon; 49.29 depth; utmZone: 10 2024-11-06T03:15:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862898.81 2024-11-06T03:15:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:02Z- waiting for 1730862902.81, time is 1730862902.45 2024-11-06T03:15:02Z- AHRS_M2 msg: 1730862902.82 epoch sec; seqNo:3586 2024-11-06T03:15:02Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.35 psi 2024-11-06T03:15:02Z- handleDepth()- Depth_Keller msg: 1730862902.83 epoch sec; seqNo:3819; depth 49.29 2024-11-06T03:15:02Z- handleDVL() - DVL msg epochMillisec: 1730862902.86 2024-11-06T03:15:02Z- handleDvl() - RDI_Pathfinder msg: 1730862902.86 sec; seqNo:1274; #beams:4 2024-11-06T03:15:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.97 , 34.25 , 43.02 , 45.47 2024-11-06T03:15:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.00 , 0.02 2024-11-06T03:15:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862902.88 epoch sec; seqNo:3819; 4061598.46 north; 594866.08 east; 36.70 lat; -121.94 lon; 49.29 depth; utmZone: 10 2024-11-06T03:15:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862898.81 2024-11-06T03:15:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:02Z- waiting for 1730862902.81, time is 1730862902.88 2024-11-06T03:15:02Z- LcmTrn::updateTrn() >>>> pose time 1730862902.88, meas time 1730862902.86 <<<< 2024-11-06T03:15:02Z- LcmTrn::updateTrn() equating pose 1730862902.86 and meas times 1730862902.86 2024-11-06T03:15:02Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862902.86 2024-11-06T03:15:02Z- TerrainNav::motionUpdate() - currEstimate = (4061598, 594866.08, 49.29) 2024-11-06T03:15:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:15:02Z- TerrainNav:: SubcloudNIS is 0.26, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:15:03Z- TNavPF::Projecting Measurements 2024-11-06T03:15:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:15:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:15:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:15:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:15:03Z- TNavFilter::4 good beams. 2024-11-06T03:15:03Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:15:03Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:15:03Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:15:03Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:15:03Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:15:03Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:15:03Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:15:03Z- TerrainMapOctree: (vehN, vehE ) = (4061598.46, 594866.08). 2024-11-06T03:15:03Z- TerrainMapOctree: Min Distance = 190.89. 2024-11-06T03:15:03Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:15:03Z- Forcing Subcloud Comparison 2024-11-06T03:15:03Z- Weighting particles with subcloud comparison 2024-11-06T03:15:03Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:03Z- meanExpectedMeasDiff: 1.289879 delta_rms_squared: 0.986246 alpha: 0.980124 2024-11-06T03:15:03Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:03Z- meanExpectedMeasDiff: -2.842925 delta_rms_squared: 0.662158 alpha: 0.970681 2024-11-06T03:15:03Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:03Z- meanExpectedMeasDiff: 1.362401 delta_rms_squared: 0.818446 alpha: 0.976146 2024-11-06T03:15:03Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:03Z- meanExpectedMeasDiff: 0.195695 delta_rms_squared: 0.861820 alpha: 0.977320 2024-11-06T03:15:03Z- Subcloud NIS: 0.276645 2024-11-06T03:15:03Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:15:03Z- TNavPF:: sumWeights = 0.186620 2024-11-06T03:15:03Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:15:03Z- TNavPF::First Term of Map Covariance Matrix is 0.34 2024-11-06T03:15:03Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T03:15:03Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862902.86 sec, ping # 0. 2024-11-06T03:15:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:15:03Z- variances([0], [2], and psi_berg) = 0.241319 0.147147 0.000000 2024-11-06T03:15:03Z- publishEstimatesDecoder() - at 1730862902.86 sec 2024-11-06T03:15:03Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:15:03Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061628.75 594849.39 58.49 2024-11-06T03:15:03Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061629.04 594848.64 58.16 lat/lon: 36.70 -121.94 2024-11-06T03:15:03Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.15 0.36 0.00 2024-11-06T03:15:03Z- AHRS_M2 msg: 1730862903.23 epoch sec; seqNo:3587 2024-11-06T03:15:03Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.35 psi 2024-11-06T03:15:03Z- handleDepth()- Depth_Keller msg: 1730862903.24 epoch sec; seqNo:3820; depth 49.27 2024-11-06T03:15:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862903.27 epoch sec; seqNo:3820; 4061598.46 north; 594866.08 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T03:15:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862902.86 2024-11-06T03:15:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:03Z- waiting for 1730862906.86, time is 1730862903.27 2024-11-06T03:15:03Z- AHRS_M2 msg: 1730862903.64 epoch sec; seqNo:3588 2024-11-06T03:15:03Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.35 psi 2024-11-06T03:15:03Z- handleDepth()- Depth_Keller msg: 1730862903.64 epoch sec; seqNo:3821; depth 49.27 2024-11-06T03:15:03Z- handleDVL() - DVL msg epochMillisec: 1730862903.65 2024-11-06T03:15:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:03Z- handleDvl() - RDI_Pathfinder msg: 1730862903.65 sec; seqNo:1275; #beams:0 2024-11-06T03:15:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.00 , 0.02 2024-11-06T03:15:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862903.66 epoch sec; seqNo:3821; 4061598.08 north; 594866.08 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T03:15:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862902.86 2024-11-06T03:15:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:03Z- waiting for 1730862906.86, time is 1730862903.66 2024-11-06T03:15:04Z- AHRS_M2 msg: 1730862904.03 epoch sec; seqNo:3589 2024-11-06T03:15:04Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.35 psi 2024-11-06T03:15:04Z- handleDepth()- Depth_Keller msg: 1730862904.06 epoch sec; seqNo:3822; depth 49.26 2024-11-06T03:15:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862904.06 epoch sec; seqNo:3822; 4061597.71 north; 594866.08 east; 36.70 lat; -121.94 lon; 49.26 depth; utmZone: 10 2024-11-06T03:15:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862902.86 2024-11-06T03:15:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:04Z- waiting for 1730862906.86, time is 1730862904.06 2024-11-06T03:15:04Z- AHRS_M2 msg: 1730862904.44 epoch sec; seqNo:3590 2024-11-06T03:15:04Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.36 psi 2024-11-06T03:15:04Z- handleDepth()- Depth_Keller msg: 1730862904.45 epoch sec; seqNo:3823; depth 49.27 2024-11-06T03:15:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862904.46 epoch sec; seqNo:3823; 4061597.33 north; 594866.09 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T03:15:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862902.86 2024-11-06T03:15:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:04Z- waiting for 1730862906.86, time is 1730862904.46 2024-11-06T03:15:04Z- AHRS_M2 msg: 1730862904.85 epoch sec; seqNo:3591 2024-11-06T03:15:04Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.36 psi 2024-11-06T03:15:04Z- handleDepth()- Depth_Keller msg: 1730862904.86 epoch sec; seqNo:3824; depth 49.27 2024-11-06T03:15:04Z- handleDVL() - DVL msg epochMillisec: 1730862904.90 2024-11-06T03:15:04Z- handleDvl() - RDI_Pathfinder msg: 1730862904.90 sec; seqNo:1276; #beams:4 2024-11-06T03:15:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.97 , 34.05 , 42.82 , 45.88 2024-11-06T03:15:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , -0.00 2024-11-06T03:15:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862904.91 epoch sec; seqNo:3824; 4061596.93 north; 594864.87 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T03:15:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862902.86 2024-11-06T03:15:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:05Z- waiting for 1730862906.86, time is 1730862904.91 2024-11-06T03:15:05Z- AHRS_M2 msg: 1730862905.26 epoch sec; seqNo:3592 2024-11-06T03:15:05Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.36 psi 2024-11-06T03:15:05Z- handleDepth()- Depth_Keller msg: 1730862905.27 epoch sec; seqNo:3825; depth 49.27 2024-11-06T03:15:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862905.28 epoch sec; seqNo:3825; 4061596.56 north; 594864.88 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T03:15:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862902.86 2024-11-06T03:15:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:05Z- waiting for 1730862906.86, time is 1730862905.28 2024-11-06T03:15:05Z- AHRS_M2 msg: 1730862905.66 epoch sec; seqNo:3593 2024-11-06T03:15:05Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.37 psi 2024-11-06T03:15:05Z- handleDepth()- Depth_Keller msg: 1730862905.66 epoch sec; seqNo:3826; depth 49.27 2024-11-06T03:15:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862905.67 epoch sec; seqNo:3826; 4061596.18 north; 594864.88 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T03:15:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862902.86 2024-11-06T03:15:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:05Z- waiting for 1730862906.86, time is 1730862905.67 2024-11-06T03:15:06Z- AHRS_M2 msg: 1730862906.06 epoch sec; seqNo:3594 2024-11-06T03:15:06Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.37 psi 2024-11-06T03:15:06Z- handleDepth()- Depth_Keller msg: 1730862906.07 epoch sec; seqNo:3827; depth 49.26 2024-11-06T03:15:06Z- handleDVL() - DVL msg epochMillisec: 1730862906.07 2024-11-06T03:15:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:06Z- handleDvl() - RDI_Pathfinder msg: 1730862906.07 sec; seqNo:1277; #beams:0 2024-11-06T03:15:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , -0.00 2024-11-06T03:15:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862906.09 epoch sec; seqNo:3827; 4061595.80 north; 594864.89 east; 36.70 lat; -121.94 lon; 49.26 depth; utmZone: 10 2024-11-06T03:15:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862902.86 2024-11-06T03:15:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:06Z- waiting for 1730862906.86, time is 1730862906.09 2024-11-06T03:15:06Z- AHRS_M2 msg: 1730862906.46 epoch sec; seqNo:3595 2024-11-06T03:15:06Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.37 psi 2024-11-06T03:15:06Z- handleDepth()- Depth_Keller msg: 1730862906.47 epoch sec; seqNo:3828; depth 49.26 2024-11-06T03:15:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862906.48 epoch sec; seqNo:3828; 4061595.42 north; 594864.89 east; 36.70 lat; -121.94 lon; 49.26 depth; utmZone: 10 2024-11-06T03:15:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862902.86 2024-11-06T03:15:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:06Z- waiting for 1730862906.86, time is 1730862906.48 2024-11-06T03:15:06Z- AHRS_M2 msg: 1730862906.87 epoch sec; seqNo:3596 2024-11-06T03:15:06Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.37 psi 2024-11-06T03:15:06Z- handleDepth()- Depth_Keller msg: 1730862906.88 epoch sec; seqNo:3829; depth 49.26 2024-11-06T03:15:06Z- handleDVL() - DVL msg epochMillisec: 1730862906.90 2024-11-06T03:15:06Z- handleDvl() - RDI_Pathfinder msg: 1730862906.90 sec; seqNo:1278; #beams:4 2024-11-06T03:15:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.77 , 33.64 , 42.41 , 45.67 2024-11-06T03:15:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.09 , 0.00 2024-11-06T03:15:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862906.94 epoch sec; seqNo:3829; 4061595.04 north; 594864.89 east; 36.70 lat; -121.94 lon; 49.26 depth; utmZone: 10 2024-11-06T03:15:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862902.86 2024-11-06T03:15:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:07Z- waiting for 1730862906.86, time is 1730862906.94 2024-11-06T03:15:07Z- LcmTrn::updateTrn() >>>> pose time 1730862906.94, meas time 1730862906.90 <<<< 2024-11-06T03:15:07Z- LcmTrn::updateTrn() equating pose 1730862906.90 and meas times 1730862906.90 2024-11-06T03:15:07Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862906.90 2024-11-06T03:15:07Z- TerrainNav::motionUpdate() - currEstimate = (4061595, 594864.89, 49.26) 2024-11-06T03:15:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:15:07Z- TerrainNav:: SubcloudNIS is 0.28, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:15:07Z- TNavPF::Projecting Measurements 2024-11-06T03:15:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:15:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:15:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:15:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:15:07Z- TNavFilter::4 good beams. 2024-11-06T03:15:07Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:15:07Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:15:07Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:15:07Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:15:07Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:15:07Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:15:07Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:15:07Z- TerrainMapOctree: (vehN, vehE ) = (4061595.04, 594864.89). 2024-11-06T03:15:07Z- TerrainMapOctree: Min Distance = 192.91. 2024-11-06T03:15:07Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:15:07Z- Forcing Subcloud Comparison 2024-11-06T03:15:07Z- Weighting particles with subcloud comparison 2024-11-06T03:15:07Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:07Z- meanExpectedMeasDiff: 0.880616 delta_rms_squared: 0.995411 alpha: 0.980304 2024-11-06T03:15:07Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:07Z- meanExpectedMeasDiff: -3.401683 delta_rms_squared: 0.661398 alpha: 0.970649 2024-11-06T03:15:07Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:07Z- meanExpectedMeasDiff: 1.082167 delta_rms_squared: 0.757483 alpha: 0.974276 2024-11-06T03:15:07Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:07Z- meanExpectedMeasDiff: -0.049111 delta_rms_squared: 0.880021 alpha: 0.977778 2024-11-06T03:15:07Z- Subcloud NIS: 0.292757 2024-11-06T03:15:07Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:15:07Z- TNavPF:: sumWeights = 0.138379 2024-11-06T03:15:07Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:15:07Z- TNavPF::First Term of Map Covariance Matrix is 0.21 2024-11-06T03:15:07Z- TNavPF::Calculated NIS Value : 0.07 numBeams normalized NIS: 0.02 2024-11-06T03:15:07Z- TerrainNav::Resampling particle filter... 2024-11-06T03:15:07Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862906.90 sec, ping # 0. 2024-11-06T03:15:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:15:07Z- variances([0], [2], and psi_berg) = 0.240865 0.146030 0.000000 2024-11-06T03:15:07Z- publishEstimatesDecoder() - at 1730862906.90 sec 2024-11-06T03:15:07Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:15:07Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061625.81 594848.10 58.87 2024-11-06T03:15:07Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061625.62 594847.46 58.32 lat/lon: 36.70 -121.94 2024-11-06T03:15:07Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.15 0.34 0.00 2024-11-06T03:15:07Z- AHRS_M2 msg: 1730862907.27 epoch sec; seqNo:3597 2024-11-06T03:15:07Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.37 psi 2024-11-06T03:15:07Z- handleDepth()- Depth_Keller msg: 1730862907.28 epoch sec; seqNo:3830; depth 49.25 2024-11-06T03:15:07Z- handleDVL() - DVL msg epochMillisec: 1730862907.28 2024-11-06T03:15:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:07Z- handleDvl() - RDI_Pathfinder msg: 1730862907.28 sec; seqNo:1279; #beams:0 2024-11-06T03:15:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.09 , 0.00 2024-11-06T03:15:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862907.31 epoch sec; seqNo:3830; 4061594.66 north; 594864.90 east; 36.70 lat; -121.94 lon; 49.25 depth; utmZone: 10 2024-11-06T03:15:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862906.90 2024-11-06T03:15:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:07Z- waiting for 1730862910.90, time is 1730862907.31 2024-11-06T03:15:07Z- handleDepth()- Depth_Keller msg: 1730862907.69 epoch sec; seqNo:3831; depth 49.26 2024-11-06T03:15:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862907.72 epoch sec; seqNo:3831; 4061594.28 north; 594864.90 east; 36.70 lat; -121.94 lon; 49.26 depth; utmZone: 10 2024-11-06T03:15:07Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:15:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862906.90 2024-11-06T03:15:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:07Z- waiting for 1730862910.90, time is 1730862907.72 2024-11-06T03:15:08Z- AHRS_M2 msg: 1730862908.08 epoch sec; seqNo:3598 2024-11-06T03:15:08Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.37 psi 2024-11-06T03:15:08Z- handleDepth()- Depth_Keller msg: 1730862908.10 epoch sec; seqNo:3832; depth 49.26 2024-11-06T03:15:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862908.11 epoch sec; seqNo:3832; 4061593.90 north; 594864.91 east; 36.70 lat; -121.94 lon; 49.26 depth; utmZone: 10 2024-11-06T03:15:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862906.90 2024-11-06T03:15:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:08Z- waiting for 1730862910.90, time is 1730862908.11 2024-11-06T03:15:08Z- AHRS_M2 msg: 1730862908.48 epoch sec; seqNo:3599 2024-11-06T03:15:08Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.37 psi 2024-11-06T03:15:08Z- handleDepth()- Depth_Keller msg: 1730862908.48 epoch sec; seqNo:3833; depth 49.27 2024-11-06T03:15:08Z- handleDVL() - DVL msg epochMillisec: 1730862908.49 2024-11-06T03:15:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:08Z- handleDvl() - RDI_Pathfinder msg: 1730862908.49 sec; seqNo:1280; #beams:0 2024-11-06T03:15:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.09 , 0.00 2024-11-06T03:15:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862908.50 epoch sec; seqNo:3833; 4061593.52 north; 594864.91 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T03:15:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862906.90 2024-11-06T03:15:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:08Z- waiting for 1730862910.90, time is 1730862908.50 2024-11-06T03:15:08Z- AHRS_M2 msg: 1730862908.88 epoch sec; seqNo:3600 2024-11-06T03:15:08Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.37 psi 2024-11-06T03:15:08Z- handleDepth()- Depth_Keller msg: 1730862908.89 epoch sec; seqNo:3834; depth 49.28 2024-11-06T03:15:08Z- handleDVL() - DVL msg epochMillisec: 1730862908.93 2024-11-06T03:15:08Z- handleDvl() - RDI_Pathfinder msg: 1730862908.93 sec; seqNo:1281; #beams:4 2024-11-06T03:15:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.77 , 32.01 , 42.41 , 45.67 2024-11-06T03:15:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.14 , 0.05 2024-11-06T03:15:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862908.95 epoch sec; seqNo:3834; 4061593.13 north; 594863.69 east; 36.70 lat; -121.94 lon; 49.28 depth; utmZone: 10 2024-11-06T03:15:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862906.90 2024-11-06T03:15:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:09Z- waiting for 1730862910.90, time is 1730862908.95 2024-11-06T03:15:09Z- AHRS_M2 msg: 1730862909.29 epoch sec; seqNo:3601 2024-11-06T03:15:09Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.37 psi 2024-11-06T03:15:09Z- handleDepth()- Depth_Keller msg: 1730862909.30 epoch sec; seqNo:3835; depth 49.29 2024-11-06T03:15:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862909.32 epoch sec; seqNo:3835; 4061592.75 north; 594863.70 east; 36.70 lat; -121.94 lon; 49.29 depth; utmZone: 10 2024-11-06T03:15:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862906.90 2024-11-06T03:15:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:09Z- waiting for 1730862910.90, time is 1730862909.32 2024-11-06T03:15:09Z- AHRS_M2 msg: 1730862909.69 epoch sec; seqNo:3602 2024-11-06T03:15:09Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.37 psi 2024-11-06T03:15:09Z- handleDepth()- Depth_Keller msg: 1730862909.70 epoch sec; seqNo:3836; depth 49.29 2024-11-06T03:15:09Z- handleDVL() - DVL msg epochMillisec: 1730862909.70 2024-11-06T03:15:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:09Z- handleDvl() - RDI_Pathfinder msg: 1730862909.70 sec; seqNo:1282; #beams:0 2024-11-06T03:15:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.14 , 0.05 2024-11-06T03:15:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862909.71 epoch sec; seqNo:3836; 4061592.37 north; 594863.70 east; 36.70 lat; -121.94 lon; 49.29 depth; utmZone: 10 2024-11-06T03:15:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862906.90 2024-11-06T03:15:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:09Z- waiting for 1730862910.90, time is 1730862909.71 2024-11-06T03:15:10Z- AHRS_M2 msg: 1730862910.10 epoch sec; seqNo:3603 2024-11-06T03:15:10Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.37 psi 2024-11-06T03:15:10Z- handleDepth()- Depth_Keller msg: 1730862910.11 epoch sec; seqNo:3837; depth 49.32 2024-11-06T03:15:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862910.11 epoch sec; seqNo:3837; 4061592.37 north; 594863.70 east; 36.70 lat; -121.94 lon; 49.32 depth; utmZone: 10 2024-11-06T03:15:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862906.90 2024-11-06T03:15:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:10Z- waiting for 1730862910.90, time is 1730862910.11 2024-11-06T03:15:10Z- AHRS_M2 msg: 1730862910.50 epoch sec; seqNo:3604 2024-11-06T03:15:10Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.37 psi 2024-11-06T03:15:10Z- handleDepth()- Depth_Keller msg: 1730862910.51 epoch sec; seqNo:3838; depth 49.32 2024-11-06T03:15:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862910.52 epoch sec; seqNo:3838; 4061592.00 north; 594863.71 east; 36.70 lat; -121.94 lon; 49.32 depth; utmZone: 10 2024-11-06T03:15:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862906.90 2024-11-06T03:15:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:10Z- waiting for 1730862910.90, time is 1730862910.52 2024-11-06T03:15:10Z- AHRS_M2 msg: 1730862910.90 epoch sec; seqNo:3605 2024-11-06T03:15:10Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.37 psi 2024-11-06T03:15:10Z- handleDepth()- Depth_Keller msg: 1730862910.92 epoch sec; seqNo:3839; depth 49.35 2024-11-06T03:15:10Z- handleDVL() - DVL msg epochMillisec: 1730862910.95 2024-11-06T03:15:10Z- handleDvl() - RDI_Pathfinder msg: 1730862910.95 sec; seqNo:1283; #beams:4 2024-11-06T03:15:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.36 , 31.81 , 41.80 , 45.47 2024-11-06T03:15:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.11 , 0.10 2024-11-06T03:15:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862910.97 epoch sec; seqNo:3839; 4061591.62 north; 594863.71 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:15:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862906.90 2024-11-06T03:15:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:11Z- waiting for 1730862910.90, time is 1730862910.97 2024-11-06T03:15:11Z- LcmTrn::updateTrn() >>>> pose time 1730862910.97, meas time 1730862910.95 <<<< 2024-11-06T03:15:11Z- LcmTrn::updateTrn() equating pose 1730862910.95 and meas times 1730862910.95 2024-11-06T03:15:11Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862910.95 2024-11-06T03:15:11Z- TerrainNav::motionUpdate() - currEstimate = (4061592, 594863.71, 49.35) 2024-11-06T03:15:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:15:11Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:15:11Z- TNavPF::Projecting Measurements 2024-11-06T03:15:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:15:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:15:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:15:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:15:11Z- TNavFilter::4 good beams. 2024-11-06T03:15:11Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:15:11Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:15:11Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:15:11Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:15:11Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:15:11Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:15:11Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:15:11Z- TerrainMapOctree: (vehN, vehE ) = (4061591.62, 594863.71). 2024-11-06T03:15:11Z- TerrainMapOctree: Min Distance = 194.98. 2024-11-06T03:15:11Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:15:11Z- Forcing Subcloud Comparison 2024-11-06T03:15:11Z- Weighting particles with subcloud comparison 2024-11-06T03:15:11Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:11Z- meanExpectedMeasDiff: 1.062365 delta_rms_squared: 0.979322 alpha: 0.979986 2024-11-06T03:15:11Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:11Z- meanExpectedMeasDiff: -5.029702 delta_rms_squared: 0.669478 alpha: 0.970993 2024-11-06T03:15:11Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:11Z- meanExpectedMeasDiff: 1.009244 delta_rms_squared: 0.743992 alpha: 0.973822 2024-11-06T03:15:11Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:11Z- meanExpectedMeasDiff: -0.118746 delta_rms_squared: 0.881953 alpha: 0.977826 2024-11-06T03:15:11Z- Subcloud NIS: 0.320033 2024-11-06T03:15:11Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:15:11Z- TNavPF:: sumWeights = 0.019858 2024-11-06T03:15:11Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:15:11Z- TNavPF::First Term of Map Covariance Matrix is 0.06 2024-11-06T03:15:11Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:15:11Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862910.95 sec, ping # 0. 2024-11-06T03:15:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:15:11Z- variances([0], [2], and psi_berg) = 0.239628 0.142642 0.000000 2024-11-06T03:15:11Z- publishEstimatesDecoder() - at 1730862910.95 sec 2024-11-06T03:15:11Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:15:11Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061621.76 594846.70 59.36 2024-11-06T03:15:11Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061622.20 594846.27 58.80 lat/lon: 36.70 -121.94 2024-11-06T03:15:11Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.14 0.35 0.00 2024-11-06T03:15:11Z- AHRS_M2 msg: 1730862911.31 epoch sec; seqNo:3606 2024-11-06T03:15:11Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.37 psi 2024-11-06T03:15:11Z- handleDepth()- Depth_Keller msg: 1730862911.31 epoch sec; seqNo:3840; depth 49.34 2024-11-06T03:15:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862911.34 epoch sec; seqNo:3840; 4061591.24 north; 594863.72 east; 36.70 lat; -121.94 lon; 49.34 depth; utmZone: 10 2024-11-06T03:15:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862910.95 2024-11-06T03:15:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:11Z- waiting for 1730862914.95, time is 1730862911.34 2024-11-06T03:15:11Z- AHRS_M2 msg: 1730862911.71 epoch sec; seqNo:3607 2024-11-06T03:15:11Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.37 psi 2024-11-06T03:15:11Z- handleDepth()- Depth_Keller msg: 1730862911.72 epoch sec; seqNo:3841; depth 49.38 2024-11-06T03:15:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862911.73 epoch sec; seqNo:3841; 4061590.86 north; 594863.72 east; 36.70 lat; -121.94 lon; 49.38 depth; utmZone: 10 2024-11-06T03:15:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862910.95 2024-11-06T03:15:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:11Z- waiting for 1730862914.95, time is 1730862911.73 2024-11-06T03:15:12Z- AHRS_M2 msg: 1730862912.12 epoch sec; seqNo:3608 2024-11-06T03:15:12Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.37 psi 2024-11-06T03:15:12Z- handleDepth()- Depth_Keller msg: 1730862912.12 epoch sec; seqNo:3842; depth 49.39 2024-11-06T03:15:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862912.13 epoch sec; seqNo:3842; 4061590.48 north; 594863.72 east; 36.70 lat; -121.94 lon; 49.39 depth; utmZone: 10 2024-11-06T03:15:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862910.95 2024-11-06T03:15:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:12Z- waiting for 1730862914.95, time is 1730862912.13 2024-11-06T03:15:12Z- AHRS_M2 msg: 1730862912.52 epoch sec; seqNo:3609 2024-11-06T03:15:12Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.37 psi 2024-11-06T03:15:12Z- handleDepth()- Depth_Keller msg: 1730862912.53 epoch sec; seqNo:3843; depth 49.41 2024-11-06T03:15:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862912.54 epoch sec; seqNo:3843; 4061590.10 north; 594863.73 east; 36.70 lat; -121.94 lon; 49.41 depth; utmZone: 10 2024-11-06T03:15:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862910.95 2024-11-06T03:15:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:12Z- waiting for 1730862914.95, time is 1730862912.54 2024-11-06T03:15:12Z- AHRS_M2 msg: 1730862912.93 epoch sec; seqNo:3610 2024-11-06T03:15:12Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.37 psi 2024-11-06T03:15:12Z- handleDepth()- Depth_Keller msg: 1730862912.94 epoch sec; seqNo:3844; depth 49.42 2024-11-06T03:15:12Z- handleDVL() - DVL msg epochMillisec: 1730862912.98 2024-11-06T03:15:12Z- handleDvl() - RDI_Pathfinder msg: 1730862912.98 sec; seqNo:1284; #beams:4 2024-11-06T03:15:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.77 , 31.60 , 41.39 , 45.47 2024-11-06T03:15:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.09 2024-11-06T03:15:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862912.99 epoch sec; seqNo:3844; 4061589.72 north; 594863.73 east; 36.70 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T03:15:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862910.95 2024-11-06T03:15:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:13Z- waiting for 1730862914.95, time is 1730862912.99 2024-11-06T03:15:13Z- AHRS_M2 msg: 1730862913.33 epoch sec; seqNo:3611 2024-11-06T03:15:13Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.37 psi 2024-11-06T03:15:13Z- handleDepth()- Depth_Keller msg: 1730862913.34 epoch sec; seqNo:3845; depth 49.44 2024-11-06T03:15:13Z- handleDVL() - DVL msg epochMillisec: 1730862913.34 2024-11-06T03:15:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:13Z- handleDvl() - RDI_Pathfinder msg: 1730862913.34 sec; seqNo:1285; #beams:0 2024-11-06T03:15:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.09 2024-11-06T03:15:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862913.37 epoch sec; seqNo:3845; 4061589.33 north; 594862.52 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:15:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862910.95 2024-11-06T03:15:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:13Z- waiting for 1730862914.95, time is 1730862913.37 2024-11-06T03:15:13Z- AHRS_M2 msg: 1730862913.73 epoch sec; seqNo:3612 2024-11-06T03:15:13Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.38 psi 2024-11-06T03:15:13Z- handleDepth()- Depth_Keller msg: 1730862913.74 epoch sec; seqNo:3846; depth 49.46 2024-11-06T03:15:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862913.75 epoch sec; seqNo:3846; 4061588.95 north; 594862.52 east; 36.70 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T03:15:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862910.95 2024-11-06T03:15:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:13Z- waiting for 1730862914.95, time is 1730862913.75 2024-11-06T03:15:14Z- AHRS_M2 msg: 1730862914.13 epoch sec; seqNo:3613 2024-11-06T03:15:14Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.38 psi 2024-11-06T03:15:14Z- handleDepth()- Depth_Keller msg: 1730862914.15 epoch sec; seqNo:3847; depth 49.48 2024-11-06T03:15:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862914.16 epoch sec; seqNo:3847; 4061588.57 north; 594862.52 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:15:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862910.95 2024-11-06T03:15:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:14Z- waiting for 1730862914.95, time is 1730862914.16 2024-11-06T03:15:14Z- AHRS_M2 msg: 1730862914.54 epoch sec; seqNo:3614 2024-11-06T03:15:14Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.38 psi 2024-11-06T03:15:14Z- handleDepth()- Depth_Keller msg: 1730862914.55 epoch sec; seqNo:3848; depth 49.49 2024-11-06T03:15:14Z- handleDVL() - DVL msg epochMillisec: 1730862914.55 2024-11-06T03:15:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:14Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:14Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:14Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:14Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:14Z- handleDvl() - RDI_Pathfinder msg: 1730862914.55 sec; seqNo:1286; #beams:0 2024-11-06T03:15:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.09 2024-11-06T03:15:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862914.57 epoch sec; seqNo:3848; 4061588.57 north; 594862.52 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:15:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862910.95 2024-11-06T03:15:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:14Z- waiting for 1730862914.95, time is 1730862914.57 2024-11-06T03:15:14Z- AHRS_M2 msg: 1730862914.94 epoch sec; seqNo:3615 2024-11-06T03:15:14Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.38 psi 2024-11-06T03:15:14Z- handleDepth()- Depth_Keller msg: 1730862914.95 epoch sec; seqNo:3849; depth 49.50 2024-11-06T03:15:15Z- handleDVL() - DVL msg epochMillisec: 1730862914.97 2024-11-06T03:15:15Z- handleDvl() - RDI_Pathfinder msg: 1730862914.97 sec; seqNo:1287; #beams:4 2024-11-06T03:15:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.36 , 31.60 , 41.80 , 45.88 2024-11-06T03:15:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , 0.05 2024-11-06T03:15:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862915.01 epoch sec; seqNo:3849; 4061587.81 north; 594862.53 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:15:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862910.95 2024-11-06T03:15:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:15Z- waiting for 1730862914.95, time is 1730862915.01 2024-11-06T03:15:15Z- LcmTrn::updateTrn() >>>> pose time 1730862915.01, meas time 1730862914.97 <<<< 2024-11-06T03:15:15Z- LcmTrn::updateTrn() equating pose 1730862914.97 and meas times 1730862914.97 2024-11-06T03:15:15Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862914.97 2024-11-06T03:15:15Z- TerrainNav::motionUpdate() - currEstimate = (4061588, 594862.53, 49.50) 2024-11-06T03:15:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:15:15Z- TerrainNav:: SubcloudNIS is 0.32, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:15:15Z- TNavPF::Projecting Measurements 2024-11-06T03:15:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:15:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:15:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:15:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:15:15Z- TNavFilter::4 good beams. 2024-11-06T03:15:15Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:15:15Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:15:15Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:15:15Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:15:15Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:15:15Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:15:15Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:15:15Z- TerrainMapOctree: (vehN, vehE ) = (4061587.81, 594862.53). 2024-11-06T03:15:15Z- TerrainMapOctree: Min Distance = 197.40. 2024-11-06T03:15:15Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:15:15Z- Forcing Subcloud Comparison 2024-11-06T03:15:15Z- Weighting particles with subcloud comparison 2024-11-06T03:15:15Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:15Z- meanExpectedMeasDiff: 1.767800 delta_rms_squared: 0.991286 alpha: 0.980223 2024-11-06T03:15:15Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:15Z- meanExpectedMeasDiff: -4.677813 delta_rms_squared: 0.670772 alpha: 0.971047 2024-11-06T03:15:15Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:15Z- meanExpectedMeasDiff: 1.695109 delta_rms_squared: 0.750432 alpha: 0.974041 2024-11-06T03:15:15Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:15Z- meanExpectedMeasDiff: 0.832127 delta_rms_squared: 0.897837 alpha: 0.978210 2024-11-06T03:15:15Z- Subcloud NIS: 0.356662 2024-11-06T03:15:15Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:15:15Z- TNavPF:: sumWeights = 0.017361 2024-11-06T03:15:15Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:15:15Z- TNavPF::First Term of Map Covariance Matrix is 0.08 2024-11-06T03:15:15Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:15:15Z- TerrainNav::Resampling particle filter... 2024-11-06T03:15:15Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862914.97 sec, ping # 0. 2024-11-06T03:15:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:15:15Z- variances([0], [2], and psi_berg) = 0.241776 0.138579 0.000000 2024-11-06T03:15:15Z- publishEstimatesDecoder() - at 1730862914.97 sec 2024-11-06T03:15:15Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:15:15Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061618.57 594845.74 60.99 2024-11-06T03:15:15Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061618.40 594845.09 59.10 lat/lon: 36.70 -121.94 2024-11-06T03:15:15Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.14 0.35 0.00 2024-11-06T03:15:15Z- AHRS_M2 msg: 1730862915.35 epoch sec; seqNo:3616 2024-11-06T03:15:15Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.38 psi 2024-11-06T03:15:15Z- handleDepth()- Depth_Keller msg: 1730862915.35 epoch sec; seqNo:3850; depth 49.54 2024-11-06T03:15:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862915.37 epoch sec; seqNo:3850; 4061587.81 north; 594862.53 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T03:15:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862914.97 2024-11-06T03:15:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:15Z- waiting for 1730862918.97, time is 1730862915.37 2024-11-06T03:15:15Z- AHRS_M2 msg: 1730862915.75 epoch sec; seqNo:3617 2024-11-06T03:15:15Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.38 psi 2024-11-06T03:15:15Z- handleDepth()- Depth_Keller msg: 1730862915.76 epoch sec; seqNo:3851; depth 49.54 2024-11-06T03:15:15Z- handleDVL() - DVL msg epochMillisec: 1730862915.76 2024-11-06T03:15:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:15Z- handleDvl() - RDI_Pathfinder msg: 1730862915.76 sec; seqNo:1288; #beams:0 2024-11-06T03:15:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , 0.05 2024-11-06T03:15:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862915.77 epoch sec; seqNo:3851; 4061587.44 north; 594862.54 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T03:15:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862914.97 2024-11-06T03:15:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:15Z- waiting for 1730862918.97, time is 1730862915.77 2024-11-06T03:15:16Z- AHRS_M2 msg: 1730862916.16 epoch sec; seqNo:3618 2024-11-06T03:15:16Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.38 psi 2024-11-06T03:15:16Z- handleDepth()- Depth_Keller msg: 1730862916.17 epoch sec; seqNo:3852; depth 49.55 2024-11-06T03:15:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862916.18 epoch sec; seqNo:3852; 4061587.06 north; 594862.54 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:15:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862914.97 2024-11-06T03:15:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:16Z- waiting for 1730862918.97, time is 1730862916.18 2024-11-06T03:15:16Z- AHRS_M2 msg: 1730862916.56 epoch sec; seqNo:3619 2024-11-06T03:15:16Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.38 psi 2024-11-06T03:15:16Z- handleDepth()- Depth_Keller msg: 1730862916.57 epoch sec; seqNo:3853; depth 49.56 2024-11-06T03:15:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862916.58 epoch sec; seqNo:3853; 4061586.68 north; 594862.55 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:15:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862914.97 2024-11-06T03:15:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:16Z- waiting for 1730862918.97, time is 1730862916.58 2024-11-06T03:15:16Z- AHRS_M2 msg: 1730862916.96 epoch sec; seqNo:3620 2024-11-06T03:15:16Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.38 psi 2024-11-06T03:15:16Z- handleDepth()- Depth_Keller msg: 1730862916.98 epoch sec; seqNo:3854; depth 49.59 2024-11-06T03:15:17Z- handleDVL() - DVL msg epochMillisec: 1730862917.00 2024-11-06T03:15:17Z- handleDvl() - RDI_Pathfinder msg: 1730862917.00 sec; seqNo:1289; #beams:4 2024-11-06T03:15:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.56 , 31.40 , 41.80 , 45.47 2024-11-06T03:15:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , 0.03 2024-11-06T03:15:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862917.01 epoch sec; seqNo:3854; 4061586.30 north; 594862.55 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:15:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862914.97 2024-11-06T03:15:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:17Z- waiting for 1730862918.97, time is 1730862917.01 2024-11-06T03:15:17Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:15:17Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:15:17Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:15:17Z- AHRS_M2 msg: 1730862917.36 epoch sec; seqNo:3621 2024-11-06T03:15:17Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.38 psi 2024-11-06T03:15:17Z- handleDepth()- Depth_Keller msg: 1730862917.39 epoch sec; seqNo:3855; depth 49.58 2024-11-06T03:15:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862917.41 epoch sec; seqNo:3855; 4061585.92 north; 594862.55 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:15:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862914.97 2024-11-06T03:15:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:17Z- waiting for 1730862918.97, time is 1730862917.41 2024-11-06T03:15:17Z- AHRS_M2 msg: 1730862917.77 epoch sec; seqNo:3622 2024-11-06T03:15:17Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.38 psi 2024-11-06T03:15:17Z- handleDepth()- Depth_Keller msg: 1730862917.78 epoch sec; seqNo:3856; depth 49.60 2024-11-06T03:15:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862917.79 epoch sec; seqNo:3856; 4061585.54 north; 594862.56 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:15:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862914.97 2024-11-06T03:15:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:17Z- waiting for 1730862918.97, time is 1730862917.79 2024-11-06T03:15:18Z- AHRS_M2 msg: 1730862918.17 epoch sec; seqNo:3623 2024-11-06T03:15:18Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.39 psi 2024-11-06T03:15:18Z- handleDepth()- Depth_Keller msg: 1730862918.18 epoch sec; seqNo:3857; depth 49.62 2024-11-06T03:15:18Z- handleDVL() - DVL msg epochMillisec: 1730862918.19 2024-11-06T03:15:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:18Z- handleDvl() - RDI_Pathfinder msg: 1730862918.19 sec; seqNo:1290; #beams:0 2024-11-06T03:15:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , 0.03 2024-11-06T03:15:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862918.20 epoch sec; seqNo:3857; 4061585.16 north; 594862.56 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:15:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862914.97 2024-11-06T03:15:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:18Z- waiting for 1730862918.97, time is 1730862918.20 2024-11-06T03:15:18Z- AHRS_M2 msg: 1730862918.58 epoch sec; seqNo:3624 2024-11-06T03:15:18Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.39 psi 2024-11-06T03:15:18Z- handleDepth()- Depth_Keller msg: 1730862918.59 epoch sec; seqNo:3858; depth 49.61 2024-11-06T03:15:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862918.60 epoch sec; seqNo:3858; 4061584.77 north; 594861.35 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:15:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862914.97 2024-11-06T03:15:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:18Z- waiting for 1730862918.97, time is 1730862918.60 2024-11-06T03:15:19Z- AHRS_M2 msg: 1730862919.00 epoch sec; seqNo:3625 2024-11-06T03:15:19Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.39 psi 2024-11-06T03:15:19Z- handleDepth()- Depth_Keller msg: 1730862919.00 epoch sec; seqNo:3859; depth 49.63 2024-11-06T03:15:19Z- handleDVL() - DVL msg epochMillisec: 1730862919.03 2024-11-06T03:15:19Z- handleDvl() - RDI_Pathfinder msg: 1730862919.03 sec; seqNo:1291; #beams:4 2024-11-06T03:15:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.56 , 31.40 , 41.59 , 45.88 2024-11-06T03:15:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.01 , 0.04 2024-11-06T03:15:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862919.04 epoch sec; seqNo:3859; 4061584.39 north; 594861.35 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:15:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862914.97 2024-11-06T03:15:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:19Z- waiting for 1730862918.97, time is 1730862919.04 2024-11-06T03:15:19Z- LcmTrn::updateTrn() >>>> pose time 1730862919.04, meas time 1730862919.03 <<<< 2024-11-06T03:15:19Z- LcmTrn::updateTrn() equating pose 1730862919.03 and meas times 1730862919.03 2024-11-06T03:15:19Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862919.03 2024-11-06T03:15:19Z- TerrainNav::motionUpdate() - currEstimate = (4061584, 594861.35, 49.63) 2024-11-06T03:15:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:15:19Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:15:19Z- TNavPF::Projecting Measurements 2024-11-06T03:15:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:15:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:15:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:15:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:15:19Z- TNavFilter::4 good beams. 2024-11-06T03:15:19Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:15:19Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:15:19Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:15:19Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:15:19Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:15:19Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:15:19Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:15:19Z- TerrainMapOctree: (vehN, vehE ) = (4061584.39, 594861.35). 2024-11-06T03:15:19Z- TerrainMapOctree: Min Distance = 199.56. 2024-11-06T03:15:19Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:15:19Z- Forcing Subcloud Comparison 2024-11-06T03:15:19Z- Weighting particles with subcloud comparison 2024-11-06T03:15:19Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:19Z- meanExpectedMeasDiff: 2.122897 delta_rms_squared: 0.996829 alpha: 0.980331 2024-11-06T03:15:19Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:19Z- meanExpectedMeasDiff: -4.518414 delta_rms_squared: 0.683858 alpha: 0.971585 2024-11-06T03:15:19Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:19Z- meanExpectedMeasDiff: 1.950933 delta_rms_squared: 0.742526 alpha: 0.973771 2024-11-06T03:15:19Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:19Z- meanExpectedMeasDiff: 0.883168 delta_rms_squared: 0.906872 alpha: 0.978422 2024-11-06T03:15:19Z- Subcloud NIS: 0.396116 2024-11-06T03:15:19Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:15:19Z- TNavPF:: sumWeights = 0.016306 2024-11-06T03:15:19Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:15:19Z- TNavPF::First Term of Map Covariance Matrix is 0.09 2024-11-06T03:15:19Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:15:19Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862919.03 sec, ping # 0. 2024-11-06T03:15:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:15:19Z- variances([0], [2], and psi_berg) = 0.240336 0.140893 0.000000 2024-11-06T03:15:19Z- publishEstimatesDecoder() - at 1730862919.03 sec 2024-11-06T03:15:19Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:15:19Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061615.02 594843.52 59.48 2024-11-06T03:15:19Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061614.97 594843.90 59.34 lat/lon: 36.70 -121.94 2024-11-06T03:15:19Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.14 0.35 0.00 2024-11-06T03:15:19Z- AHRS_M2 msg: 1730862919.39 epoch sec; seqNo:3626 2024-11-06T03:15:19Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.39 psi 2024-11-06T03:15:19Z- handleDepth()- Depth_Keller msg: 1730862919.40 epoch sec; seqNo:3860; depth 49.63 2024-11-06T03:15:19Z- handleDVL() - DVL msg epochMillisec: 1730862919.40 2024-11-06T03:15:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:19Z- handleDvl() - RDI_Pathfinder msg: 1730862919.40 sec; seqNo:1292; #beams:0 2024-11-06T03:15:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.01 , 0.04 2024-11-06T03:15:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862919.43 epoch sec; seqNo:3860; 4061584.01 north; 594861.35 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:15:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862919.03 2024-11-06T03:15:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:19Z- waiting for 1730862923.03, time is 1730862919.43 2024-11-06T03:15:19Z- AHRS_M2 msg: 1730862919.79 epoch sec; seqNo:3627 2024-11-06T03:15:19Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.39 psi 2024-11-06T03:15:19Z- handleDepth()- Depth_Keller msg: 1730862919.80 epoch sec; seqNo:3861; depth 49.65 2024-11-06T03:15:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862919.81 epoch sec; seqNo:3861; 4061583.63 north; 594861.36 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:15:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862919.03 2024-11-06T03:15:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:19Z- waiting for 1730862923.03, time is 1730862919.81 2024-11-06T03:15:20Z- AHRS_M2 msg: 1730862920.20 epoch sec; seqNo:3628 2024-11-06T03:15:20Z- AHRS_M2 msg: 0.16 phi; -0.09 theta; 3.39 psi 2024-11-06T03:15:20Z- handleDepth()- Depth_Keller msg: 1730862920.21 epoch sec; seqNo:3862; depth 49.65 2024-11-06T03:15:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862920.21 epoch sec; seqNo:3862; 4061583.25 north; 594861.36 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:15:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862919.03 2024-11-06T03:15:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:20Z- waiting for 1730862923.03, time is 1730862920.21 2024-11-06T03:15:20Z- AHRS_M2 msg: 1730862920.60 epoch sec; seqNo:3629 2024-11-06T03:15:20Z- AHRS_M2 msg: 0.15 phi; -0.09 theta; 3.39 psi 2024-11-06T03:15:20Z- handleDepth()- Depth_Keller msg: 1730862920.61 epoch sec; seqNo:3863; depth 49.66 2024-11-06T03:15:20Z- handleDVL() - DVL msg epochMillisec: 1730862920.61 2024-11-06T03:15:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:20Z- handleDvl() - RDI_Pathfinder msg: 1730862920.61 sec; seqNo:1293; #beams:0 2024-11-06T03:15:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.01 , 0.04 2024-11-06T03:15:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862920.63 epoch sec; seqNo:3863; 4061582.88 north; 594861.37 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:15:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862919.03 2024-11-06T03:15:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:20Z- waiting for 1730862923.03, time is 1730862920.63 2024-11-06T03:15:21Z- AHRS_M2 msg: 1730862921.00 epoch sec; seqNo:3630 2024-11-06T03:15:21Z- AHRS_M2 msg: 0.15 phi; -0.09 theta; 3.40 psi 2024-11-06T03:15:21Z- handleDepth()- Depth_Keller msg: 1730862921.01 epoch sec; seqNo:3864; depth 49.67 2024-11-06T03:15:21Z- handleDVL() - DVL msg epochMillisec: 1730862921.04 2024-11-06T03:15:21Z- handleDvl() - RDI_Pathfinder msg: 1730862921.04 sec; seqNo:1294; #beams:4 2024-11-06T03:15:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.36 , 32.62 , 41.39 , 45.88 2024-11-06T03:15:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , 0.02 2024-11-06T03:15:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862921.06 epoch sec; seqNo:3864; 4061582.50 north; 594861.37 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:15:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862919.03 2024-11-06T03:15:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:21Z- waiting for 1730862923.03, time is 1730862921.06 2024-11-06T03:15:21Z- AHRS_M2 msg: 1730862921.41 epoch sec; seqNo:3631 2024-11-06T03:15:21Z- AHRS_M2 msg: 0.15 phi; -0.09 theta; 3.40 psi 2024-11-06T03:15:21Z- handleDepth()- Depth_Keller msg: 1730862921.41 epoch sec; seqNo:3865; depth 49.67 2024-11-06T03:15:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862921.43 epoch sec; seqNo:3865; 4061582.12 north; 594861.38 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:15:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862919.03 2024-11-06T03:15:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:21Z- waiting for 1730862923.03, time is 1730862921.43 2024-11-06T03:15:21Z- AHRS_M2 msg: 1730862921.81 epoch sec; seqNo:3632 2024-11-06T03:15:21Z- AHRS_M2 msg: 0.15 phi; -0.09 theta; 3.40 psi 2024-11-06T03:15:21Z- handleDepth()- Depth_Keller msg: 1730862921.82 epoch sec; seqNo:3866; depth 49.70 2024-11-06T03:15:21Z- handleDVL() - DVL msg epochMillisec: 1730862921.83 2024-11-06T03:15:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:21Z- handleDvl() - RDI_Pathfinder msg: 1730862921.83 sec; seqNo:1295; #beams:0 2024-11-06T03:15:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , 0.02 2024-11-06T03:15:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862921.84 epoch sec; seqNo:3866; 4061581.74 north; 594861.38 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:15:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862919.03 2024-11-06T03:15:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:21Z- waiting for 1730862923.03, time is 1730862921.84 2024-11-06T03:15:22Z- AHRS_M2 msg: 1730862922.22 epoch sec; seqNo:3633 2024-11-06T03:15:22Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.40 psi 2024-11-06T03:15:22Z- handleDepth()- Depth_Keller msg: 1730862922.23 epoch sec; seqNo:3867; depth 49.70 2024-11-06T03:15:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862922.25 epoch sec; seqNo:3867; 4061581.36 north; 594861.38 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:15:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862919.03 2024-11-06T03:15:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:22Z- waiting for 1730862923.03, time is 1730862922.25 2024-11-06T03:15:22Z- AHRS_M2 msg: 1730862922.63 epoch sec; seqNo:3634 2024-11-06T03:15:22Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.40 psi 2024-11-06T03:15:22Z- handleDepth()- Depth_Keller msg: 1730862922.64 epoch sec; seqNo:3868; depth 49.70 2024-11-06T03:15:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862922.65 epoch sec; seqNo:3868; 4061580.98 north; 594861.39 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:15:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862919.03 2024-11-06T03:15:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:22Z- waiting for 1730862923.03, time is 1730862922.65 2024-11-06T03:15:23Z- AHRS_M2 msg: 1730862923.03 epoch sec; seqNo:3635 2024-11-06T03:15:23Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.40 psi 2024-11-06T03:15:23Z- handleDepth()- Depth_Keller msg: 1730862923.04 epoch sec; seqNo:3869; depth 49.71 2024-11-06T03:15:23Z- handleDVL() - DVL msg epochMillisec: 1730862923.06 2024-11-06T03:15:23Z- handleDvl() - RDI_Pathfinder msg: 1730862923.06 sec; seqNo:1296; #beams:4 2024-11-06T03:15:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.16 , 31.40 , 41.39 , 45.67 2024-11-06T03:15:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , 0.00 2024-11-06T03:15:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862923.11 epoch sec; seqNo:3869; 4061580.60 north; 594861.39 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:15:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862919.03 2024-11-06T03:15:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:23Z- waiting for 1730862923.03, time is 1730862923.11 2024-11-06T03:15:23Z- LcmTrn::updateTrn() >>>> pose time 1730862923.11, meas time 1730862923.06 <<<< 2024-11-06T03:15:23Z- LcmTrn::updateTrn() equating pose 1730862923.06 and meas times 1730862923.06 2024-11-06T03:15:23Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862923.06 2024-11-06T03:15:23Z- TerrainNav::motionUpdate() - currEstimate = (4061581, 594861.39, 49.71) 2024-11-06T03:15:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:15:23Z- TerrainNav:: SubcloudNIS is 0.40, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:15:23Z- TNavPF::Projecting Measurements 2024-11-06T03:15:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:15:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:15:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:15:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:15:23Z- TNavFilter::4 good beams. 2024-11-06T03:15:23Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:15:23Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:15:23Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:15:23Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:15:23Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:15:23Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:15:23Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:15:23Z- TerrainMapOctree: (vehN, vehE ) = (4061580.60, 594861.39). 2024-11-06T03:15:23Z- TerrainMapOctree: Min Distance = 202.74. 2024-11-06T03:15:23Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:15:23Z- Forcing Subcloud Comparison 2024-11-06T03:15:23Z- Weighting particles with subcloud comparison 2024-11-06T03:15:23Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:23Z- meanExpectedMeasDiff: 1.917927 delta_rms_squared: 0.995874 alpha: 0.980313 2024-11-06T03:15:23Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:23Z- meanExpectedMeasDiff: -4.324460 delta_rms_squared: 0.682280 alpha: 0.971521 2024-11-06T03:15:23Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:23Z- meanExpectedMeasDiff: 1.923413 delta_rms_squared: 0.741810 alpha: 0.973747 2024-11-06T03:15:23Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:23Z- meanExpectedMeasDiff: 0.907471 delta_rms_squared: 0.904609 alpha: 0.978369 2024-11-06T03:15:23Z- Subcloud NIS: 0.427762 2024-11-06T03:15:23Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:15:23Z- TNavPF:: sumWeights = 0.022474 2024-11-06T03:15:23Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:15:23Z- TNavPF::First Term of Map Covariance Matrix is 0.10 2024-11-06T03:15:23Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:15:23Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862923.06 sec, ping # 0. 2024-11-06T03:15:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:15:23Z- variances([0], [2], and psi_berg) = 0.240918 0.142881 0.000000 2024-11-06T03:15:23Z- publishEstimatesDecoder() - at 1730862923.06 sec 2024-11-06T03:15:23Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:15:23Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061611.21 594844.03 59.63 2024-11-06T03:15:23Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061611.19 594843.95 59.50 lat/lon: 36.70 -121.94 2024-11-06T03:15:23Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.14 0.34 0.00 2024-11-06T03:15:23Z- AHRS_M2 msg: 1730862923.44 epoch sec; seqNo:3636 2024-11-06T03:15:23Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.40 psi 2024-11-06T03:15:23Z- handleDepth()- Depth_Keller msg: 1730862923.45 epoch sec; seqNo:3870; depth 49.71 2024-11-06T03:15:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862923.49 epoch sec; seqNo:3870; 4061580.21 north; 594860.18 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:15:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862923.06 2024-11-06T03:15:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:23Z- waiting for 1730862927.06, time is 1730862923.49 2024-11-06T03:15:23Z- AHRS_M2 msg: 1730862923.83 epoch sec; seqNo:3637 2024-11-06T03:15:23Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.40 psi 2024-11-06T03:15:23Z- handleDepth()- Depth_Keller msg: 1730862923.84 epoch sec; seqNo:3871; depth 49.74 2024-11-06T03:15:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862923.85 epoch sec; seqNo:3871; 4061579.83 north; 594860.18 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:15:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862923.06 2024-11-06T03:15:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:23Z- waiting for 1730862927.06, time is 1730862923.85 2024-11-06T03:15:24Z- AHRS_M2 msg: 1730862924.23 epoch sec; seqNo:3638 2024-11-06T03:15:24Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.40 psi 2024-11-06T03:15:24Z- handleDepth()- Depth_Keller msg: 1730862924.24 epoch sec; seqNo:3872; depth 49.73 2024-11-06T03:15:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862924.25 epoch sec; seqNo:3872; 4061579.45 north; 594860.18 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:15:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862923.06 2024-11-06T03:15:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:24Z- waiting for 1730862927.06, time is 1730862924.25 2024-11-06T03:15:24Z- AHRS_M2 msg: 1730862924.65 epoch sec; seqNo:3639 2024-11-06T03:15:24Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.40 psi 2024-11-06T03:15:24Z- handleDepth()- Depth_Keller msg: 1730862924.65 epoch sec; seqNo:3873; depth 49.75 2024-11-06T03:15:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862924.66 epoch sec; seqNo:3873; 4061579.07 north; 594860.19 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:15:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862923.06 2024-11-06T03:15:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:24Z- waiting for 1730862927.06, time is 1730862924.66 2024-11-06T03:15:25Z- AHRS_M2 msg: 1730862925.04 epoch sec; seqNo:3640 2024-11-06T03:15:25Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.40 psi 2024-11-06T03:15:25Z- handleDepth()- Depth_Keller msg: 1730862925.05 epoch sec; seqNo:3874; depth 49.76 2024-11-06T03:15:25Z- handleDVL() - DVL msg epochMillisec: 1730862925.07 2024-11-06T03:15:25Z- handleDvl() - RDI_Pathfinder msg: 1730862925.07 sec; seqNo:1297; #beams:4 2024-11-06T03:15:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.16 , 34.05 , 41.19 , 45.47 2024-11-06T03:15:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.12 , 0.01 2024-11-06T03:15:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862925.09 epoch sec; seqNo:3874; 4061579.07 north; 594860.19 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:15:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862923.06 2024-11-06T03:15:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:25Z- waiting for 1730862927.06, time is 1730862925.09 2024-11-06T03:15:25Z- AHRS_M2 msg: 1730862925.45 epoch sec; seqNo:3641 2024-11-06T03:15:25Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.40 psi 2024-11-06T03:15:25Z- handleDepth()- Depth_Keller msg: 1730862925.45 epoch sec; seqNo:3875; depth 49.78 2024-11-06T03:15:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862925.47 epoch sec; seqNo:3875; 4061578.70 north; 594860.19 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:15:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862923.06 2024-11-06T03:15:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:25Z- waiting for 1730862927.06, time is 1730862925.47 2024-11-06T03:15:25Z- AHRS_M2 msg: 1730862925.85 epoch sec; seqNo:3642 2024-11-06T03:15:25Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.40 psi 2024-11-06T03:15:25Z- handleDepth()- Depth_Keller msg: 1730862925.86 epoch sec; seqNo:3876; depth 49.80 2024-11-06T03:15:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862925.87 epoch sec; seqNo:3876; 4061578.32 north; 594860.20 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:15:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862923.06 2024-11-06T03:15:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:26Z- waiting for 1730862927.06, time is 1730862925.87 2024-11-06T03:15:26Z- AHRS_M2 msg: 1730862926.26 epoch sec; seqNo:3643 2024-11-06T03:15:26Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.40 psi 2024-11-06T03:15:26Z- handleDepth()- Depth_Keller msg: 1730862926.26 epoch sec; seqNo:3877; depth 49.82 2024-11-06T03:15:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862926.29 epoch sec; seqNo:3877; 4061577.94 north; 594860.20 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:15:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862923.06 2024-11-06T03:15:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:26Z- waiting for 1730862927.06, time is 1730862926.29 2024-11-06T03:15:26Z- handleDepth()- Depth_Keller msg: 1730862926.68 epoch sec; seqNo:3878; depth 49.83 2024-11-06T03:15:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862926.72 epoch sec; seqNo:3878; 4061577.56 north; 594860.21 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:15:26Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:15:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862923.06 2024-11-06T03:15:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:26Z- waiting for 1730862927.06, time is 1730862926.72 2024-11-06T03:15:27Z- AHRS_M2 msg: 1730862927.06 epoch sec; seqNo:3644 2024-11-06T03:15:27Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.40 psi 2024-11-06T03:15:27Z- handleDepth()- Depth_Keller msg: 1730862927.07 epoch sec; seqNo:3879; depth 49.87 2024-11-06T03:15:27Z- handleDVL() - DVL msg epochMillisec: 1730862927.09 2024-11-06T03:15:27Z- handleDvl() - RDI_Pathfinder msg: 1730862927.09 sec; seqNo:1298; #beams:4 2024-11-06T03:15:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.54 , 34.05 , 40.78 , 44.65 2024-11-06T03:15:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.14 , 0.06 2024-11-06T03:15:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862927.11 epoch sec; seqNo:3879; 4061577.18 north; 594860.21 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:15:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862923.06 2024-11-06T03:15:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:27Z- waiting for 1730862927.06, time is 1730862927.11 2024-11-06T03:15:27Z- LcmTrn::updateTrn() >>>> pose time 1730862927.11, meas time 1730862927.09 <<<< 2024-11-06T03:15:27Z- LcmTrn::updateTrn() equating pose 1730862927.09 and meas times 1730862927.09 2024-11-06T03:15:27Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862927.09 2024-11-06T03:15:27Z- TerrainNav::motionUpdate() - currEstimate = (4061577, 594860.21, 49.87) 2024-11-06T03:15:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:15:27Z- TerrainNav:: SubcloudNIS is 0.43, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:15:27Z- TNavPF::Projecting Measurements 2024-11-06T03:15:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:15:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:15:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:15:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:15:27Z- TNavFilter::4 good beams. 2024-11-06T03:15:27Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:15:27Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:15:27Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:15:27Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:15:27Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:15:27Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:15:27Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:15:27Z- TerrainMapOctree: (vehN, vehE ) = (4061577.18, 594860.21). 2024-11-06T03:15:27Z- TerrainMapOctree: Min Distance = 204.97. 2024-11-06T03:15:27Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:15:27Z- Forcing Subcloud Comparison 2024-11-06T03:15:27Z- Weighting particles with subcloud comparison 2024-11-06T03:15:27Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:27Z- meanExpectedMeasDiff: 2.074484 delta_rms_squared: 1.113955 alpha: 0.982363 2024-11-06T03:15:27Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:27Z- meanExpectedMeasDiff: -1.515138 delta_rms_squared: 0.677049 alpha: 0.971308 2024-11-06T03:15:27Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:27Z- meanExpectedMeasDiff: 1.533168 delta_rms_squared: 0.733734 alpha: 0.973465 2024-11-06T03:15:27Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:27Z- meanExpectedMeasDiff: 0.480955 delta_rms_squared: 0.880575 alpha: 0.977792 2024-11-06T03:15:27Z- Subcloud NIS: 0.441894 2024-11-06T03:15:27Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:15:27Z- TNavPF:: sumWeights = 0.307786 2024-11-06T03:15:27Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:15:27Z- TNavPF::First Term of Map Covariance Matrix is 0.66 2024-11-06T03:15:27Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T03:15:27Z- TerrainNav::Resampling particle filter... 2024-11-06T03:15:27Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730862927.09 sec, ping # 0. 2024-11-06T03:15:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:15:27Z- variances([0], [2], and psi_berg) = 0.244646 0.145065 0.000000 2024-11-06T03:15:27Z- publishEstimatesDecoder() - at 1730862927.09 sec 2024-11-06T03:15:27Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:15:27Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061608.12 594843.34 59.61 2024-11-06T03:15:27Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061607.76 594842.77 59.48 lat/lon: 36.70 -121.94 2024-11-06T03:15:27Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.15 0.34 0.00 2024-11-06T03:15:27Z- AHRS_M2 msg: 1730862927.47 epoch sec; seqNo:3645 2024-11-06T03:15:27Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.39 psi 2024-11-06T03:15:27Z- handleDepth()- Depth_Keller msg: 1730862927.48 epoch sec; seqNo:3880; depth 49.88 2024-11-06T03:15:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862927.50 epoch sec; seqNo:3880; 4061576.79 north; 594858.99 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:15:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862927.09 2024-11-06T03:15:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:27Z- waiting for 1730862931.09, time is 1730862927.50 2024-11-06T03:15:27Z- handleDepth()- Depth_Keller msg: 1730862927.88 epoch sec; seqNo:3881; depth 49.91 2024-11-06T03:15:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862927.89 epoch sec; seqNo:3881; 4061576.41 north; 594859.00 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:15:28Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:15:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862927.09 2024-11-06T03:15:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:28Z- waiting for 1730862931.09, time is 1730862927.89 2024-11-06T03:15:28Z- AHRS_M2 msg: 1730862928.28 epoch sec; seqNo:3646 2024-11-06T03:15:28Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.39 psi 2024-11-06T03:15:28Z- handleDepth()- Depth_Keller msg: 1730862928.29 epoch sec; seqNo:3882; depth 49.93 2024-11-06T03:15:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862928.30 epoch sec; seqNo:3882; 4061576.41 north; 594859.00 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:15:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862927.09 2024-11-06T03:15:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:28Z- waiting for 1730862931.09, time is 1730862928.30 2024-11-06T03:15:28Z- AHRS_M2 msg: 1730862928.68 epoch sec; seqNo:3647 2024-11-06T03:15:28Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.39 psi 2024-11-06T03:15:28Z- handleDepth()- Depth_Keller msg: 1730862928.70 epoch sec; seqNo:3883; depth 49.95 2024-11-06T03:15:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862928.75 epoch sec; seqNo:3883; 4061576.03 north; 594859.00 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:15:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862927.09 2024-11-06T03:15:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:28Z- waiting for 1730862931.09, time is 1730862928.75 2024-11-06T03:15:29Z- AHRS_M2 msg: 1730862929.08 epoch sec; seqNo:3648 2024-11-06T03:15:29Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.39 psi 2024-11-06T03:15:29Z- handleDepth()- Depth_Keller msg: 1730862929.09 epoch sec; seqNo:3884; depth 49.96 2024-11-06T03:15:29Z- handleDVL() - DVL msg epochMillisec: 1730862929.12 2024-11-06T03:15:29Z- handleDvl() - RDI_Pathfinder msg: 1730862929.12 sec; seqNo:1299; #beams:4 2024-11-06T03:15:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.34 , 33.03 , 40.98 , 44.65 2024-11-06T03:15:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.07 , 0.11 2024-11-06T03:15:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862929.14 epoch sec; seqNo:3884; 4061575.65 north; 594859.01 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:15:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862927.09 2024-11-06T03:15:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:29Z- waiting for 1730862931.09, time is 1730862929.14 2024-11-06T03:15:29Z- AHRS_M2 msg: 1730862929.49 epoch sec; seqNo:3649 2024-11-06T03:15:29Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.39 psi 2024-11-06T03:15:29Z- handleDepth()- Depth_Keller msg: 1730862929.49 epoch sec; seqNo:3885; depth 49.99 2024-11-06T03:15:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862929.51 epoch sec; seqNo:3885; 4061575.27 north; 594859.01 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:15:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862927.09 2024-11-06T03:15:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:29Z- waiting for 1730862931.09, time is 1730862929.51 2024-11-06T03:15:29Z- AHRS_M2 msg: 1730862929.89 epoch sec; seqNo:3650 2024-11-06T03:15:29Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.39 psi 2024-11-06T03:15:29Z- handleDepth()- Depth_Keller msg: 1730862929.90 epoch sec; seqNo:3886; depth 50.01 2024-11-06T03:15:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862929.91 epoch sec; seqNo:3886; 4061574.89 north; 594859.01 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:15:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862927.09 2024-11-06T03:15:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:30Z- waiting for 1730862931.09, time is 1730862929.91 2024-11-06T03:15:30Z- AHRS_M2 msg: 1730862930.30 epoch sec; seqNo:3651 2024-11-06T03:15:30Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.39 psi 2024-11-06T03:15:30Z- handleDepth()- Depth_Keller msg: 1730862930.30 epoch sec; seqNo:3887; depth 50.03 2024-11-06T03:15:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862930.32 epoch sec; seqNo:3887; 4061574.51 north; 594859.02 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:15:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862927.09 2024-11-06T03:15:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:30Z- waiting for 1730862931.09, time is 1730862930.32 2024-11-06T03:15:30Z- AHRS_M2 msg: 1730862930.74 epoch sec; seqNo:3652 2024-11-06T03:15:30Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.39 psi 2024-11-06T03:15:30Z- handleDepth()- Depth_Keller msg: 1730862930.75 epoch sec; seqNo:3888; depth 50.07 2024-11-06T03:15:30Z- handleDVL() - DVL msg epochMillisec: 1730862930.78 2024-11-06T03:15:30Z- handleDvl() - RDI_Pathfinder msg: 1730862930.78 sec; seqNo:1300; #beams:4 2024-11-06T03:15:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.34 , 32.83 , 40.78 , 44.65 2024-11-06T03:15:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.09 2024-11-06T03:15:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862930.80 epoch sec; seqNo:3888; 4061574.14 north; 594859.02 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:15:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862927.09 2024-11-06T03:15:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:30Z- waiting for 1730862931.09, time is 1730862930.80 2024-11-06T03:15:31Z- AHRS_M2 msg: 1730862931.10 epoch sec; seqNo:3653 2024-11-06T03:15:31Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.40 psi 2024-11-06T03:15:31Z- handleDepth()- Depth_Keller msg: 1730862931.11 epoch sec; seqNo:3889; depth 50.08 2024-11-06T03:15:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862931.12 epoch sec; seqNo:3889; 4061573.76 north; 594859.03 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:15:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862927.09 2024-11-06T03:15:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:31Z- waiting for 1730862931.09, time is 1730862931.12 2024-11-06T03:15:31Z- LcmTrn::updateTrn() >>>> pose time 1730862931.12, meas time 1730862930.78 <<<< 2024-11-06T03:15:31Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862931.12 2024-11-06T03:15:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862930.78 sec, ping # 0; waiting for INS data more recent than 1730862927.09... 2024-11-06T03:15:31Z- TerrainNav::motionUpdate() - currEstimate = (4061574, 594859.03, 50.08) 2024-11-06T03:15:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:15:31Z- TerrainNav:: SubcloudNIS is 0.44, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:15:31Z- TNavPF::Projecting Measurements 2024-11-06T03:15:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:15:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:15:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:15:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:15:31Z- TNavFilter::4 good beams. 2024-11-06T03:15:31Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:15:31Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:15:31Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:15:31Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:15:31Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:15:31Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:15:31Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:15:31Z- TerrainMapOctree: (vehN, vehE ) = (4061574.05, 594859.13). 2024-11-06T03:15:31Z- TerrainMapOctree: Min Distance = 207.05. 2024-11-06T03:15:31Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:15:31Z- Forcing Subcloud Comparison 2024-11-06T03:15:31Z- Weighting particles with subcloud comparison 2024-11-06T03:15:31Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:31Z- meanExpectedMeasDiff: 2.064521 delta_rms_squared: 1.468551 alpha: 0.986564 2024-11-06T03:15:31Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:31Z- meanExpectedMeasDiff: -2.187272 delta_rms_squared: 0.522065 alpha: 0.963104 2024-11-06T03:15:31Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:31Z- meanExpectedMeasDiff: 1.508017 delta_rms_squared: 0.739640 alpha: 0.973672 2024-11-06T03:15:31Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:31Z- meanExpectedMeasDiff: 0.595625 delta_rms_squared: 0.881821 alpha: 0.977823 2024-11-06T03:15:31Z- Subcloud NIS: 0.451158 2024-11-06T03:15:31Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:15:31Z- TNavPF:: sumWeights = 0.223926 2024-11-06T03:15:31Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:15:31Z- TNavPF::First Term of Map Covariance Matrix is 0.55 2024-11-06T03:15:31Z- TNavPF::Calculated NIS Value : 0.14 numBeams normalized NIS: 0.03 2024-11-06T03:15:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862930.78 sec. 2024-11-06T03:15:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:15:31Z- variances([0], [2], and psi_berg) = 0.232748 0.135867 0.000000 2024-11-06T03:15:31Z- publishEstimatesDecoder() - at 1730862931.12 sec 2024-11-06T03:15:31Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:15:31Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061603.72 594841.20 59.31 2024-11-06T03:15:31Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061604.32 594841.51 59.48 lat/lon: 36.70 -121.94 2024-11-06T03:15:31Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.14 0.58 0.00 2024-11-06T03:15:31Z- AHRS_M2 msg: 1730862931.51 epoch sec; seqNo:3654 2024-11-06T03:15:31Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.40 psi 2024-11-06T03:15:31Z- handleDepth()- Depth_Keller msg: 1730862931.52 epoch sec; seqNo:3890; depth 50.11 2024-11-06T03:15:31Z- handleDVL() - DVL msg epochMillisec: 1730862931.52 2024-11-06T03:15:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:31Z- handleDvl() - RDI_Pathfinder msg: 1730862931.52 sec; seqNo:1301; #beams:0 2024-11-06T03:15:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.09 2024-11-06T03:15:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862931.53 epoch sec; seqNo:3890; 4061573.36 north; 594857.81 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:15:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862931.12 2024-11-06T03:15:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:31Z- waiting for 1730862935.12, time is 1730862931.53 2024-11-06T03:15:31Z- AHRS_M2 msg: 1730862931.91 epoch sec; seqNo:3655 2024-11-06T03:15:31Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.40 psi 2024-11-06T03:15:31Z- handleDepth()- Depth_Keller msg: 1730862931.93 epoch sec; seqNo:3891; depth 50.12 2024-11-06T03:15:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862931.94 epoch sec; seqNo:3891; 4061572.99 north; 594857.82 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:15:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862931.12 2024-11-06T03:15:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:32Z- waiting for 1730862935.12, time is 1730862931.94 2024-11-06T03:15:32Z- AHRS_M2 msg: 1730862932.31 epoch sec; seqNo:3656 2024-11-06T03:15:32Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.40 psi 2024-11-06T03:15:32Z- handleDepth()- Depth_Keller msg: 1730862932.33 epoch sec; seqNo:3892; depth 50.13 2024-11-06T03:15:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862932.34 epoch sec; seqNo:3892; 4061572.61 north; 594857.82 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:15:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862931.12 2024-11-06T03:15:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:32Z- waiting for 1730862935.12, time is 1730862932.34 2024-11-06T03:15:32Z- AHRS_M2 msg: 1730862932.76 epoch sec; seqNo:3657 2024-11-06T03:15:32Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.40 psi 2024-11-06T03:15:32Z- handleDepth()- Depth_Keller msg: 1730862932.76 epoch sec; seqNo:3893; depth 50.16 2024-11-06T03:15:32Z- handleDVL() - DVL msg epochMillisec: 1730862932.79 2024-11-06T03:15:32Z- handleDvl() - RDI_Pathfinder msg: 1730862932.79 sec; seqNo:1302; #beams:4 2024-11-06T03:15:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.75 , 32.62 , 40.98 , 44.45 2024-11-06T03:15:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.08 , 0.04 2024-11-06T03:15:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862932.80 epoch sec; seqNo:3893; 4061572.23 north; 594857.82 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:15:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862931.12 2024-11-06T03:15:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:32Z- waiting for 1730862935.12, time is 1730862932.80 2024-11-06T03:15:33Z- AHRS_M2 msg: 1730862933.12 epoch sec; seqNo:3658 2024-11-06T03:15:33Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.40 psi 2024-11-06T03:15:33Z- handleDepth()- Depth_Keller msg: 1730862933.13 epoch sec; seqNo:3894; depth 50.16 2024-11-06T03:15:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862933.16 epoch sec; seqNo:3894; 4061571.85 north; 594857.83 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:15:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862931.12 2024-11-06T03:15:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:33Z- waiting for 1730862935.12, time is 1730862933.16 2024-11-06T03:15:33Z- AHRS_M2 msg: 1730862933.53 epoch sec; seqNo:3659 2024-11-06T03:15:33Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.40 psi 2024-11-06T03:15:33Z- handleDepth()- Depth_Keller msg: 1730862933.53 epoch sec; seqNo:3895; depth 50.16 2024-11-06T03:15:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862933.54 epoch sec; seqNo:3895; 4061571.47 north; 594857.83 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:15:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862931.12 2024-11-06T03:15:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:33Z- waiting for 1730862935.12, time is 1730862933.54 2024-11-06T03:15:33Z- AHRS_M2 msg: 1730862933.93 epoch sec; seqNo:3660 2024-11-06T03:15:33Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 3.40 psi 2024-11-06T03:15:33Z- handleDepth()- Depth_Keller msg: 1730862933.94 epoch sec; seqNo:3896; depth 50.17 2024-11-06T03:15:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862933.95 epoch sec; seqNo:3896; 4061571.09 north; 594857.84 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:15:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862931.12 2024-11-06T03:15:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:34Z- waiting for 1730862935.12, time is 1730862933.95 2024-11-06T03:15:34Z- AHRS_M2 msg: 1730862934.34 epoch sec; seqNo:3661 2024-11-06T03:15:34Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 3.41 psi 2024-11-06T03:15:34Z- handleDepth()- Depth_Keller msg: 1730862934.34 epoch sec; seqNo:3897; depth 50.18 2024-11-06T03:15:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862934.35 epoch sec; seqNo:3897; 4061570.71 north; 594857.84 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:15:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862931.12 2024-11-06T03:15:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:34Z- waiting for 1730862935.12, time is 1730862934.35 2024-11-06T03:15:34Z- AHRS_M2 msg: 1730862934.77 epoch sec; seqNo:3662 2024-11-06T03:15:34Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 3.41 psi 2024-11-06T03:15:34Z- handleDepth()- Depth_Keller msg: 1730862934.78 epoch sec; seqNo:3898; depth 50.20 2024-11-06T03:15:34Z- handleDVL() - DVL msg epochMillisec: 1730862934.80 2024-11-06T03:15:34Z- handleDvl() - RDI_Pathfinder msg: 1730862934.80 sec; seqNo:1303; #beams:4 2024-11-06T03:15:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.14 , 33.03 , 40.78 , 44.04 2024-11-06T03:15:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.03 2024-11-06T03:15:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862934.83 epoch sec; seqNo:3898; 4061570.33 north; 594857.84 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:15:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862931.12 2024-11-06T03:15:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:34Z- waiting for 1730862935.12, time is 1730862934.83 2024-11-06T03:15:35Z- AHRS_M2 msg: 1730862935.14 epoch sec; seqNo:3663 2024-11-06T03:15:35Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 3.41 psi 2024-11-06T03:15:35Z- handleDepth()- Depth_Keller msg: 1730862935.15 epoch sec; seqNo:3899; depth 50.17 2024-11-06T03:15:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862935.17 epoch sec; seqNo:3899; 4061569.95 north; 594857.85 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:15:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862931.12 2024-11-06T03:15:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:35Z- waiting for 1730862935.12, time is 1730862935.17 2024-11-06T03:15:35Z- LcmTrn::updateTrn() >>>> pose time 1730862935.17, meas time 1730862934.80 <<<< 2024-11-06T03:15:35Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862935.17 2024-11-06T03:15:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862934.80 sec, ping # 0; waiting for INS data more recent than 1730862931.12... 2024-11-06T03:15:35Z- TerrainNav::motionUpdate() - currEstimate = (4061570, 594857.85, 50.17) 2024-11-06T03:15:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:15:35Z- TerrainNav:: SubcloudNIS is 0.45, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:15:35Z- TNavPF::Projecting Measurements 2024-11-06T03:15:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:15:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:15:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:15:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:15:35Z- TNavFilter::4 good beams. 2024-11-06T03:15:35Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:15:35Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:15:35Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:15:35Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:15:35Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:15:35Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:15:35Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:15:35Z- TerrainMapOctree: (vehN, vehE ) = (4061570.30, 594857.96). 2024-11-06T03:15:35Z- TerrainMapOctree: Min Distance = 209.63. 2024-11-06T03:15:35Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:15:35Z- Forcing Subcloud Comparison 2024-11-06T03:15:35Z- Weighting particles with subcloud comparison 2024-11-06T03:15:35Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:35Z- meanExpectedMeasDiff: 2.827876 delta_rms_squared: 2.485844 alpha: 0.992019 2024-11-06T03:15:35Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:35Z- meanExpectedMeasDiff: -1.670222 delta_rms_squared: 0.957881 alpha: 0.979548 2024-11-06T03:15:35Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:35Z- meanExpectedMeasDiff: 1.301933 delta_rms_squared: 1.132629 alpha: 0.982648 2024-11-06T03:15:35Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:35Z- meanExpectedMeasDiff: 0.340734 delta_rms_squared: 1.853798 alpha: 0.989326 2024-11-06T03:15:35Z- Subcloud NIS: 0.456304 2024-11-06T03:15:35Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:15:35Z- TNavPF:: sumWeights = 0.192483 2024-11-06T03:15:35Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:15:35Z- TNavPF::First Term of Map Covariance Matrix is 0.96 2024-11-06T03:15:35Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.02 2024-11-06T03:15:35Z- TerrainNav::Resampling particle filter... 2024-11-06T03:15:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862934.80 sec. 2024-11-06T03:15:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:15:35Z- variances([0], [2], and psi_berg) = 0.233233 0.132151 0.000000 2024-11-06T03:15:35Z- publishEstimatesDecoder() - at 1730862935.17 sec 2024-11-06T03:15:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:15:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061600.83 594840.92 59.25 2024-11-06T03:15:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061600.53 594840.38 59.26 lat/lon: 36.70 -121.94 2024-11-06T03:15:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.13 0.63 0.00 2024-11-06T03:15:35Z- AHRS_M2 msg: 1730862935.55 epoch sec; seqNo:3664 2024-11-06T03:15:35Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 3.41 psi 2024-11-06T03:15:35Z- handleDepth()- Depth_Keller msg: 1730862935.57 epoch sec; seqNo:3900; depth 50.18 2024-11-06T03:15:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862935.60 epoch sec; seqNo:3900; 4061569.58 north; 594857.85 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:15:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862935.17 2024-11-06T03:15:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:35Z- waiting for 1730862939.17, time is 1730862935.60 2024-11-06T03:15:35Z- AHRS_M2 msg: 1730862935.95 epoch sec; seqNo:3665 2024-11-06T03:15:35Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 3.41 psi 2024-11-06T03:15:35Z- handleDepth()- Depth_Keller msg: 1730862935.96 epoch sec; seqNo:3901; depth 50.18 2024-11-06T03:15:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862935.97 epoch sec; seqNo:3901; 4061569.20 north; 594857.86 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:15:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862935.17 2024-11-06T03:15:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:36Z- waiting for 1730862939.17, time is 1730862935.97 2024-11-06T03:15:36Z- AHRS_M2 msg: 1730862936.36 epoch sec; seqNo:3666 2024-11-06T03:15:36Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 3.42 psi 2024-11-06T03:15:36Z- handleDepth()- Depth_Keller msg: 1730862936.36 epoch sec; seqNo:3902; depth 50.17 2024-11-06T03:15:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862936.38 epoch sec; seqNo:3902; 4061568.82 north; 594857.86 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:15:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862935.17 2024-11-06T03:15:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:36Z- waiting for 1730862939.17, time is 1730862936.38 2024-11-06T03:15:36Z- AHRS_M2 msg: 1730862936.77 epoch sec; seqNo:3667 2024-11-06T03:15:36Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.42 psi 2024-11-06T03:15:36Z- handleDepth()- Depth_Keller msg: 1730862936.78 epoch sec; seqNo:3903; depth 50.16 2024-11-06T03:15:36Z- handleDVL() - DVL msg epochMillisec: 1730862936.80 2024-11-06T03:15:36Z- handleDvl() - RDI_Pathfinder msg: 1730862936.80 sec; seqNo:1304; #beams:4 2024-11-06T03:15:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.14 , 33.03 , 40.78 , 43.84 2024-11-06T03:15:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.06 2024-11-06T03:15:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862936.81 epoch sec; seqNo:3903; 4061568.82 north; 594857.86 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:15:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862935.17 2024-11-06T03:15:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:36Z- waiting for 1730862939.17, time is 1730862936.81 2024-11-06T03:15:37Z- handleDepth()- Depth_Keller msg: 1730862937.19 epoch sec; seqNo:3904; depth 50.17 2024-11-06T03:15:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862937.20 epoch sec; seqNo:3904; 4061568.44 north; 594857.87 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:15:37Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:15:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862935.17 2024-11-06T03:15:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:37Z- waiting for 1730862939.17, time is 1730862937.20 2024-11-06T03:15:37Z- AHRS_M2 msg: 1730862937.57 epoch sec; seqNo:3668 2024-11-06T03:15:37Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.42 psi 2024-11-06T03:15:37Z- handleDepth()- Depth_Keller msg: 1730862937.57 epoch sec; seqNo:3905; depth 50.17 2024-11-06T03:15:37Z- handleDVL() - DVL msg epochMillisec: 1730862937.57 2024-11-06T03:15:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:37Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:37Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:37Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:37Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:37Z- handleDvl() - RDI_Pathfinder msg: 1730862937.57 sec; seqNo:1305; #beams:0 2024-11-06T03:15:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.06 2024-11-06T03:15:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862937.59 epoch sec; seqNo:3905; 4061568.05 north; 594856.65 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:15:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862935.17 2024-11-06T03:15:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:37Z- waiting for 1730862939.17, time is 1730862937.59 2024-11-06T03:15:37Z- AHRS_M2 msg: 1730862937.97 epoch sec; seqNo:3669 2024-11-06T03:15:37Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.42 psi 2024-11-06T03:15:37Z- handleDepth()- Depth_Keller msg: 1730862937.97 epoch sec; seqNo:3906; depth 50.16 2024-11-06T03:15:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862937.99 epoch sec; seqNo:3906; 4061567.67 north; 594856.65 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:15:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862935.17 2024-11-06T03:15:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:38Z- waiting for 1730862939.17, time is 1730862937.99 2024-11-06T03:15:38Z- AHRS_M2 msg: 1730862938.38 epoch sec; seqNo:3670 2024-11-06T03:15:38Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.42 psi 2024-11-06T03:15:38Z- handleDepth()- Depth_Keller msg: 1730862938.38 epoch sec; seqNo:3907; depth 50.16 2024-11-06T03:15:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862938.40 epoch sec; seqNo:3907; 4061567.29 north; 594856.66 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:15:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862935.17 2024-11-06T03:15:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:38Z- waiting for 1730862939.17, time is 1730862938.40 2024-11-06T03:15:38Z- AHRS_M2 msg: 1730862938.78 epoch sec; seqNo:3671 2024-11-06T03:15:38Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.41 psi 2024-11-06T03:15:38Z- handleDepth()- Depth_Keller msg: 1730862938.79 epoch sec; seqNo:3908; depth 50.17 2024-11-06T03:15:38Z- handleDVL() - DVL msg epochMillisec: 1730862938.81 2024-11-06T03:15:38Z- handleDvl() - RDI_Pathfinder msg: 1730862938.81 sec; seqNo:1306; #beams:4 2024-11-06T03:15:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.54 , 33.44 , 40.57 , 43.63 2024-11-06T03:15:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.15 , -0.01 2024-11-06T03:15:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862938.83 epoch sec; seqNo:3908; 4061566.91 north; 594856.66 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:15:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862935.17 2024-11-06T03:15:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:38Z- waiting for 1730862939.17, time is 1730862938.83 2024-11-06T03:15:39Z- AHRS_M2 msg: 1730862939.18 epoch sec; seqNo:3672 2024-11-06T03:15:39Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.41 psi 2024-11-06T03:15:39Z- handleDepth()- Depth_Keller msg: 1730862939.19 epoch sec; seqNo:3909; depth 50.17 2024-11-06T03:15:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862939.22 epoch sec; seqNo:3909; 4061566.53 north; 594856.67 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:15:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862935.17 2024-11-06T03:15:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:39Z- waiting for 1730862939.17, time is 1730862939.22 2024-11-06T03:15:39Z- LcmTrn::updateTrn() >>>> pose time 1730862939.22, meas time 1730862938.81 <<<< 2024-11-06T03:15:39Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862939.22 2024-11-06T03:15:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862938.81 sec, ping # 0; waiting for INS data more recent than 1730862935.17... 2024-11-06T03:15:39Z- TerrainNav::motionUpdate() - currEstimate = (4061567, 594856.67, 50.17) 2024-11-06T03:15:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:15:39Z- TerrainNav:: SubcloudNIS is 0.46, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:15:39Z- TNavPF::Projecting Measurements 2024-11-06T03:15:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:15:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:15:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:15:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:15:39Z- TNavFilter::4 good beams. 2024-11-06T03:15:39Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:15:39Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:15:39Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:15:39Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:15:39Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:15:39Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:15:39Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:15:39Z- TerrainMapOctree: (vehN, vehE ) = (4061566.87, 594856.78). 2024-11-06T03:15:39Z- TerrainMapOctree: Min Distance = 211.99. 2024-11-06T03:15:39Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:15:39Z- Forcing Subcloud Comparison 2024-11-06T03:15:39Z- Weighting particles with subcloud comparison 2024-11-06T03:15:39Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:39Z- meanExpectedMeasDiff: 1.827980 delta_rms_squared: 2.852977 alpha: 0.993039 2024-11-06T03:15:39Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:39Z- meanExpectedMeasDiff: -1.352364 delta_rms_squared: 0.996185 alpha: 0.980319 2024-11-06T03:15:39Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:39Z- meanExpectedMeasDiff: 1.485303 delta_rms_squared: 1.110041 alpha: 0.982302 2024-11-06T03:15:39Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:39Z- meanExpectedMeasDiff: 0.789473 delta_rms_squared: 1.514383 alpha: 0.986965 2024-11-06T03:15:39Z- Subcloud NIS: 0.458107 2024-11-06T03:15:39Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:15:39Z- TNavPF:: sumWeights = 0.310767 2024-11-06T03:15:39Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:15:39Z- TNavPF::First Term of Map Covariance Matrix is 0.57 2024-11-06T03:15:39Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T03:15:39Z- TerrainNav::Resampling particle filter... 2024-11-06T03:15:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862938.81 sec. 2024-11-06T03:15:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:15:39Z- variances([0], [2], and psi_berg) = 0.231874 0.128167 0.000000 2024-11-06T03:15:39Z- publishEstimatesDecoder() - at 1730862939.22 sec 2024-11-06T03:15:39Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:15:39Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061597.45 594839.71 58.88 2024-11-06T03:15:39Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061597.10 594839.22 58.97 lat/lon: 36.70 -121.94 2024-11-06T03:15:39Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.13 0.62 0.00 2024-11-06T03:15:39Z- AHRS_M2 msg: 1730862939.59 epoch sec; seqNo:3673 2024-11-06T03:15:39Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.41 psi 2024-11-06T03:15:39Z- handleDepth()- Depth_Keller msg: 1730862939.60 epoch sec; seqNo:3910; depth 50.18 2024-11-06T03:15:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862939.63 epoch sec; seqNo:3910; 4061566.15 north; 594856.67 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:15:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862939.22 2024-11-06T03:15:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:39Z- waiting for 1730862943.22, time is 1730862939.63 2024-11-06T03:15:40Z- AHRS_M2 msg: 1730862940.00 epoch sec; seqNo:3674 2024-11-06T03:15:40Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.41 psi 2024-11-06T03:15:40Z- handleDepth()- Depth_Keller msg: 1730862940.02 epoch sec; seqNo:3911; depth 50.18 2024-11-06T03:15:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862940.02 epoch sec; seqNo:3911; 4061566.15 north; 594856.67 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:15:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862939.22 2024-11-06T03:15:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:40Z- waiting for 1730862943.22, time is 1730862940.02 2024-11-06T03:15:40Z- AHRS_M2 msg: 1730862940.41 epoch sec; seqNo:3675 2024-11-06T03:15:40Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.41 psi 2024-11-06T03:15:40Z- handleDepth()- Depth_Keller msg: 1730862940.42 epoch sec; seqNo:3912; depth 50.20 2024-11-06T03:15:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862940.43 epoch sec; seqNo:3912; 4061565.77 north; 594856.67 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:15:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862939.22 2024-11-06T03:15:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:40Z- waiting for 1730862943.22, time is 1730862940.43 2024-11-06T03:15:40Z- AHRS_M2 msg: 1730862940.80 epoch sec; seqNo:3676 2024-11-06T03:15:40Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.41 psi 2024-11-06T03:15:40Z- handleDepth()- Depth_Keller msg: 1730862940.81 epoch sec; seqNo:3913; depth 50.22 2024-11-06T03:15:40Z- handleDVL() - DVL msg epochMillisec: 1730862940.83 2024-11-06T03:15:40Z- handleDvl() - RDI_Pathfinder msg: 1730862940.83 sec; seqNo:1307; #beams:4 2024-11-06T03:15:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.14 , 33.44 , 40.57 , 43.43 2024-11-06T03:15:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.15 , 0.08 2024-11-06T03:15:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862940.84 epoch sec; seqNo:3913; 4061565.38 north; 594855.46 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:15:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862939.22 2024-11-06T03:15:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:40Z- waiting for 1730862943.22, time is 1730862940.84 2024-11-06T03:15:41Z- AHRS_M2 msg: 1730862941.20 epoch sec; seqNo:3677 2024-11-06T03:15:41Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.41 psi 2024-11-06T03:15:41Z- handleDepth()- Depth_Keller msg: 1730862941.21 epoch sec; seqNo:3914; depth 50.23 2024-11-06T03:15:41Z- handleDVL() - DVL msg epochMillisec: 1730862941.21 2024-11-06T03:15:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:41Z- handleDvl() - RDI_Pathfinder msg: 1730862941.21 sec; seqNo:1308; #beams:0 2024-11-06T03:15:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.15 , 0.08 2024-11-06T03:15:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862941.23 epoch sec; seqNo:3914; 4061565.00 north; 594855.46 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:15:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862939.22 2024-11-06T03:15:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:41Z- waiting for 1730862943.22, time is 1730862941.23 2024-11-06T03:15:41Z- handleDepth()- Depth_Keller msg: 1730862941.63 epoch sec; seqNo:3915; depth 50.23 2024-11-06T03:15:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862941.64 epoch sec; seqNo:3915; 4061564.62 north; 594855.47 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:15:41Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:15:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862939.22 2024-11-06T03:15:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:41Z- waiting for 1730862943.22, time is 1730862941.64 2024-11-06T03:15:42Z- AHRS_M2 msg: 1730862942.01 epoch sec; seqNo:3678 2024-11-06T03:15:42Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.40 psi 2024-11-06T03:15:42Z- handleDepth()- Depth_Keller msg: 1730862942.02 epoch sec; seqNo:3916; depth 50.25 2024-11-06T03:15:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862942.03 epoch sec; seqNo:3916; 4061564.24 north; 594855.47 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:15:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862939.22 2024-11-06T03:15:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:42Z- waiting for 1730862943.22, time is 1730862942.03 2024-11-06T03:15:42Z- AHRS_M2 msg: 1730862942.41 epoch sec; seqNo:3679 2024-11-06T03:15:42Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 3.40 psi 2024-11-06T03:15:42Z- handleDepth()- Depth_Keller msg: 1730862942.42 epoch sec; seqNo:3917; depth 50.26 2024-11-06T03:15:42Z- handleDVL() - DVL msg epochMillisec: 1730862942.42 2024-11-06T03:15:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:42Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:42Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:42Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:42Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:42Z- handleDvl() - RDI_Pathfinder msg: 1730862942.42 sec; seqNo:1309; #beams:0 2024-11-06T03:15:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.15 , 0.08 2024-11-06T03:15:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862942.43 epoch sec; seqNo:3917; 4061564.24 north; 594855.47 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:15:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862939.22 2024-11-06T03:15:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:42Z- waiting for 1730862943.22, time is 1730862942.43 2024-11-06T03:15:42Z- AHRS_M2 msg: 1730862942.82 epoch sec; seqNo:3680 2024-11-06T03:15:42Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 3.40 psi 2024-11-06T03:15:42Z- handleDepth()- Depth_Keller msg: 1730862942.83 epoch sec; seqNo:3918; depth 50.29 2024-11-06T03:15:42Z- handleDVL() - DVL msg epochMillisec: 1730862942.85 2024-11-06T03:15:42Z- handleDvl() - RDI_Pathfinder msg: 1730862942.85 sec; seqNo:1310; #beams:4 2024-11-06T03:15:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.41 , 33.27 , 40.56 , 42.99 2024-11-06T03:15:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.07 , 0.12 2024-11-06T03:15:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862942.88 epoch sec; seqNo:3918; 4061563.87 north; 594855.48 east; 36.69 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:15:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862939.22 2024-11-06T03:15:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:42Z- waiting for 1730862943.22, time is 1730862942.88 2024-11-06T03:15:43Z- AHRS_M2 msg: 1730862943.22 epoch sec; seqNo:3681 2024-11-06T03:15:43Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 3.39 psi 2024-11-06T03:15:43Z- handleDepth()- Depth_Keller msg: 1730862943.23 epoch sec; seqNo:3919; depth 50.30 2024-11-06T03:15:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862943.25 epoch sec; seqNo:3919; 4061563.49 north; 594855.48 east; 36.69 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:15:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862939.22 2024-11-06T03:15:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:43Z- waiting for 1730862943.22, time is 1730862943.25 2024-11-06T03:15:43Z- LcmTrn::updateTrn() >>>> pose time 1730862943.25, meas time 1730862942.85 <<<< 2024-11-06T03:15:43Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862943.25 2024-11-06T03:15:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862942.85 sec, ping # 0; waiting for INS data more recent than 1730862939.22... 2024-11-06T03:15:43Z- TerrainNav::motionUpdate() - currEstimate = (4061563, 594855.48, 50.30) 2024-11-06T03:15:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:15:43Z- TerrainNav:: SubcloudNIS is 0.46, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:15:43Z- TNavPF::Projecting Measurements 2024-11-06T03:15:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:15:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:15:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:15:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:15:43Z- TNavFilter::4 good beams. 2024-11-06T03:15:43Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:15:43Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:15:43Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:15:43Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:15:43Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:15:43Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:15:43Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:15:43Z- TerrainMapOctree: (vehN, vehE ) = (4061563.79, 594855.60). 2024-11-06T03:15:43Z- TerrainMapOctree: Min Distance = 214.07. 2024-11-06T03:15:43Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:15:43Z- Forcing Subcloud Comparison 2024-11-06T03:15:43Z- Weighting particles with subcloud comparison 2024-11-06T03:15:43Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:43Z- meanExpectedMeasDiff: 1.859743 delta_rms_squared: 1.378997 alpha: 0.985704 2024-11-06T03:15:43Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:43Z- meanExpectedMeasDiff: -1.881506 delta_rms_squared: 0.966016 alpha: 0.979716 2024-11-06T03:15:43Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:43Z- meanExpectedMeasDiff: 0.870300 delta_rms_squared: 1.105562 alpha: 0.982231 2024-11-06T03:15:43Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:43Z- meanExpectedMeasDiff: 0.709519 delta_rms_squared: 1.186400 alpha: 0.983422 2024-11-06T03:15:43Z- Subcloud NIS: 0.460453 2024-11-06T03:15:43Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:15:43Z- TNavPF:: sumWeights = 0.277471 2024-11-06T03:15:43Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:15:43Z- TNavPF::First Term of Map Covariance Matrix is 0.62 2024-11-06T03:15:43Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T03:15:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862942.85 sec. 2024-11-06T03:15:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:15:43Z- variances([0], [2], and psi_berg) = 0.233652 0.125857 0.000000 2024-11-06T03:15:43Z- publishEstimatesDecoder() - at 1730862943.25 sec 2024-11-06T03:15:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:15:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061594.31 594837.65 59.12 2024-11-06T03:15:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061594.06 594838.03 59.00 lat/lon: 36.70 -121.94 2024-11-06T03:15:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.13 0.67 0.00 2024-11-06T03:15:43Z- AHRS_M2 msg: 1730862943.64 epoch sec; seqNo:3682 2024-11-06T03:15:43Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 3.39 psi 2024-11-06T03:15:43Z- handleDepth()- Depth_Keller msg: 1730862943.66 epoch sec; seqNo:3920; depth 50.31 2024-11-06T03:15:43Z- handleDVL() - DVL msg epochMillisec: 1730862943.67 2024-11-06T03:15:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:43Z- handleDvl() - RDI_Pathfinder msg: 1730862943.67 sec; seqNo:1311; #beams:0 2024-11-06T03:15:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.07 , 0.12 2024-11-06T03:15:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862943.69 epoch sec; seqNo:3920; 4061563.11 north; 594855.48 east; 36.69 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:15:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862943.25 2024-11-06T03:15:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:43Z- waiting for 1730862947.25, time is 1730862943.69 2024-11-06T03:15:44Z- AHRS_M2 msg: 1730862944.04 epoch sec; seqNo:3683 2024-11-06T03:15:44Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 3.39 psi 2024-11-06T03:15:44Z- handleDepth()- Depth_Keller msg: 1730862944.04 epoch sec; seqNo:3921; depth 50.33 2024-11-06T03:15:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862944.06 epoch sec; seqNo:3921; 4061562.73 north; 594855.49 east; 36.69 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:15:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862943.25 2024-11-06T03:15:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:44Z- waiting for 1730862947.25, time is 1730862944.06 2024-11-06T03:15:44Z- AHRS_M2 msg: 1730862944.43 epoch sec; seqNo:3684 2024-11-06T03:15:44Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 3.39 psi 2024-11-06T03:15:44Z- handleDepth()- Depth_Keller msg: 1730862944.45 epoch sec; seqNo:3922; depth 50.33 2024-11-06T03:15:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862944.46 epoch sec; seqNo:3922; 4061562.35 north; 594855.49 east; 36.69 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:15:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862943.25 2024-11-06T03:15:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:44Z- waiting for 1730862947.25, time is 1730862944.46 2024-11-06T03:15:44Z- AHRS_M2 msg: 1730862944.84 epoch sec; seqNo:3685 2024-11-06T03:15:44Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 3.38 psi 2024-11-06T03:15:44Z- handleDepth()- Depth_Keller msg: 1730862944.84 epoch sec; seqNo:3923; depth 50.34 2024-11-06T03:15:44Z- handleDVL() - DVL msg epochMillisec: 1730862944.87 2024-11-06T03:15:44Z- handleDvl() - RDI_Pathfinder msg: 1730862944.87 sec; seqNo:1312; #beams:4 2024-11-06T03:15:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.03 , 34.20 , 41.12 , 42.61 2024-11-06T03:15:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , 0.09 2024-11-06T03:15:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862944.88 epoch sec; seqNo:3923; 4061561.97 north; 594855.50 east; 36.69 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:15:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862943.25 2024-11-06T03:15:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:44Z- waiting for 1730862947.25, time is 1730862944.88 2024-11-06T03:15:45Z- AHRS_M2 msg: 1730862945.24 epoch sec; seqNo:3686 2024-11-06T03:15:45Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 3.39 psi 2024-11-06T03:15:45Z- handleDepth()- Depth_Keller msg: 1730862945.25 epoch sec; seqNo:3924; depth 50.36 2024-11-06T03:15:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862945.26 epoch sec; seqNo:3924; 4061561.58 north; 594854.28 east; 36.69 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:15:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862943.25 2024-11-06T03:15:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:45Z- waiting for 1730862947.25, time is 1730862945.26 2024-11-06T03:15:45Z- AHRS_M2 msg: 1730862945.65 epoch sec; seqNo:3687 2024-11-06T03:15:45Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 3.39 psi 2024-11-06T03:15:45Z- handleDepth()- Depth_Keller msg: 1730862945.65 epoch sec; seqNo:3925; depth 50.34 2024-11-06T03:15:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862945.66 epoch sec; seqNo:3925; 4061561.20 north; 594854.28 east; 36.69 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:15:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862943.25 2024-11-06T03:15:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:45Z- waiting for 1730862947.25, time is 1730862945.66 2024-11-06T03:15:46Z- AHRS_M2 msg: 1730862946.07 epoch sec; seqNo:3688 2024-11-06T03:15:46Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 3.39 psi 2024-11-06T03:15:46Z- handleDepth()- Depth_Keller msg: 1730862946.07 epoch sec; seqNo:3926; depth 50.35 2024-11-06T03:15:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862946.09 epoch sec; seqNo:3926; 4061560.82 north; 594854.29 east; 36.69 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:15:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862943.25 2024-11-06T03:15:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:46Z- waiting for 1730862947.25, time is 1730862946.09 2024-11-06T03:15:46Z- AHRS_M2 msg: 1730862946.46 epoch sec; seqNo:3689 2024-11-06T03:15:46Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 3.40 psi 2024-11-06T03:15:46Z- handleDepth()- Depth_Keller msg: 1730862946.46 epoch sec; seqNo:3927; depth 50.34 2024-11-06T03:15:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862946.48 epoch sec; seqNo:3927; 4061560.44 north; 594854.29 east; 36.69 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:15:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862943.25 2024-11-06T03:15:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:46Z- waiting for 1730862947.25, time is 1730862946.48 2024-11-06T03:15:46Z- AHRS_M2 msg: 1730862946.86 epoch sec; seqNo:3690 2024-11-06T03:15:46Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 3.40 psi 2024-11-06T03:15:46Z- handleDepth()- Depth_Keller msg: 1730862946.87 epoch sec; seqNo:3928; depth 50.34 2024-11-06T03:15:46Z- handleDVL() - DVL msg epochMillisec: 1730862946.89 2024-11-06T03:15:46Z- handleDvl() - RDI_Pathfinder msg: 1730862946.89 sec; seqNo:1313; #beams:4 2024-11-06T03:15:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.66 , 34.39 , 41.49 , 41.68 2024-11-06T03:15:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , 0.04 2024-11-06T03:15:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862946.90 epoch sec; seqNo:3928; 4061560.06 north; 594854.30 east; 36.69 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:15:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862943.25 2024-11-06T03:15:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:47Z- waiting for 1730862947.25, time is 1730862946.90 2024-11-06T03:15:47Z- AHRS_M2 msg: 1730862947.26 epoch sec; seqNo:3691 2024-11-06T03:15:47Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.40 psi 2024-11-06T03:15:47Z- handleDepth()- Depth_Keller msg: 1730862947.27 epoch sec; seqNo:3929; depth 50.35 2024-11-06T03:15:47Z- handleDVL() - DVL msg epochMillisec: 1730862947.27 2024-11-06T03:15:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:47Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:47Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:47Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:47Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:47Z- handleDvl() - RDI_Pathfinder msg: 1730862947.27 sec; seqNo:1314; #beams:0 2024-11-06T03:15:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , 0.04 2024-11-06T03:15:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862947.29 epoch sec; seqNo:3929; 4061559.69 north; 594854.30 east; 36.69 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:15:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862943.25 2024-11-06T03:15:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:47Z- waiting for 1730862947.25, time is 1730862947.29 2024-11-06T03:15:47Z- LcmTrn::updateTrn() >>>> pose time 1730862947.29, meas time 1730862947.27 <<<< 2024-11-06T03:15:47Z- LcmTrn::updateTrn() equating pose 1730862947.27 and meas times 1730862947.27 2024-11-06T03:15:47Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862947.27 2024-11-06T03:15:47Z- TerrainNav::motionUpdate() - currEstimate = (4061560, 594854.30, 50.35) 2024-11-06T03:15:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:15:47Z- TerrainNav:: SubcloudNIS is 0.46, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:15:47Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:15:47Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:15:47Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:15:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:15:47Z- variances([0], [2], and psi_berg) = 0.236321 0.128369 0.000000 2024-11-06T03:15:47Z- publishEstimatesDecoder() - at 1730862947.27 sec 2024-11-06T03:15:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:15:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061590.52 594836.29 59.14 2024-11-06T03:15:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061590.25 594836.85 59.05 lat/lon: 36.70 -121.94 2024-11-06T03:15:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.13 0.92 0.00 2024-11-06T03:15:47Z- AHRS_M2 msg: 1730862947.67 epoch sec; seqNo:3692 2024-11-06T03:15:47Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.40 psi 2024-11-06T03:15:47Z- handleDepth()- Depth_Keller msg: 1730862947.68 epoch sec; seqNo:3930; depth 50.33 2024-11-06T03:15:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862947.69 epoch sec; seqNo:3930; 4061559.31 north; 594854.31 east; 36.69 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:15:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862947.27 2024-11-06T03:15:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:47Z- waiting for 1730862951.27, time is 1730862947.69 2024-11-06T03:15:48Z- AHRS_M2 msg: 1730862948.08 epoch sec; seqNo:3693 2024-11-06T03:15:48Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.40 psi 2024-11-06T03:15:48Z- handleDepth()- Depth_Keller msg: 1730862948.08 epoch sec; seqNo:3931; depth 50.31 2024-11-06T03:15:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862948.13 epoch sec; seqNo:3931; 4061558.93 north; 594854.31 east; 36.69 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:15:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862947.27 2024-11-06T03:15:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:48Z- waiting for 1730862951.27, time is 1730862948.13 2024-11-06T03:15:48Z- AHRS_M2 msg: 1730862948.47 epoch sec; seqNo:3694 2024-11-06T03:15:48Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.40 psi 2024-11-06T03:15:48Z- handleDepth()- Depth_Keller msg: 1730862948.48 epoch sec; seqNo:3932; depth 50.30 2024-11-06T03:15:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862948.49 epoch sec; seqNo:3932; 4061558.55 north; 594854.31 east; 36.69 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:15:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862947.27 2024-11-06T03:15:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:48Z- waiting for 1730862951.27, time is 1730862948.49 2024-11-06T03:15:48Z- AHRS_M2 msg: 1730862948.88 epoch sec; seqNo:3695 2024-11-06T03:15:48Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.40 psi 2024-11-06T03:15:48Z- handleDepth()- Depth_Keller msg: 1730862948.89 epoch sec; seqNo:3933; depth 50.29 2024-11-06T03:15:48Z- handleDVL() - DVL msg epochMillisec: 1730862948.91 2024-11-06T03:15:48Z- handleDvl() - RDI_Pathfinder msg: 1730862948.91 sec; seqNo:1315; #beams:4 2024-11-06T03:15:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.41 , 34.39 , 41.87 , 42.24 2024-11-06T03:15:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.03 2024-11-06T03:15:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862948.93 epoch sec; seqNo:3933; 4061558.17 north; 594854.32 east; 36.69 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:15:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862947.27 2024-11-06T03:15:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:49Z- waiting for 1730862951.27, time is 1730862948.93 2024-11-06T03:15:49Z- AHRS_M2 msg: 1730862949.28 epoch sec; seqNo:3696 2024-11-06T03:15:49Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.41 psi 2024-11-06T03:15:49Z- handleDepth()- Depth_Keller msg: 1730862949.29 epoch sec; seqNo:3934; depth 50.26 2024-11-06T03:15:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862949.31 epoch sec; seqNo:3934; 4061557.79 north; 594854.32 east; 36.69 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:15:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862947.27 2024-11-06T03:15:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:49Z- waiting for 1730862951.27, time is 1730862949.31 2024-11-06T03:15:49Z- AHRS_M2 msg: 1730862949.69 epoch sec; seqNo:3697 2024-11-06T03:15:49Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.41 psi 2024-11-06T03:15:49Z- handleDepth()- Depth_Keller msg: 1730862949.70 epoch sec; seqNo:3935; depth 50.25 2024-11-06T03:15:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862949.71 epoch sec; seqNo:3935; 4061557.41 north; 594854.33 east; 36.69 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:15:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862947.27 2024-11-06T03:15:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:49Z- waiting for 1730862951.27, time is 1730862949.71 2024-11-06T03:15:50Z- AHRS_M2 msg: 1730862950.10 epoch sec; seqNo:3698 2024-11-06T03:15:50Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.41 psi 2024-11-06T03:15:50Z- handleDepth()- Depth_Keller msg: 1730862950.10 epoch sec; seqNo:3936; depth 50.23 2024-11-06T03:15:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862950.11 epoch sec; seqNo:3936; 4061557.03 north; 594854.33 east; 36.69 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:15:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862947.27 2024-11-06T03:15:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:50Z- waiting for 1730862951.27, time is 1730862950.11 2024-11-06T03:15:50Z- AHRS_M2 msg: 1730862950.49 epoch sec; seqNo:3699 2024-11-06T03:15:50Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.41 psi 2024-11-06T03:15:50Z- handleDepth()- Depth_Keller msg: 1730862950.51 epoch sec; seqNo:3937; depth 50.21 2024-11-06T03:15:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862950.52 epoch sec; seqNo:3937; 4061556.65 north; 594854.33 east; 36.69 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:15:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862947.27 2024-11-06T03:15:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:50Z- waiting for 1730862951.27, time is 1730862950.52 2024-11-06T03:15:50Z- AHRS_M2 msg: 1730862950.90 epoch sec; seqNo:3700 2024-11-06T03:15:50Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.41 psi 2024-11-06T03:15:50Z- handleDepth()- Depth_Keller msg: 1730862950.91 epoch sec; seqNo:3938; depth 50.18 2024-11-06T03:15:50Z- handleDVL() - DVL msg epochMillisec: 1730862950.94 2024-11-06T03:15:50Z- handleDvl() - RDI_Pathfinder msg: 1730862950.94 sec; seqNo:1316; #beams:4 2024-11-06T03:15:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.32 , 34.46 , 41.59 , 42.21 2024-11-06T03:15:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , -0.06 2024-11-06T03:15:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862950.95 epoch sec; seqNo:3938; 4061556.65 north; 594854.33 east; 36.69 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:15:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862947.27 2024-11-06T03:15:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:51Z- waiting for 1730862951.27, time is 1730862950.95 2024-11-06T03:15:51Z- AHRS_M2 msg: 1730862951.30 epoch sec; seqNo:3701 2024-11-06T03:15:51Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.41 psi 2024-11-06T03:15:51Z- handleDepth()- Depth_Keller msg: 1730862951.31 epoch sec; seqNo:3939; depth 50.17 2024-11-06T03:15:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862951.32 epoch sec; seqNo:3939; 4061556.28 north; 594854.34 east; 36.69 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:15:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862947.27 2024-11-06T03:15:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:51Z- waiting for 1730862951.27, time is 1730862951.32 2024-11-06T03:15:51Z- LcmTrn::updateTrn() >>>> pose time 1730862951.32, meas time 1730862950.94 <<<< 2024-11-06T03:15:51Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862951.32 2024-11-06T03:15:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862950.94 sec, ping # 0; waiting for INS data more recent than 1730862947.27... 2024-11-06T03:15:51Z- TerrainNav::motionUpdate() - currEstimate = (4061556, 594854.34, 50.17) 2024-11-06T03:15:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:15:51Z- TerrainNav:: SubcloudNIS is 0.46, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:15:51Z- TNavPF::Projecting Measurements 2024-11-06T03:15:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:15:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:15:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:15:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:15:51Z- TNavFilter::4 good beams. 2024-11-06T03:15:51Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:15:51Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:15:51Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:15:51Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:15:51Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:15:51Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:15:51Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:15:51Z- TerrainMapOctree: (vehN, vehE ) = (4061556.60, 594854.34). 2024-11-06T03:15:51Z- TerrainMapOctree: Min Distance = 219.75. 2024-11-06T03:15:51Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:15:51Z- Forcing Subcloud Comparison 2024-11-06T03:15:51Z- Weighting particles with subcloud comparison 2024-11-06T03:15:51Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:51Z- meanExpectedMeasDiff: 1.343866 delta_rms_squared: 1.903103 alpha: 0.989600 2024-11-06T03:15:51Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:51Z- meanExpectedMeasDiff: -0.815274 delta_rms_squared: 1.340209 alpha: 0.985296 2024-11-06T03:15:51Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:51Z- meanExpectedMeasDiff: 2.533082 delta_rms_squared: 1.546861 alpha: 0.987236 2024-11-06T03:15:51Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:51Z- meanExpectedMeasDiff: -0.014494 delta_rms_squared: 1.613810 alpha: 0.987759 2024-11-06T03:15:51Z- Subcloud NIS: 0.467256 2024-11-06T03:15:51Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:15:51Z- TNavPF:: sumWeights = 0.277235 2024-11-06T03:15:51Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:15:51Z- TNavPF::First Term of Map Covariance Matrix is 0.35 2024-11-06T03:15:51Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T03:15:51Z- TerrainNav::Resampling particle filter... 2024-11-06T03:15:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862950.94 sec. 2024-11-06T03:15:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:15:51Z- variances([0], [2], and psi_berg) = 0.237085 0.129073 0.000000 2024-11-06T03:15:51Z- publishEstimatesDecoder() - at 1730862951.32 sec 2024-11-06T03:15:51Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:15:51Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061587.12 594837.28 58.00 2024-11-06T03:15:51Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061586.84 594836.89 58.54 lat/lon: 36.70 -121.94 2024-11-06T03:15:51Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.13 0.73 0.00 2024-11-06T03:15:51Z- AHRS_M2 msg: 1730862951.73 epoch sec; seqNo:3702 2024-11-06T03:15:51Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.41 psi 2024-11-06T03:15:51Z- handleDepth()- Depth_Keller msg: 1730862951.74 epoch sec; seqNo:3940; depth 50.17 2024-11-06T03:15:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862951.75 epoch sec; seqNo:3940; 4061555.88 north; 594853.12 east; 36.69 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:15:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862951.32 2024-11-06T03:15:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:51Z- waiting for 1730862955.32, time is 1730862951.75 2024-11-06T03:15:52Z- AHRS_M2 msg: 1730862952.11 epoch sec; seqNo:3703 2024-11-06T03:15:52Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.40 psi 2024-11-06T03:15:52Z- handleDepth()- Depth_Keller msg: 1730862952.12 epoch sec; seqNo:3941; depth 50.14 2024-11-06T03:15:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862952.14 epoch sec; seqNo:3941; 4061555.50 north; 594853.13 east; 36.69 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:15:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862951.32 2024-11-06T03:15:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:52Z- waiting for 1730862955.32, time is 1730862952.14 2024-11-06T03:15:52Z- AHRS_M2 msg: 1730862952.52 epoch sec; seqNo:3704 2024-11-06T03:15:52Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.40 psi 2024-11-06T03:15:52Z- handleDepth()- Depth_Keller msg: 1730862952.52 epoch sec; seqNo:3942; depth 50.13 2024-11-06T03:15:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862952.53 epoch sec; seqNo:3942; 4061555.13 north; 594853.13 east; 36.69 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:15:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862951.32 2024-11-06T03:15:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:52Z- waiting for 1730862955.32, time is 1730862952.53 2024-11-06T03:15:52Z- AHRS_M2 msg: 1730862952.92 epoch sec; seqNo:3705 2024-11-06T03:15:52Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.40 psi 2024-11-06T03:15:52Z- handleDepth()- Depth_Keller msg: 1730862952.92 epoch sec; seqNo:3943; depth 50.13 2024-11-06T03:15:52Z- handleDVL() - DVL msg epochMillisec: 1730862952.94 2024-11-06T03:15:52Z- handleDvl() - RDI_Pathfinder msg: 1730862952.94 sec; seqNo:1317; #beams:4 2024-11-06T03:15:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.59 , 34.39 , 41.49 , 42.05 2024-11-06T03:15:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.12 , -0.03 2024-11-06T03:15:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862952.96 epoch sec; seqNo:3943; 4061554.75 north; 594853.14 east; 36.69 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:15:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862951.32 2024-11-06T03:15:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:53Z- waiting for 1730862955.32, time is 1730862952.96 2024-11-06T03:15:53Z- AHRS_M2 msg: 1730862953.32 epoch sec; seqNo:3706 2024-11-06T03:15:53Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T03:15:53Z- handleDepth()- Depth_Keller msg: 1730862953.34 epoch sec; seqNo:3944; depth 50.11 2024-11-06T03:15:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862953.35 epoch sec; seqNo:3944; 4061554.37 north; 594853.14 east; 36.69 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:15:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862951.32 2024-11-06T03:15:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:53Z- waiting for 1730862955.32, time is 1730862953.35 2024-11-06T03:15:53Z- AHRS_M2 msg: 1730862953.72 epoch sec; seqNo:3707 2024-11-06T03:15:53Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T03:15:53Z- handleDepth()- Depth_Keller msg: 1730862953.73 epoch sec; seqNo:3945; depth 50.10 2024-11-06T03:15:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862953.76 epoch sec; seqNo:3945; 4061553.99 north; 594853.14 east; 36.69 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:15:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862951.32 2024-11-06T03:15:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:53Z- waiting for 1730862955.32, time is 1730862953.76 2024-11-06T03:15:54Z- AHRS_M2 msg: 1730862954.13 epoch sec; seqNo:3708 2024-11-06T03:15:54Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T03:15:54Z- handleDepth()- Depth_Keller msg: 1730862954.13 epoch sec; seqNo:3946; depth 50.09 2024-11-06T03:15:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862954.15 epoch sec; seqNo:3946; 4061553.99 north; 594853.14 east; 36.69 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:15:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862951.32 2024-11-06T03:15:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:54Z- waiting for 1730862955.32, time is 1730862954.15 2024-11-06T03:15:54Z- AHRS_M2 msg: 1730862954.54 epoch sec; seqNo:3709 2024-11-06T03:15:54Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T03:15:54Z- handleDepth()- Depth_Keller msg: 1730862954.55 epoch sec; seqNo:3947; depth 50.09 2024-11-06T03:15:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862954.56 epoch sec; seqNo:3947; 4061553.61 north; 594853.15 east; 36.69 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:15:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862951.32 2024-11-06T03:15:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:54Z- waiting for 1730862955.32, time is 1730862954.56 2024-11-06T03:15:54Z- AHRS_M2 msg: 1730862954.94 epoch sec; seqNo:3710 2024-11-06T03:15:54Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.38 psi 2024-11-06T03:15:54Z- handleDepth()- Depth_Keller msg: 1730862954.95 epoch sec; seqNo:3948; depth 50.10 2024-11-06T03:15:54Z- handleDVL() - DVL msg epochMillisec: 1730862954.97 2024-11-06T03:15:54Z- handleDvl() - RDI_Pathfinder msg: 1730862954.97 sec; seqNo:1318; #beams:4 2024-11-06T03:15:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.66 , 34.39 , 41.12 , 41.49 2024-11-06T03:15:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.16 , 0.04 2024-11-06T03:15:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862954.98 epoch sec; seqNo:3948; 4061553.23 north; 594853.15 east; 36.69 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:15:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862951.32 2024-11-06T03:15:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:55Z- waiting for 1730862955.32, time is 1730862954.98 2024-11-06T03:15:55Z- AHRS_M2 msg: 1730862955.34 epoch sec; seqNo:3711 2024-11-06T03:15:55Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.38 psi 2024-11-06T03:15:55Z- handleDepth()- Depth_Keller msg: 1730862955.35 epoch sec; seqNo:3949; depth 50.09 2024-11-06T03:15:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862955.37 epoch sec; seqNo:3949; 4061552.84 north; 594851.94 east; 36.69 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:15:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862951.32 2024-11-06T03:15:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:55Z- waiting for 1730862955.32, time is 1730862955.37 2024-11-06T03:15:55Z- LcmTrn::updateTrn() >>>> pose time 1730862955.37, meas time 1730862954.97 <<<< 2024-11-06T03:15:55Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862955.37 2024-11-06T03:15:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862954.97 sec, ping # 0; waiting for INS data more recent than 1730862951.32... 2024-11-06T03:15:55Z- TerrainNav::motionUpdate() - currEstimate = (4061553, 594851.94, 50.09) 2024-11-06T03:15:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:15:55Z- TerrainNav:: SubcloudNIS is 0.47, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:15:55Z- TNavPF::Projecting Measurements 2024-11-06T03:15:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:15:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:15:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:15:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:15:55Z- TNavFilter::4 good beams. 2024-11-06T03:15:55Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:15:55Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:15:55Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:15:55Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:15:55Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:15:55Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:15:55Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:15:55Z- TerrainMapOctree: (vehN, vehE ) = (4061553.18, 594852.17). 2024-11-06T03:15:55Z- TerrainMapOctree: Min Distance = 221.76. 2024-11-06T03:15:55Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:15:55Z- Forcing Subcloud Comparison 2024-11-06T03:15:55Z- Weighting particles with subcloud comparison 2024-11-06T03:15:55Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:55Z- meanExpectedMeasDiff: 0.418273 delta_rms_squared: 1.586443 alpha: 0.987550 2024-11-06T03:15:55Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:55Z- meanExpectedMeasDiff: -0.350994 delta_rms_squared: 1.142158 alpha: 0.982791 2024-11-06T03:15:55Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:55Z- meanExpectedMeasDiff: 2.712190 delta_rms_squared: 1.310861 alpha: 0.984972 2024-11-06T03:15:55Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:55Z- meanExpectedMeasDiff: -1.105237 delta_rms_squared: 1.360642 alpha: 0.985514 2024-11-06T03:15:55Z- Subcloud NIS: 0.468754 2024-11-06T03:15:55Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:15:55Z- TNavPF:: sumWeights = 0.261414 2024-11-06T03:15:55Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:15:55Z- TNavPF::First Term of Map Covariance Matrix is 0.20 2024-11-06T03:15:55Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T03:15:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862954.97 sec. 2024-11-06T03:15:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:15:55Z- variances([0], [2], and psi_berg) = 0.241535 0.131103 0.000000 2024-11-06T03:15:55Z- publishEstimatesDecoder() - at 1730862955.37 sec 2024-11-06T03:15:55Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:15:55Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061583.38 594835.66 58.04 2024-11-06T03:15:55Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061583.40 594834.49 58.26 lat/lon: 36.70 -121.94 2024-11-06T03:15:55Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.13 0.70 0.00 2024-11-06T03:15:55Z- AHRS_M2 msg: 1730862955.76 epoch sec; seqNo:3712 2024-11-06T03:15:55Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.38 psi 2024-11-06T03:15:55Z- handleDepth()- Depth_Keller msg: 1730862955.78 epoch sec; seqNo:3950; depth 50.10 2024-11-06T03:15:55Z- handleDVL() - DVL msg epochMillisec: 1730862955.79 2024-11-06T03:15:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:15:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:15:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:15:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:15:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:15:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:15:55Z- handleDvl() - RDI_Pathfinder msg: 1730862955.79 sec; seqNo:1319; #beams:0 2024-11-06T03:15:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:15:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.16 , 0.04 2024-11-06T03:15:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862955.80 epoch sec; seqNo:3950; 4061552.46 north; 594851.94 east; 36.69 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:15:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862955.37 2024-11-06T03:15:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:55Z- waiting for 1730862959.37, time is 1730862955.80 2024-11-06T03:15:56Z- AHRS_M2 msg: 1730862956.15 epoch sec; seqNo:3713 2024-11-06T03:15:56Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.38 psi 2024-11-06T03:15:56Z- handleDepth()- Depth_Keller msg: 1730862956.16 epoch sec; seqNo:3951; depth 50.09 2024-11-06T03:15:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862956.18 epoch sec; seqNo:3951; 4061552.08 north; 594851.94 east; 36.69 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:15:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862955.37 2024-11-06T03:15:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:56Z- waiting for 1730862959.37, time is 1730862956.18 2024-11-06T03:15:56Z- AHRS_M2 msg: 1730862956.55 epoch sec; seqNo:3714 2024-11-06T03:15:56Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.38 psi 2024-11-06T03:15:56Z- handleDepth()- Depth_Keller msg: 1730862956.56 epoch sec; seqNo:3952; depth 50.11 2024-11-06T03:15:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862956.57 epoch sec; seqNo:3952; 4061551.70 north; 594851.95 east; 36.69 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:15:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862955.37 2024-11-06T03:15:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:56Z- waiting for 1730862959.37, time is 1730862956.57 2024-11-06T03:15:56Z- AHRS_M2 msg: 1730862956.96 epoch sec; seqNo:3715 2024-11-06T03:15:56Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.38 psi 2024-11-06T03:15:56Z- handleDepth()- Depth_Keller msg: 1730862956.97 epoch sec; seqNo:3953; depth 50.13 2024-11-06T03:15:57Z- handleDVL() - DVL msg epochMillisec: 1730862956.99 2024-11-06T03:15:57Z- handleDvl() - RDI_Pathfinder msg: 1730862956.99 sec; seqNo:1320; #beams:4 2024-11-06T03:15:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.66 , 34.58 , 41.12 , 41.87 2024-11-06T03:15:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , 0.10 2024-11-06T03:15:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862957.01 epoch sec; seqNo:3953; 4061551.32 north; 594851.95 east; 36.69 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:15:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862955.37 2024-11-06T03:15:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:57Z- waiting for 1730862959.37, time is 1730862957.01 2024-11-06T03:15:57Z- AHRS_M2 msg: 1730862957.37 epoch sec; seqNo:3716 2024-11-06T03:15:57Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.37 psi 2024-11-06T03:15:57Z- handleDepth()- Depth_Keller msg: 1730862957.37 epoch sec; seqNo:3954; depth 50.13 2024-11-06T03:15:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862957.39 epoch sec; seqNo:3954; 4061551.32 north; 594851.95 east; 36.69 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:15:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862955.37 2024-11-06T03:15:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:57Z- waiting for 1730862959.37, time is 1730862957.39 2024-11-06T03:15:57Z- AHRS_M2 msg: 1730862957.77 epoch sec; seqNo:3717 2024-11-06T03:15:57Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.37 psi 2024-11-06T03:15:57Z- handleDepth()- Depth_Keller msg: 1730862957.78 epoch sec; seqNo:3955; depth 50.15 2024-11-06T03:15:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862957.80 epoch sec; seqNo:3955; 4061550.94 north; 594851.96 east; 36.69 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:15:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862955.37 2024-11-06T03:15:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:57Z- waiting for 1730862959.37, time is 1730862957.80 2024-11-06T03:15:58Z- AHRS_M2 msg: 1730862958.17 epoch sec; seqNo:3718 2024-11-06T03:15:58Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.37 psi 2024-11-06T03:15:58Z- handleDepth()- Depth_Keller msg: 1730862958.18 epoch sec; seqNo:3956; depth 50.16 2024-11-06T03:15:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862958.20 epoch sec; seqNo:3956; 4061550.57 north; 594851.96 east; 36.69 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:15:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862955.37 2024-11-06T03:15:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:58Z- waiting for 1730862959.37, time is 1730862958.20 2024-11-06T03:15:58Z- AHRS_M2 msg: 1730862958.58 epoch sec; seqNo:3719 2024-11-06T03:15:58Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.38 psi 2024-11-06T03:15:58Z- handleDepth()- Depth_Keller msg: 1730862958.58 epoch sec; seqNo:3957; depth 50.17 2024-11-06T03:15:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862958.60 epoch sec; seqNo:3957; 4061550.19 north; 594851.97 east; 36.69 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:15:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862955.37 2024-11-06T03:15:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:58Z- waiting for 1730862959.37, time is 1730862958.60 2024-11-06T03:15:58Z- AHRS_M2 msg: 1730862958.98 epoch sec; seqNo:3720 2024-11-06T03:15:58Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.37 psi 2024-11-06T03:15:58Z- handleDepth()- Depth_Keller msg: 1730862958.99 epoch sec; seqNo:3958; depth 50.17 2024-11-06T03:15:59Z- handleDVL() - DVL msg epochMillisec: 1730862959.01 2024-11-06T03:15:59Z- handleDvl() - RDI_Pathfinder msg: 1730862959.01 sec; seqNo:1321; #beams:4 2024-11-06T03:15:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.85 , 34.76 , 41.12 , 42.05 2024-11-06T03:15:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , 0.11 2024-11-06T03:15:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862959.03 epoch sec; seqNo:3958; 4061549.81 north; 594851.97 east; 36.69 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:15:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:15:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862955.37 2024-11-06T03:15:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:59Z- waiting for 1730862959.37, time is 1730862959.03 2024-11-06T03:15:59Z- AHRS_M2 msg: 1730862959.38 epoch sec; seqNo:3721 2024-11-06T03:15:59Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.38 psi 2024-11-06T03:15:59Z- handleDepth()- Depth_Keller msg: 1730862959.39 epoch sec; seqNo:3959; depth 50.20 2024-11-06T03:15:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862959.41 epoch sec; seqNo:3959; 4061549.42 north; 594850.75 east; 36.69 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:15:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862955.37 2024-11-06T03:15:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:59Z- waiting for 1730862959.37, time is 1730862959.41 2024-11-06T03:15:59Z- LcmTrn::updateTrn() >>>> pose time 1730862959.41, meas time 1730862959.01 <<<< 2024-11-06T03:15:59Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862959.41 2024-11-06T03:15:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862959.01 sec, ping # 0; waiting for INS data more recent than 1730862955.37... 2024-11-06T03:15:59Z- TerrainNav::motionUpdate() - currEstimate = (4061549, 594850.75, 50.20) 2024-11-06T03:15:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:15:59Z- TerrainNav:: SubcloudNIS is 0.47, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:15:59Z- TNavPF::Projecting Measurements 2024-11-06T03:15:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:15:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:15:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:15:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:15:59Z- TNavFilter::4 good beams. 2024-11-06T03:15:59Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:15:59Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:15:59Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:15:59Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:15:59Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:15:59Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:15:59Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:15:59Z- TerrainMapOctree: (vehN, vehE ) = (4061549.75, 594850.87). 2024-11-06T03:15:59Z- TerrainMapOctree: Min Distance = 224.22. 2024-11-06T03:15:59Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:15:59Z- Forcing Subcloud Comparison 2024-11-06T03:15:59Z- Weighting particles with subcloud comparison 2024-11-06T03:15:59Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:59Z- meanExpectedMeasDiff: 0.576990 delta_rms_squared: 1.525126 alpha: 0.987056 2024-11-06T03:15:59Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:59Z- meanExpectedMeasDiff: 0.775258 delta_rms_squared: 1.022043 alpha: 0.980807 2024-11-06T03:15:59Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:59Z- meanExpectedMeasDiff: 2.808523 delta_rms_squared: 1.281490 alpha: 0.984633 2024-11-06T03:15:59Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:15:59Z- meanExpectedMeasDiff: -0.866616 delta_rms_squared: 1.326255 alpha: 0.985144 2024-11-06T03:15:59Z- Subcloud NIS: 0.465659 2024-11-06T03:15:59Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:15:59Z- TNavPF:: sumWeights = 0.275980 2024-11-06T03:15:59Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:15:59Z- TNavPF::First Term of Map Covariance Matrix is 0.20 2024-11-06T03:15:59Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T03:15:59Z- TerrainNav::Resampling particle filter... 2024-11-06T03:15:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862959.01 sec. 2024-11-06T03:15:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:15:59Z- variances([0], [2], and psi_berg) = 0.243961 0.121928 0.000000 2024-11-06T03:15:59Z- publishEstimatesDecoder() - at 1730862959.41 sec 2024-11-06T03:15:59Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:15:59Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061580.33 594833.58 59.97 2024-11-06T03:15:59Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061579.98 594833.32 57.98 lat/lon: 36.70 -121.94 2024-11-06T03:15:59Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.12 0.70 0.00 2024-11-06T03:15:59Z- AHRS_M2 msg: 1730862959.79 epoch sec; seqNo:3722 2024-11-06T03:15:59Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.38 psi 2024-11-06T03:15:59Z- handleDepth()- Depth_Keller msg: 1730862959.79 epoch sec; seqNo:3960; depth 50.20 2024-11-06T03:15:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862959.82 epoch sec; seqNo:3960; 4061549.04 north; 594850.76 east; 36.69 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:15:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:15:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862959.41 2024-11-06T03:15:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:15:59Z- waiting for 1730862963.41, time is 1730862959.82 2024-11-06T03:16:00Z- AHRS_M2 msg: 1730862960.19 epoch sec; seqNo:3723 2024-11-06T03:16:00Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.38 psi 2024-11-06T03:16:00Z- handleDepth()- Depth_Keller msg: 1730862960.20 epoch sec; seqNo:3961; depth 50.21 2024-11-06T03:16:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862960.21 epoch sec; seqNo:3961; 4061548.66 north; 594850.76 east; 36.69 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:16:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862959.41 2024-11-06T03:16:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:00Z- waiting for 1730862963.41, time is 1730862960.21 2024-11-06T03:16:00Z- AHRS_M2 msg: 1730862960.60 epoch sec; seqNo:3724 2024-11-06T03:16:00Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.38 psi 2024-11-06T03:16:00Z- handleDepth()- Depth_Keller msg: 1730862960.60 epoch sec; seqNo:3962; depth 50.21 2024-11-06T03:16:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862960.61 epoch sec; seqNo:3962; 4061548.28 north; 594850.77 east; 36.69 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:16:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862959.41 2024-11-06T03:16:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:00Z- waiting for 1730862963.41, time is 1730862960.61 2024-11-06T03:16:01Z- AHRS_M2 msg: 1730862961.00 epoch sec; seqNo:3725 2024-11-06T03:16:01Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.38 psi 2024-11-06T03:16:01Z- handleDepth()- Depth_Keller msg: 1730862961.01 epoch sec; seqNo:3963; depth 50.22 2024-11-06T03:16:01Z- handleDVL() - DVL msg epochMillisec: 1730862961.03 2024-11-06T03:16:01Z- handleDvl() - RDI_Pathfinder msg: 1730862961.03 sec; seqNo:1322; #beams:4 2024-11-06T03:16:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.22 , 34.76 , 41.49 , 42.42 2024-11-06T03:16:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.09 , 0.03 2024-11-06T03:16:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862961.06 epoch sec; seqNo:3963; 4061547.90 north; 594850.77 east; 36.69 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:16:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862959.41 2024-11-06T03:16:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:01Z- waiting for 1730862963.41, time is 1730862961.06 2024-11-06T03:16:01Z- AHRS_M2 msg: 1730862961.40 epoch sec; seqNo:3726 2024-11-06T03:16:01Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.39 psi 2024-11-06T03:16:01Z- handleDepth()- Depth_Keller msg: 1730862961.41 epoch sec; seqNo:3964; depth 50.21 2024-11-06T03:16:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862961.42 epoch sec; seqNo:3964; 4061547.52 north; 594850.77 east; 36.69 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:16:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862959.41 2024-11-06T03:16:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:01Z- waiting for 1730862963.41, time is 1730862961.42 2024-11-06T03:16:01Z- AHRS_M2 msg: 1730862961.81 epoch sec; seqNo:3727 2024-11-06T03:16:01Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:01Z- handleDepth()- Depth_Keller msg: 1730862961.83 epoch sec; seqNo:3965; depth 50.23 2024-11-06T03:16:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862961.84 epoch sec; seqNo:3965; 4061547.14 north; 594850.78 east; 36.69 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:16:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862959.41 2024-11-06T03:16:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:01Z- waiting for 1730862963.41, time is 1730862961.84 2024-11-06T03:16:02Z- AHRS_M2 msg: 1730862962.22 epoch sec; seqNo:3728 2024-11-06T03:16:02Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:02Z- handleDepth()- Depth_Keller msg: 1730862962.22 epoch sec; seqNo:3966; depth 50.21 2024-11-06T03:16:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862962.23 epoch sec; seqNo:3966; 4061546.76 north; 594850.78 east; 36.69 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:16:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862959.41 2024-11-06T03:16:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:02Z- waiting for 1730862963.41, time is 1730862962.23 2024-11-06T03:16:02Z- AHRS_M2 msg: 1730862962.62 epoch sec; seqNo:3729 2024-11-06T03:16:02Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:02Z- handleDepth()- Depth_Keller msg: 1730862962.62 epoch sec; seqNo:3967; depth 50.21 2024-11-06T03:16:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862962.64 epoch sec; seqNo:3967; 4061546.39 north; 594850.79 east; 36.69 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:16:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862959.41 2024-11-06T03:16:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:02Z- waiting for 1730862963.41, time is 1730862962.64 2024-11-06T03:16:03Z- AHRS_M2 msg: 1730862963.02 epoch sec; seqNo:3730 2024-11-06T03:16:03Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:03Z- handleDepth()- Depth_Keller msg: 1730862963.03 epoch sec; seqNo:3968; depth 50.21 2024-11-06T03:16:03Z- handleDVL() - DVL msg epochMillisec: 1730862963.05 2024-11-06T03:16:03Z- handleDvl() - RDI_Pathfinder msg: 1730862963.05 sec; seqNo:1323; #beams:4 2024-11-06T03:16:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.91 , 35.48 , 41.59 , 42.61 2024-11-06T03:16:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.05 2024-11-06T03:16:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862963.07 epoch sec; seqNo:3968; 4061546.01 north; 594850.79 east; 36.69 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:16:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862959.41 2024-11-06T03:16:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:03Z- waiting for 1730862963.41, time is 1730862963.07 2024-11-06T03:16:03Z- AHRS_M2 msg: 1730862963.42 epoch sec; seqNo:3731 2024-11-06T03:16:03Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:03Z- handleDepth()- Depth_Keller msg: 1730862963.43 epoch sec; seqNo:3969; depth 50.20 2024-11-06T03:16:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862963.45 epoch sec; seqNo:3969; 4061545.63 north; 594850.80 east; 36.69 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:16:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862959.41 2024-11-06T03:16:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:03Z- waiting for 1730862963.41, time is 1730862963.45 2024-11-06T03:16:03Z- LcmTrn::updateTrn() >>>> pose time 1730862963.45, meas time 1730862963.05 <<<< 2024-11-06T03:16:03Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862963.45 2024-11-06T03:16:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862963.05 sec, ping # 0; waiting for INS data more recent than 1730862959.41... 2024-11-06T03:16:03Z- TerrainNav::motionUpdate() - currEstimate = (4061546, 594850.80, 50.20) 2024-11-06T03:16:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:16:03Z- TerrainNav:: SubcloudNIS is 0.47, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:16:03Z- TNavPF::Projecting Measurements 2024-11-06T03:16:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:16:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:16:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:16:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:16:03Z- TNavFilter::4 good beams. 2024-11-06T03:16:03Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:16:03Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:16:03Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:16:03Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:16:03Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:16:03Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:16:03Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:16:03Z- TerrainMapOctree: (vehN, vehE ) = (4061546.00, 594850.79). 2024-11-06T03:16:03Z- TerrainMapOctree: Min Distance = 227.54. 2024-11-06T03:16:03Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:16:03Z- Forcing Subcloud Comparison 2024-11-06T03:16:03Z- Weighting particles with subcloud comparison 2024-11-06T03:16:03Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:03Z- meanExpectedMeasDiff: 0.044567 delta_rms_squared: 1.572287 alpha: 0.987439 2024-11-06T03:16:03Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:03Z- meanExpectedMeasDiff: 1.269838 delta_rms_squared: 1.090565 alpha: 0.981991 2024-11-06T03:16:03Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:03Z- meanExpectedMeasDiff: 3.633582 delta_rms_squared: 1.113782 alpha: 0.982360 2024-11-06T03:16:03Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:03Z- meanExpectedMeasDiff: -0.529498 delta_rms_squared: 1.297367 alpha: 0.984818 2024-11-06T03:16:03Z- Subcloud NIS: 0.474892 2024-11-06T03:16:03Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:16:03Z- TNavPF:: sumWeights = 0.167497 2024-11-06T03:16:03Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:16:03Z- TNavPF::First Term of Map Covariance Matrix is 0.17 2024-11-06T03:16:03Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T03:16:03Z- TerrainNav::Resampling particle filter... 2024-11-06T03:16:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862963.05 sec. 2024-11-06T03:16:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:16:03Z- variances([0], [2], and psi_berg) = 0.253856 0.135307 0.000000 2024-11-06T03:16:03Z- publishEstimatesDecoder() - at 1730862963.45 sec 2024-11-06T03:16:03Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:16:03Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061576.65 594833.77 58.68 2024-11-06T03:16:03Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061576.21 594833.38 57.46 lat/lon: 36.70 -121.94 2024-11-06T03:16:03Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.14 0.72 0.00 2024-11-06T03:16:03Z- AHRS_M2 msg: 1730862963.83 epoch sec; seqNo:3732 2024-11-06T03:16:03Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:03Z- handleDepth()- Depth_Keller msg: 1730862963.83 epoch sec; seqNo:3970; depth 50.16 2024-11-06T03:16:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862963.86 epoch sec; seqNo:3970; 4061545.25 north; 594850.80 east; 36.69 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:16:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862963.45 2024-11-06T03:16:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:03Z- waiting for 1730862967.45, time is 1730862963.86 2024-11-06T03:16:04Z- AHRS_M2 msg: 1730862964.24 epoch sec; seqNo:3733 2024-11-06T03:16:04Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:04Z- handleDepth()- Depth_Keller msg: 1730862964.24 epoch sec; seqNo:3971; depth 50.17 2024-11-06T03:16:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862964.27 epoch sec; seqNo:3971; 4061544.87 north; 594850.80 east; 36.69 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:16:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862963.45 2024-11-06T03:16:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:04Z- waiting for 1730862967.45, time is 1730862964.27 2024-11-06T03:16:04Z- AHRS_M2 msg: 1730862964.64 epoch sec; seqNo:3734 2024-11-06T03:16:04Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:04Z- handleDepth()- Depth_Keller msg: 1730862964.65 epoch sec; seqNo:3972; depth 50.15 2024-11-06T03:16:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862964.66 epoch sec; seqNo:3972; 4061544.49 north; 594850.81 east; 36.69 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:16:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862963.45 2024-11-06T03:16:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:04Z- waiting for 1730862967.45, time is 1730862964.66 2024-11-06T03:16:05Z- AHRS_M2 msg: 1730862965.04 epoch sec; seqNo:3735 2024-11-06T03:16:05Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:05Z- handleDepth()- Depth_Keller msg: 1730862965.05 epoch sec; seqNo:3973; depth 50.13 2024-11-06T03:16:05Z- handleDVL() - DVL msg epochMillisec: 1730862965.07 2024-11-06T03:16:05Z- handleDvl() - RDI_Pathfinder msg: 1730862965.07 sec; seqNo:1324; #beams:4 2024-11-06T03:16:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.91 , 35.48 , 41.59 , 42.41 2024-11-06T03:16:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , -0.06 2024-11-06T03:16:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862965.08 epoch sec; seqNo:3973; 4061544.11 north; 594850.81 east; 36.69 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:16:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862963.45 2024-11-06T03:16:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:05Z- waiting for 1730862967.45, time is 1730862965.08 2024-11-06T03:16:05Z- AHRS_M2 msg: 1730862965.44 epoch sec; seqNo:3736 2024-11-06T03:16:05Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:05Z- handleDepth()- Depth_Keller msg: 1730862965.45 epoch sec; seqNo:3974; depth 50.11 2024-11-06T03:16:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862965.47 epoch sec; seqNo:3974; 4061543.73 north; 594850.82 east; 36.69 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:16:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862963.45 2024-11-06T03:16:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:05Z- waiting for 1730862967.45, time is 1730862965.47 2024-11-06T03:16:05Z- AHRS_M2 msg: 1730862965.85 epoch sec; seqNo:3737 2024-11-06T03:16:05Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:05Z- handleDepth()- Depth_Keller msg: 1730862965.85 epoch sec; seqNo:3975; depth 50.09 2024-11-06T03:16:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862965.86 epoch sec; seqNo:3975; 4061543.34 north; 594849.60 east; 36.69 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:16:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862963.45 2024-11-06T03:16:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:06Z- waiting for 1730862967.45, time is 1730862965.86 2024-11-06T03:16:06Z- AHRS_M2 msg: 1730862966.25 epoch sec; seqNo:3738 2024-11-06T03:16:06Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:06Z- handleDepth()- Depth_Keller msg: 1730862966.26 epoch sec; seqNo:3976; depth 50.09 2024-11-06T03:16:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862966.27 epoch sec; seqNo:3976; 4061543.34 north; 594849.60 east; 36.69 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:16:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862963.45 2024-11-06T03:16:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:06Z- waiting for 1730862967.45, time is 1730862966.27 2024-11-06T03:16:06Z- AHRS_M2 msg: 1730862966.66 epoch sec; seqNo:3739 2024-11-06T03:16:06Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:06Z- handleDepth()- Depth_Keller msg: 1730862966.66 epoch sec; seqNo:3977; depth 50.07 2024-11-06T03:16:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862966.67 epoch sec; seqNo:3977; 4061542.96 north; 594849.60 east; 36.69 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:16:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862963.45 2024-11-06T03:16:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:06Z- waiting for 1730862967.45, time is 1730862966.67 2024-11-06T03:16:07Z- AHRS_M2 msg: 1730862967.06 epoch sec; seqNo:3740 2024-11-06T03:16:07Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:07Z- handleDepth()- Depth_Keller msg: 1730862967.07 epoch sec; seqNo:3978; depth 50.05 2024-11-06T03:16:07Z- handleDVL() - DVL msg epochMillisec: 1730862967.10 2024-11-06T03:16:07Z- handleDvl() - RDI_Pathfinder msg: 1730862967.10 sec; seqNo:1325; #beams:4 2024-11-06T03:16:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.12 , 35.88 , 41.39 , 42.61 2024-11-06T03:16:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.09 , 0.00 2024-11-06T03:16:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862967.11 epoch sec; seqNo:3978; 4061542.58 north; 594849.61 east; 36.69 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:16:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862963.45 2024-11-06T03:16:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:07Z- waiting for 1730862967.45, time is 1730862967.11 2024-11-06T03:16:07Z- handleDepth()- Depth_Keller msg: 1730862967.49 epoch sec; seqNo:3979; depth 50.04 2024-11-06T03:16:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862967.51 epoch sec; seqNo:3979; 4061542.20 north; 594849.61 east; 36.69 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:16:07Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:16:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862963.45 2024-11-06T03:16:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:07Z- waiting for 1730862967.45, time is 1730862967.51 2024-11-06T03:16:07Z- LcmTrn::updateTrn() >>>> pose time 1730862967.51, meas time 1730862967.10 <<<< 2024-11-06T03:16:07Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862967.51 2024-11-06T03:16:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862967.10 sec, ping # 0; waiting for INS data more recent than 1730862963.45... 2024-11-06T03:16:07Z- TerrainNav::motionUpdate() - currEstimate = (4061542, 594849.61, 50.04) 2024-11-06T03:16:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:16:07Z- TerrainNav:: SubcloudNIS is 0.47, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:16:07Z- TNavPF::Projecting Measurements 2024-11-06T03:16:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:16:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:16:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:16:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:16:07Z- TNavFilter::4 good beams. 2024-11-06T03:16:07Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:16:07Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:16:07Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:16:07Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:16:07Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:16:07Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:16:07Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:16:07Z- TerrainMapOctree: (vehN, vehE ) = (4061542.55, 594849.73). 2024-11-06T03:16:07Z- TerrainMapOctree: Min Distance = 230.18. 2024-11-06T03:16:07Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:16:07Z- Forcing Subcloud Comparison 2024-11-06T03:16:07Z- Weighting particles with subcloud comparison 2024-11-06T03:16:07Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:07Z- meanExpectedMeasDiff: -1.442182 delta_rms_squared: 2.983289 alpha: 0.993341 2024-11-06T03:16:07Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:07Z- meanExpectedMeasDiff: 1.090648 delta_rms_squared: 1.095655 alpha: 0.982073 2024-11-06T03:16:07Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:07Z- meanExpectedMeasDiff: 3.794956 delta_rms_squared: 1.141186 alpha: 0.982776 2024-11-06T03:16:07Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:07Z- meanExpectedMeasDiff: -1.306339 delta_rms_squared: 1.311695 alpha: 0.984982 2024-11-06T03:16:07Z- Subcloud NIS: 0.496201 2024-11-06T03:16:07Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:16:07Z- TNavPF:: sumWeights = 0.079849 2024-11-06T03:16:07Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:16:07Z- TNavPF::First Term of Map Covariance Matrix is 0.31 2024-11-06T03:16:07Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T03:16:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862967.10 sec. 2024-11-06T03:16:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:16:07Z- variances([0], [2], and psi_berg) = 0.285425 0.146344 0.000000 2024-11-06T03:16:07Z- publishEstimatesDecoder() - at 1730862967.51 sec 2024-11-06T03:16:07Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:16:07Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061573.96 594832.93 57.24 2024-11-06T03:16:07Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061572.85 594832.18 57.14 lat/lon: 36.70 -121.94 2024-11-06T03:16:07Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.29 0.15 0.70 0.00 2024-11-06T03:16:07Z- AHRS_M2 msg: 1730862967.87 epoch sec; seqNo:3741 2024-11-06T03:16:07Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:07Z- handleDepth()- Depth_Keller msg: 1730862967.88 epoch sec; seqNo:3980; depth 50.02 2024-11-06T03:16:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862967.89 epoch sec; seqNo:3980; 4061541.83 north; 594849.62 east; 36.69 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:16:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862967.51 2024-11-06T03:16:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:08Z- waiting for 1730862971.51, time is 1730862967.89 2024-11-06T03:16:08Z- AHRS_M2 msg: 1730862968.28 epoch sec; seqNo:3742 2024-11-06T03:16:08Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:08Z- handleDepth()- Depth_Keller msg: 1730862968.28 epoch sec; seqNo:3981; depth 50.00 2024-11-06T03:16:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862968.30 epoch sec; seqNo:3981; 4061541.45 north; 594849.62 east; 36.69 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:16:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862967.51 2024-11-06T03:16:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:08Z- waiting for 1730862971.51, time is 1730862968.30 2024-11-06T03:16:08Z- AHRS_M2 msg: 1730862968.68 epoch sec; seqNo:3743 2024-11-06T03:16:08Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:08Z- handleDepth()- Depth_Keller msg: 1730862968.68 epoch sec; seqNo:3982; depth 50.00 2024-11-06T03:16:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862968.73 epoch sec; seqNo:3982; 4061541.07 north; 594849.63 east; 36.69 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:16:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862967.51 2024-11-06T03:16:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:08Z- waiting for 1730862971.51, time is 1730862968.73 2024-11-06T03:16:09Z- AHRS_M2 msg: 1730862969.09 epoch sec; seqNo:3744 2024-11-06T03:16:09Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:09Z- handleDepth()- Depth_Keller msg: 1730862969.10 epoch sec; seqNo:3983; depth 49.99 2024-11-06T03:16:09Z- handleDVL() - DVL msg epochMillisec: 1730862969.12 2024-11-06T03:16:09Z- handleDvl() - RDI_Pathfinder msg: 1730862969.12 sec; seqNo:1326; #beams:4 2024-11-06T03:16:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.12 , 34.87 , 41.39 , 42.61 2024-11-06T03:16:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.10 , 0.05 2024-11-06T03:16:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862969.14 epoch sec; seqNo:3983; 4061540.69 north; 594849.63 east; 36.69 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:16:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862967.51 2024-11-06T03:16:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:09Z- waiting for 1730862971.51, time is 1730862969.14 2024-11-06T03:16:09Z- AHRS_M2 msg: 1730862969.48 epoch sec; seqNo:3745 2024-11-06T03:16:09Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:09Z- handleDepth()- Depth_Keller msg: 1730862969.49 epoch sec; seqNo:3984; depth 49.99 2024-11-06T03:16:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862969.51 epoch sec; seqNo:3984; 4061540.31 north; 594849.63 east; 36.69 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:16:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862967.51 2024-11-06T03:16:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:09Z- waiting for 1730862971.51, time is 1730862969.51 2024-11-06T03:16:09Z- AHRS_M2 msg: 1730862969.89 epoch sec; seqNo:3746 2024-11-06T03:16:09Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:09Z- handleDepth()- Depth_Keller msg: 1730862969.90 epoch sec; seqNo:3985; depth 49.99 2024-11-06T03:16:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862969.92 epoch sec; seqNo:3985; 4061540.31 north; 594849.63 east; 36.69 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:16:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862967.51 2024-11-06T03:16:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:10Z- waiting for 1730862971.51, time is 1730862969.92 2024-11-06T03:16:10Z- AHRS_M2 msg: 1730862970.29 epoch sec; seqNo:3747 2024-11-06T03:16:10Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:10Z- handleDepth()- Depth_Keller msg: 1730862970.30 epoch sec; seqNo:3986; depth 49.98 2024-11-06T03:16:10Z- handleDVL() - DVL msg epochMillisec: 1730862970.30 2024-11-06T03:16:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:16:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:16:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:16:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:16:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:16:10Z- handleDvl() - RDI_Pathfinder msg: 1730862970.30 sec; seqNo:1327; #beams:0 2024-11-06T03:16:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:16:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.10 , 0.05 2024-11-06T03:16:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862970.31 epoch sec; seqNo:3986; 4061539.92 north; 594848.42 east; 36.69 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:16:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862967.51 2024-11-06T03:16:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:10Z- waiting for 1730862971.51, time is 1730862970.31 2024-11-06T03:16:10Z- AHRS_M2 msg: 1730862970.72 epoch sec; seqNo:3748 2024-11-06T03:16:10Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:10Z- handleDepth()- Depth_Keller msg: 1730862970.73 epoch sec; seqNo:3987; depth 49.98 2024-11-06T03:16:10Z- handleDVL() - DVL msg epochMillisec: 1730862970.79 2024-11-06T03:16:10Z- handleDvl() - RDI_Pathfinder msg: 1730862970.79 sec; seqNo:1328; #beams:4 2024-11-06T03:16:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.32 , 35.48 , 41.39 , 42.82 2024-11-06T03:16:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , 0.06 2024-11-06T03:16:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862970.81 epoch sec; seqNo:3987; 4061539.16 north; 594848.43 east; 36.69 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:16:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862967.51 2024-11-06T03:16:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:10Z- waiting for 1730862971.51, time is 1730862970.81 2024-11-06T03:16:11Z- AHRS_M2 msg: 1730862971.10 epoch sec; seqNo:3749 2024-11-06T03:16:11Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:11Z- handleDepth()- Depth_Keller msg: 1730862971.11 epoch sec; seqNo:3988; depth 49.96 2024-11-06T03:16:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862971.12 epoch sec; seqNo:3988; 4061539.16 north; 594848.43 east; 36.69 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:16:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862967.51 2024-11-06T03:16:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:11Z- waiting for 1730862971.51, time is 1730862971.12 2024-11-06T03:16:11Z- AHRS_M2 msg: 1730862971.50 epoch sec; seqNo:3750 2024-11-06T03:16:11Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:11Z- handleDepth()- Depth_Keller msg: 1730862971.51 epoch sec; seqNo:3989; depth 49.98 2024-11-06T03:16:11Z- handleDVL() - DVL msg epochMillisec: 1730862971.52 2024-11-06T03:16:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:16:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:16:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:16:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:16:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:16:11Z- handleDvl() - RDI_Pathfinder msg: 1730862971.52 sec; seqNo:1329; #beams:0 2024-11-06T03:16:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:16:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.08 , 0.06 2024-11-06T03:16:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862971.53 epoch sec; seqNo:3989; 4061538.78 north; 594848.43 east; 36.69 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:16:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862967.51 2024-11-06T03:16:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:11Z- waiting for 1730862971.51, time is 1730862971.53 2024-11-06T03:16:11Z- LcmTrn::updateTrn() >>>> pose time 1730862971.53, meas time 1730862971.52 <<<< 2024-11-06T03:16:11Z- LcmTrn::updateTrn() equating pose 1730862971.52 and meas times 1730862971.52 2024-11-06T03:16:11Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730862971.52 2024-11-06T03:16:11Z- TerrainNav::motionUpdate() - currEstimate = (4061539, 594848.43, 49.98) 2024-11-06T03:16:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:16:11Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:16:11Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:16:11Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:16:11Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:16:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:16:11Z- variances([0], [2], and psi_berg) = 0.288919 0.148701 0.000000 2024-11-06T03:16:11Z- publishEstimatesDecoder() - at 1730862971.52 sec 2024-11-06T03:16:11Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:16:11Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061570.55 594831.72 57.14 2024-11-06T03:16:11Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061569.42 594831.00 57.07 lat/lon: 36.70 -121.94 2024-11-06T03:16:11Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.29 0.15 0.95 0.00 2024-11-06T03:16:11Z- AHRS_M2 msg: 1730862971.91 epoch sec; seqNo:3751 2024-11-06T03:16:11Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.39 psi 2024-11-06T03:16:11Z- handleDepth()- Depth_Keller msg: 1730862971.92 epoch sec; seqNo:3990; depth 49.98 2024-11-06T03:16:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862971.93 epoch sec; seqNo:3990; 4061538.40 north; 594848.43 east; 36.69 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:16:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862971.52 2024-11-06T03:16:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:12Z- waiting for 1730862975.52, time is 1730862971.93 2024-11-06T03:16:12Z- AHRS_M2 msg: 1730862972.32 epoch sec; seqNo:3752 2024-11-06T03:16:12Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.39 psi 2024-11-06T03:16:12Z- handleDepth()- Depth_Keller msg: 1730862972.33 epoch sec; seqNo:3991; depth 49.98 2024-11-06T03:16:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862972.34 epoch sec; seqNo:3991; 4061538.02 north; 594848.44 east; 36.69 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:16:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862971.52 2024-11-06T03:16:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:12Z- waiting for 1730862975.52, time is 1730862972.34 2024-11-06T03:16:12Z- AHRS_M2 msg: 1730862972.76 epoch sec; seqNo:3753 2024-11-06T03:16:12Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:12Z- handleDepth()- Depth_Keller msg: 1730862972.77 epoch sec; seqNo:3992; depth 50.00 2024-11-06T03:16:12Z- handleDVL() - DVL msg epochMillisec: 1730862972.79 2024-11-06T03:16:12Z- handleDvl() - RDI_Pathfinder msg: 1730862972.79 sec; seqNo:1330; #beams:4 2024-11-06T03:16:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.91 , 35.48 , 41.80 , 43.02 2024-11-06T03:16:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , 0.07 2024-11-06T03:16:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862972.81 epoch sec; seqNo:3992; 4061537.64 north; 594848.44 east; 36.69 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:16:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862971.52 2024-11-06T03:16:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:12Z- waiting for 1730862975.52, time is 1730862972.81 2024-11-06T03:16:13Z- AHRS_M2 msg: 1730862973.15 epoch sec; seqNo:3754 2024-11-06T03:16:13Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:13Z- handleDepth()- Depth_Keller msg: 1730862973.16 epoch sec; seqNo:3993; depth 49.99 2024-11-06T03:16:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862973.17 epoch sec; seqNo:3993; 4061537.27 north; 594848.45 east; 36.69 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:16:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862971.52 2024-11-06T03:16:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:13Z- waiting for 1730862975.52, time is 1730862973.17 2024-11-06T03:16:13Z- handleDepth()- Depth_Keller msg: 1730862973.54 epoch sec; seqNo:3994; depth 50.00 2024-11-06T03:16:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862973.55 epoch sec; seqNo:3994; 4061536.89 north; 594848.45 east; 36.69 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:16:13Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:16:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862971.52 2024-11-06T03:16:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:13Z- waiting for 1730862975.52, time is 1730862973.55 2024-11-06T03:16:13Z- AHRS_M2 msg: 1730862973.93 epoch sec; seqNo:3755 2024-11-06T03:16:13Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:13Z- handleDepth()- Depth_Keller msg: 1730862973.94 epoch sec; seqNo:3995; depth 50.00 2024-11-06T03:16:13Z- handleDVL() - DVL msg epochMillisec: 1730862973.94 2024-11-06T03:16:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:16:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:16:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:16:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:16:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:16:13Z- handleDvl() - RDI_Pathfinder msg: 1730862973.94 sec; seqNo:1331; #beams:0 2024-11-06T03:16:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:16:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , 0.07 2024-11-06T03:16:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862973.95 epoch sec; seqNo:3995; 4061536.51 north; 594848.46 east; 36.69 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:16:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862971.52 2024-11-06T03:16:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:14Z- waiting for 1730862975.52, time is 1730862973.95 2024-11-06T03:16:14Z- AHRS_M2 msg: 1730862974.34 epoch sec; seqNo:3756 2024-11-06T03:16:14Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:14Z- handleDepth()- Depth_Keller msg: 1730862974.36 epoch sec; seqNo:3996; depth 50.01 2024-11-06T03:16:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862974.37 epoch sec; seqNo:3996; 4061536.12 north; 594847.24 east; 36.69 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:16:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862971.52 2024-11-06T03:16:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:14Z- waiting for 1730862975.52, time is 1730862974.37 2024-11-06T03:16:14Z- AHRS_M2 msg: 1730862974.76 epoch sec; seqNo:3757 2024-11-06T03:16:14Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:14Z- handleDepth()- Depth_Keller msg: 1730862974.77 epoch sec; seqNo:3997; depth 49.99 2024-11-06T03:16:14Z- handleDVL() - DVL msg epochMillisec: 1730862974.79 2024-11-06T03:16:14Z- handleDvl() - RDI_Pathfinder msg: 1730862974.79 sec; seqNo:1332; #beams:4 2024-11-06T03:16:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.32 , 35.68 , 41.59 , 42.82 2024-11-06T03:16:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.00 , 0.06 2024-11-06T03:16:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862974.80 epoch sec; seqNo:3997; 4061535.74 north; 594847.24 east; 36.69 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:16:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862971.52 2024-11-06T03:16:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:14Z- waiting for 1730862975.52, time is 1730862974.80 2024-11-06T03:16:15Z- AHRS_M2 msg: 1730862975.15 epoch sec; seqNo:3758 2024-11-06T03:16:15Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:15Z- handleDepth()- Depth_Keller msg: 1730862975.15 epoch sec; seqNo:3998; depth 50.02 2024-11-06T03:16:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862975.17 epoch sec; seqNo:3998; 4061535.36 north; 594847.25 east; 36.69 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:16:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862971.52 2024-11-06T03:16:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:15Z- waiting for 1730862975.52, time is 1730862975.17 2024-11-06T03:16:15Z- AHRS_M2 msg: 1730862975.54 epoch sec; seqNo:3759 2024-11-06T03:16:15Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:15Z- handleDepth()- Depth_Keller msg: 1730862975.55 epoch sec; seqNo:3999; depth 50.02 2024-11-06T03:16:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862975.56 epoch sec; seqNo:3999; 4061534.98 north; 594847.25 east; 36.69 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:16:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862971.52 2024-11-06T03:16:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:15Z- waiting for 1730862975.52, time is 1730862975.56 2024-11-06T03:16:15Z- LcmTrn::updateTrn() >>>> pose time 1730862975.56, meas time 1730862974.79 <<<< 2024-11-06T03:16:15Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862975.56 2024-11-06T03:16:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862974.79 sec, ping # 0; waiting for INS data more recent than 1730862971.52... 2024-11-06T03:16:15Z- TerrainNav::motionUpdate() - currEstimate = (4061535, 594847.25, 50.02) 2024-11-06T03:16:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:16:15Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:16:15Z- TNavPF::Projecting Measurements 2024-11-06T03:16:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:16:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:16:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:16:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:16:15Z- TNavFilter::4 good beams. 2024-11-06T03:16:15Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:16:15Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:16:15Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:16:15Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:16:15Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:16:15Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:16:15Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:16:15Z- TerrainMapOctree: (vehN, vehE ) = (4061535.70, 594847.48). 2024-11-06T03:16:15Z- TerrainMapOctree: Min Distance = 235.42. 2024-11-06T03:16:15Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:16:15Z- Forcing Subcloud Comparison 2024-11-06T03:16:15Z- Weighting particles with subcloud comparison 2024-11-06T03:16:15Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:15Z- meanExpectedMeasDiff: -1.300877 delta_rms_squared: 2.038501 alpha: 0.990284 2024-11-06T03:16:15Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:15Z- meanExpectedMeasDiff: 1.824808 delta_rms_squared: 1.364154 alpha: 0.985551 2024-11-06T03:16:15Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:15Z- meanExpectedMeasDiff: 4.130246 delta_rms_squared: 1.594123 alpha: 0.987609 2024-11-06T03:16:15Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:15Z- meanExpectedMeasDiff: -1.708790 delta_rms_squared: 1.695278 alpha: 0.988340 2024-11-06T03:16:15Z- Subcloud NIS: 0.523713 2024-11-06T03:16:15Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:16:15Z- TNavPF:: sumWeights = 0.049337 2024-11-06T03:16:15Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:16:15Z- TNavPF::First Term of Map Covariance Matrix is 0.18 2024-11-06T03:16:15Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:16:15Z- TerrainNav::Resampling particle filter... 2024-11-06T03:16:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862974.79 sec. 2024-11-06T03:16:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:16:15Z- variances([0], [2], and psi_berg) = 0.274186 0.134012 0.000000 2024-11-06T03:16:15Z- publishEstimatesDecoder() - at 1730862975.56 sec 2024-11-06T03:16:15Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:16:15Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061566.08 594830.25 57.49 2024-11-06T03:16:15Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061565.63 594829.85 56.73 lat/lon: 36.70 -121.94 2024-11-06T03:16:15Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.13 0.74 0.00 2024-11-06T03:16:15Z- AHRS_M2 msg: 1730862975.95 epoch sec; seqNo:3760 2024-11-06T03:16:15Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.40 psi 2024-11-06T03:16:15Z- handleDepth()- Depth_Keller msg: 1730862975.95 epoch sec; seqNo:4000; depth 50.01 2024-11-06T03:16:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862975.99 epoch sec; seqNo:4000; 4061534.60 north; 594847.26 east; 36.69 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:16:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862975.56 2024-11-06T03:16:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:16Z- waiting for 1730862979.56, time is 1730862975.99 2024-11-06T03:16:16Z- AHRS_M2 msg: 1730862976.35 epoch sec; seqNo:3761 2024-11-06T03:16:16Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.40 psi 2024-11-06T03:16:16Z- handleDepth()- Depth_Keller msg: 1730862976.36 epoch sec; seqNo:4001; depth 50.01 2024-11-06T03:16:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862976.36 epoch sec; seqNo:4001; 4061534.22 north; 594847.26 east; 36.69 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:16:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862975.56 2024-11-06T03:16:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:16Z- waiting for 1730862979.56, time is 1730862976.36 2024-11-06T03:16:16Z- AHRS_M2 msg: 1730862976.77 epoch sec; seqNo:3762 2024-11-06T03:16:16Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.40 psi 2024-11-06T03:16:16Z- handleDepth()- Depth_Keller msg: 1730862976.77 epoch sec; seqNo:4002; depth 50.01 2024-11-06T03:16:16Z- handleDVL() - DVL msg epochMillisec: 1730862976.80 2024-11-06T03:16:16Z- handleDvl() - RDI_Pathfinder msg: 1730862976.80 sec; seqNo:1333; #beams:4 2024-11-06T03:16:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.32 , 35.27 , 41.80 , 43.02 2024-11-06T03:16:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , 0.02 2024-11-06T03:16:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862976.83 epoch sec; seqNo:4002; 4061533.84 north; 594847.26 east; 36.69 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:16:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862975.56 2024-11-06T03:16:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:16Z- waiting for 1730862979.56, time is 1730862976.83 2024-11-06T03:16:17Z- AHRS_M2 msg: 1730862977.16 epoch sec; seqNo:3763 2024-11-06T03:16:17Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.41 psi 2024-11-06T03:16:17Z- handleDepth()- Depth_Keller msg: 1730862977.16 epoch sec; seqNo:4003; depth 50.02 2024-11-06T03:16:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862977.19 epoch sec; seqNo:4003; 4061533.84 north; 594847.26 east; 36.69 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:16:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862975.56 2024-11-06T03:16:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:17Z- waiting for 1730862979.56, time is 1730862977.19 2024-11-06T03:16:17Z- AHRS_M2 msg: 1730862977.56 epoch sec; seqNo:3764 2024-11-06T03:16:17Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.41 psi 2024-11-06T03:16:17Z- handleDepth()- Depth_Keller msg: 1730862977.57 epoch sec; seqNo:4004; depth 50.01 2024-11-06T03:16:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862977.58 epoch sec; seqNo:4004; 4061533.46 north; 594847.27 east; 36.69 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:16:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862975.56 2024-11-06T03:16:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:17Z- waiting for 1730862979.56, time is 1730862977.58 2024-11-06T03:16:17Z- AHRS_M2 msg: 1730862977.97 epoch sec; seqNo:3765 2024-11-06T03:16:17Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.41 psi 2024-11-06T03:16:17Z- handleDepth()- Depth_Keller msg: 1730862977.97 epoch sec; seqNo:4005; depth 50.01 2024-11-06T03:16:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862977.99 epoch sec; seqNo:4005; 4061533.08 north; 594847.27 east; 36.69 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:16:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862975.56 2024-11-06T03:16:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:18Z- waiting for 1730862979.56, time is 1730862977.99 2024-11-06T03:16:18Z- AHRS_M2 msg: 1730862978.37 epoch sec; seqNo:3766 2024-11-06T03:16:18Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.41 psi 2024-11-06T03:16:18Z- handleDepth()- Depth_Keller msg: 1730862978.38 epoch sec; seqNo:4006; depth 50.01 2024-11-06T03:16:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862978.40 epoch sec; seqNo:4006; 4061532.71 north; 594847.28 east; 36.69 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:16:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862975.56 2024-11-06T03:16:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:18Z- waiting for 1730862979.56, time is 1730862978.40 2024-11-06T03:16:18Z- AHRS_M2 msg: 1730862978.77 epoch sec; seqNo:3767 2024-11-06T03:16:18Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.41 psi 2024-11-06T03:16:18Z- handleDepth()- Depth_Keller msg: 1730862978.78 epoch sec; seqNo:4007; depth 50.00 2024-11-06T03:16:18Z- handleDVL() - DVL msg epochMillisec: 1730862978.81 2024-11-06T03:16:18Z- handleDvl() - RDI_Pathfinder msg: 1730862978.81 sec; seqNo:1334; #beams:4 2024-11-06T03:16:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.91 , 36.09 , 41.59 , 42.82 2024-11-06T03:16:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , -0.03 2024-11-06T03:16:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862978.82 epoch sec; seqNo:4007; 4061532.33 north; 594847.28 east; 36.69 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:16:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862975.56 2024-11-06T03:16:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:18Z- waiting for 1730862979.56, time is 1730862978.82 2024-11-06T03:16:19Z- AHRS_M2 msg: 1730862979.18 epoch sec; seqNo:3768 2024-11-06T03:16:19Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.41 psi 2024-11-06T03:16:19Z- handleDepth()- Depth_Keller msg: 1730862979.18 epoch sec; seqNo:4008; depth 49.99 2024-11-06T03:16:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862979.20 epoch sec; seqNo:4008; 4061531.95 north; 594847.29 east; 36.69 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:16:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862975.56 2024-11-06T03:16:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:19Z- waiting for 1730862979.56, time is 1730862979.20 2024-11-06T03:16:19Z- AHRS_M2 msg: 1730862979.58 epoch sec; seqNo:3769 2024-11-06T03:16:19Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.41 psi 2024-11-06T03:16:19Z- handleDepth()- Depth_Keller msg: 1730862979.59 epoch sec; seqNo:4009; depth 50.01 2024-11-06T03:16:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862979.60 epoch sec; seqNo:4009; 4061531.57 north; 594847.29 east; 36.69 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:16:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862975.56 2024-11-06T03:16:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:19Z- waiting for 1730862979.56, time is 1730862979.60 2024-11-06T03:16:19Z- LcmTrn::updateTrn() >>>> pose time 1730862979.60, meas time 1730862978.81 <<<< 2024-11-06T03:16:19Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862979.60 2024-11-06T03:16:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862978.81 sec, ping # 0; waiting for INS data more recent than 1730862975.56... 2024-11-06T03:16:19Z- TerrainNav::motionUpdate() - currEstimate = (4061532, 594847.29, 50.01) 2024-11-06T03:16:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:16:19Z- TerrainNav:: SubcloudNIS is 0.52, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:16:19Z- TNavPF::Projecting Measurements 2024-11-06T03:16:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:16:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:16:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:16:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:16:19Z- TNavFilter::4 good beams. 2024-11-06T03:16:19Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:16:19Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:16:19Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:16:19Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:16:19Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:16:19Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:16:19Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:16:19Z- TerrainMapOctree: (vehN, vehE ) = (4061532.24, 594847.28). 2024-11-06T03:16:19Z- TerrainMapOctree: Min Distance = 238.51. 2024-11-06T03:16:19Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:16:19Z- Forcing Subcloud Comparison 2024-11-06T03:16:19Z- Weighting particles with subcloud comparison 2024-11-06T03:16:19Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:19Z- meanExpectedMeasDiff: -2.113258 delta_rms_squared: 1.651321 alpha: 0.988033 2024-11-06T03:16:19Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:19Z- meanExpectedMeasDiff: 1.806148 delta_rms_squared: 1.142536 alpha: 0.982796 2024-11-06T03:16:19Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:19Z- meanExpectedMeasDiff: 4.808727 delta_rms_squared: 1.304624 alpha: 0.984901 2024-11-06T03:16:19Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:19Z- meanExpectedMeasDiff: -2.265611 delta_rms_squared: 1.412707 alpha: 0.986040 2024-11-06T03:16:19Z- Subcloud NIS: 0.553152 2024-11-06T03:16:19Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:16:19Z- TNavPF:: sumWeights = 0.015031 2024-11-06T03:16:19Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:16:19Z- TNavPF::First Term of Map Covariance Matrix is 0.10 2024-11-06T03:16:19Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:16:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862978.81 sec. 2024-11-06T03:16:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:16:19Z- variances([0], [2], and psi_berg) = 0.284982 0.137137 0.000000 2024-11-06T03:16:19Z- publishEstimatesDecoder() - at 1730862979.60 sec 2024-11-06T03:16:19Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:16:19Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061563.93 594830.95 56.03 2024-11-06T03:16:19Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061562.23 594829.88 56.43 lat/lon: 36.69 -121.94 2024-11-06T03:16:19Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.28 0.14 0.72 0.00 2024-11-06T03:16:19Z- AHRS_M2 msg: 1730862979.99 epoch sec; seqNo:3770 2024-11-06T03:16:19Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.41 psi 2024-11-06T03:16:19Z- handleDepth()- Depth_Keller msg: 1730862979.99 epoch sec; seqNo:4010; depth 49.99 2024-11-06T03:16:19Z- handleDVL() - DVL msg epochMillisec: 1730862979.99 2024-11-06T03:16:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:16:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:16:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:16:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:16:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:16:19Z- handleDvl() - RDI_Pathfinder msg: 1730862979.99 sec; seqNo:1335; #beams:0 2024-11-06T03:16:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:16:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , -0.03 2024-11-06T03:16:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862980.00 epoch sec; seqNo:4010; 4061531.18 north; 594846.07 east; 36.69 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:16:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862979.60 2024-11-06T03:16:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:20Z- waiting for 1730862983.60, time is 1730862980.00 2024-11-06T03:16:20Z- AHRS_M2 msg: 1730862980.39 epoch sec; seqNo:3771 2024-11-06T03:16:20Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.41 psi 2024-11-06T03:16:20Z- handleDepth()- Depth_Keller msg: 1730862980.39 epoch sec; seqNo:4011; depth 49.99 2024-11-06T03:16:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862980.41 epoch sec; seqNo:4011; 4061530.80 north; 594846.08 east; 36.69 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:16:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862979.60 2024-11-06T03:16:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:20Z- waiting for 1730862983.60, time is 1730862980.41 2024-11-06T03:16:20Z- AHRS_M2 msg: 1730862980.80 epoch sec; seqNo:3772 2024-11-06T03:16:20Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.41 psi 2024-11-06T03:16:20Z- handleDepth()- Depth_Keller msg: 1730862980.80 epoch sec; seqNo:4012; depth 49.98 2024-11-06T03:16:20Z- handleDVL() - DVL msg epochMillisec: 1730862980.82 2024-11-06T03:16:20Z- handleDvl() - RDI_Pathfinder msg: 1730862980.82 sec; seqNo:1336; #beams:4 2024-11-06T03:16:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.12 , 35.88 , 41.39 , 42.61 2024-11-06T03:16:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.07 , -0.03 2024-11-06T03:16:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862980.85 epoch sec; seqNo:4012; 4061530.42 north; 594846.08 east; 36.69 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:16:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862979.60 2024-11-06T03:16:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:20Z- waiting for 1730862983.60, time is 1730862980.85 2024-11-06T03:16:21Z- AHRS_M2 msg: 1730862981.20 epoch sec; seqNo:3773 2024-11-06T03:16:21Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.41 psi 2024-11-06T03:16:21Z- handleDepth()- Depth_Keller msg: 1730862981.21 epoch sec; seqNo:4013; depth 49.98 2024-11-06T03:16:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862981.22 epoch sec; seqNo:4013; 4061530.04 north; 594846.09 east; 36.69 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:16:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862979.60 2024-11-06T03:16:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:21Z- waiting for 1730862983.60, time is 1730862981.22 2024-11-06T03:16:21Z- AHRS_M2 msg: 1730862981.61 epoch sec; seqNo:3774 2024-11-06T03:16:21Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.41 psi 2024-11-06T03:16:21Z- handleDepth()- Depth_Keller msg: 1730862981.62 epoch sec; seqNo:4014; depth 49.98 2024-11-06T03:16:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862981.63 epoch sec; seqNo:4014; 4061529.66 north; 594846.09 east; 36.69 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:16:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862979.60 2024-11-06T03:16:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:21Z- waiting for 1730862983.60, time is 1730862981.63 2024-11-06T03:16:22Z- AHRS_M2 msg: 1730862982.01 epoch sec; seqNo:3775 2024-11-06T03:16:22Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.41 psi 2024-11-06T03:16:22Z- handleDepth()- Depth_Keller msg: 1730862982.01 epoch sec; seqNo:4015; depth 49.98 2024-11-06T03:16:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862982.02 epoch sec; seqNo:4015; 4061529.66 north; 594846.09 east; 36.69 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:16:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862979.60 2024-11-06T03:16:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:22Z- waiting for 1730862983.60, time is 1730862982.02 2024-11-06T03:16:22Z- AHRS_M2 msg: 1730862982.41 epoch sec; seqNo:3776 2024-11-06T03:16:22Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.41 psi 2024-11-06T03:16:22Z- handleDepth()- Depth_Keller msg: 1730862982.42 epoch sec; seqNo:4016; depth 49.98 2024-11-06T03:16:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862982.43 epoch sec; seqNo:4016; 4061529.28 north; 594846.09 east; 36.69 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:16:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862979.60 2024-11-06T03:16:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:22Z- waiting for 1730862983.60, time is 1730862982.43 2024-11-06T03:16:22Z- AHRS_M2 msg: 1730862982.82 epoch sec; seqNo:3777 2024-11-06T03:16:22Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.41 psi 2024-11-06T03:16:22Z- handleDepth()- Depth_Keller msg: 1730862982.82 epoch sec; seqNo:4017; depth 49.99 2024-11-06T03:16:22Z- handleDVL() - DVL msg epochMillisec: 1730862982.84 2024-11-06T03:16:22Z- handleDvl() - RDI_Pathfinder msg: 1730862982.84 sec; seqNo:1337; #beams:4 2024-11-06T03:16:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.91 , 35.88 , 40.98 , 42.61 2024-11-06T03:16:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , 0.02 2024-11-06T03:16:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862982.86 epoch sec; seqNo:4017; 4061528.90 north; 594846.10 east; 36.69 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:16:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862979.60 2024-11-06T03:16:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:22Z- waiting for 1730862983.60, time is 1730862982.86 2024-11-06T03:16:23Z- AHRS_M2 msg: 1730862983.22 epoch sec; seqNo:3778 2024-11-06T03:16:23Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.41 psi 2024-11-06T03:16:23Z- handleDepth()- Depth_Keller msg: 1730862983.23 epoch sec; seqNo:4018; depth 50.00 2024-11-06T03:16:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862983.28 epoch sec; seqNo:4018; 4061528.53 north; 594846.10 east; 36.69 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:16:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862979.60 2024-11-06T03:16:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:23Z- waiting for 1730862983.60, time is 1730862983.28 2024-11-06T03:16:23Z- AHRS_M2 msg: 1730862983.63 epoch sec; seqNo:3779 2024-11-06T03:16:23Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.41 psi 2024-11-06T03:16:23Z- handleDepth()- Depth_Keller msg: 1730862983.63 epoch sec; seqNo:4019; depth 49.99 2024-11-06T03:16:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862983.66 epoch sec; seqNo:4019; 4061528.13 north; 594844.89 east; 36.69 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:16:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862979.60 2024-11-06T03:16:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:23Z- waiting for 1730862983.60, time is 1730862983.66 2024-11-06T03:16:23Z- LcmTrn::updateTrn() >>>> pose time 1730862983.66, meas time 1730862982.84 <<<< 2024-11-06T03:16:23Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862983.66 2024-11-06T03:16:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862982.84 sec, ping # 0; waiting for INS data more recent than 1730862979.60... 2024-11-06T03:16:23Z- TerrainNav::motionUpdate() - currEstimate = (4061528, 594844.89, 49.99) 2024-11-06T03:16:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:16:23Z- TerrainNav:: SubcloudNIS is 0.55, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:16:23Z- TNavPF::Projecting Measurements 2024-11-06T03:16:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:16:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:16:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:16:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:16:23Z- TNavFilter::4 good beams. 2024-11-06T03:16:23Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:16:23Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:16:23Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:16:23Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:16:23Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:16:23Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:16:23Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:16:23Z- TerrainMapOctree: (vehN, vehE ) = (4061528.82, 594845.37). 2024-11-06T03:16:23Z- TerrainMapOctree: Min Distance = 240.86. 2024-11-06T03:16:23Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:16:23Z- Forcing Subcloud Comparison 2024-11-06T03:16:23Z- Weighting particles with subcloud comparison 2024-11-06T03:16:23Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:23Z- meanExpectedMeasDiff: -2.654423 delta_rms_squared: 1.602888 alpha: 0.987676 2024-11-06T03:16:23Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:23Z- meanExpectedMeasDiff: 2.537432 delta_rms_squared: 1.074304 alpha: 0.981724 2024-11-06T03:16:23Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:23Z- meanExpectedMeasDiff: 5.265916 delta_rms_squared: 1.723025 alpha: 0.988526 2024-11-06T03:16:23Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:23Z- meanExpectedMeasDiff: -2.370685 delta_rms_squared: 2.561571 alpha: 0.992253 2024-11-06T03:16:23Z- Subcloud NIS: 0.589350 2024-11-06T03:16:23Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:16:23Z- TNavPF:: sumWeights = 0.004229 2024-11-06T03:16:23Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:16:23Z- TNavPF::First Term of Map Covariance Matrix is 0.04 2024-11-06T03:16:23Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:16:23Z- TerrainNav::Resampling particle filter... 2024-11-06T03:16:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862982.84 sec. 2024-11-06T03:16:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:16:23Z- variances([0], [2], and psi_berg) = 0.363343 0.166859 0.000000 2024-11-06T03:16:23Z- publishEstimatesDecoder() - at 1730862983.66 sec 2024-11-06T03:16:23Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:16:23Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061559.17 594827.79 57.72 2024-11-06T03:16:23Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061558.79 594827.56 56.18 lat/lon: 36.69 -121.94 2024-11-06T03:16:23Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.36 0.17 0.68 0.00 2024-11-06T03:16:24Z- AHRS_M2 msg: 1730862984.03 epoch sec; seqNo:3780 2024-11-06T03:16:24Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.41 psi 2024-11-06T03:16:24Z- handleDepth()- Depth_Keller msg: 1730862984.05 epoch sec; seqNo:4020; depth 50.00 2024-11-06T03:16:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862984.07 epoch sec; seqNo:4020; 4061527.75 north; 594844.89 east; 36.69 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:16:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862983.66 2024-11-06T03:16:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:24Z- waiting for 1730862987.66, time is 1730862984.07 2024-11-06T03:16:24Z- AHRS_M2 msg: 1730862984.44 epoch sec; seqNo:3781 2024-11-06T03:16:24Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.41 psi 2024-11-06T03:16:24Z- handleDepth()- Depth_Keller msg: 1730862984.44 epoch sec; seqNo:4021; depth 50.01 2024-11-06T03:16:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862984.45 epoch sec; seqNo:4021; 4061527.38 north; 594844.89 east; 36.69 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:16:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862983.66 2024-11-06T03:16:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:24Z- waiting for 1730862987.66, time is 1730862984.45 2024-11-06T03:16:24Z- AHRS_M2 msg: 1730862984.83 epoch sec; seqNo:3782 2024-11-06T03:16:24Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.41 psi 2024-11-06T03:16:24Z- handleDepth()- Depth_Keller msg: 1730862984.84 epoch sec; seqNo:4022; depth 50.02 2024-11-06T03:16:24Z- handleDVL() - DVL msg epochMillisec: 1730862984.86 2024-11-06T03:16:24Z- handleDvl() - RDI_Pathfinder msg: 1730862984.86 sec; seqNo:1338; #beams:4 2024-11-06T03:16:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.12 , 35.68 , 40.98 , 42.61 2024-11-06T03:16:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.05 , 0.06 2024-11-06T03:16:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862984.88 epoch sec; seqNo:4022; 4061527.00 north; 594844.90 east; 36.69 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:16:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862983.66 2024-11-06T03:16:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:24Z- waiting for 1730862987.66, time is 1730862984.88 2024-11-06T03:16:25Z- AHRS_M2 msg: 1730862985.24 epoch sec; seqNo:3783 2024-11-06T03:16:25Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.41 psi 2024-11-06T03:16:25Z- handleDepth()- Depth_Keller msg: 1730862985.25 epoch sec; seqNo:4023; depth 50.04 2024-11-06T03:16:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862985.28 epoch sec; seqNo:4023; 4061526.62 north; 594844.90 east; 36.69 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:16:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862983.66 2024-11-06T03:16:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:25Z- waiting for 1730862987.66, time is 1730862985.28 2024-11-06T03:16:25Z- AHRS_M2 msg: 1730862985.65 epoch sec; seqNo:3784 2024-11-06T03:16:25Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.41 psi 2024-11-06T03:16:25Z- handleDepth()- Depth_Keller msg: 1730862985.65 epoch sec; seqNo:4024; depth 50.04 2024-11-06T03:16:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862985.66 epoch sec; seqNo:4024; 4061526.24 north; 594844.91 east; 36.69 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:16:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862983.66 2024-11-06T03:16:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:25Z- waiting for 1730862987.66, time is 1730862985.66 2024-11-06T03:16:26Z- AHRS_M2 msg: 1730862986.05 epoch sec; seqNo:3785 2024-11-06T03:16:26Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.41 psi 2024-11-06T03:16:26Z- handleDepth()- Depth_Keller msg: 1730862986.05 epoch sec; seqNo:4025; depth 50.07 2024-11-06T03:16:26Z- handleDVL() - DVL msg epochMillisec: 1730862986.06 2024-11-06T03:16:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:16:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:16:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:16:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:16:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:16:26Z- handleDvl() - RDI_Pathfinder msg: 1730862986.06 sec; seqNo:1339; #beams:0 2024-11-06T03:16:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:16:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.05 , 0.06 2024-11-06T03:16:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862986.06 epoch sec; seqNo:4025; 4061526.24 north; 594844.91 east; 36.69 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:16:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862983.66 2024-11-06T03:16:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:26Z- waiting for 1730862987.66, time is 1730862986.06 2024-11-06T03:16:26Z- handleDepth()- Depth_Keller msg: 1730862986.47 epoch sec; seqNo:4026; depth 50.06 2024-11-06T03:16:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862986.48 epoch sec; seqNo:4026; 4061525.86 north; 594844.91 east; 36.69 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:16:26Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:16:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862983.66 2024-11-06T03:16:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:26Z- waiting for 1730862987.66, time is 1730862986.48 2024-11-06T03:16:26Z- AHRS_M2 msg: 1730862986.85 epoch sec; seqNo:3786 2024-11-06T03:16:26Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.41 psi 2024-11-06T03:16:26Z- handleDepth()- Depth_Keller msg: 1730862986.87 epoch sec; seqNo:4027; depth 50.08 2024-11-06T03:16:26Z- handleDVL() - DVL msg epochMillisec: 1730862986.89 2024-11-06T03:16:26Z- handleDvl() - RDI_Pathfinder msg: 1730862986.89 sec; seqNo:1340; #beams:4 2024-11-06T03:16:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.12 , 35.88 , 40.78 , 42.61 2024-11-06T03:16:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.03 , 0.04 2024-11-06T03:16:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862986.90 epoch sec; seqNo:4027; 4061525.48 north; 594844.92 east; 36.69 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:16:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862983.66 2024-11-06T03:16:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:27Z- waiting for 1730862987.66, time is 1730862986.90 2024-11-06T03:16:27Z- AHRS_M2 msg: 1730862987.26 epoch sec; seqNo:3787 2024-11-06T03:16:27Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.41 psi 2024-11-06T03:16:27Z- handleDepth()- Depth_Keller msg: 1730862987.27 epoch sec; seqNo:4028; depth 50.11 2024-11-06T03:16:27Z- handleDVL() - DVL msg epochMillisec: 1730862987.27 2024-11-06T03:16:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:16:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:16:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:16:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:16:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:16:27Z- handleDvl() - RDI_Pathfinder msg: 1730862987.27 sec; seqNo:1341; #beams:0 2024-11-06T03:16:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:16:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.03 , 0.04 2024-11-06T03:16:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862987.29 epoch sec; seqNo:4028; 4061525.10 north; 594844.92 east; 36.69 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:16:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862983.66 2024-11-06T03:16:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:27Z- waiting for 1730862987.66, time is 1730862987.29 2024-11-06T03:16:27Z- AHRS_M2 msg: 1730862987.66 epoch sec; seqNo:3788 2024-11-06T03:16:27Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.41 psi 2024-11-06T03:16:27Z- handleDepth()- Depth_Keller msg: 1730862987.67 epoch sec; seqNo:4029; depth 50.11 2024-11-06T03:16:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862987.68 epoch sec; seqNo:4029; 4061524.72 north; 594844.92 east; 36.69 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:16:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862983.66 2024-11-06T03:16:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:27Z- waiting for 1730862987.66, time is 1730862987.68 2024-11-06T03:16:27Z- LcmTrn::updateTrn() >>>> pose time 1730862987.68, meas time 1730862987.27 <<<< 2024-11-06T03:16:27Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862987.68 2024-11-06T03:16:27Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:16:27Z- TerrainNav::motionUpdate() - currEstimate = (4061525, 594844.92, 50.11) 2024-11-06T03:16:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:16:27Z- TerrainNav:: SubcloudNIS is 0.59, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:16:27Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:16:27Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:16:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:16:27Z- variances([0], [2], and psi_berg) = 0.364765 0.168639 0.000000 2024-11-06T03:16:27Z- publishEstimatesDecoder() - at 1730862987.68 sec 2024-11-06T03:16:27Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:16:27Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061555.71 594827.84 58.26 2024-11-06T03:16:27Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061555.38 594827.59 56.30 lat/lon: 36.69 -121.94 2024-11-06T03:16:27Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.36 0.17 0.94 0.00 2024-11-06T03:16:28Z- AHRS_M2 msg: 1730862988.07 epoch sec; seqNo:3789 2024-11-06T03:16:28Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.41 psi 2024-11-06T03:16:28Z- handleDepth()- Depth_Keller msg: 1730862988.08 epoch sec; seqNo:4030; depth 50.12 2024-11-06T03:16:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862988.09 epoch sec; seqNo:4030; 4061524.33 north; 594843.71 east; 36.69 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:16:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862987.68 2024-11-06T03:16:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:28Z- waiting for 1730862991.68, time is 1730862988.09 2024-11-06T03:16:28Z- AHRS_M2 msg: 1730862988.47 epoch sec; seqNo:3790 2024-11-06T03:16:28Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.41 psi 2024-11-06T03:16:28Z- handleDepth()- Depth_Keller msg: 1730862988.48 epoch sec; seqNo:4031; depth 50.15 2024-11-06T03:16:28Z- handleDVL() - DVL msg epochMillisec: 1730862988.48 2024-11-06T03:16:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:16:28Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:16:28Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:16:28Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:16:28Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:16:28Z- handleDvl() - RDI_Pathfinder msg: 1730862988.48 sec; seqNo:1342; #beams:0 2024-11-06T03:16:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:16:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.03 , 0.04 2024-11-06T03:16:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862988.49 epoch sec; seqNo:4031; 4061523.95 north; 594843.71 east; 36.69 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:16:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862987.68 2024-11-06T03:16:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:28Z- waiting for 1730862991.68, time is 1730862988.49 2024-11-06T03:16:28Z- AHRS_M2 msg: 1730862988.88 epoch sec; seqNo:3791 2024-11-06T03:16:28Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.40 psi 2024-11-06T03:16:28Z- handleDepth()- Depth_Keller msg: 1730862988.89 epoch sec; seqNo:4032; depth 50.15 2024-11-06T03:16:28Z- handleDVL() - DVL msg epochMillisec: 1730862988.92 2024-11-06T03:16:28Z- handleDvl() - RDI_Pathfinder msg: 1730862988.92 sec; seqNo:1343; #beams:4 2024-11-06T03:16:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.91 , 35.88 , 40.78 , 42.41 2024-11-06T03:16:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.01 2024-11-06T03:16:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862988.94 epoch sec; seqNo:4032; 4061523.57 north; 594843.72 east; 36.69 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:16:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862987.68 2024-11-06T03:16:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:29Z- waiting for 1730862991.68, time is 1730862988.94 2024-11-06T03:16:29Z- AHRS_M2 msg: 1730862989.28 epoch sec; seqNo:3792 2024-11-06T03:16:29Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.40 psi 2024-11-06T03:16:29Z- handleDepth()- Depth_Keller msg: 1730862989.29 epoch sec; seqNo:4033; depth 50.16 2024-11-06T03:16:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862989.31 epoch sec; seqNo:4033; 4061523.19 north; 594843.72 east; 36.69 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:16:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862987.68 2024-11-06T03:16:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:29Z- waiting for 1730862991.68, time is 1730862989.31 2024-11-06T03:16:29Z- AHRS_M2 msg: 1730862989.68 epoch sec; seqNo:3793 2024-11-06T03:16:29Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.40 psi 2024-11-06T03:16:29Z- handleDepth()- Depth_Keller msg: 1730862989.69 epoch sec; seqNo:4034; depth 50.18 2024-11-06T03:16:29Z- handleDVL() - DVL msg epochMillisec: 1730862989.69 2024-11-06T03:16:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:16:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:16:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:16:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:16:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:16:29Z- handleDvl() - RDI_Pathfinder msg: 1730862989.69 sec; seqNo:1344; #beams:0 2024-11-06T03:16:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:16:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.01 2024-11-06T03:16:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862989.70 epoch sec; seqNo:4034; 4061522.82 north; 594843.72 east; 36.69 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:16:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862987.68 2024-11-06T03:16:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:29Z- waiting for 1730862991.68, time is 1730862989.70 2024-11-06T03:16:30Z- AHRS_M2 msg: 1730862990.09 epoch sec; seqNo:3794 2024-11-06T03:16:30Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.40 psi 2024-11-06T03:16:30Z- handleDepth()- Depth_Keller msg: 1730862990.09 epoch sec; seqNo:4035; depth 50.20 2024-11-06T03:16:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862990.10 epoch sec; seqNo:4035; 4061522.44 north; 594843.73 east; 36.69 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:16:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862987.68 2024-11-06T03:16:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:30Z- waiting for 1730862991.68, time is 1730862990.10 2024-11-06T03:16:30Z- AHRS_M2 msg: 1730862990.49 epoch sec; seqNo:3795 2024-11-06T03:16:30Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.40 psi 2024-11-06T03:16:30Z- handleDepth()- Depth_Keller msg: 1730862990.51 epoch sec; seqNo:4036; depth 50.21 2024-11-06T03:16:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862990.52 epoch sec; seqNo:4036; 4061522.06 north; 594843.73 east; 36.69 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:16:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862987.68 2024-11-06T03:16:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:30Z- waiting for 1730862991.68, time is 1730862990.52 2024-11-06T03:16:30Z- AHRS_M2 msg: 1730862990.89 epoch sec; seqNo:3796 2024-11-06T03:16:30Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.40 psi 2024-11-06T03:16:30Z- handleDepth()- Depth_Keller msg: 1730862990.90 epoch sec; seqNo:4037; depth 50.23 2024-11-06T03:16:30Z- handleDVL() - DVL msg epochMillisec: 1730862990.93 2024-11-06T03:16:30Z- handleDvl() - RDI_Pathfinder msg: 1730862990.93 sec; seqNo:1345; #beams:4 2024-11-06T03:16:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.51 , 35.88 , 40.78 , 42.21 2024-11-06T03:16:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , -0.02 2024-11-06T03:16:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862990.95 epoch sec; seqNo:4037; 4061521.68 north; 594843.74 east; 36.69 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:16:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862987.68 2024-11-06T03:16:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:31Z- waiting for 1730862991.68, time is 1730862990.95 2024-11-06T03:16:31Z- AHRS_M2 msg: 1730862991.32 epoch sec; seqNo:3797 2024-11-06T03:16:31Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.40 psi 2024-11-06T03:16:31Z- handleDepth()- Depth_Keller msg: 1730862991.32 epoch sec; seqNo:4038; depth 50.24 2024-11-06T03:16:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862991.34 epoch sec; seqNo:4038; 4061521.30 north; 594843.74 east; 36.69 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:16:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862987.68 2024-11-06T03:16:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:31Z- waiting for 1730862991.68, time is 1730862991.34 2024-11-06T03:16:31Z- AHRS_M2 msg: 1730862991.71 epoch sec; seqNo:3798 2024-11-06T03:16:31Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.40 psi 2024-11-06T03:16:31Z- handleDepth()- Depth_Keller msg: 1730862991.71 epoch sec; seqNo:4039; depth 50.26 2024-11-06T03:16:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862991.72 epoch sec; seqNo:4039; 4061520.92 north; 594843.75 east; 36.69 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:16:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862987.68 2024-11-06T03:16:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:31Z- waiting for 1730862991.68, time is 1730862991.72 2024-11-06T03:16:31Z- LcmTrn::updateTrn() >>>> pose time 1730862991.72, meas time 1730862990.93 <<<< 2024-11-06T03:16:31Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862991.72 2024-11-06T03:16:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862990.93 sec, ping # 0; waiting for INS data more recent than 1730862987.68... 2024-11-06T03:16:31Z- TerrainNav::motionUpdate() - currEstimate = (4061521, 594843.75, 50.26) 2024-11-06T03:16:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:16:31Z- TerrainNav:: SubcloudNIS is 0.59, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:16:31Z- TNavPF::Projecting Measurements 2024-11-06T03:16:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:16:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:16:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:16:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:16:31Z- TNavFilter::4 good beams. 2024-11-06T03:16:31Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:16:31Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:16:31Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:16:31Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:16:31Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:16:31Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:16:31Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:16:31Z- TerrainMapOctree: (vehN, vehE ) = (4061521.66, 594843.98). 2024-11-06T03:16:31Z- TerrainMapOctree: Min Distance = 246.92. 2024-11-06T03:16:31Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:16:32Z- Forcing Subcloud Comparison 2024-11-06T03:16:32Z- Weighting particles with subcloud comparison 2024-11-06T03:16:32Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:32Z- meanExpectedMeasDiff: -2.899595 delta_rms_squared: 1.965414 alpha: 0.989927 2024-11-06T03:16:32Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:32Z- meanExpectedMeasDiff: 2.482218 delta_rms_squared: 1.093409 alpha: 0.982037 2024-11-06T03:16:32Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:32Z- meanExpectedMeasDiff: 3.749415 delta_rms_squared: 4.372352 alpha: 0.995447 2024-11-06T03:16:32Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:32Z- meanExpectedMeasDiff: -2.027055 delta_rms_squared: 2.493431 alpha: 0.992043 2024-11-06T03:16:32Z- Subcloud NIS: 0.614039 2024-11-06T03:16:32Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:16:32Z- TNavPF:: sumWeights = 0.022054 2024-11-06T03:16:32Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:16:32Z- TNavPF::First Term of Map Covariance Matrix is 0.23 2024-11-06T03:16:32Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:16:32Z- TerrainNav::Resampling particle filter... 2024-11-06T03:16:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862990.93 sec. 2024-11-06T03:16:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:16:32Z- variances([0], [2], and psi_berg) = 0.262846 0.143288 0.000000 2024-11-06T03:16:32Z- publishEstimatesDecoder() - at 1730862991.72 sec 2024-11-06T03:16:32Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:16:32Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061552.06 594826.67 55.90 2024-11-06T03:16:32Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061551.32 594826.45 56.24 lat/lon: 36.69 -121.94 2024-11-06T03:16:32Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.14 0.67 0.00 2024-11-06T03:16:32Z- handleDepth()- Depth_Keller msg: 1730862992.14 epoch sec; seqNo:4040; depth 50.27 2024-11-06T03:16:32Z- handleDVL() - DVL msg epochMillisec: 1730862992.14 2024-11-06T03:16:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:16:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:16:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:16:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:16:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:16:32Z- handleDvl() - RDI_Pathfinder msg: 1730862992.14 sec; seqNo:1346; #beams:0 2024-11-06T03:16:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:16:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , -0.02 2024-11-06T03:16:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862992.16 epoch sec; seqNo:4040; 4061520.54 north; 594843.75 east; 36.69 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:16:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862991.72 2024-11-06T03:16:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:32Z- waiting for 1730862995.72, time is 1730862992.16 2024-11-06T03:16:32Z- AHRS_M2 msg: 1730862992.51 epoch sec; seqNo:3799 2024-11-06T03:16:32Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.40 psi 2024-11-06T03:16:32Z- handleDepth()- Depth_Keller msg: 1730862992.53 epoch sec; seqNo:4041; depth 50.28 2024-11-06T03:16:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862992.54 epoch sec; seqNo:4041; 4061520.54 north; 594843.75 east; 36.69 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:16:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862991.72 2024-11-06T03:16:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:32Z- waiting for 1730862995.72, time is 1730862992.54 2024-11-06T03:16:32Z- AHRS_M2 msg: 1730862992.91 epoch sec; seqNo:3800 2024-11-06T03:16:32Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.40 psi 2024-11-06T03:16:32Z- handleDepth()- Depth_Keller msg: 1730862992.92 epoch sec; seqNo:4042; depth 50.28 2024-11-06T03:16:32Z- handleDVL() - DVL msg epochMillisec: 1730862992.94 2024-11-06T03:16:32Z- handleDvl() - RDI_Pathfinder msg: 1730862992.94 sec; seqNo:1347; #beams:4 2024-11-06T03:16:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.66 , 35.51 , 40.74 , 42.05 2024-11-06T03:16:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , -0.01 2024-11-06T03:16:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862992.96 epoch sec; seqNo:4042; 4061520.15 north; 594842.53 east; 36.69 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:16:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862991.72 2024-11-06T03:16:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:33Z- waiting for 1730862995.72, time is 1730862992.96 2024-11-06T03:16:33Z- AHRS_M2 msg: 1730862993.33 epoch sec; seqNo:3801 2024-11-06T03:16:33Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.40 psi 2024-11-06T03:16:33Z- handleDepth()- Depth_Keller msg: 1730862993.34 epoch sec; seqNo:4043; depth 50.29 2024-11-06T03:16:33Z- handleDVL() - DVL msg epochMillisec: 1730862993.34 2024-11-06T03:16:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:16:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:16:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:16:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:16:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:16:33Z- handleDvl() - RDI_Pathfinder msg: 1730862993.34 sec; seqNo:1348; #beams:0 2024-11-06T03:16:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:16:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , -0.01 2024-11-06T03:16:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862993.36 epoch sec; seqNo:4043; 4061519.77 north; 594842.54 east; 36.69 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:16:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862991.72 2024-11-06T03:16:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:33Z- waiting for 1730862995.72, time is 1730862993.36 2024-11-06T03:16:33Z- AHRS_M2 msg: 1730862993.73 epoch sec; seqNo:3802 2024-11-06T03:16:33Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.40 psi 2024-11-06T03:16:33Z- handleDepth()- Depth_Keller msg: 1730862993.73 epoch sec; seqNo:4044; depth 50.31 2024-11-06T03:16:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862993.74 epoch sec; seqNo:4044; 4061519.39 north; 594842.54 east; 36.69 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:16:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862991.72 2024-11-06T03:16:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:33Z- waiting for 1730862995.72, time is 1730862993.74 2024-11-06T03:16:34Z- AHRS_M2 msg: 1730862994.13 epoch sec; seqNo:3803 2024-11-06T03:16:34Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.40 psi 2024-11-06T03:16:34Z- handleDepth()- Depth_Keller msg: 1730862994.14 epoch sec; seqNo:4045; depth 50.31 2024-11-06T03:16:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862994.15 epoch sec; seqNo:4045; 4061519.01 north; 594842.55 east; 36.69 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:16:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862991.72 2024-11-06T03:16:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:34Z- waiting for 1730862995.72, time is 1730862994.15 2024-11-06T03:16:34Z- AHRS_M2 msg: 1730862994.53 epoch sec; seqNo:3804 2024-11-06T03:16:34Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.40 psi 2024-11-06T03:16:34Z- handleDepth()- Depth_Keller msg: 1730862994.54 epoch sec; seqNo:4046; depth 50.33 2024-11-06T03:16:34Z- handleDVL() - DVL msg epochMillisec: 1730862994.54 2024-11-06T03:16:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:16:34Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:16:34Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:16:34Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:16:34Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:16:34Z- handleDvl() - RDI_Pathfinder msg: 1730862994.54 sec; seqNo:1349; #beams:0 2024-11-06T03:16:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:16:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , -0.01 2024-11-06T03:16:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862994.55 epoch sec; seqNo:4046; 4061518.63 north; 594842.55 east; 36.69 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:16:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862991.72 2024-11-06T03:16:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:34Z- waiting for 1730862995.72, time is 1730862994.55 2024-11-06T03:16:34Z- AHRS_M2 msg: 1730862994.93 epoch sec; seqNo:3805 2024-11-06T03:16:34Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:34Z- handleDepth()- Depth_Keller msg: 1730862994.94 epoch sec; seqNo:4047; depth 50.32 2024-11-06T03:16:34Z- handleDVL() - DVL msg epochMillisec: 1730862994.96 2024-11-06T03:16:34Z- handleDvl() - RDI_Pathfinder msg: 1730862994.96 sec; seqNo:1350; #beams:4 2024-11-06T03:16:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.28 , 35.32 , 40.93 , 42.05 2024-11-06T03:16:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , 0.00 2024-11-06T03:16:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862995.00 epoch sec; seqNo:4047; 4061518.26 north; 594842.55 east; 36.69 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:16:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862991.72 2024-11-06T03:16:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:35Z- waiting for 1730862995.72, time is 1730862995.00 2024-11-06T03:16:35Z- AHRS_M2 msg: 1730862995.34 epoch sec; seqNo:3806 2024-11-06T03:16:35Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:35Z- handleDepth()- Depth_Keller msg: 1730862995.35 epoch sec; seqNo:4048; depth 50.34 2024-11-06T03:16:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862995.38 epoch sec; seqNo:4048; 4061517.88 north; 594842.56 east; 36.69 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:16:35Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:16:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862991.72 2024-11-06T03:16:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:35Z- waiting for 1730862995.72, time is 1730862995.38 2024-11-06T03:16:35Z- AHRS_M2 msg: 1730862995.74 epoch sec; seqNo:3807 2024-11-06T03:16:35Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:35Z- handleDepth()- Depth_Keller msg: 1730862995.75 epoch sec; seqNo:4049; depth 50.33 2024-11-06T03:16:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862995.76 epoch sec; seqNo:4049; 4061517.88 north; 594842.56 east; 36.69 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:16:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862991.72 2024-11-06T03:16:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:35Z- waiting for 1730862995.72, time is 1730862995.76 2024-11-06T03:16:35Z- LcmTrn::updateTrn() >>>> pose time 1730862995.76, meas time 1730862994.96 <<<< 2024-11-06T03:16:35Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862995.76 2024-11-06T03:16:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862994.96 sec, ping # 0; waiting for INS data more recent than 1730862991.72... 2024-11-06T03:16:35Z- TerrainNav::motionUpdate() - currEstimate = (4061518, 594842.56, 50.33) 2024-11-06T03:16:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:16:35Z- TerrainNav:: SubcloudNIS is 0.61, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:16:35Z- TNavPF::Projecting Measurements 2024-11-06T03:16:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:16:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:16:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:16:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:16:35Z- TNavFilter::4 good beams. 2024-11-06T03:16:35Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:16:35Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:16:35Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:16:35Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:16:35Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:16:35Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:16:35Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:16:36Z- TerrainMapOctree: (vehN, vehE ) = (4061518.48, 594842.80). 2024-11-06T03:16:36Z- TerrainMapOctree: Min Distance = 249.42. 2024-11-06T03:16:36Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:16:36Z- Forcing Subcloud Comparison 2024-11-06T03:16:36Z- Weighting particles with subcloud comparison 2024-11-06T03:16:36Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:36Z- meanExpectedMeasDiff: -3.276850 delta_rms_squared: 1.511978 alpha: 0.986945 2024-11-06T03:16:36Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:36Z- meanExpectedMeasDiff: 2.714821 delta_rms_squared: 1.040938 alpha: 0.981149 2024-11-06T03:16:36Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:36Z- meanExpectedMeasDiff: 2.492974 delta_rms_squared: 1.226506 alpha: 0.983955 2024-11-06T03:16:36Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:36Z- meanExpectedMeasDiff: -1.127556 delta_rms_squared: 1.360871 alpha: 0.985516 2024-11-06T03:16:36Z- Subcloud NIS: 0.637594 2024-11-06T03:16:36Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:16:36Z- TNavPF:: sumWeights = 0.043373 2024-11-06T03:16:36Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:16:36Z- TNavPF::First Term of Map Covariance Matrix is 0.49 2024-11-06T03:16:36Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:16:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862994.96 sec. 2024-11-06T03:16:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:16:36Z- variances([0], [2], and psi_berg) = 0.291797 0.143465 0.000000 2024-11-06T03:16:36Z- publishEstimatesDecoder() - at 1730862995.76 sec 2024-11-06T03:16:36Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:16:36Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061549.94 594826.10 56.89 2024-11-06T03:16:36Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061548.34 594825.30 56.21 lat/lon: 36.69 -121.94 2024-11-06T03:16:36Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.29 0.14 0.70 0.00 2024-11-06T03:16:36Z- AHRS_M2 msg: 1730862996.15 epoch sec; seqNo:3808 2024-11-06T03:16:36Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:36Z- handleDepth()- Depth_Keller msg: 1730862996.15 epoch sec; seqNo:4050; depth 50.35 2024-11-06T03:16:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862996.16 epoch sec; seqNo:4050; 4061517.50 north; 594842.56 east; 36.69 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:16:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862995.76 2024-11-06T03:16:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:36Z- waiting for 1730862999.76, time is 1730862996.16 2024-11-06T03:16:36Z- AHRS_M2 msg: 1730862996.55 epoch sec; seqNo:3809 2024-11-06T03:16:36Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:36Z- handleDepth()- Depth_Keller msg: 1730862996.56 epoch sec; seqNo:4051; depth 50.36 2024-11-06T03:16:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862996.57 epoch sec; seqNo:4051; 4061517.12 north; 594842.57 east; 36.69 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:16:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862995.76 2024-11-06T03:16:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:36Z- waiting for 1730862999.76, time is 1730862996.57 2024-11-06T03:16:36Z- AHRS_M2 msg: 1730862996.96 epoch sec; seqNo:3810 2024-11-06T03:16:36Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:36Z- handleDepth()- Depth_Keller msg: 1730862996.97 epoch sec; seqNo:4052; depth 50.37 2024-11-06T03:16:37Z- handleDVL() - DVL msg epochMillisec: 1730862997.05 2024-11-06T03:16:37Z- handleDvl() - RDI_Pathfinder msg: 1730862997.05 sec; seqNo:1351; #beams:4 2024-11-06T03:16:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 35.51 , 41.12 , 41.87 2024-11-06T03:16:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , 0.01 2024-11-06T03:16:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862997.07 epoch sec; seqNo:4052; 4061516.74 north; 594842.57 east; 36.69 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:16:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862995.76 2024-11-06T03:16:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:37Z- waiting for 1730862999.76, time is 1730862997.07 2024-11-06T03:16:37Z- AHRS_M2 msg: 1730862997.36 epoch sec; seqNo:3811 2024-11-06T03:16:37Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:37Z- handleDepth()- Depth_Keller msg: 1730862997.36 epoch sec; seqNo:4053; depth 50.37 2024-11-06T03:16:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862997.39 epoch sec; seqNo:4053; 4061516.36 north; 594842.58 east; 36.69 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:16:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862995.76 2024-11-06T03:16:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:37Z- waiting for 1730862999.76, time is 1730862997.39 2024-11-06T03:16:37Z- AHRS_M2 msg: 1730862997.77 epoch sec; seqNo:3812 2024-11-06T03:16:37Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:37Z- handleDepth()- Depth_Keller msg: 1730862997.77 epoch sec; seqNo:4054; depth 50.37 2024-11-06T03:16:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862997.78 epoch sec; seqNo:4054; 4061515.98 north; 594842.58 east; 36.69 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:16:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862995.76 2024-11-06T03:16:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:37Z- waiting for 1730862999.76, time is 1730862997.78 2024-11-06T03:16:38Z- AHRS_M2 msg: 1730862998.17 epoch sec; seqNo:3813 2024-11-06T03:16:38Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:38Z- handleDepth()- Depth_Keller msg: 1730862998.17 epoch sec; seqNo:4055; depth 50.38 2024-11-06T03:16:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862998.18 epoch sec; seqNo:4055; 4061515.59 north; 594841.36 east; 36.69 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:16:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862995.76 2024-11-06T03:16:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:38Z- waiting for 1730862999.76, time is 1730862998.18 2024-11-06T03:16:38Z- AHRS_M2 msg: 1730862998.57 epoch sec; seqNo:3814 2024-11-06T03:16:38Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:38Z- handleDepth()- Depth_Keller msg: 1730862998.58 epoch sec; seqNo:4056; depth 50.38 2024-11-06T03:16:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862998.58 epoch sec; seqNo:4056; 4061515.21 north; 594841.37 east; 36.69 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:16:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862995.76 2024-11-06T03:16:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:38Z- waiting for 1730862999.76, time is 1730862998.58 2024-11-06T03:16:38Z- AHRS_M2 msg: 1730862998.97 epoch sec; seqNo:3815 2024-11-06T03:16:38Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:38Z- handleDepth()- Depth_Keller msg: 1730862998.98 epoch sec; seqNo:4057; depth 50.39 2024-11-06T03:16:39Z- handleDVL() - DVL msg epochMillisec: 1730862999.01 2024-11-06T03:16:39Z- handleDvl() - RDI_Pathfinder msg: 1730862999.01 sec; seqNo:1352; #beams:4 2024-11-06T03:16:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.47 , 35.51 , 41.12 , 41.87 2024-11-06T03:16:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.00 , 0.01 2024-11-06T03:16:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862999.03 epoch sec; seqNo:4057; 4061514.83 north; 594841.37 east; 36.69 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:16:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862995.76 2024-11-06T03:16:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:39Z- waiting for 1730862999.76, time is 1730862999.03 2024-11-06T03:16:39Z- AHRS_M2 msg: 1730862999.38 epoch sec; seqNo:3816 2024-11-06T03:16:39Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:39Z- handleDepth()- Depth_Keller msg: 1730862999.38 epoch sec; seqNo:4058; depth 50.38 2024-11-06T03:16:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862999.40 epoch sec; seqNo:4058; 4061514.45 north; 594841.38 east; 36.69 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:16:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862995.76 2024-11-06T03:16:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:39Z- waiting for 1730862999.76, time is 1730862999.40 2024-11-06T03:16:39Z- AHRS_M2 msg: 1730862999.79 epoch sec; seqNo:3817 2024-11-06T03:16:39Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:39Z- handleDepth()- Depth_Keller msg: 1730862999.79 epoch sec; seqNo:4059; depth 50.40 2024-11-06T03:16:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730862999.80 epoch sec; seqNo:4059; 4061514.45 north; 594841.38 east; 36.69 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:16:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862995.76 2024-11-06T03:16:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:39Z- waiting for 1730862999.76, time is 1730862999.80 2024-11-06T03:16:39Z- LcmTrn::updateTrn() >>>> pose time 1730862999.80, meas time 1730862999.01 <<<< 2024-11-06T03:16:39Z- LcmTrn::updateTrn() measUpdate first last update time: 1730862999.80 2024-11-06T03:16:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730862999.01 sec, ping # 0; waiting for INS data more recent than 1730862995.76... 2024-11-06T03:16:39Z- TerrainNav::motionUpdate() - currEstimate = (4061514, 594841.38, 50.40) 2024-11-06T03:16:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:16:39Z- TerrainNav:: SubcloudNIS is 0.64, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:16:39Z- TNavPF::Projecting Measurements 2024-11-06T03:16:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:16:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:16:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:16:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:16:39Z- TNavFilter::4 good beams. 2024-11-06T03:16:39Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:16:39Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:16:39Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:16:39Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:16:39Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:16:39Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:16:39Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:16:39Z- TerrainMapOctree: (vehN, vehE ) = (4061515.12, 594841.61). 2024-11-06T03:16:39Z- TerrainMapOctree: Min Distance = 252.11. 2024-11-06T03:16:39Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:16:40Z- Forcing Subcloud Comparison 2024-11-06T03:16:40Z- Weighting particles with subcloud comparison 2024-11-06T03:16:40Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:40Z- meanExpectedMeasDiff: -3.145818 delta_rms_squared: 1.541155 alpha: 0.987189 2024-11-06T03:16:40Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:40Z- meanExpectedMeasDiff: 4.038572 delta_rms_squared: 0.915852 alpha: 0.978629 2024-11-06T03:16:40Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:40Z- meanExpectedMeasDiff: 2.539092 delta_rms_squared: 1.246525 alpha: 0.984209 2024-11-06T03:16:40Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:40Z- meanExpectedMeasDiff: -0.220460 delta_rms_squared: 1.872390 alpha: 0.989431 2024-11-06T03:16:40Z- Subcloud NIS: 0.658240 2024-11-06T03:16:40Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:16:40Z- TNavPF:: sumWeights = 0.018874 2024-11-06T03:16:40Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:16:40Z- TNavPF::First Term of Map Covariance Matrix is 0.25 2024-11-06T03:16:40Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:16:40Z- TerrainNav::Resampling particle filter... 2024-11-06T03:16:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730862999.01 sec. 2024-11-06T03:16:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:16:40Z- variances([0], [2], and psi_berg) = 0.288439 0.122891 0.000000 2024-11-06T03:16:40Z- publishEstimatesDecoder() - at 1730862999.80 sec 2024-11-06T03:16:40Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:16:40Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061545.76 594824.27 56.78 2024-11-06T03:16:40Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061544.91 594824.12 55.92 lat/lon: 36.69 -121.94 2024-11-06T03:16:40Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.29 0.12 0.65 0.00 2024-11-06T03:16:40Z- AHRS_M2 msg: 1730863000.19 epoch sec; seqNo:3818 2024-11-06T03:16:40Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:40Z- handleDepth()- Depth_Keller msg: 1730863000.22 epoch sec; seqNo:4060; depth 50.39 2024-11-06T03:16:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863000.25 epoch sec; seqNo:4060; 4061514.08 north; 594841.38 east; 36.69 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:16:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862999.80 2024-11-06T03:16:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:40Z- waiting for 1730863003.80, time is 1730863000.25 2024-11-06T03:16:40Z- AHRS_M2 msg: 1730863000.59 epoch sec; seqNo:3819 2024-11-06T03:16:40Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.40 psi 2024-11-06T03:16:40Z- handleDepth()- Depth_Keller msg: 1730863000.60 epoch sec; seqNo:4061; depth 50.39 2024-11-06T03:16:40Z- handleDVL() - DVL msg epochMillisec: 1730863000.60 2024-11-06T03:16:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:16:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:16:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:16:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:16:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:16:40Z- handleDvl() - RDI_Pathfinder msg: 1730863000.60 sec; seqNo:1353; #beams:0 2024-11-06T03:16:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:16:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.00 , 0.01 2024-11-06T03:16:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863000.62 epoch sec; seqNo:4061; 4061513.70 north; 594841.38 east; 36.69 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:16:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862999.80 2024-11-06T03:16:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:40Z- waiting for 1730863003.80, time is 1730863000.62 2024-11-06T03:16:41Z- AHRS_M2 msg: 1730863000.99 epoch sec; seqNo:3820 2024-11-06T03:16:41Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.41 psi 2024-11-06T03:16:41Z- handleDepth()- Depth_Keller msg: 1730863001.01 epoch sec; seqNo:4062; depth 50.40 2024-11-06T03:16:41Z- handleDVL() - DVL msg epochMillisec: 1730863001.03 2024-11-06T03:16:41Z- handleDvl() - RDI_Pathfinder msg: 1730863001.03 sec; seqNo:1354; #beams:4 2024-11-06T03:16:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.28 , 35.51 , 41.12 , 41.68 2024-11-06T03:16:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.01 2024-11-06T03:16:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863001.05 epoch sec; seqNo:4062; 4061513.32 north; 594841.39 east; 36.69 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:16:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862999.80 2024-11-06T03:16:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:41Z- waiting for 1730863003.80, time is 1730863001.05 2024-11-06T03:16:41Z- AHRS_M2 msg: 1730863001.40 epoch sec; seqNo:3821 2024-11-06T03:16:41Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.41 psi 2024-11-06T03:16:41Z- handleDepth()- Depth_Keller msg: 1730863001.41 epoch sec; seqNo:4063; depth 50.40 2024-11-06T03:16:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863001.42 epoch sec; seqNo:4063; 4061512.94 north; 594841.39 east; 36.69 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:16:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862999.80 2024-11-06T03:16:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:41Z- waiting for 1730863003.80, time is 1730863001.42 2024-11-06T03:16:41Z- AHRS_M2 msg: 1730863001.81 epoch sec; seqNo:3822 2024-11-06T03:16:41Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.41 psi 2024-11-06T03:16:41Z- handleDepth()- Depth_Keller msg: 1730863001.81 epoch sec; seqNo:4064; depth 50.40 2024-11-06T03:16:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863001.82 epoch sec; seqNo:4064; 4061512.56 north; 594841.40 east; 36.69 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:16:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862999.80 2024-11-06T03:16:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:41Z- waiting for 1730863003.80, time is 1730863001.82 2024-11-06T03:16:42Z- AHRS_M2 msg: 1730863002.21 epoch sec; seqNo:3823 2024-11-06T03:16:42Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.41 psi 2024-11-06T03:16:42Z- handleDepth()- Depth_Keller msg: 1730863002.22 epoch sec; seqNo:4065; depth 50.39 2024-11-06T03:16:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863002.23 epoch sec; seqNo:4065; 4061512.18 north; 594841.40 east; 36.69 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:16:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862999.80 2024-11-06T03:16:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:42Z- waiting for 1730863003.80, time is 1730863002.23 2024-11-06T03:16:42Z- AHRS_M2 msg: 1730863002.61 epoch sec; seqNo:3824 2024-11-06T03:16:42Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.41 psi 2024-11-06T03:16:42Z- handleDepth()- Depth_Keller msg: 1730863002.62 epoch sec; seqNo:4066; depth 50.40 2024-11-06T03:16:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863002.65 epoch sec; seqNo:4066; 4061511.80 north; 594841.41 east; 36.69 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:16:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862999.80 2024-11-06T03:16:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:42Z- waiting for 1730863003.80, time is 1730863002.65 2024-11-06T03:16:43Z- AHRS_M2 msg: 1730863003.02 epoch sec; seqNo:3825 2024-11-06T03:16:43Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.41 psi 2024-11-06T03:16:43Z- handleDepth()- Depth_Keller msg: 1730863003.02 epoch sec; seqNo:4067; depth 50.41 2024-11-06T03:16:43Z- handleDVL() - DVL msg epochMillisec: 1730863003.04 2024-11-06T03:16:43Z- handleDvl() - RDI_Pathfinder msg: 1730863003.04 sec; seqNo:1355; #beams:4 2024-11-06T03:16:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.28 , 35.88 , 41.30 , 41.68 2024-11-06T03:16:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , -0.02 2024-11-06T03:16:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863003.05 epoch sec; seqNo:4067; 4061511.42 north; 594841.41 east; 36.69 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:16:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862999.80 2024-11-06T03:16:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:43Z- waiting for 1730863003.80, time is 1730863003.05 2024-11-06T03:16:43Z- AHRS_M2 msg: 1730863003.42 epoch sec; seqNo:3826 2024-11-06T03:16:43Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.41 psi 2024-11-06T03:16:43Z- handleDepth()- Depth_Keller msg: 1730863003.43 epoch sec; seqNo:4068; depth 50.41 2024-11-06T03:16:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863003.44 epoch sec; seqNo:4068; 4061511.03 north; 594840.19 east; 36.69 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:16:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862999.80 2024-11-06T03:16:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:43Z- waiting for 1730863003.80, time is 1730863003.44 2024-11-06T03:16:43Z- AHRS_M2 msg: 1730863003.82 epoch sec; seqNo:3827 2024-11-06T03:16:43Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.41 psi 2024-11-06T03:16:43Z- handleDepth()- Depth_Keller msg: 1730863003.84 epoch sec; seqNo:4069; depth 50.42 2024-11-06T03:16:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863003.85 epoch sec; seqNo:4069; 4061510.65 north; 594840.20 east; 36.69 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:16:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730862999.80 2024-11-06T03:16:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:43Z- waiting for 1730863003.80, time is 1730863003.85 2024-11-06T03:16:43Z- LcmTrn::updateTrn() >>>> pose time 1730863003.85, meas time 1730863003.04 <<<< 2024-11-06T03:16:43Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863003.85 2024-11-06T03:16:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863003.04 sec, ping # 0; waiting for INS data more recent than 1730862999.80... 2024-11-06T03:16:43Z- TerrainNav::motionUpdate() - currEstimate = (4061511, 594840.20, 50.42) 2024-11-06T03:16:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:16:43Z- TerrainNav:: SubcloudNIS is 0.66, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:16:44Z- TNavPF::Projecting Measurements 2024-11-06T03:16:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:16:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:16:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:16:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:16:44Z- TNavFilter::4 good beams. 2024-11-06T03:16:44Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:16:44Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:16:44Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:16:44Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:16:44Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:16:44Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:16:44Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:16:44Z- TerrainMapOctree: (vehN, vehE ) = (4061511.41, 594840.43). 2024-11-06T03:16:44Z- TerrainMapOctree: Min Distance = 255.15. 2024-11-06T03:16:44Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:16:44Z- Forcing Subcloud Comparison 2024-11-06T03:16:44Z- Weighting particles with subcloud comparison 2024-11-06T03:16:44Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:44Z- meanExpectedMeasDiff: -3.872142 delta_rms_squared: 1.477998 alpha: 0.986649 2024-11-06T03:16:44Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:44Z- meanExpectedMeasDiff: 4.999016 delta_rms_squared: 0.958819 alpha: 0.979567 2024-11-06T03:16:44Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:44Z- meanExpectedMeasDiff: 1.711400 delta_rms_squared: 2.210147 alpha: 0.991032 2024-11-06T03:16:44Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:44Z- meanExpectedMeasDiff: -0.609840 delta_rms_squared: 1.280706 alpha: 0.984624 2024-11-06T03:16:44Z- Subcloud NIS: 0.671805 2024-11-06T03:16:44Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:16:44Z- TNavPF:: sumWeights = 0.005225 2024-11-06T03:16:44Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:16:44Z- TNavPF::First Term of Map Covariance Matrix is 0.10 2024-11-06T03:16:44Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:16:44Z- TerrainNav::Resampling particle filter... 2024-11-06T03:16:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863003.04 sec. 2024-11-06T03:16:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:16:44Z- variances([0], [2], and psi_berg) = 0.358923 0.150984 0.000000 2024-11-06T03:16:44Z- publishEstimatesDecoder() - at 1730863003.85 sec 2024-11-06T03:16:44Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:16:44Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061541.93 594823.18 56.74 2024-11-06T03:16:44Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061541.14 594823.03 55.63 lat/lon: 36.69 -121.94 2024-11-06T03:16:44Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.36 0.15 0.67 0.00 2024-11-06T03:16:44Z- AHRS_M2 msg: 1730863004.23 epoch sec; seqNo:3828 2024-11-06T03:16:44Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.41 psi 2024-11-06T03:16:44Z- handleDepth()- Depth_Keller msg: 1730863004.24 epoch sec; seqNo:4070; depth 50.41 2024-11-06T03:16:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863004.27 epoch sec; seqNo:4070; 4061510.27 north; 594840.20 east; 36.69 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:16:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863003.85 2024-11-06T03:16:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:44Z- waiting for 1730863007.85, time is 1730863004.27 2024-11-06T03:16:44Z- AHRS_M2 msg: 1730863004.64 epoch sec; seqNo:3829 2024-11-06T03:16:44Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.41 psi 2024-11-06T03:16:44Z- handleDepth()- Depth_Keller msg: 1730863004.64 epoch sec; seqNo:4071; depth 50.42 2024-11-06T03:16:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863004.66 epoch sec; seqNo:4071; 4061510.27 north; 594840.20 east; 36.69 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:16:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863003.85 2024-11-06T03:16:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:44Z- waiting for 1730863007.85, time is 1730863004.66 2024-11-06T03:16:45Z- AHRS_M2 msg: 1730863005.03 epoch sec; seqNo:3830 2024-11-06T03:16:45Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.41 psi 2024-11-06T03:16:45Z- handleDepth()- Depth_Keller msg: 1730863005.04 epoch sec; seqNo:4072; depth 50.42 2024-11-06T03:16:45Z- handleDVL() - DVL msg epochMillisec: 1730863005.07 2024-11-06T03:16:45Z- handleDvl() - RDI_Pathfinder msg: 1730863005.07 sec; seqNo:1356; #beams:4 2024-11-06T03:16:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 35.32 , 41.12 , 41.68 2024-11-06T03:16:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , -0.00 2024-11-06T03:16:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863005.08 epoch sec; seqNo:4072; 4061509.89 north; 594840.21 east; 36.69 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:16:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863003.85 2024-11-06T03:16:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:45Z- waiting for 1730863007.85, time is 1730863005.08 2024-11-06T03:16:45Z- AHRS_M2 msg: 1730863005.44 epoch sec; seqNo:3831 2024-11-06T03:16:45Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.41 psi 2024-11-06T03:16:45Z- handleDepth()- Depth_Keller msg: 1730863005.45 epoch sec; seqNo:4073; depth 50.42 2024-11-06T03:16:45Z- handleDVL() - DVL msg epochMillisec: 1730863005.45 2024-11-06T03:16:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:16:45Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:16:45Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:16:45Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:16:45Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:16:45Z- handleDvl() - RDI_Pathfinder msg: 1730863005.45 sec; seqNo:1357; #beams:0 2024-11-06T03:16:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:16:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , -0.00 2024-11-06T03:16:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863005.47 epoch sec; seqNo:4073; 4061509.52 north; 594840.21 east; 36.69 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:16:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863003.85 2024-11-06T03:16:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:45Z- waiting for 1730863007.85, time is 1730863005.47 2024-11-06T03:16:45Z- handleDepth()- Depth_Keller msg: 1730863005.87 epoch sec; seqNo:4074; depth 50.43 2024-11-06T03:16:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863005.88 epoch sec; seqNo:4074; 4061509.14 north; 594840.21 east; 36.69 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:16:46Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:16:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863003.85 2024-11-06T03:16:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:46Z- waiting for 1730863007.85, time is 1730863005.88 2024-11-06T03:16:46Z- AHRS_M2 msg: 1730863006.25 epoch sec; seqNo:3832 2024-11-06T03:16:46Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.41 psi 2024-11-06T03:16:46Z- handleDepth()- Depth_Keller msg: 1730863006.25 epoch sec; seqNo:4075; depth 50.43 2024-11-06T03:16:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863006.27 epoch sec; seqNo:4075; 4061508.76 north; 594840.22 east; 36.69 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:16:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863003.85 2024-11-06T03:16:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:46Z- waiting for 1730863007.85, time is 1730863006.27 2024-11-06T03:16:46Z- AHRS_M2 msg: 1730863006.65 epoch sec; seqNo:3833 2024-11-06T03:16:46Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.41 psi 2024-11-06T03:16:46Z- handleDepth()- Depth_Keller msg: 1730863006.66 epoch sec; seqNo:4076; depth 50.44 2024-11-06T03:16:46Z- handleDVL() - DVL msg epochMillisec: 1730863006.66 2024-11-06T03:16:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:16:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:16:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:16:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:16:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:16:46Z- handleDvl() - RDI_Pathfinder msg: 1730863006.66 sec; seqNo:1358; #beams:0 2024-11-06T03:16:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:16:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , -0.00 2024-11-06T03:16:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863006.67 epoch sec; seqNo:4076; 4061508.38 north; 594840.22 east; 36.69 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:16:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863003.85 2024-11-06T03:16:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:46Z- waiting for 1730863007.85, time is 1730863006.67 2024-11-06T03:16:47Z- AHRS_M2 msg: 1730863007.06 epoch sec; seqNo:3834 2024-11-06T03:16:47Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.41 psi 2024-11-06T03:16:47Z- handleDepth()- Depth_Keller msg: 1730863007.06 epoch sec; seqNo:4077; depth 50.44 2024-11-06T03:16:47Z- handleDVL() - DVL msg epochMillisec: 1730863007.08 2024-11-06T03:16:47Z- handleDvl() - RDI_Pathfinder msg: 1730863007.08 sec; seqNo:1359; #beams:4 2024-11-06T03:16:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 35.32 , 41.12 , 41.49 2024-11-06T03:16:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , 0.03 2024-11-06T03:16:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863007.12 epoch sec; seqNo:4077; 4061508.00 north; 594840.23 east; 36.69 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:16:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863003.85 2024-11-06T03:16:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:47Z- waiting for 1730863007.85, time is 1730863007.12 2024-11-06T03:16:47Z- AHRS_M2 msg: 1730863007.46 epoch sec; seqNo:3835 2024-11-06T03:16:47Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.41 psi 2024-11-06T03:16:47Z- handleDepth()- Depth_Keller msg: 1730863007.46 epoch sec; seqNo:4078; depth 50.44 2024-11-06T03:16:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863007.48 epoch sec; seqNo:4078; 4061507.62 north; 594840.23 east; 36.69 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:16:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863003.85 2024-11-06T03:16:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:47Z- waiting for 1730863007.85, time is 1730863007.48 2024-11-06T03:16:47Z- AHRS_M2 msg: 1730863007.86 epoch sec; seqNo:3836 2024-11-06T03:16:47Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.41 psi 2024-11-06T03:16:47Z- handleDepth()- Depth_Keller msg: 1730863007.87 epoch sec; seqNo:4079; depth 50.46 2024-11-06T03:16:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863007.89 epoch sec; seqNo:4079; 4061507.61 north; 594839.01 east; 36.69 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:16:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863003.85 2024-11-06T03:16:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:48Z- waiting for 1730863007.85, time is 1730863007.89 2024-11-06T03:16:48Z- LcmTrn::updateTrn() >>>> pose time 1730863007.89, meas time 1730863007.08 <<<< 2024-11-06T03:16:48Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863007.89 2024-11-06T03:16:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863007.08 sec, ping # 0; waiting for INS data more recent than 1730863003.85... 2024-11-06T03:16:48Z- TerrainNav::motionUpdate() - currEstimate = (4061508, 594839.01, 50.46) 2024-11-06T03:16:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:16:48Z- TerrainNav:: SubcloudNIS is 0.67, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:16:48Z- TNavPF::Projecting Measurements 2024-11-06T03:16:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:16:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:16:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:16:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:16:48Z- TNavFilter::4 good beams. 2024-11-06T03:16:48Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:16:48Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:16:48Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:16:48Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:16:48Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:16:48Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:16:48Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:16:48Z- TerrainMapOctree: (vehN, vehE ) = (4061508.22, 594839.25). 2024-11-06T03:16:48Z- TerrainMapOctree: Min Distance = 257.73. 2024-11-06T03:16:48Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:16:48Z- Forcing Subcloud Comparison 2024-11-06T03:16:48Z- Weighting particles with subcloud comparison 2024-11-06T03:16:48Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:48Z- meanExpectedMeasDiff: -4.273891 delta_rms_squared: 1.500877 alpha: 0.986850 2024-11-06T03:16:48Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:48Z- meanExpectedMeasDiff: 4.207939 delta_rms_squared: 1.039038 alpha: 0.981115 2024-11-06T03:16:48Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:48Z- meanExpectedMeasDiff: 0.869546 delta_rms_squared: 1.617132 alpha: 0.987784 2024-11-06T03:16:48Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:48Z- meanExpectedMeasDiff: -0.280886 delta_rms_squared: 2.241982 alpha: 0.991158 2024-11-06T03:16:48Z- Subcloud NIS: 0.680945 2024-11-06T03:16:48Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:16:48Z- TNavPF:: sumWeights = 0.010512 2024-11-06T03:16:48Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:16:48Z- TNavPF::First Term of Map Covariance Matrix is 0.21 2024-11-06T03:16:48Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:16:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863007.08 sec. 2024-11-06T03:16:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:16:48Z- variances([0], [2], and psi_berg) = 0.349861 0.147873 0.000000 2024-11-06T03:16:48Z- publishEstimatesDecoder() - at 1730863007.89 sec 2024-11-06T03:16:48Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:16:48Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061538.32 594821.87 56.01 2024-11-06T03:16:48Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061538.05 594821.84 55.56 lat/lon: 36.69 -121.94 2024-11-06T03:16:48Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.35 0.15 0.65 0.00 2024-11-06T03:16:48Z- AHRS_M2 msg: 1730863008.28 epoch sec; seqNo:3837 2024-11-06T03:16:48Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.42 psi 2024-11-06T03:16:48Z- handleDepth()- Depth_Keller msg: 1730863008.30 epoch sec; seqNo:4080; depth 50.48 2024-11-06T03:16:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863008.31 epoch sec; seqNo:4080; 4061507.23 north; 594839.02 east; 36.69 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:16:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863007.89 2024-11-06T03:16:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:48Z- waiting for 1730863011.89, time is 1730863008.31 2024-11-06T03:16:48Z- AHRS_M2 msg: 1730863008.68 epoch sec; seqNo:3838 2024-11-06T03:16:48Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.42 psi 2024-11-06T03:16:48Z- handleDepth()- Depth_Keller msg: 1730863008.69 epoch sec; seqNo:4081; depth 50.48 2024-11-06T03:16:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863008.74 epoch sec; seqNo:4081; 4061506.85 north; 594839.02 east; 36.69 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:16:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863007.89 2024-11-06T03:16:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:48Z- waiting for 1730863011.89, time is 1730863008.74 2024-11-06T03:16:49Z- AHRS_M2 msg: 1730863009.08 epoch sec; seqNo:3839 2024-11-06T03:16:49Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.42 psi 2024-11-06T03:16:49Z- handleDepth()- Depth_Keller msg: 1730863009.09 epoch sec; seqNo:4082; depth 50.48 2024-11-06T03:16:49Z- handleDVL() - DVL msg epochMillisec: 1730863009.11 2024-11-06T03:16:49Z- handleDvl() - RDI_Pathfinder msg: 1730863009.11 sec; seqNo:1360; #beams:4 2024-11-06T03:16:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 36.07 , 41.12 , 41.49 2024-11-06T03:16:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , 0.04 2024-11-06T03:16:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863009.12 epoch sec; seqNo:4082; 4061506.47 north; 594839.02 east; 36.69 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:16:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863007.89 2024-11-06T03:16:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:49Z- waiting for 1730863011.89, time is 1730863009.12 2024-11-06T03:16:49Z- AHRS_M2 msg: 1730863009.48 epoch sec; seqNo:3840 2024-11-06T03:16:49Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.42 psi 2024-11-06T03:16:49Z- handleDepth()- Depth_Keller msg: 1730863009.48 epoch sec; seqNo:4083; depth 50.50 2024-11-06T03:16:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863009.51 epoch sec; seqNo:4083; 4061506.09 north; 594839.03 east; 36.69 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:16:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863007.89 2024-11-06T03:16:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:49Z- waiting for 1730863011.89, time is 1730863009.51 2024-11-06T03:16:49Z- AHRS_M2 msg: 1730863009.88 epoch sec; seqNo:3841 2024-11-06T03:16:49Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.42 psi 2024-11-06T03:16:49Z- handleDepth()- Depth_Keller msg: 1730863009.89 epoch sec; seqNo:4084; depth 50.50 2024-11-06T03:16:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863009.90 epoch sec; seqNo:4084; 4061505.71 north; 594839.03 east; 36.69 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:16:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863007.89 2024-11-06T03:16:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:50Z- waiting for 1730863011.89, time is 1730863009.90 2024-11-06T03:16:50Z- AHRS_M2 msg: 1730863010.29 epoch sec; seqNo:3842 2024-11-06T03:16:50Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.42 psi 2024-11-06T03:16:50Z- handleDepth()- Depth_Keller msg: 1730863010.30 epoch sec; seqNo:4085; depth 50.53 2024-11-06T03:16:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863010.31 epoch sec; seqNo:4085; 4061505.33 north; 594839.04 east; 36.69 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:16:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863007.89 2024-11-06T03:16:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:50Z- waiting for 1730863011.89, time is 1730863010.31 2024-11-06T03:16:50Z- AHRS_M2 msg: 1730863010.70 epoch sec; seqNo:3843 2024-11-06T03:16:50Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.43 psi 2024-11-06T03:16:50Z- handleDepth()- Depth_Keller msg: 1730863010.71 epoch sec; seqNo:4086; depth 50.52 2024-11-06T03:16:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863010.76 epoch sec; seqNo:4086; 4061504.96 north; 594839.04 east; 36.69 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:16:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863007.89 2024-11-06T03:16:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:50Z- waiting for 1730863011.89, time is 1730863010.76 2024-11-06T03:16:51Z- AHRS_M2 msg: 1730863011.09 epoch sec; seqNo:3844 2024-11-06T03:16:51Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.43 psi 2024-11-06T03:16:51Z- handleDepth()- Depth_Keller msg: 1730863011.10 epoch sec; seqNo:4087; depth 50.52 2024-11-06T03:16:51Z- handleDVL() - DVL msg epochMillisec: 1730863011.12 2024-11-06T03:16:51Z- handleDvl() - RDI_Pathfinder msg: 1730863011.12 sec; seqNo:1361; #beams:4 2024-11-06T03:16:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 36.07 , 41.12 , 41.68 2024-11-06T03:16:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.01 2024-11-06T03:16:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863011.14 epoch sec; seqNo:4087; 4061504.58 north; 594839.04 east; 36.69 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:16:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863007.89 2024-11-06T03:16:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:51Z- waiting for 1730863011.89, time is 1730863011.14 2024-11-06T03:16:51Z- AHRS_M2 msg: 1730863011.50 epoch sec; seqNo:3845 2024-11-06T03:16:51Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.43 psi 2024-11-06T03:16:51Z- handleDepth()- Depth_Keller msg: 1730863011.51 epoch sec; seqNo:4088; depth 50.52 2024-11-06T03:16:51Z- handleDVL() - DVL msg epochMillisec: 1730863011.51 2024-11-06T03:16:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:16:51Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:16:51Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:16:51Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:16:51Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:16:51Z- handleDvl() - RDI_Pathfinder msg: 1730863011.51 sec; seqNo:1362; #beams:0 2024-11-06T03:16:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:16:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.01 2024-11-06T03:16:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863011.53 epoch sec; seqNo:4088; 4061504.20 north; 594839.05 east; 36.69 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:16:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863007.89 2024-11-06T03:16:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:51Z- waiting for 1730863011.89, time is 1730863011.53 2024-11-06T03:16:51Z- AHRS_M2 msg: 1730863011.90 epoch sec; seqNo:3846 2024-11-06T03:16:51Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.43 psi 2024-11-06T03:16:51Z- handleDepth()- Depth_Keller msg: 1730863011.91 epoch sec; seqNo:4089; depth 50.53 2024-11-06T03:16:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863011.92 epoch sec; seqNo:4089; 4061504.20 north; 594839.05 east; 36.69 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:16:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863007.89 2024-11-06T03:16:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:52Z- waiting for 1730863011.89, time is 1730863011.92 2024-11-06T03:16:52Z- LcmTrn::updateTrn() >>>> pose time 1730863011.92, meas time 1730863011.51 <<<< 2024-11-06T03:16:52Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863011.92 2024-11-06T03:16:52Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:16:52Z- TerrainNav::motionUpdate() - currEstimate = (4061504, 594839.05, 50.53) 2024-11-06T03:16:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:16:52Z- TerrainNav:: SubcloudNIS is 0.68, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:16:52Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:16:52Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:16:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:16:52Z- variances([0], [2], and psi_berg) = 0.352330 0.149723 0.000000 2024-11-06T03:16:52Z- publishEstimatesDecoder() - at 1730863011.92 sec 2024-11-06T03:16:52Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:16:52Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061534.95 594821.92 55.86 2024-11-06T03:16:52Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061534.64 594821.88 55.63 lat/lon: 36.69 -121.94 2024-11-06T03:16:52Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.35 0.15 0.92 0.00 2024-11-06T03:16:52Z- AHRS_M2 msg: 1730863012.31 epoch sec; seqNo:3847 2024-11-06T03:16:52Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.43 psi 2024-11-06T03:16:52Z- handleDepth()- Depth_Keller msg: 1730863012.31 epoch sec; seqNo:4090; depth 50.55 2024-11-06T03:16:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863012.34 epoch sec; seqNo:4090; 4061503.82 north; 594839.05 east; 36.69 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:16:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863011.92 2024-11-06T03:16:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:52Z- waiting for 1730863015.92, time is 1730863012.34 2024-11-06T03:16:52Z- AHRS_M2 msg: 1730863012.75 epoch sec; seqNo:3848 2024-11-06T03:16:52Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.43 psi 2024-11-06T03:16:52Z- handleDepth()- Depth_Keller msg: 1730863012.76 epoch sec; seqNo:4091; depth 50.54 2024-11-06T03:16:52Z- handleDVL() - DVL msg epochMillisec: 1730863012.78 2024-11-06T03:16:52Z- handleDvl() - RDI_Pathfinder msg: 1730863012.78 sec; seqNo:1363; #beams:4 2024-11-06T03:16:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.28 , 36.07 , 41.49 , 41.49 2024-11-06T03:16:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.01 2024-11-06T03:16:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863012.81 epoch sec; seqNo:4091; 4061503.43 north; 594837.84 east; 36.69 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:16:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863011.92 2024-11-06T03:16:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:52Z- waiting for 1730863015.92, time is 1730863012.81 2024-11-06T03:16:53Z- AHRS_M2 msg: 1730863013.12 epoch sec; seqNo:3849 2024-11-06T03:16:53Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.43 psi 2024-11-06T03:16:53Z- handleDepth()- Depth_Keller msg: 1730863013.12 epoch sec; seqNo:4092; depth 50.52 2024-11-06T03:16:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863013.14 epoch sec; seqNo:4092; 4061503.05 north; 594837.84 east; 36.69 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:16:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863011.92 2024-11-06T03:16:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:53Z- waiting for 1730863015.92, time is 1730863013.14 2024-11-06T03:16:53Z- AHRS_M2 msg: 1730863013.52 epoch sec; seqNo:3850 2024-11-06T03:16:53Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.43 psi 2024-11-06T03:16:53Z- handleDepth()- Depth_Keller msg: 1730863013.53 epoch sec; seqNo:4093; depth 50.52 2024-11-06T03:16:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863013.54 epoch sec; seqNo:4093; 4061502.67 north; 594837.85 east; 36.69 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:16:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863011.92 2024-11-06T03:16:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:53Z- waiting for 1730863015.92, time is 1730863013.54 2024-11-06T03:16:53Z- AHRS_M2 msg: 1730863013.92 epoch sec; seqNo:3851 2024-11-06T03:16:53Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.43 psi 2024-11-06T03:16:53Z- handleDepth()- Depth_Keller msg: 1730863013.93 epoch sec; seqNo:4094; depth 50.52 2024-11-06T03:16:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863013.94 epoch sec; seqNo:4094; 4061502.29 north; 594837.85 east; 36.69 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:16:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863011.92 2024-11-06T03:16:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:54Z- waiting for 1730863015.92, time is 1730863013.94 2024-11-06T03:16:54Z- AHRS_M2 msg: 1730863014.33 epoch sec; seqNo:3852 2024-11-06T03:16:54Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.43 psi 2024-11-06T03:16:54Z- handleDepth()- Depth_Keller msg: 1730863014.34 epoch sec; seqNo:4095; depth 50.53 2024-11-06T03:16:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863014.35 epoch sec; seqNo:4095; 4061501.91 north; 594837.85 east; 36.69 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:16:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863011.92 2024-11-06T03:16:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:54Z- waiting for 1730863015.92, time is 1730863014.35 2024-11-06T03:16:54Z- AHRS_M2 msg: 1730863014.76 epoch sec; seqNo:3853 2024-11-06T03:16:54Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.43 psi 2024-11-06T03:16:54Z- handleDepth()- Depth_Keller msg: 1730863014.77 epoch sec; seqNo:4096; depth 50.54 2024-11-06T03:16:54Z- handleDVL() - DVL msg epochMillisec: 1730863014.79 2024-11-06T03:16:54Z- handleDvl() - RDI_Pathfinder msg: 1730863014.79 sec; seqNo:1364; #beams:4 2024-11-06T03:16:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 36.07 , 41.30 , 41.49 2024-11-06T03:16:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.00 2024-11-06T03:16:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863014.82 epoch sec; seqNo:4096; 4061501.53 north; 594837.86 east; 36.69 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:16:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863011.92 2024-11-06T03:16:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:54Z- waiting for 1730863015.92, time is 1730863014.82 2024-11-06T03:16:55Z- AHRS_M2 msg: 1730863015.13 epoch sec; seqNo:3854 2024-11-06T03:16:55Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.44 psi 2024-11-06T03:16:55Z- handleDepth()- Depth_Keller msg: 1730863015.15 epoch sec; seqNo:4097; depth 50.51 2024-11-06T03:16:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863015.17 epoch sec; seqNo:4097; 4061501.15 north; 594837.86 east; 36.69 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:16:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863011.92 2024-11-06T03:16:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:55Z- waiting for 1730863015.92, time is 1730863015.17 2024-11-06T03:16:55Z- AHRS_M2 msg: 1730863015.54 epoch sec; seqNo:3855 2024-11-06T03:16:55Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.44 psi 2024-11-06T03:16:55Z- handleDepth()- Depth_Keller msg: 1730863015.55 epoch sec; seqNo:4098; depth 50.50 2024-11-06T03:16:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863015.56 epoch sec; seqNo:4098; 4061500.77 north; 594837.87 east; 36.69 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:16:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863011.92 2024-11-06T03:16:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:55Z- waiting for 1730863015.92, time is 1730863015.56 2024-11-06T03:16:55Z- AHRS_M2 msg: 1730863015.95 epoch sec; seqNo:3856 2024-11-06T03:16:55Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.44 psi 2024-11-06T03:16:55Z- handleDepth()- Depth_Keller msg: 1730863015.96 epoch sec; seqNo:4099; depth 50.51 2024-11-06T03:16:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863015.97 epoch sec; seqNo:4099; 4061500.77 north; 594837.87 east; 36.69 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:16:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863011.92 2024-11-06T03:16:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:56Z- waiting for 1730863015.92, time is 1730863015.97 2024-11-06T03:16:56Z- LcmTrn::updateTrn() >>>> pose time 1730863015.97, meas time 1730863014.79 <<<< 2024-11-06T03:16:56Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863015.97 2024-11-06T03:16:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863014.79 sec, ping # 0; waiting for INS data more recent than 1730863011.92... 2024-11-06T03:16:56Z- TerrainNav::motionUpdate() - currEstimate = (4061501, 594837.87, 50.51) 2024-11-06T03:16:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:16:56Z- TerrainNav:: SubcloudNIS is 0.68, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:16:56Z- TNavPF::Projecting Measurements 2024-11-06T03:16:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:16:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:16:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:16:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:16:56Z- TNavFilter::4 good beams. 2024-11-06T03:16:56Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:16:56Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:16:56Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:16:56Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:16:56Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:16:56Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:16:56Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:16:56Z- TerrainMapOctree: (vehN, vehE ) = (4061501.77, 594838.21). 2024-11-06T03:16:56Z- TerrainMapOctree: Min Distance = 263.44. 2024-11-06T03:16:56Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:16:56Z- Forcing Subcloud Comparison 2024-11-06T03:16:56Z- Weighting particles with subcloud comparison 2024-11-06T03:16:56Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:56Z- meanExpectedMeasDiff: -4.471591 delta_rms_squared: 1.914588 alpha: 0.989662 2024-11-06T03:16:56Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:56Z- meanExpectedMeasDiff: 4.002651 delta_rms_squared: 1.271235 alpha: 0.984511 2024-11-06T03:16:56Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:56Z- meanExpectedMeasDiff: 0.290207 delta_rms_squared: 2.138093 alpha: 0.990733 2024-11-06T03:16:56Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:16:56Z- meanExpectedMeasDiff: 1.422893 delta_rms_squared: 1.600918 alpha: 0.987661 2024-11-06T03:16:56Z- Subcloud NIS: 0.684840 2024-11-06T03:16:56Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:16:56Z- TNavPF:: sumWeights = 0.010431 2024-11-06T03:16:56Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:16:56Z- TNavPF::First Term of Map Covariance Matrix is 0.23 2024-11-06T03:16:56Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:16:56Z- TerrainNav::Resampling particle filter... 2024-11-06T03:16:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863014.79 sec. 2024-11-06T03:16:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:16:56Z- variances([0], [2], and psi_berg) = 0.245787 0.140514 0.000000 2024-11-06T03:16:56Z- publishEstimatesDecoder() - at 1730863015.97 sec 2024-11-06T03:16:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:16:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061532.28 594820.75 56.32 2024-11-06T03:16:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061531.13 594820.68 55.42 lat/lon: 36.69 -121.94 2024-11-06T03:16:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.14 0.73 0.00 2024-11-06T03:16:56Z- AHRS_M2 msg: 1730863016.35 epoch sec; seqNo:3857 2024-11-06T03:16:56Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.44 psi 2024-11-06T03:16:56Z- handleDepth()- Depth_Keller msg: 1730863016.36 epoch sec; seqNo:4100; depth 50.50 2024-11-06T03:16:56Z- handleDVL() - DVL msg epochMillisec: 1730863016.37 2024-11-06T03:16:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:16:56Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:16:56Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:16:56Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:16:56Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:16:56Z- handleDvl() - RDI_Pathfinder msg: 1730863016.37 sec; seqNo:1365; #beams:0 2024-11-06T03:16:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:16:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.00 2024-11-06T03:16:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863016.39 epoch sec; seqNo:4100; 4061500.40 north; 594837.87 east; 36.69 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:16:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863015.97 2024-11-06T03:16:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:56Z- waiting for 1730863019.97, time is 1730863016.39 2024-11-06T03:16:56Z- AHRS_M2 msg: 1730863016.77 epoch sec; seqNo:3858 2024-11-06T03:16:56Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.44 psi 2024-11-06T03:16:56Z- handleDepth()- Depth_Keller msg: 1730863016.77 epoch sec; seqNo:4101; depth 50.50 2024-11-06T03:16:56Z- handleDVL() - DVL msg epochMillisec: 1730863016.80 2024-11-06T03:16:56Z- handleDvl() - RDI_Pathfinder msg: 1730863016.80 sec; seqNo:1366; #beams:4 2024-11-06T03:16:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.28 , 36.25 , 41.67 , 41.11 2024-11-06T03:16:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , -0.01 2024-11-06T03:16:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863016.82 epoch sec; seqNo:4101; 4061500.02 north; 594837.87 east; 36.69 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:16:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863015.97 2024-11-06T03:16:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:56Z- waiting for 1730863019.97, time is 1730863016.82 2024-11-06T03:16:57Z- AHRS_M2 msg: 1730863017.16 epoch sec; seqNo:3859 2024-11-06T03:16:57Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.44 psi 2024-11-06T03:16:57Z- handleDepth()- Depth_Keller msg: 1730863017.16 epoch sec; seqNo:4102; depth 50.49 2024-11-06T03:16:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863017.18 epoch sec; seqNo:4102; 4061499.62 north; 594836.66 east; 36.69 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:16:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863015.97 2024-11-06T03:16:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:57Z- waiting for 1730863019.97, time is 1730863017.18 2024-11-06T03:16:57Z- AHRS_M2 msg: 1730863017.56 epoch sec; seqNo:3860 2024-11-06T03:16:57Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.45 psi 2024-11-06T03:16:57Z- handleDepth()- Depth_Keller msg: 1730863017.57 epoch sec; seqNo:4103; depth 50.49 2024-11-06T03:16:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863017.59 epoch sec; seqNo:4103; 4061499.25 north; 594836.66 east; 36.69 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:16:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863015.97 2024-11-06T03:16:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:57Z- waiting for 1730863019.97, time is 1730863017.59 2024-11-06T03:16:57Z- AHRS_M2 msg: 1730863017.96 epoch sec; seqNo:3861 2024-11-06T03:16:57Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.45 psi 2024-11-06T03:16:57Z- handleDepth()- Depth_Keller msg: 1730863017.97 epoch sec; seqNo:4104; depth 50.49 2024-11-06T03:16:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863017.98 epoch sec; seqNo:4104; 4061498.87 north; 594836.67 east; 36.69 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:16:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863015.97 2024-11-06T03:16:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:58Z- waiting for 1730863019.97, time is 1730863017.98 2024-11-06T03:16:58Z- AHRS_M2 msg: 1730863018.37 epoch sec; seqNo:3862 2024-11-06T03:16:58Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.45 psi 2024-11-06T03:16:58Z- handleDepth()- Depth_Keller msg: 1730863018.39 epoch sec; seqNo:4105; depth 50.48 2024-11-06T03:16:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863018.40 epoch sec; seqNo:4105; 4061498.49 north; 594836.67 east; 36.69 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:16:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863015.97 2024-11-06T03:16:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:58Z- waiting for 1730863019.97, time is 1730863018.40 2024-11-06T03:16:58Z- AHRS_M2 msg: 1730863018.79 epoch sec; seqNo:3863 2024-11-06T03:16:58Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.45 psi 2024-11-06T03:16:58Z- handleDepth()- Depth_Keller msg: 1730863018.80 epoch sec; seqNo:4106; depth 50.47 2024-11-06T03:16:58Z- handleDVL() - DVL msg epochMillisec: 1730863018.83 2024-11-06T03:16:58Z- handleDvl() - RDI_Pathfinder msg: 1730863018.83 sec; seqNo:1367; #beams:4 2024-11-06T03:16:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.91 , 36.44 , 41.67 , 40.74 2024-11-06T03:16:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , -0.01 2024-11-06T03:16:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863018.84 epoch sec; seqNo:4106; 4061498.11 north; 594836.68 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:16:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:16:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863015.97 2024-11-06T03:16:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:58Z- waiting for 1730863019.97, time is 1730863018.84 2024-11-06T03:16:59Z- AHRS_M2 msg: 1730863019.18 epoch sec; seqNo:3864 2024-11-06T03:16:59Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.45 psi 2024-11-06T03:16:59Z- handleDepth()- Depth_Keller msg: 1730863019.18 epoch sec; seqNo:4107; depth 50.47 2024-11-06T03:16:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863019.20 epoch sec; seqNo:4107; 4061497.73 north; 594836.68 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:16:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863015.97 2024-11-06T03:16:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:59Z- waiting for 1730863019.97, time is 1730863019.20 2024-11-06T03:16:59Z- AHRS_M2 msg: 1730863019.58 epoch sec; seqNo:3865 2024-11-06T03:16:59Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.45 psi 2024-11-06T03:16:59Z- handleDepth()- Depth_Keller msg: 1730863019.60 epoch sec; seqNo:4108; depth 50.48 2024-11-06T03:16:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863019.61 epoch sec; seqNo:4108; 4061497.35 north; 594836.68 east; 36.69 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:16:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:16:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863015.97 2024-11-06T03:16:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:16:59Z- waiting for 1730863019.97, time is 1730863019.61 2024-11-06T03:16:59Z- AHRS_M2 msg: 1730863019.98 epoch sec; seqNo:3866 2024-11-06T03:16:59Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.45 psi 2024-11-06T03:16:59Z- handleDepth()- Depth_Keller msg: 1730863019.99 epoch sec; seqNo:4109; depth 50.47 2024-11-06T03:16:59Z- handleDVL() - DVL msg epochMillisec: 1730863019.99 2024-11-06T03:16:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:16:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:16:59Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:16:59Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:16:59Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:16:59Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:16:59Z- handleDvl() - RDI_Pathfinder msg: 1730863019.99 sec; seqNo:1368; #beams:0 2024-11-06T03:16:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:16:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , -0.01 2024-11-06T03:17:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863020.02 epoch sec; seqNo:4109; 4061496.97 north; 594836.69 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:17:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863015.97 2024-11-06T03:17:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:00Z- waiting for 1730863019.97, time is 1730863020.02 2024-11-06T03:17:00Z- LcmTrn::updateTrn() >>>> pose time 1730863020.02, meas time 1730863019.99 <<<< 2024-11-06T03:17:00Z- LcmTrn::updateTrn() equating pose 1730863019.99 and meas times 1730863019.99 2024-11-06T03:17:00Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863019.99 2024-11-06T03:17:00Z- TerrainNav::motionUpdate() - currEstimate = (4061497, 594836.69, 50.47) 2024-11-06T03:17:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:17:00Z- TerrainNav:: SubcloudNIS is 0.68, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:17:00Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:17:00Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:17:00Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:17:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:17:00Z- variances([0], [2], and psi_berg) = 0.248299 0.142973 0.000000 2024-11-06T03:17:00Z- publishEstimatesDecoder() - at 1730863019.99 sec 2024-11-06T03:17:00Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:17:00Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061528.49 594819.64 57.00 2024-11-06T03:17:00Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061527.32 594819.50 55.39 lat/lon: 36.69 -121.94 2024-11-06T03:17:00Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.14 0.97 0.00 2024-11-06T03:17:00Z- AHRS_M2 msg: 1730863020.39 epoch sec; seqNo:3867 2024-11-06T03:17:00Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.45 psi 2024-11-06T03:17:00Z- handleDepth()- Depth_Keller msg: 1730863020.40 epoch sec; seqNo:4110; depth 50.47 2024-11-06T03:17:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863020.41 epoch sec; seqNo:4110; 4061496.59 north; 594836.69 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:17:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863019.99 2024-11-06T03:17:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:00Z- waiting for 1730863023.99, time is 1730863020.41 2024-11-06T03:17:00Z- AHRS_M2 msg: 1730863020.79 epoch sec; seqNo:3868 2024-11-06T03:17:00Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 3.45 psi 2024-11-06T03:17:00Z- handleDepth()- Depth_Keller msg: 1730863020.80 epoch sec; seqNo:4111; depth 50.48 2024-11-06T03:17:00Z- handleDVL() - DVL msg epochMillisec: 1730863020.83 2024-11-06T03:17:00Z- handleDvl() - RDI_Pathfinder msg: 1730863020.83 sec; seqNo:1369; #beams:4 2024-11-06T03:17:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.09 , 36.44 , 41.86 , 40.93 2024-11-06T03:17:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , 0.01 2024-11-06T03:17:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863020.84 epoch sec; seqNo:4111; 4061496.22 north; 594836.70 east; 36.69 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:17:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863019.99 2024-11-06T03:17:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:00Z- waiting for 1730863023.99, time is 1730863020.84 2024-11-06T03:17:01Z- AHRS_M2 msg: 1730863021.19 epoch sec; seqNo:3869 2024-11-06T03:17:01Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 3.45 psi 2024-11-06T03:17:01Z- handleDepth()- Depth_Keller msg: 1730863021.20 epoch sec; seqNo:4112; depth 50.47 2024-11-06T03:17:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863021.22 epoch sec; seqNo:4112; 4061495.82 north; 594835.48 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:17:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863019.99 2024-11-06T03:17:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:01Z- waiting for 1730863023.99, time is 1730863021.22 2024-11-06T03:17:01Z- AHRS_M2 msg: 1730863021.60 epoch sec; seqNo:3870 2024-11-06T03:17:01Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 3.45 psi 2024-11-06T03:17:01Z- handleDepth()- Depth_Keller msg: 1730863021.61 epoch sec; seqNo:4113; depth 50.46 2024-11-06T03:17:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863021.62 epoch sec; seqNo:4113; 4061495.82 north; 594835.48 east; 36.69 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:17:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863019.99 2024-11-06T03:17:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:01Z- waiting for 1730863023.99, time is 1730863021.62 2024-11-06T03:17:02Z- AHRS_M2 msg: 1730863022.00 epoch sec; seqNo:3871 2024-11-06T03:17:02Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 3.45 psi 2024-11-06T03:17:02Z- handleDepth()- Depth_Keller msg: 1730863022.01 epoch sec; seqNo:4114; depth 50.48 2024-11-06T03:17:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863022.02 epoch sec; seqNo:4114; 4061495.44 north; 594835.48 east; 36.69 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:17:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863019.99 2024-11-06T03:17:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:02Z- waiting for 1730863023.99, time is 1730863022.02 2024-11-06T03:17:02Z- AHRS_M2 msg: 1730863022.41 epoch sec; seqNo:3872 2024-11-06T03:17:02Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.45 psi 2024-11-06T03:17:02Z- handleDepth()- Depth_Keller msg: 1730863022.43 epoch sec; seqNo:4115; depth 50.48 2024-11-06T03:17:02Z- handleDVL() - DVL msg epochMillisec: 1730863022.43 2024-11-06T03:17:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:02Z- handleDvl() - RDI_Pathfinder msg: 1730863022.43 sec; seqNo:1370; #beams:0 2024-11-06T03:17:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , 0.01 2024-11-06T03:17:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863022.44 epoch sec; seqNo:4115; 4061495.07 north; 594835.49 east; 36.69 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:17:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863019.99 2024-11-06T03:17:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:02Z- waiting for 1730863023.99, time is 1730863022.44 2024-11-06T03:17:02Z- AHRS_M2 msg: 1730863022.82 epoch sec; seqNo:3873 2024-11-06T03:17:02Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.45 psi 2024-11-06T03:17:02Z- handleDepth()- Depth_Keller msg: 1730863022.83 epoch sec; seqNo:4116; depth 50.48 2024-11-06T03:17:02Z- handleDVL() - DVL msg epochMillisec: 1730863022.89 2024-11-06T03:17:02Z- handleDvl() - RDI_Pathfinder msg: 1730863022.89 sec; seqNo:1371; #beams:4 2024-11-06T03:17:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.28 , 36.63 , 41.86 , 40.93 2024-11-06T03:17:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , 0.04 2024-11-06T03:17:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863022.92 epoch sec; seqNo:4116; 4061494.69 north; 594835.49 east; 36.69 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:17:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863019.99 2024-11-06T03:17:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:02Z- waiting for 1730863023.99, time is 1730863022.92 2024-11-06T03:17:03Z- AHRS_M2 msg: 1730863023.21 epoch sec; seqNo:3874 2024-11-06T03:17:03Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.45 psi 2024-11-06T03:17:03Z- handleDepth()- Depth_Keller msg: 1730863023.24 epoch sec; seqNo:4117; depth 50.49 2024-11-06T03:17:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863023.25 epoch sec; seqNo:4117; 4061494.31 north; 594835.50 east; 36.69 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:17:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863019.99 2024-11-06T03:17:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:03Z- waiting for 1730863023.99, time is 1730863023.25 2024-11-06T03:17:03Z- AHRS_M2 msg: 1730863023.62 epoch sec; seqNo:3875 2024-11-06T03:17:03Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.45 psi 2024-11-06T03:17:03Z- handleDepth()- Depth_Keller msg: 1730863023.63 epoch sec; seqNo:4118; depth 50.48 2024-11-06T03:17:03Z- handleDVL() - DVL msg epochMillisec: 1730863023.63 2024-11-06T03:17:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:03Z- handleDvl() - RDI_Pathfinder msg: 1730863023.63 sec; seqNo:1372; #beams:0 2024-11-06T03:17:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.05 , 0.04 2024-11-06T03:17:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863023.65 epoch sec; seqNo:4118; 4061493.93 north; 594835.50 east; 36.69 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:17:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863019.99 2024-11-06T03:17:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:03Z- waiting for 1730863023.99, time is 1730863023.65 2024-11-06T03:17:04Z- AHRS_M2 msg: 1730863024.02 epoch sec; seqNo:3876 2024-11-06T03:17:04Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.45 psi 2024-11-06T03:17:04Z- handleDepth()- Depth_Keller msg: 1730863024.03 epoch sec; seqNo:4119; depth 50.49 2024-11-06T03:17:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863024.04 epoch sec; seqNo:4119; 4061493.55 north; 594835.51 east; 36.69 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:17:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863019.99 2024-11-06T03:17:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:04Z- waiting for 1730863023.99, time is 1730863024.04 2024-11-06T03:17:04Z- LcmTrn::updateTrn() >>>> pose time 1730863024.04, meas time 1730863023.63 <<<< 2024-11-06T03:17:04Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863024.04 2024-11-06T03:17:04Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:17:04Z- TerrainNav::motionUpdate() - currEstimate = (4061494, 594835.51, 50.49) 2024-11-06T03:17:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:17:04Z- TerrainNav:: SubcloudNIS is 0.68, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:17:04Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:17:04Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:17:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:17:04Z- variances([0], [2], and psi_berg) = 0.250744 0.145241 0.000000 2024-11-06T03:17:04Z- publishEstimatesDecoder() - at 1730863024.04 sec 2024-11-06T03:17:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:17:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061525.08 594818.45 57.00 2024-11-06T03:17:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061523.90 594818.32 55.41 lat/lon: 36.69 -121.94 2024-11-06T03:17:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.15 1.23 0.00 2024-11-06T03:17:04Z- AHRS_M2 msg: 1730863024.43 epoch sec; seqNo:3877 2024-11-06T03:17:04Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.45 psi 2024-11-06T03:17:04Z- handleDepth()- Depth_Keller msg: 1730863024.44 epoch sec; seqNo:4120; depth 50.49 2024-11-06T03:17:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863024.45 epoch sec; seqNo:4120; 4061493.17 north; 594835.51 east; 36.69 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:17:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863024.04 2024-11-06T03:17:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:04Z- waiting for 1730863028.04, time is 1730863024.45 2024-11-06T03:17:04Z- AHRS_M2 msg: 1730863024.83 epoch sec; seqNo:3878 2024-11-06T03:17:04Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.45 psi 2024-11-06T03:17:04Z- handleDepth()- Depth_Keller msg: 1730863024.84 epoch sec; seqNo:4121; depth 50.48 2024-11-06T03:17:04Z- handleDVL() - DVL msg epochMillisec: 1730863024.88 2024-11-06T03:17:04Z- handleDvl() - RDI_Pathfinder msg: 1730863024.88 sec; seqNo:1373; #beams:4 2024-11-06T03:17:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.72 , 36.81 , 41.67 , 40.93 2024-11-06T03:17:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , 0.04 2024-11-06T03:17:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863024.90 epoch sec; seqNo:4121; 4061492.78 north; 594834.29 east; 36.69 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:17:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863024.04 2024-11-06T03:17:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:04Z- waiting for 1730863028.04, time is 1730863024.90 2024-11-06T03:17:05Z- AHRS_M2 msg: 1730863025.23 epoch sec; seqNo:3879 2024-11-06T03:17:05Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.45 psi 2024-11-06T03:17:05Z- handleDepth()- Depth_Keller msg: 1730863025.25 epoch sec; seqNo:4122; depth 50.49 2024-11-06T03:17:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863025.27 epoch sec; seqNo:4122; 4061492.78 north; 594834.29 east; 36.69 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:17:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863024.04 2024-11-06T03:17:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:05Z- waiting for 1730863028.04, time is 1730863025.27 2024-11-06T03:17:05Z- AHRS_M2 msg: 1730863025.65 epoch sec; seqNo:3880 2024-11-06T03:17:05Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.45 psi 2024-11-06T03:17:05Z- handleDepth()- Depth_Keller msg: 1730863025.65 epoch sec; seqNo:4123; depth 50.48 2024-11-06T03:17:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863025.66 epoch sec; seqNo:4123; 4061492.40 north; 594834.30 east; 36.69 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:17:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863024.04 2024-11-06T03:17:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:05Z- waiting for 1730863028.04, time is 1730863025.66 2024-11-06T03:17:06Z- AHRS_M2 msg: 1730863026.05 epoch sec; seqNo:3881 2024-11-06T03:17:06Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.45 psi 2024-11-06T03:17:06Z- handleDepth()- Depth_Keller msg: 1730863026.06 epoch sec; seqNo:4124; depth 50.48 2024-11-06T03:17:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863026.07 epoch sec; seqNo:4124; 4061492.02 north; 594834.30 east; 36.69 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:17:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863024.04 2024-11-06T03:17:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:06Z- waiting for 1730863028.04, time is 1730863026.07 2024-11-06T03:17:06Z- AHRS_M2 msg: 1730863026.45 epoch sec; seqNo:3882 2024-11-06T03:17:06Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 3.45 psi 2024-11-06T03:17:06Z- handleDepth()- Depth_Keller msg: 1730863026.45 epoch sec; seqNo:4125; depth 50.47 2024-11-06T03:17:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863026.46 epoch sec; seqNo:4125; 4061491.64 north; 594834.31 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:17:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863024.04 2024-11-06T03:17:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:06Z- waiting for 1730863028.04, time is 1730863026.46 2024-11-06T03:17:06Z- AHRS_M2 msg: 1730863026.85 epoch sec; seqNo:3883 2024-11-06T03:17:06Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 3.45 psi 2024-11-06T03:17:06Z- handleDepth()- Depth_Keller msg: 1730863026.86 epoch sec; seqNo:4126; depth 50.47 2024-11-06T03:17:06Z- handleDVL() - DVL msg epochMillisec: 1730863026.88 2024-11-06T03:17:06Z- handleDvl() - RDI_Pathfinder msg: 1730863026.88 sec; seqNo:1374; #beams:4 2024-11-06T03:17:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.09 , 37.00 , 42.05 , 41.11 2024-11-06T03:17:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , 0.01 2024-11-06T03:17:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863026.91 epoch sec; seqNo:4126; 4061491.26 north; 594834.31 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:17:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863024.04 2024-11-06T03:17:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:07Z- waiting for 1730863028.04, time is 1730863026.91 2024-11-06T03:17:07Z- AHRS_M2 msg: 1730863027.26 epoch sec; seqNo:3884 2024-11-06T03:17:07Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 3.45 psi 2024-11-06T03:17:07Z- handleDepth()- Depth_Keller msg: 1730863027.27 epoch sec; seqNo:4127; depth 50.47 2024-11-06T03:17:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863027.29 epoch sec; seqNo:4127; 4061490.88 north; 594834.31 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:17:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863024.04 2024-11-06T03:17:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:07Z- waiting for 1730863028.04, time is 1730863027.29 2024-11-06T03:17:07Z- handleDepth()- Depth_Keller msg: 1730863027.68 epoch sec; seqNo:4128; depth 50.47 2024-11-06T03:17:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863027.69 epoch sec; seqNo:4128; 4061490.51 north; 594834.32 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:17:07Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:17:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863024.04 2024-11-06T03:17:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:07Z- waiting for 1730863028.04, time is 1730863027.69 2024-11-06T03:17:08Z- AHRS_M2 msg: 1730863028.06 epoch sec; seqNo:3885 2024-11-06T03:17:08Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 3.46 psi 2024-11-06T03:17:08Z- handleDepth()- Depth_Keller msg: 1730863028.07 epoch sec; seqNo:4129; depth 50.46 2024-11-06T03:17:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863028.08 epoch sec; seqNo:4129; 4061490.13 north; 594834.32 east; 36.69 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:17:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863024.04 2024-11-06T03:17:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:08Z- waiting for 1730863028.04, time is 1730863028.08 2024-11-06T03:17:08Z- LcmTrn::updateTrn() >>>> pose time 1730863028.08, meas time 1730863026.88 <<<< 2024-11-06T03:17:08Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863028.08 2024-11-06T03:17:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863026.88 sec, ping # 0; waiting for INS data more recent than 1730863024.04... 2024-11-06T03:17:08Z- TerrainNav::motionUpdate() - currEstimate = (4061490, 594834.32, 50.46) 2024-11-06T03:17:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:17:08Z- TerrainNav:: SubcloudNIS is 0.68, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:17:08Z- TNavPF::Projecting Measurements 2024-11-06T03:17:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:17:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:17:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:17:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:17:08Z- TNavFilter::4 good beams. 2024-11-06T03:17:08Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:17:08Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:17:08Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:17:08Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:17:08Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:17:08Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:17:08Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:17:08Z- TerrainMapOctree: (vehN, vehE ) = (4061491.15, 594834.68). 2024-11-06T03:17:08Z- TerrainMapOctree: Min Distance = 255.58. 2024-11-06T03:17:08Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:17:08Z- TerrainMapOctree: Switching to tile 2. 2024-11-06T03:17:09Z- TerrainMapOctree::Octree tile load /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile2.bo took 0.970184 seconds. 2024-11-06T03:17:09Z- Forcing Subcloud Comparison 2024-11-06T03:17:09Z- Weighting particles with subcloud comparison 2024-11-06T03:17:09Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:17:09Z- meanExpectedMeasDiff: -4.614849 delta_rms_squared: 3.159009 alpha: 0.993709 2024-11-06T03:17:09Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:17:09Z- meanExpectedMeasDiff: 4.477462 delta_rms_squared: 1.688813 alpha: 0.988296 2024-11-06T03:17:09Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:17:09Z- meanExpectedMeasDiff: -0.060878 delta_rms_squared: 3.289411 alpha: 0.993957 2024-11-06T03:17:09Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:17:09Z- meanExpectedMeasDiff: 3.286150 delta_rms_squared: 3.818429 alpha: 0.994790 2024-11-06T03:17:09Z- Subcloud NIS: 0.704446 2024-11-06T03:17:09Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:17:09Z- TNavPF:: sumWeights = 0.002124 2024-11-06T03:17:09Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:17:09Z- TNavPF::First Term of Map Covariance Matrix is 0.06 2024-11-06T03:17:09Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:17:09Z- TerrainNav::Resampling particle filter... 2024-11-06T03:17:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863026.88 sec. 2024-11-06T03:17:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:17:09Z- variances([0], [2], and psi_berg) = 0.246078 0.155556 0.000000 2024-11-06T03:17:09Z- publishEstimatesDecoder() - at 1730863028.08 sec 2024-11-06T03:17:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:17:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061521.57 594817.21 52.43 2024-11-06T03:17:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061520.49 594817.21 54.87 lat/lon: 36.69 -121.94 2024-11-06T03:17:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.16 0.71 0.00 2024-11-06T03:17:09Z- AHRS_M2 msg: 1730863028.47 epoch sec; seqNo:3886 2024-11-06T03:17:09Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.46 psi 2024-11-06T03:17:09Z- handleDepth()- Depth_Keller msg: 1730863028.47 epoch sec; seqNo:4130; depth 50.46 2024-11-06T03:17:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863028.48 epoch sec; seqNo:4130; 4061489.75 north; 594834.33 east; 36.69 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:17:09Z- AHRS_M2 msg: 1730863028.87 epoch sec; seqNo:3887 2024-11-06T03:17:09Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.46 psi 2024-11-06T03:17:09Z- handleDepth()- Depth_Keller msg: 1730863028.88 epoch sec; seqNo:4131; depth 50.44 2024-11-06T03:17:09Z- handleDVL() - DVL msg epochMillisec: 1730863028.90 2024-11-06T03:17:09Z- handleDvl() - RDI_Pathfinder msg: 1730863028.90 sec; seqNo:1375; #beams:4 2024-11-06T03:17:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 37.10 , 42.00 , 41.38 2024-11-06T03:17:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , -0.04 2024-11-06T03:17:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863028.91 epoch sec; seqNo:4131; 4061489.37 north; 594834.33 east; 36.69 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:17:09Z- AHRS_M2 msg: 1730863029.27 epoch sec; seqNo:3888 2024-11-06T03:17:09Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.46 psi 2024-11-06T03:17:09Z- handleDepth()- Depth_Keller msg: 1730863029.28 epoch sec; seqNo:4132; depth 50.44 2024-11-06T03:17:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863029.30 epoch sec; seqNo:4132; 4061489.37 north; 594834.33 east; 36.69 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:17:09Z- AHRS_M2 msg: 1730863029.68 epoch sec; seqNo:3889 2024-11-06T03:17:09Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.46 psi 2024-11-06T03:17:09Z- handleDepth()- Depth_Keller msg: 1730863029.69 epoch sec; seqNo:4133; depth 50.43 2024-11-06T03:17:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863029.74 epoch sec; seqNo:4133; 4061488.98 north; 594833.11 east; 36.69 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:17:09Z- 13 msgs in 200 ms and 14 handle calls 2024-11-06T03:17:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863028.08 2024-11-06T03:17:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:09Z- waiting for 1730863032.08, time is 1730863029.74 2024-11-06T03:17:10Z- AHRS_M2 msg: 1730863030.09 epoch sec; seqNo:3890 2024-11-06T03:17:10Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.46 psi 2024-11-06T03:17:10Z- handleDepth()- Depth_Keller msg: 1730863030.09 epoch sec; seqNo:4134; depth 50.44 2024-11-06T03:17:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863030.10 epoch sec; seqNo:4134; 4061488.60 north; 594833.12 east; 36.69 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:17:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863028.08 2024-11-06T03:17:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:10Z- waiting for 1730863032.08, time is 1730863030.10 2024-11-06T03:17:10Z- AHRS_M2 msg: 1730863030.49 epoch sec; seqNo:3891 2024-11-06T03:17:10Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.46 psi 2024-11-06T03:17:10Z- handleDepth()- Depth_Keller msg: 1730863030.50 epoch sec; seqNo:4135; depth 50.41 2024-11-06T03:17:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863030.51 epoch sec; seqNo:4135; 4061488.22 north; 594833.12 east; 36.69 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:17:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863028.08 2024-11-06T03:17:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:10Z- waiting for 1730863032.08, time is 1730863030.51 2024-11-06T03:17:10Z- AHRS_M2 msg: 1730863030.89 epoch sec; seqNo:3892 2024-11-06T03:17:10Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.46 psi 2024-11-06T03:17:10Z- handleDepth()- Depth_Keller msg: 1730863030.90 epoch sec; seqNo:4136; depth 50.41 2024-11-06T03:17:10Z- handleDVL() - DVL msg epochMillisec: 1730863030.93 2024-11-06T03:17:10Z- handleDvl() - RDI_Pathfinder msg: 1730863030.93 sec; seqNo:1376; #beams:4 2024-11-06T03:17:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 36.90 , 42.20 , 40.77 2024-11-06T03:17:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.05 , -0.05 2024-11-06T03:17:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863030.94 epoch sec; seqNo:4136; 4061487.84 north; 594833.13 east; 36.69 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:17:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863028.08 2024-11-06T03:17:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:11Z- waiting for 1730863032.08, time is 1730863030.94 2024-11-06T03:17:11Z- AHRS_M2 msg: 1730863031.30 epoch sec; seqNo:3893 2024-11-06T03:17:11Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.46 psi 2024-11-06T03:17:11Z- handleDepth()- Depth_Keller msg: 1730863031.30 epoch sec; seqNo:4137; depth 50.41 2024-11-06T03:17:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863031.32 epoch sec; seqNo:4137; 4061487.46 north; 594833.13 east; 36.69 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:17:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863028.08 2024-11-06T03:17:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:11Z- waiting for 1730863032.08, time is 1730863031.32 2024-11-06T03:17:11Z- AHRS_M2 msg: 1730863031.70 epoch sec; seqNo:3894 2024-11-06T03:17:11Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.46 psi 2024-11-06T03:17:11Z- handleDepth()- Depth_Keller msg: 1730863031.71 epoch sec; seqNo:4138; depth 50.40 2024-11-06T03:17:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863031.73 epoch sec; seqNo:4138; 4061487.08 north; 594833.14 east; 36.69 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:17:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863028.08 2024-11-06T03:17:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:11Z- waiting for 1730863032.08, time is 1730863031.73 2024-11-06T03:17:12Z- AHRS_M2 msg: 1730863032.11 epoch sec; seqNo:3895 2024-11-06T03:17:12Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.45 psi 2024-11-06T03:17:12Z- handleDepth()- Depth_Keller msg: 1730863032.12 epoch sec; seqNo:4139; depth 50.38 2024-11-06T03:17:12Z- handleDVL() - DVL msg epochMillisec: 1730863032.12 2024-11-06T03:17:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:12Z- handleDvl() - RDI_Pathfinder msg: 1730863032.12 sec; seqNo:1377; #beams:0 2024-11-06T03:17:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.05 , -0.05 2024-11-06T03:17:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863032.13 epoch sec; seqNo:4139; 4061486.70 north; 594833.14 east; 36.69 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:17:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863028.08 2024-11-06T03:17:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:12Z- waiting for 1730863032.08, time is 1730863032.13 2024-11-06T03:17:12Z- LcmTrn::updateTrn() >>>> pose time 1730863032.13, meas time 1730863032.12 <<<< 2024-11-06T03:17:12Z- LcmTrn::updateTrn() equating pose 1730863032.12 and meas times 1730863032.12 2024-11-06T03:17:12Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863032.12 2024-11-06T03:17:12Z- TerrainNav::motionUpdate() - currEstimate = (4061487, 594833.14, 50.38) 2024-11-06T03:17:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:17:12Z- TerrainNav:: SubcloudNIS is 0.70, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:17:12Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:17:12Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:17:12Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:17:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:17:12Z- variances([0], [2], and psi_berg) = 0.248437 0.157488 0.000000 2024-11-06T03:17:12Z- publishEstimatesDecoder() - at 1730863032.12 sec 2024-11-06T03:17:12Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:17:12Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061518.23 594816.01 52.89 2024-11-06T03:17:12Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061517.06 594816.03 54.79 lat/lon: 36.69 -121.94 2024-11-06T03:17:12Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.16 0.97 0.00 2024-11-06T03:17:12Z- AHRS_M2 msg: 1730863032.51 epoch sec; seqNo:3896 2024-11-06T03:17:12Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.45 psi 2024-11-06T03:17:12Z- handleDepth()- Depth_Keller msg: 1730863032.52 epoch sec; seqNo:4140; depth 50.40 2024-11-06T03:17:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863032.53 epoch sec; seqNo:4140; 4061486.32 north; 594833.14 east; 36.69 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:17:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863032.12 2024-11-06T03:17:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:12Z- waiting for 1730863036.12, time is 1730863032.53 2024-11-06T03:17:12Z- AHRS_M2 msg: 1730863032.91 epoch sec; seqNo:3897 2024-11-06T03:17:12Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.45 psi 2024-11-06T03:17:12Z- handleDepth()- Depth_Keller msg: 1730863032.92 epoch sec; seqNo:4141; depth 50.38 2024-11-06T03:17:12Z- handleDVL() - DVL msg epochMillisec: 1730863032.94 2024-11-06T03:17:12Z- handleDvl() - RDI_Pathfinder msg: 1730863032.94 sec; seqNo:1378; #beams:4 2024-11-06T03:17:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.69 , 37.10 , 42.20 , 40.77 2024-11-06T03:17:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , -0.02 2024-11-06T03:17:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863032.97 epoch sec; seqNo:4141; 4061485.95 north; 594833.15 east; 36.69 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:17:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863032.12 2024-11-06T03:17:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:13Z- waiting for 1730863036.12, time is 1730863032.97 2024-11-06T03:17:13Z- AHRS_M2 msg: 1730863033.32 epoch sec; seqNo:3898 2024-11-06T03:17:13Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.45 psi 2024-11-06T03:17:13Z- handleDepth()- Depth_Keller msg: 1730863033.33 epoch sec; seqNo:4142; depth 50.40 2024-11-06T03:17:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863033.35 epoch sec; seqNo:4142; 4061485.55 north; 594831.93 east; 36.69 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:17:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863032.12 2024-11-06T03:17:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:13Z- waiting for 1730863036.12, time is 1730863033.35 2024-11-06T03:17:13Z- AHRS_M2 msg: 1730863033.72 epoch sec; seqNo:3899 2024-11-06T03:17:13Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.45 psi 2024-11-06T03:17:13Z- handleDepth()- Depth_Keller msg: 1730863033.72 epoch sec; seqNo:4143; depth 50.39 2024-11-06T03:17:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863033.74 epoch sec; seqNo:4143; 4061485.17 north; 594831.94 east; 36.69 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:17:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863032.12 2024-11-06T03:17:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:13Z- waiting for 1730863036.12, time is 1730863033.74 2024-11-06T03:17:14Z- AHRS_M2 msg: 1730863034.12 epoch sec; seqNo:3900 2024-11-06T03:17:14Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.45 psi 2024-11-06T03:17:14Z- handleDepth()- Depth_Keller msg: 1730863034.13 epoch sec; seqNo:4144; depth 50.39 2024-11-06T03:17:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863034.16 epoch sec; seqNo:4144; 4061484.80 north; 594831.94 east; 36.69 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:17:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863032.12 2024-11-06T03:17:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:14Z- waiting for 1730863036.12, time is 1730863034.16 2024-11-06T03:17:14Z- AHRS_M2 msg: 1730863034.53 epoch sec; seqNo:3901 2024-11-06T03:17:14Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.44 psi 2024-11-06T03:17:14Z- handleDepth()- Depth_Keller msg: 1730863034.53 epoch sec; seqNo:4145; depth 50.40 2024-11-06T03:17:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863034.54 epoch sec; seqNo:4145; 4061484.80 north; 594831.94 east; 36.69 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:17:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863032.12 2024-11-06T03:17:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:14Z- waiting for 1730863036.12, time is 1730863034.54 2024-11-06T03:17:14Z- AHRS_M2 msg: 1730863034.93 epoch sec; seqNo:3902 2024-11-06T03:17:14Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.44 psi 2024-11-06T03:17:14Z- handleDepth()- Depth_Keller msg: 1730863034.94 epoch sec; seqNo:4146; depth 50.39 2024-11-06T03:17:14Z- handleDVL() - DVL msg epochMillisec: 1730863034.96 2024-11-06T03:17:14Z- handleDvl() - RDI_Pathfinder msg: 1730863034.96 sec; seqNo:1379; #beams:4 2024-11-06T03:17:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 37.31 , 42.20 , 40.37 2024-11-06T03:17:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.08 , 0.03 2024-11-06T03:17:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863034.99 epoch sec; seqNo:4146; 4061484.42 north; 594831.94 east; 36.69 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:17:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863032.12 2024-11-06T03:17:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:15Z- waiting for 1730863036.12, time is 1730863034.99 2024-11-06T03:17:15Z- handleDepth()- Depth_Keller msg: 1730863035.35 epoch sec; seqNo:4147; depth 50.39 2024-11-06T03:17:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863035.36 epoch sec; seqNo:4147; 4061484.04 north; 594831.95 east; 36.69 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:17:15Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:17:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863032.12 2024-11-06T03:17:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:15Z- waiting for 1730863036.12, time is 1730863035.36 2024-11-06T03:17:15Z- AHRS_M2 msg: 1730863035.74 epoch sec; seqNo:3903 2024-11-06T03:17:15Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.44 psi 2024-11-06T03:17:15Z- handleDepth()- Depth_Keller msg: 1730863035.75 epoch sec; seqNo:4148; depth 50.39 2024-11-06T03:17:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863035.77 epoch sec; seqNo:4148; 4061483.66 north; 594831.95 east; 36.69 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:17:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863032.12 2024-11-06T03:17:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:15Z- waiting for 1730863036.12, time is 1730863035.77 2024-11-06T03:17:16Z- AHRS_M2 msg: 1730863036.15 epoch sec; seqNo:3904 2024-11-06T03:17:16Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.44 psi 2024-11-06T03:17:16Z- handleDepth()- Depth_Keller msg: 1730863036.15 epoch sec; seqNo:4149; depth 50.40 2024-11-06T03:17:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863036.16 epoch sec; seqNo:4149; 4061483.28 north; 594831.96 east; 36.69 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:17:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863032.12 2024-11-06T03:17:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:16Z- waiting for 1730863036.12, time is 1730863036.16 2024-11-06T03:17:16Z- LcmTrn::updateTrn() >>>> pose time 1730863036.16, meas time 1730863034.96 <<<< 2024-11-06T03:17:16Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863036.16 2024-11-06T03:17:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863034.96 sec, ping # 0; waiting for INS data more recent than 1730863032.12... 2024-11-06T03:17:16Z- TerrainNav::motionUpdate() - currEstimate = (4061483, 594831.96, 50.40) 2024-11-06T03:17:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:17:16Z- TerrainNav:: SubcloudNIS is 0.70, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:17:16Z- TNavPF::Projecting Measurements 2024-11-06T03:17:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:17:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:17:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:17:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:17:16Z- TNavFilter::4 good beams. 2024-11-06T03:17:16Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:17:16Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:17:16Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:17:16Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:17:16Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:17:16Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:17:16Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:17:16Z- TerrainMapOctree: (vehN, vehE ) = (4061484.30, 594832.31). 2024-11-06T03:17:16Z- TerrainMapOctree: Min Distance = 248.33. 2024-11-06T03:17:16Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:17:16Z- Forcing Subcloud Comparison 2024-11-06T03:17:16Z- Weighting particles with subcloud comparison 2024-11-06T03:17:16Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:17:16Z- meanExpectedMeasDiff: -4.631083 delta_rms_squared: 1.991337 alpha: 0.990056 2024-11-06T03:17:16Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:17:16Z- meanExpectedMeasDiff: 3.681018 delta_rms_squared: 1.019029 alpha: 0.980751 2024-11-06T03:17:16Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:17:16Z- meanExpectedMeasDiff: 0.005471 delta_rms_squared: 1.682798 alpha: 0.988255 2024-11-06T03:17:16Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:17:16Z- meanExpectedMeasDiff: 0.039210 delta_rms_squared: 1.942778 alpha: 0.989810 2024-11-06T03:17:16Z- Subcloud NIS: 0.728472 2024-11-06T03:17:16Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:17:16Z- TNavPF:: sumWeights = 0.016409 2024-11-06T03:17:16Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:17:16Z- TNavPF::First Term of Map Covariance Matrix is 0.31 2024-11-06T03:17:16Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:17:16Z- TerrainNav::Resampling particle filter... 2024-11-06T03:17:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863034.96 sec. 2024-11-06T03:17:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:17:16Z- variances([0], [2], and psi_berg) = 0.255872 0.163412 0.000000 2024-11-06T03:17:16Z- publishEstimatesDecoder() - at 1730863036.16 sec 2024-11-06T03:17:16Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:17:16Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061514.14 594814.66 54.78 2024-11-06T03:17:16Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061513.55 594814.93 55.04 lat/lon: 36.69 -121.94 2024-11-06T03:17:16Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.16 0.80 0.00 2024-11-06T03:17:16Z- AHRS_M2 msg: 1730863036.55 epoch sec; seqNo:3905 2024-11-06T03:17:16Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.44 psi 2024-11-06T03:17:16Z- handleDepth()- Depth_Keller msg: 1730863036.55 epoch sec; seqNo:4150; depth 50.39 2024-11-06T03:17:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863036.56 epoch sec; seqNo:4150; 4061482.90 north; 594831.96 east; 36.69 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:17:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863036.16 2024-11-06T03:17:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:16Z- waiting for 1730863040.16, time is 1730863036.56 2024-11-06T03:17:16Z- AHRS_M2 msg: 1730863036.95 epoch sec; seqNo:3906 2024-11-06T03:17:16Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.44 psi 2024-11-06T03:17:16Z- handleDepth()- Depth_Keller msg: 1730863036.96 epoch sec; seqNo:4151; depth 50.40 2024-11-06T03:17:16Z- handleDVL() - DVL msg epochMillisec: 1730863036.99 2024-11-06T03:17:16Z- handleDvl() - RDI_Pathfinder msg: 1730863036.99 sec; seqNo:1380; #beams:4 2024-11-06T03:17:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.35 , 37.38 , 42.42 , 40.74 2024-11-06T03:17:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , 0.05 2024-11-06T03:17:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863037.00 epoch sec; seqNo:4151; 4061482.51 north; 594830.75 east; 36.69 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:17:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863036.16 2024-11-06T03:17:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:17Z- waiting for 1730863040.16, time is 1730863037.00 2024-11-06T03:17:17Z- AHRS_M2 msg: 1730863037.36 epoch sec; seqNo:3907 2024-11-06T03:17:17Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.44 psi 2024-11-06T03:17:17Z- handleDepth()- Depth_Keller msg: 1730863037.37 epoch sec; seqNo:4152; depth 50.40 2024-11-06T03:17:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863037.39 epoch sec; seqNo:4152; 4061482.13 north; 594830.75 east; 36.69 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:17:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863036.16 2024-11-06T03:17:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:17Z- waiting for 1730863040.16, time is 1730863037.39 2024-11-06T03:17:17Z- AHRS_M2 msg: 1730863037.76 epoch sec; seqNo:3908 2024-11-06T03:17:17Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.44 psi 2024-11-06T03:17:17Z- handleDepth()- Depth_Keller msg: 1730863037.76 epoch sec; seqNo:4153; depth 50.40 2024-11-06T03:17:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863037.77 epoch sec; seqNo:4153; 4061481.75 north; 594830.75 east; 36.69 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:17:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863036.16 2024-11-06T03:17:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:17Z- waiting for 1730863040.16, time is 1730863037.77 2024-11-06T03:17:18Z- AHRS_M2 msg: 1730863038.16 epoch sec; seqNo:3909 2024-11-06T03:17:18Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.44 psi 2024-11-06T03:17:18Z- handleDepth()- Depth_Keller msg: 1730863038.17 epoch sec; seqNo:4154; depth 50.39 2024-11-06T03:17:18Z- handleDVL() - DVL msg epochMillisec: 1730863038.17 2024-11-06T03:17:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:18Z- handleDvl() - RDI_Pathfinder msg: 1730863038.17 sec; seqNo:1381; #beams:0 2024-11-06T03:17:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.03 , 0.05 2024-11-06T03:17:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863038.18 epoch sec; seqNo:4154; 4061481.37 north; 594830.76 east; 36.69 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:17:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863036.16 2024-11-06T03:17:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:18Z- waiting for 1730863040.16, time is 1730863038.18 2024-11-06T03:17:18Z- AHRS_M2 msg: 1730863038.57 epoch sec; seqNo:3910 2024-11-06T03:17:18Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.44 psi 2024-11-06T03:17:18Z- handleDepth()- Depth_Keller msg: 1730863038.57 epoch sec; seqNo:4155; depth 50.40 2024-11-06T03:17:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863038.59 epoch sec; seqNo:4155; 4061481.37 north; 594830.76 east; 36.69 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:17:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863036.16 2024-11-06T03:17:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:18Z- waiting for 1730863040.16, time is 1730863038.59 2024-11-06T03:17:18Z- AHRS_M2 msg: 1730863038.97 epoch sec; seqNo:3911 2024-11-06T03:17:18Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.44 psi 2024-11-06T03:17:18Z- handleDepth()- Depth_Keller msg: 1730863038.97 epoch sec; seqNo:4156; depth 50.38 2024-11-06T03:17:19Z- handleDVL() - DVL msg epochMillisec: 1730863039.01 2024-11-06T03:17:19Z- handleDvl() - RDI_Pathfinder msg: 1730863039.01 sec; seqNo:1382; #beams:4 2024-11-06T03:17:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.48 , 37.51 , 42.61 , 40.57 2024-11-06T03:17:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.03 2024-11-06T03:17:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863039.02 epoch sec; seqNo:4156; 4061480.99 north; 594830.76 east; 36.69 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:17:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863036.16 2024-11-06T03:17:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:19Z- waiting for 1730863040.16, time is 1730863039.02 2024-11-06T03:17:19Z- AHRS_M2 msg: 1730863039.37 epoch sec; seqNo:3912 2024-11-06T03:17:19Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.44 psi 2024-11-06T03:17:19Z- handleDepth()- Depth_Keller msg: 1730863039.39 epoch sec; seqNo:4157; depth 50.39 2024-11-06T03:17:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863039.40 epoch sec; seqNo:4157; 4061480.61 north; 594830.77 east; 36.69 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:17:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863036.16 2024-11-06T03:17:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:19Z- waiting for 1730863040.16, time is 1730863039.40 2024-11-06T03:17:19Z- AHRS_M2 msg: 1730863039.78 epoch sec; seqNo:3913 2024-11-06T03:17:19Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.44 psi 2024-11-06T03:17:19Z- handleDepth()- Depth_Keller msg: 1730863039.79 epoch sec; seqNo:4158; depth 50.38 2024-11-06T03:17:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863039.80 epoch sec; seqNo:4158; 4061480.24 north; 594830.77 east; 36.69 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:17:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863036.16 2024-11-06T03:17:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:19Z- waiting for 1730863040.16, time is 1730863039.80 2024-11-06T03:17:20Z- AHRS_M2 msg: 1730863040.18 epoch sec; seqNo:3914 2024-11-06T03:17:20Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.44 psi 2024-11-06T03:17:20Z- handleDepth()- Depth_Keller msg: 1730863040.19 epoch sec; seqNo:4159; depth 50.36 2024-11-06T03:17:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863040.21 epoch sec; seqNo:4159; 4061479.86 north; 594830.77 east; 36.69 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:17:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863036.16 2024-11-06T03:17:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:20Z- waiting for 1730863040.16, time is 1730863040.21 2024-11-06T03:17:20Z- LcmTrn::updateTrn() >>>> pose time 1730863040.21, meas time 1730863039.01 <<<< 2024-11-06T03:17:20Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863040.21 2024-11-06T03:17:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863039.01 sec, ping # 0; waiting for INS data more recent than 1730863036.16... 2024-11-06T03:17:20Z- TerrainNav::motionUpdate() - currEstimate = (4061480, 594830.77, 50.36) 2024-11-06T03:17:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:17:20Z- TerrainNav:: SubcloudNIS is 0.73, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:17:20Z- TNavPF::Projecting Measurements 2024-11-06T03:17:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:17:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:17:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:17:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:17:20Z- TNavFilter::4 good beams. 2024-11-06T03:17:20Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:17:20Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:17:20Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:17:20Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:17:20Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:17:20Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:17:20Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:17:20Z- TerrainMapOctree: (vehN, vehE ) = (4061480.88, 594831.13). 2024-11-06T03:17:20Z- TerrainMapOctree: Min Distance = 244.71. 2024-11-06T03:17:20Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:17:20Z- Forcing Subcloud Comparison 2024-11-06T03:17:20Z- Weighting particles with subcloud comparison 2024-11-06T03:17:20Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:17:20Z- meanExpectedMeasDiff: -3.983344 delta_rms_squared: 1.661473 alpha: 0.988106 2024-11-06T03:17:20Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:17:20Z- meanExpectedMeasDiff: 3.438282 delta_rms_squared: 0.984321 alpha: 0.980086 2024-11-06T03:17:20Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:17:20Z- meanExpectedMeasDiff: 0.581155 delta_rms_squared: 1.423311 alpha: 0.986143 2024-11-06T03:17:20Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:17:20Z- meanExpectedMeasDiff: 0.449616 delta_rms_squared: 1.457093 alpha: 0.986460 2024-11-06T03:17:20Z- Subcloud NIS: 0.745953 2024-11-06T03:17:20Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:17:20Z- TNavPF:: sumWeights = 0.034206 2024-11-06T03:17:20Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:17:20Z- TNavPF::First Term of Map Covariance Matrix is 0.55 2024-11-06T03:17:20Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:17:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863039.01 sec. 2024-11-06T03:17:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:17:20Z- variances([0], [2], and psi_berg) = 0.243364 0.152830 0.000000 2024-11-06T03:17:20Z- publishEstimatesDecoder() - at 1730863040.21 sec 2024-11-06T03:17:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:17:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061509.94 594813.79 54.89 2024-11-06T03:17:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061510.08 594813.78 54.95 lat/lon: 36.69 -121.94 2024-11-06T03:17:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.15 0.71 0.00 2024-11-06T03:17:20Z- AHRS_M2 msg: 1730863040.59 epoch sec; seqNo:3915 2024-11-06T03:17:20Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.44 psi 2024-11-06T03:17:20Z- handleDepth()- Depth_Keller msg: 1730863040.60 epoch sec; seqNo:4160; depth 50.35 2024-11-06T03:17:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863040.62 epoch sec; seqNo:4160; 4061479.48 north; 594830.78 east; 36.69 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:17:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863040.21 2024-11-06T03:17:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:20Z- waiting for 1730863044.21, time is 1730863040.62 2024-11-06T03:17:20Z- AHRS_M2 msg: 1730863040.99 epoch sec; seqNo:3916 2024-11-06T03:17:20Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.44 psi 2024-11-06T03:17:20Z- handleDepth()- Depth_Keller msg: 1730863040.99 epoch sec; seqNo:4161; depth 50.35 2024-11-06T03:17:21Z- handleDVL() - DVL msg epochMillisec: 1730863041.02 2024-11-06T03:17:21Z- handleDvl() - RDI_Pathfinder msg: 1730863041.02 sec; seqNo:1383; #beams:4 2024-11-06T03:17:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 37.72 , 42.61 , 40.98 2024-11-06T03:17:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , -0.03 2024-11-06T03:17:21Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:17:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863040.21 2024-11-06T03:17:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:21Z- waiting for 1730863044.21, time is 1730863041.02 2024-11-06T03:17:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863041.04 epoch sec; seqNo:4161; 4061479.10 north; 594830.78 east; 36.69 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:17:21Z- 1 msgs in 200 ms and 2 handle calls 2024-11-06T03:17:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863040.21 2024-11-06T03:17:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:21Z- waiting for 1730863044.21, time is 1730863041.04 2024-11-06T03:17:21Z- AHRS_M2 msg: 1730863041.39 epoch sec; seqNo:3917 2024-11-06T03:17:21Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.44 psi 2024-11-06T03:17:21Z- handleDepth()- Depth_Keller msg: 1730863041.40 epoch sec; seqNo:4162; depth 50.35 2024-11-06T03:17:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863041.42 epoch sec; seqNo:4162; 4061478.72 north; 594830.79 east; 36.69 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:17:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863040.21 2024-11-06T03:17:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:21Z- waiting for 1730863044.21, time is 1730863041.42 2024-11-06T03:17:21Z- AHRS_M2 msg: 1730863041.80 epoch sec; seqNo:3918 2024-11-06T03:17:21Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.44 psi 2024-11-06T03:17:21Z- handleDepth()- Depth_Keller msg: 1730863041.81 epoch sec; seqNo:4163; depth 50.31 2024-11-06T03:17:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863041.81 epoch sec; seqNo:4163; 4061478.33 north; 594829.57 east; 36.69 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:17:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863040.21 2024-11-06T03:17:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:22Z- waiting for 1730863044.21, time is 1730863041.81 2024-11-06T03:17:22Z- AHRS_M2 msg: 1730863042.20 epoch sec; seqNo:3919 2024-11-06T03:17:22Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.44 psi 2024-11-06T03:17:22Z- handleDepth()- Depth_Keller msg: 1730863042.22 epoch sec; seqNo:4164; depth 50.32 2024-11-06T03:17:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863042.23 epoch sec; seqNo:4164; 4061477.95 north; 594829.58 east; 36.69 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:17:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863040.21 2024-11-06T03:17:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:22Z- waiting for 1730863044.21, time is 1730863042.23 2024-11-06T03:17:22Z- AHRS_M2 msg: 1730863042.61 epoch sec; seqNo:3920 2024-11-06T03:17:22Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.44 psi 2024-11-06T03:17:22Z- handleDepth()- Depth_Keller msg: 1730863042.61 epoch sec; seqNo:4165; depth 50.29 2024-11-06T03:17:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863042.63 epoch sec; seqNo:4165; 4061477.57 north; 594829.58 east; 36.69 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:17:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863040.21 2024-11-06T03:17:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:22Z- waiting for 1730863044.21, time is 1730863042.63 2024-11-06T03:17:23Z- AHRS_M2 msg: 1730863043.02 epoch sec; seqNo:3921 2024-11-06T03:17:23Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.44 psi 2024-11-06T03:17:23Z- handleDepth()- Depth_Keller msg: 1730863043.03 epoch sec; seqNo:4166; depth 50.28 2024-11-06T03:17:23Z- handleDVL() - DVL msg epochMillisec: 1730863043.05 2024-11-06T03:17:23Z- handleDvl() - RDI_Pathfinder msg: 1730863043.05 sec; seqNo:1384; #beams:4 2024-11-06T03:17:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 37.51 , 42.61 , 40.77 2024-11-06T03:17:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.04 , -0.07 2024-11-06T03:17:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863043.07 epoch sec; seqNo:4166; 4061477.19 north; 594829.58 east; 36.69 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:17:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863040.21 2024-11-06T03:17:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:23Z- waiting for 1730863044.21, time is 1730863043.07 2024-11-06T03:17:23Z- AHRS_M2 msg: 1730863043.42 epoch sec; seqNo:3922 2024-11-06T03:17:23Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.43 psi 2024-11-06T03:17:23Z- handleDepth()- Depth_Keller msg: 1730863043.44 epoch sec; seqNo:4167; depth 50.28 2024-11-06T03:17:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863043.45 epoch sec; seqNo:4167; 4061477.19 north; 594829.58 east; 36.69 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:17:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863040.21 2024-11-06T03:17:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:23Z- waiting for 1730863044.21, time is 1730863043.45 2024-11-06T03:17:23Z- AHRS_M2 msg: 1730863043.82 epoch sec; seqNo:3923 2024-11-06T03:17:23Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.43 psi 2024-11-06T03:17:23Z- handleDepth()- Depth_Keller msg: 1730863043.83 epoch sec; seqNo:4168; depth 50.26 2024-11-06T03:17:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863043.83 epoch sec; seqNo:4168; 4061476.81 north; 594829.59 east; 36.69 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:17:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863040.21 2024-11-06T03:17:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:23Z- waiting for 1730863044.21, time is 1730863043.83 2024-11-06T03:17:24Z- AHRS_M2 msg: 1730863044.22 epoch sec; seqNo:3924 2024-11-06T03:17:24Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.43 psi 2024-11-06T03:17:24Z- handleDepth()- Depth_Keller msg: 1730863044.23 epoch sec; seqNo:4169; depth 50.26 2024-11-06T03:17:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863044.24 epoch sec; seqNo:4169; 4061476.43 north; 594829.59 east; 36.69 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:17:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863040.21 2024-11-06T03:17:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:24Z- waiting for 1730863044.21, time is 1730863044.24 2024-11-06T03:17:24Z- LcmTrn::updateTrn() >>>> pose time 1730863044.24, meas time 1730863043.05 <<<< 2024-11-06T03:17:24Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863044.24 2024-11-06T03:17:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863043.05 sec, ping # 0; waiting for INS data more recent than 1730863040.21... 2024-11-06T03:17:24Z- TerrainNav::motionUpdate() - currEstimate = (4061476, 594829.59, 50.26) 2024-11-06T03:17:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:17:24Z- TerrainNav:: SubcloudNIS is 0.75, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:17:24Z- TNavPF::Projecting Measurements 2024-11-06T03:17:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:17:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:17:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:17:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:17:24Z- TNavFilter::4 good beams. 2024-11-06T03:17:24Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:17:24Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:17:24Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:17:24Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:17:24Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:17:24Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:17:24Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:17:24Z- TerrainMapOctree: (vehN, vehE ) = (4061477.45, 594829.94). 2024-11-06T03:17:24Z- TerrainMapOctree: Min Distance = 241.09. 2024-11-06T03:17:24Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:17:24Z- Forcing Subcloud Comparison 2024-11-06T03:17:24Z- Weighting particles with subcloud comparison 2024-11-06T03:17:24Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:17:24Z- meanExpectedMeasDiff: -3.713207 delta_rms_squared: 1.564471 alpha: 0.987377 2024-11-06T03:17:24Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:17:24Z- meanExpectedMeasDiff: 3.174639 delta_rms_squared: 1.074383 alpha: 0.981725 2024-11-06T03:17:24Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:17:24Z- meanExpectedMeasDiff: 0.392637 delta_rms_squared: 1.387920 alpha: 0.985795 2024-11-06T03:17:24Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:17:24Z- meanExpectedMeasDiff: 0.496745 delta_rms_squared: 1.293386 alpha: 0.984772 2024-11-06T03:17:24Z- Subcloud NIS: 0.761601 2024-11-06T03:17:24Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:17:24Z- TNavPF:: sumWeights = 0.052272 2024-11-06T03:17:24Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:17:24Z- TNavPF::First Term of Map Covariance Matrix is 0.72 2024-11-06T03:17:24Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T03:17:24Z- TerrainNav::Resampling particle filter... 2024-11-06T03:17:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863043.05 sec. 2024-11-06T03:17:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:17:24Z- variances([0], [2], and psi_berg) = 0.247737 0.151182 0.000000 2024-11-06T03:17:24Z- publishEstimatesDecoder() - at 1730863044.24 sec 2024-11-06T03:17:24Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:17:24Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061507.38 594812.27 55.79 2024-11-06T03:17:24Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061506.66 594812.60 54.79 lat/lon: 36.69 -121.94 2024-11-06T03:17:24Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.15 0.70 0.00 2024-11-06T03:17:24Z- AHRS_M2 msg: 1730863044.63 epoch sec; seqNo:3925 2024-11-06T03:17:24Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.43 psi 2024-11-06T03:17:24Z- handleDepth()- Depth_Keller msg: 1730863044.64 epoch sec; seqNo:4170; depth 50.25 2024-11-06T03:17:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863044.67 epoch sec; seqNo:4170; 4061476.06 north; 594829.60 east; 36.69 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:17:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863044.24 2024-11-06T03:17:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:24Z- waiting for 1730863048.24, time is 1730863044.67 2024-11-06T03:17:25Z- AHRS_M2 msg: 1730863045.03 epoch sec; seqNo:3926 2024-11-06T03:17:25Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.42 psi 2024-11-06T03:17:25Z- handleDepth()- Depth_Keller msg: 1730863045.04 epoch sec; seqNo:4171; depth 50.25 2024-11-06T03:17:25Z- handleDVL() - DVL msg epochMillisec: 1730863045.06 2024-11-06T03:17:25Z- handleDvl() - RDI_Pathfinder msg: 1730863045.06 sec; seqNo:1385; #beams:4 2024-11-06T03:17:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.48 , 37.51 , 42.40 , 40.57 2024-11-06T03:17:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.10 , -0.03 2024-11-06T03:17:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863045.07 epoch sec; seqNo:4171; 4061475.68 north; 594829.60 east; 36.69 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:17:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863044.24 2024-11-06T03:17:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:25Z- waiting for 1730863048.24, time is 1730863045.07 2024-11-06T03:17:25Z- AHRS_M2 msg: 1730863045.43 epoch sec; seqNo:3927 2024-11-06T03:17:25Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.42 psi 2024-11-06T03:17:25Z- handleDepth()- Depth_Keller msg: 1730863045.45 epoch sec; seqNo:4172; depth 50.23 2024-11-06T03:17:25Z- handleDVL() - DVL msg epochMillisec: 1730863045.45 2024-11-06T03:17:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:25Z- handleDvl() - RDI_Pathfinder msg: 1730863045.45 sec; seqNo:1386; #beams:0 2024-11-06T03:17:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.10 , -0.03 2024-11-06T03:17:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863045.47 epoch sec; seqNo:4172; 4061475.30 north; 594829.60 east; 36.69 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:17:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863044.24 2024-11-06T03:17:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:25Z- waiting for 1730863048.24, time is 1730863045.47 2024-11-06T03:17:25Z- AHRS_M2 msg: 1730863045.84 epoch sec; seqNo:3928 2024-11-06T03:17:25Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.42 psi 2024-11-06T03:17:25Z- handleDepth()- Depth_Keller msg: 1730863045.85 epoch sec; seqNo:4173; depth 50.22 2024-11-06T03:17:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863045.86 epoch sec; seqNo:4173; 4061474.91 north; 594828.39 east; 36.69 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:17:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863044.24 2024-11-06T03:17:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:25Z- waiting for 1730863048.24, time is 1730863045.86 2024-11-06T03:17:26Z- AHRS_M2 msg: 1730863046.24 epoch sec; seqNo:3929 2024-11-06T03:17:26Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.42 psi 2024-11-06T03:17:26Z- handleDepth()- Depth_Keller msg: 1730863046.25 epoch sec; seqNo:4174; depth 50.23 2024-11-06T03:17:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863046.26 epoch sec; seqNo:4174; 4061474.53 north; 594828.39 east; 36.69 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:17:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863044.24 2024-11-06T03:17:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:26Z- waiting for 1730863048.24, time is 1730863046.26 2024-11-06T03:17:26Z- AHRS_M2 msg: 1730863046.65 epoch sec; seqNo:3930 2024-11-06T03:17:26Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.42 psi 2024-11-06T03:17:26Z- handleDepth()- Depth_Keller msg: 1730863046.66 epoch sec; seqNo:4175; depth 50.24 2024-11-06T03:17:26Z- handleDVL() - DVL msg epochMillisec: 1730863046.66 2024-11-06T03:17:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:26Z- handleDvl() - RDI_Pathfinder msg: 1730863046.66 sec; seqNo:1387; #beams:0 2024-11-06T03:17:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.10 , -0.03 2024-11-06T03:17:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863046.68 epoch sec; seqNo:4175; 4061474.15 north; 594828.40 east; 36.69 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:17:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863044.24 2024-11-06T03:17:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:26Z- waiting for 1730863048.24, time is 1730863046.68 2024-11-06T03:17:27Z- AHRS_M2 msg: 1730863047.05 epoch sec; seqNo:3931 2024-11-06T03:17:27Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.42 psi 2024-11-06T03:17:27Z- handleDepth()- Depth_Keller msg: 1730863047.06 epoch sec; seqNo:4176; depth 50.23 2024-11-06T03:17:27Z- handleDVL() - DVL msg epochMillisec: 1730863047.08 2024-11-06T03:17:27Z- handleDvl() - RDI_Pathfinder msg: 1730863047.08 sec; seqNo:1388; #beams:4 2024-11-06T03:17:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.69 , 37.72 , 42.61 , 40.57 2024-11-06T03:17:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.10 , 0.05 2024-11-06T03:17:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863047.10 epoch sec; seqNo:4176; 4061473.77 north; 594828.40 east; 36.69 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:17:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863044.24 2024-11-06T03:17:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:27Z- waiting for 1730863048.24, time is 1730863047.10 2024-11-06T03:17:27Z- AHRS_M2 msg: 1730863047.46 epoch sec; seqNo:3932 2024-11-06T03:17:27Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.41 psi 2024-11-06T03:17:27Z- handleDepth()- Depth_Keller msg: 1730863047.46 epoch sec; seqNo:4177; depth 50.21 2024-11-06T03:17:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863047.48 epoch sec; seqNo:4177; 4061473.39 north; 594828.41 east; 36.69 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:17:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863044.24 2024-11-06T03:17:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:27Z- waiting for 1730863048.24, time is 1730863047.48 2024-11-06T03:17:27Z- AHRS_M2 msg: 1730863047.86 epoch sec; seqNo:3933 2024-11-06T03:17:27Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.41 psi 2024-11-06T03:17:27Z- handleDepth()- Depth_Keller msg: 1730863047.87 epoch sec; seqNo:4178; depth 50.20 2024-11-06T03:17:27Z- handleDVL() - DVL msg epochMillisec: 1730863047.87 2024-11-06T03:17:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:27Z- handleDvl() - RDI_Pathfinder msg: 1730863047.87 sec; seqNo:1389; #beams:0 2024-11-06T03:17:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.10 , 0.05 2024-11-06T03:17:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863047.88 epoch sec; seqNo:4178; 4061473.39 north; 594828.41 east; 36.69 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:17:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863044.24 2024-11-06T03:17:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:28Z- waiting for 1730863048.24, time is 1730863047.88 2024-11-06T03:17:28Z- AHRS_M2 msg: 1730863048.27 epoch sec; seqNo:3934 2024-11-06T03:17:28Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.41 psi 2024-11-06T03:17:28Z- handleDepth()- Depth_Keller msg: 1730863048.27 epoch sec; seqNo:4179; depth 50.20 2024-11-06T03:17:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863048.29 epoch sec; seqNo:4179; 4061473.01 north; 594828.41 east; 36.69 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:17:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863044.24 2024-11-06T03:17:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:28Z- waiting for 1730863048.24, time is 1730863048.29 2024-11-06T03:17:28Z- LcmTrn::updateTrn() >>>> pose time 1730863048.29, meas time 1730863047.87 <<<< 2024-11-06T03:17:28Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863048.29 2024-11-06T03:17:28Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:17:28Z- TerrainNav::motionUpdate() - currEstimate = (4061473, 594828.41, 50.20) 2024-11-06T03:17:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:17:28Z- TerrainNav:: SubcloudNIS is 0.76, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:17:28Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:17:28Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:17:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:17:28Z- variances([0], [2], and psi_berg) = 0.249914 0.153529 0.000000 2024-11-06T03:17:28Z- publishEstimatesDecoder() - at 1730863048.29 sec 2024-11-06T03:17:28Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:17:28Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061503.95 594811.11 55.83 2024-11-06T03:17:28Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061503.24 594811.42 54.73 lat/lon: 36.69 -121.94 2024-11-06T03:17:28Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.15 0.94 0.00 2024-11-06T03:17:28Z- AHRS_M2 msg: 1730863048.67 epoch sec; seqNo:3935 2024-11-06T03:17:28Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.41 psi 2024-11-06T03:17:28Z- handleDepth()- Depth_Keller msg: 1730863048.68 epoch sec; seqNo:4180; depth 50.20 2024-11-06T03:17:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863048.69 epoch sec; seqNo:4180; 4061472.63 north; 594828.41 east; 36.69 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:17:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863048.29 2024-11-06T03:17:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:28Z- waiting for 1730863052.29, time is 1730863048.69 2024-11-06T03:17:29Z- AHRS_M2 msg: 1730863049.07 epoch sec; seqNo:3936 2024-11-06T03:17:29Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.41 psi 2024-11-06T03:17:29Z- handleDepth()- Depth_Keller msg: 1730863049.08 epoch sec; seqNo:4181; depth 50.20 2024-11-06T03:17:29Z- handleDVL() - DVL msg epochMillisec: 1730863049.10 2024-11-06T03:17:29Z- handleDvl() - RDI_Pathfinder msg: 1730863049.10 sec; seqNo:1390; #beams:4 2024-11-06T03:17:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.48 , 37.72 , 42.61 , 40.57 2024-11-06T03:17:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , 0.06 2024-11-06T03:17:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863049.11 epoch sec; seqNo:4181; 4061472.25 north; 594828.42 east; 36.69 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:17:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863048.29 2024-11-06T03:17:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:29Z- waiting for 1730863052.29, time is 1730863049.11 2024-11-06T03:17:29Z- AHRS_M2 msg: 1730863049.47 epoch sec; seqNo:3937 2024-11-06T03:17:29Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.41 psi 2024-11-06T03:17:29Z- handleDepth()- Depth_Keller msg: 1730863049.48 epoch sec; seqNo:4182; depth 50.20 2024-11-06T03:17:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863049.50 epoch sec; seqNo:4182; 4061471.86 north; 594827.20 east; 36.69 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:17:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863048.29 2024-11-06T03:17:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:29Z- waiting for 1730863052.29, time is 1730863049.50 2024-11-06T03:17:29Z- AHRS_M2 msg: 1730863049.88 epoch sec; seqNo:3938 2024-11-06T03:17:29Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.41 psi 2024-11-06T03:17:29Z- handleDepth()- Depth_Keller msg: 1730863049.88 epoch sec; seqNo:4183; depth 50.20 2024-11-06T03:17:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863049.89 epoch sec; seqNo:4183; 4061471.48 north; 594827.21 east; 36.69 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:17:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863048.29 2024-11-06T03:17:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:30Z- waiting for 1730863052.29, time is 1730863049.89 2024-11-06T03:17:30Z- AHRS_M2 msg: 1730863050.28 epoch sec; seqNo:3939 2024-11-06T03:17:30Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.41 psi 2024-11-06T03:17:30Z- handleDepth()- Depth_Keller msg: 1730863050.29 epoch sec; seqNo:4184; depth 50.21 2024-11-06T03:17:30Z- handleDVL() - DVL msg epochMillisec: 1730863050.30 2024-11-06T03:17:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:30Z- handleDvl() - RDI_Pathfinder msg: 1730863050.30 sec; seqNo:1391; #beams:0 2024-11-06T03:17:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , 0.06 2024-11-06T03:17:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863050.31 epoch sec; seqNo:4184; 4061471.10 north; 594827.21 east; 36.69 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:17:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863048.29 2024-11-06T03:17:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:30Z- waiting for 1730863052.29, time is 1730863050.31 2024-11-06T03:17:30Z- AHRS_M2 msg: 1730863050.69 epoch sec; seqNo:3940 2024-11-06T03:17:30Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.41 psi 2024-11-06T03:17:30Z- handleDepth()- Depth_Keller msg: 1730863050.71 epoch sec; seqNo:4185; depth 50.19 2024-11-06T03:17:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863050.76 epoch sec; seqNo:4185; 4061470.72 north; 594827.21 east; 36.69 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:17:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863048.29 2024-11-06T03:17:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:30Z- waiting for 1730863052.29, time is 1730863050.76 2024-11-06T03:17:31Z- AHRS_M2 msg: 1730863051.09 epoch sec; seqNo:3941 2024-11-06T03:17:31Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.41 psi 2024-11-06T03:17:31Z- handleDepth()- Depth_Keller msg: 1730863051.10 epoch sec; seqNo:4186; depth 50.18 2024-11-06T03:17:31Z- handleDVL() - DVL msg epochMillisec: 1730863051.12 2024-11-06T03:17:31Z- handleDvl() - RDI_Pathfinder msg: 1730863051.12 sec; seqNo:1392; #beams:4 2024-11-06T03:17:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 37.92 , 43.02 , 40.77 2024-11-06T03:17:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , 0.01 2024-11-06T03:17:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863051.13 epoch sec; seqNo:4186; 4061470.35 north; 594827.22 east; 36.69 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:17:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863048.29 2024-11-06T03:17:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:31Z- waiting for 1730863052.29, time is 1730863051.13 2024-11-06T03:17:31Z- AHRS_M2 msg: 1730863051.49 epoch sec; seqNo:3942 2024-11-06T03:17:31Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.41 psi 2024-11-06T03:17:31Z- handleDepth()- Depth_Keller msg: 1730863051.50 epoch sec; seqNo:4187; depth 50.17 2024-11-06T03:17:31Z- handleDVL() - DVL msg epochMillisec: 1730863051.51 2024-11-06T03:17:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:31Z- handleDvl() - RDI_Pathfinder msg: 1730863051.51 sec; seqNo:1393; #beams:0 2024-11-06T03:17:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , 0.01 2024-11-06T03:17:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863051.53 epoch sec; seqNo:4187; 4061469.97 north; 594827.22 east; 36.69 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:17:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863048.29 2024-11-06T03:17:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:31Z- waiting for 1730863052.29, time is 1730863051.53 2024-11-06T03:17:31Z- AHRS_M2 msg: 1730863051.90 epoch sec; seqNo:3943 2024-11-06T03:17:31Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.41 psi 2024-11-06T03:17:31Z- handleDepth()- Depth_Keller msg: 1730863051.90 epoch sec; seqNo:4188; depth 50.17 2024-11-06T03:17:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863051.91 epoch sec; seqNo:4188; 4061469.59 north; 594827.23 east; 36.69 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:17:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863048.29 2024-11-06T03:17:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:32Z- waiting for 1730863052.29, time is 1730863051.91 2024-11-06T03:17:32Z- AHRS_M2 msg: 1730863052.31 epoch sec; seqNo:3944 2024-11-06T03:17:32Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.41 psi 2024-11-06T03:17:32Z- handleDepth()- Depth_Keller msg: 1730863052.32 epoch sec; seqNo:4189; depth 50.16 2024-11-06T03:17:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863052.33 epoch sec; seqNo:4189; 4061469.21 north; 594827.23 east; 36.69 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:17:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863048.29 2024-11-06T03:17:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:32Z- waiting for 1730863052.29, time is 1730863052.33 2024-11-06T03:17:32Z- LcmTrn::updateTrn() >>>> pose time 1730863052.33, meas time 1730863051.51 <<<< 2024-11-06T03:17:32Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863052.33 2024-11-06T03:17:32Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:17:32Z- TerrainNav::motionUpdate() - currEstimate = (4061469, 594827.23, 50.16) 2024-11-06T03:17:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:17:32Z- TerrainNav:: SubcloudNIS is 0.76, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:17:32Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:17:32Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:17:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:17:32Z- variances([0], [2], and psi_berg) = 0.252026 0.156727 0.000000 2024-11-06T03:17:32Z- publishEstimatesDecoder() - at 1730863052.33 sec 2024-11-06T03:17:32Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:17:32Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061500.21 594809.86 55.74 2024-11-06T03:17:32Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061499.44 594810.24 54.68 lat/lon: 36.69 -121.94 2024-11-06T03:17:32Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.16 1.19 0.00 2024-11-06T03:17:32Z- AHRS_M2 msg: 1730863052.74 epoch sec; seqNo:3945 2024-11-06T03:17:32Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.41 psi 2024-11-06T03:17:32Z- handleDepth()- Depth_Keller msg: 1730863052.76 epoch sec; seqNo:4190; depth 50.15 2024-11-06T03:17:32Z- handleDVL() - DVL msg epochMillisec: 1730863052.79 2024-11-06T03:17:32Z- handleDvl() - RDI_Pathfinder msg: 1730863052.79 sec; seqNo:1394; #beams:4 2024-11-06T03:17:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 38.12 , 43.02 , 40.77 2024-11-06T03:17:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.04 2024-11-06T03:17:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863052.80 epoch sec; seqNo:4190; 4061468.83 north; 594827.24 east; 36.69 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:17:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863052.33 2024-11-06T03:17:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:32Z- waiting for 1730863056.33, time is 1730863052.80 2024-11-06T03:17:33Z- AHRS_M2 msg: 1730863053.11 epoch sec; seqNo:3946 2024-11-06T03:17:33Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.41 psi 2024-11-06T03:17:33Z- handleDepth()- Depth_Keller msg: 1730863053.12 epoch sec; seqNo:4191; depth 50.13 2024-11-06T03:17:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863053.15 epoch sec; seqNo:4191; 4061468.45 north; 594827.24 east; 36.69 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:17:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863052.33 2024-11-06T03:17:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:33Z- waiting for 1730863056.33, time is 1730863053.15 2024-11-06T03:17:33Z- AHRS_M2 msg: 1730863053.52 epoch sec; seqNo:3947 2024-11-06T03:17:33Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.41 psi 2024-11-06T03:17:33Z- handleDepth()- Depth_Keller msg: 1730863053.53 epoch sec; seqNo:4192; depth 50.12 2024-11-06T03:17:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863053.54 epoch sec; seqNo:4192; 4061468.07 north; 594827.24 east; 36.69 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:17:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863052.33 2024-11-06T03:17:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:33Z- waiting for 1730863056.33, time is 1730863053.54 2024-11-06T03:17:33Z- AHRS_M2 msg: 1730863053.92 epoch sec; seqNo:3948 2024-11-06T03:17:33Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.41 psi 2024-11-06T03:17:33Z- handleDepth()- Depth_Keller msg: 1730863053.92 epoch sec; seqNo:4193; depth 50.12 2024-11-06T03:17:33Z- handleDVL() - DVL msg epochMillisec: 1730863053.93 2024-11-06T03:17:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:33Z- handleDvl() - RDI_Pathfinder msg: 1730863053.93 sec; seqNo:1395; #beams:0 2024-11-06T03:17:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.04 2024-11-06T03:17:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863053.94 epoch sec; seqNo:4193; 4061468.07 north; 594827.24 east; 36.69 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:17:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863052.33 2024-11-06T03:17:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:34Z- waiting for 1730863056.33, time is 1730863053.94 2024-11-06T03:17:34Z- AHRS_M2 msg: 1730863054.32 epoch sec; seqNo:3949 2024-11-06T03:17:34Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.41 psi 2024-11-06T03:17:34Z- handleDepth()- Depth_Keller msg: 1730863054.33 epoch sec; seqNo:4194; depth 50.10 2024-11-06T03:17:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863054.36 epoch sec; seqNo:4194; 4061467.69 north; 594827.25 east; 36.69 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:17:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863052.33 2024-11-06T03:17:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:34Z- waiting for 1730863056.33, time is 1730863054.36 2024-11-06T03:17:34Z- AHRS_M2 msg: 1730863054.76 epoch sec; seqNo:3950 2024-11-06T03:17:34Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.41 psi 2024-11-06T03:17:34Z- handleDepth()- Depth_Keller msg: 1730863054.77 epoch sec; seqNo:4195; depth 50.10 2024-11-06T03:17:34Z- handleDVL() - DVL msg epochMillisec: 1730863054.79 2024-11-06T03:17:34Z- handleDvl() - RDI_Pathfinder msg: 1730863054.79 sec; seqNo:1396; #beams:4 2024-11-06T03:17:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 38.12 , 42.61 , 40.77 2024-11-06T03:17:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , -0.04 2024-11-06T03:17:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863054.82 epoch sec; seqNo:4195; 4061467.30 north; 594826.03 east; 36.69 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:17:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863052.33 2024-11-06T03:17:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:34Z- waiting for 1730863056.33, time is 1730863054.82 2024-11-06T03:17:35Z- AHRS_M2 msg: 1730863055.13 epoch sec; seqNo:3951 2024-11-06T03:17:35Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:35Z- handleDepth()- Depth_Keller msg: 1730863055.13 epoch sec; seqNo:4196; depth 50.07 2024-11-06T03:17:35Z- handleDVL() - DVL msg epochMillisec: 1730863055.14 2024-11-06T03:17:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:35Z- handleDvl() - RDI_Pathfinder msg: 1730863055.14 sec; seqNo:1397; #beams:0 2024-11-06T03:17:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , -0.04 2024-11-06T03:17:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863055.15 epoch sec; seqNo:4196; 4061466.92 north; 594826.04 east; 36.69 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:17:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863052.33 2024-11-06T03:17:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:35Z- waiting for 1730863056.33, time is 1730863055.15 2024-11-06T03:17:35Z- AHRS_M2 msg: 1730863055.54 epoch sec; seqNo:3952 2024-11-06T03:17:35Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:35Z- handleDepth()- Depth_Keller msg: 1730863055.55 epoch sec; seqNo:4197; depth 50.06 2024-11-06T03:17:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863055.56 epoch sec; seqNo:4197; 4061466.54 north; 594826.04 east; 36.69 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:17:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863052.33 2024-11-06T03:17:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:35Z- waiting for 1730863056.33, time is 1730863055.56 2024-11-06T03:17:35Z- AHRS_M2 msg: 1730863055.94 epoch sec; seqNo:3953 2024-11-06T03:17:35Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:35Z- handleDepth()- Depth_Keller msg: 1730863055.95 epoch sec; seqNo:4198; depth 50.05 2024-11-06T03:17:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863055.96 epoch sec; seqNo:4198; 4061466.16 north; 594826.04 east; 36.69 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:17:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863052.33 2024-11-06T03:17:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:36Z- waiting for 1730863056.33, time is 1730863055.96 2024-11-06T03:17:36Z- AHRS_M2 msg: 1730863056.35 epoch sec; seqNo:3954 2024-11-06T03:17:36Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:36Z- handleDepth()- Depth_Keller msg: 1730863056.35 epoch sec; seqNo:4199; depth 50.05 2024-11-06T03:17:36Z- handleDVL() - DVL msg epochMillisec: 1730863056.35 2024-11-06T03:17:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:36Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:36Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:36Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:36Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:36Z- handleDvl() - RDI_Pathfinder msg: 1730863056.35 sec; seqNo:1398; #beams:0 2024-11-06T03:17:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.05 , -0.04 2024-11-06T03:17:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863056.36 epoch sec; seqNo:4199; 4061465.79 north; 594826.05 east; 36.69 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:17:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863052.33 2024-11-06T03:17:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:36Z- waiting for 1730863056.33, time is 1730863056.36 2024-11-06T03:17:36Z- LcmTrn::updateTrn() >>>> pose time 1730863056.36, meas time 1730863056.35 <<<< 2024-11-06T03:17:36Z- LcmTrn::updateTrn() equating pose 1730863056.35 and meas times 1730863056.35 2024-11-06T03:17:36Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863056.35 2024-11-06T03:17:36Z- TerrainNav::motionUpdate() - currEstimate = (4061466, 594826.05, 50.05) 2024-11-06T03:17:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:17:36Z- TerrainNav:: SubcloudNIS is 0.76, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:17:36Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:17:36Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:17:36Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:17:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:17:36Z- variances([0], [2], and psi_berg) = 0.253997 0.158779 0.000000 2024-11-06T03:17:36Z- publishEstimatesDecoder() - at 1730863056.35 sec 2024-11-06T03:17:36Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:17:36Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061496.73 594808.64 55.80 2024-11-06T03:17:36Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061496.02 594809.06 54.58 lat/lon: 36.69 -121.94 2024-11-06T03:17:36Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.16 1.42 0.00 2024-11-06T03:17:36Z- handleDepth()- Depth_Keller msg: 1730863056.79 epoch sec; seqNo:4200; depth 50.03 2024-11-06T03:17:36Z- handleDVL() - DVL msg epochMillisec: 1730863056.81 2024-11-06T03:17:36Z- handleDvl() - RDI_Pathfinder msg: 1730863056.81 sec; seqNo:1399; #beams:4 2024-11-06T03:17:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.09 , 37.92 , 42.81 , 41.18 2024-11-06T03:17:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.07 , -0.01 2024-11-06T03:17:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863056.83 epoch sec; seqNo:4200; 4061465.41 north; 594826.05 east; 36.69 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:17:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863056.35 2024-11-06T03:17:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:36Z- waiting for 1730863060.35, time is 1730863056.83 2024-11-06T03:17:37Z- handleDepth()- Depth_Keller msg: 1730863057.17 epoch sec; seqNo:4201; depth 50.02 2024-11-06T03:17:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863057.19 epoch sec; seqNo:4201; 4061465.03 north; 594826.06 east; 36.69 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:17:37Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:17:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863056.35 2024-11-06T03:17:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:37Z- waiting for 1730863060.35, time is 1730863057.19 2024-11-06T03:17:37Z- AHRS_M2 msg: 1730863057.57 epoch sec; seqNo:3955 2024-11-06T03:17:37Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:37Z- handleDepth()- Depth_Keller msg: 1730863057.58 epoch sec; seqNo:4202; depth 49.99 2024-11-06T03:17:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863057.58 epoch sec; seqNo:4202; 4061464.65 north; 594826.06 east; 36.69 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:17:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863056.35 2024-11-06T03:17:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:37Z- waiting for 1730863060.35, time is 1730863057.58 2024-11-06T03:17:37Z- AHRS_M2 msg: 1730863057.96 epoch sec; seqNo:3956 2024-11-06T03:17:37Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:37Z- handleDepth()- Depth_Keller msg: 1730863057.97 epoch sec; seqNo:4203; depth 50.01 2024-11-06T03:17:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863057.99 epoch sec; seqNo:4203; 4061464.65 north; 594826.06 east; 36.69 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:17:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863056.35 2024-11-06T03:17:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:38Z- waiting for 1730863060.35, time is 1730863057.99 2024-11-06T03:17:38Z- AHRS_M2 msg: 1730863058.36 epoch sec; seqNo:3957 2024-11-06T03:17:38Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:38Z- handleDepth()- Depth_Keller msg: 1730863058.38 epoch sec; seqNo:4204; depth 50.00 2024-11-06T03:17:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863058.38 epoch sec; seqNo:4204; 4061464.27 north; 594826.07 east; 36.69 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:17:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863056.35 2024-11-06T03:17:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:38Z- waiting for 1730863060.35, time is 1730863058.38 2024-11-06T03:17:38Z- AHRS_M2 msg: 1730863058.78 epoch sec; seqNo:3958 2024-11-06T03:17:38Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:38Z- handleDepth()- Depth_Keller msg: 1730863058.79 epoch sec; seqNo:4205; depth 49.99 2024-11-06T03:17:38Z- handleDVL() - DVL msg epochMillisec: 1730863058.82 2024-11-06T03:17:38Z- handleDvl() - RDI_Pathfinder msg: 1730863058.82 sec; seqNo:1400; #beams:4 2024-11-06T03:17:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.09 , 37.92 , 42.81 , 41.18 2024-11-06T03:17:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.08 , 0.01 2024-11-06T03:17:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863058.83 epoch sec; seqNo:4205; 4061463.88 north; 594824.85 east; 36.69 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:17:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863056.35 2024-11-06T03:17:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:38Z- waiting for 1730863060.35, time is 1730863058.83 2024-11-06T03:17:39Z- AHRS_M2 msg: 1730863059.17 epoch sec; seqNo:3959 2024-11-06T03:17:39Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:39Z- handleDepth()- Depth_Keller msg: 1730863059.18 epoch sec; seqNo:4206; depth 49.98 2024-11-06T03:17:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863059.20 epoch sec; seqNo:4206; 4061463.50 north; 594824.85 east; 36.69 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:17:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863056.35 2024-11-06T03:17:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:39Z- waiting for 1730863060.35, time is 1730863059.20 2024-11-06T03:17:39Z- AHRS_M2 msg: 1730863059.59 epoch sec; seqNo:3960 2024-11-06T03:17:39Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:39Z- handleDepth()- Depth_Keller msg: 1730863059.60 epoch sec; seqNo:4207; depth 49.98 2024-11-06T03:17:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863059.62 epoch sec; seqNo:4207; 4061463.12 north; 594824.86 east; 36.69 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:17:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863056.35 2024-11-06T03:17:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:39Z- waiting for 1730863060.35, time is 1730863059.62 2024-11-06T03:17:40Z- handleDepth()- Depth_Keller msg: 1730863060.00 epoch sec; seqNo:4208; depth 49.97 2024-11-06T03:17:40Z- handleDVL() - DVL msg epochMillisec: 1730863060.00 2024-11-06T03:17:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:40Z- handleDvl() - RDI_Pathfinder msg: 1730863060.00 sec; seqNo:1401; #beams:0 2024-11-06T03:17:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.08 , 0.01 2024-11-06T03:17:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863060.01 epoch sec; seqNo:4208; 4061462.74 north; 594824.86 east; 36.69 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:17:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863056.35 2024-11-06T03:17:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:40Z- waiting for 1730863060.35, time is 1730863060.01 2024-11-06T03:17:40Z- AHRS_M2 msg: 1730863060.39 epoch sec; seqNo:3961 2024-11-06T03:17:40Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:40Z- handleDepth()- Depth_Keller msg: 1730863060.40 epoch sec; seqNo:4209; depth 49.96 2024-11-06T03:17:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863060.41 epoch sec; seqNo:4209; 4061462.36 north; 594824.87 east; 36.69 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:17:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863056.35 2024-11-06T03:17:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:40Z- waiting for 1730863060.35, time is 1730863060.41 2024-11-06T03:17:40Z- LcmTrn::updateTrn() >>>> pose time 1730863060.41, meas time 1730863060.00 <<<< 2024-11-06T03:17:40Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863060.41 2024-11-06T03:17:40Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:17:40Z- TerrainNav::motionUpdate() - currEstimate = (4061462, 594824.87, 49.96) 2024-11-06T03:17:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:17:40Z- TerrainNav:: SubcloudNIS is 0.76, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:17:40Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:17:40Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:17:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:17:40Z- variances([0], [2], and psi_berg) = 0.256528 0.160918 0.000000 2024-11-06T03:17:40Z- publishEstimatesDecoder() - at 1730863060.41 sec 2024-11-06T03:17:40Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:17:40Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061493.42 594807.45 55.55 2024-11-06T03:17:40Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061492.59 594807.88 54.49 lat/lon: 36.69 -121.94 2024-11-06T03:17:40Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.16 1.68 0.00 2024-11-06T03:17:40Z- AHRS_M2 msg: 1730863060.79 epoch sec; seqNo:3962 2024-11-06T03:17:40Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:40Z- handleDepth()- Depth_Keller msg: 1730863060.79 epoch sec; seqNo:4210; depth 49.96 2024-11-06T03:17:40Z- handleDVL() - DVL msg epochMillisec: 1730863060.82 2024-11-06T03:17:40Z- handleDvl() - RDI_Pathfinder msg: 1730863060.82 sec; seqNo:1402; #beams:4 2024-11-06T03:17:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.09 , 38.12 , 43.02 , 40.98 2024-11-06T03:17:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.09 , 0.02 2024-11-06T03:17:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863060.84 epoch sec; seqNo:4210; 4061461.98 north; 594824.87 east; 36.69 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:17:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863060.41 2024-11-06T03:17:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:40Z- waiting for 1730863064.41, time is 1730863060.84 2024-11-06T03:17:41Z- AHRS_M2 msg: 1730863061.19 epoch sec; seqNo:3963 2024-11-06T03:17:41Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:41Z- handleDepth()- Depth_Keller msg: 1730863061.20 epoch sec; seqNo:4211; depth 49.95 2024-11-06T03:17:41Z- handleDVL() - DVL msg epochMillisec: 1730863061.20 2024-11-06T03:17:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:41Z- handleDvl() - RDI_Pathfinder msg: 1730863061.20 sec; seqNo:1403; #beams:0 2024-11-06T03:17:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.09 , 0.02 2024-11-06T03:17:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863061.21 epoch sec; seqNo:4211; 4061461.60 north; 594824.87 east; 36.69 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:17:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863060.41 2024-11-06T03:17:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:41Z- waiting for 1730863064.41, time is 1730863061.21 2024-11-06T03:17:41Z- AHRS_M2 msg: 1730863061.60 epoch sec; seqNo:3964 2024-11-06T03:17:41Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:41Z- handleDepth()- Depth_Keller msg: 1730863061.61 epoch sec; seqNo:4212; depth 49.96 2024-11-06T03:17:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863061.64 epoch sec; seqNo:4212; 4061461.23 north; 594824.88 east; 36.69 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:17:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863060.41 2024-11-06T03:17:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:41Z- waiting for 1730863064.41, time is 1730863061.64 2024-11-06T03:17:42Z- AHRS_M2 msg: 1730863062.00 epoch sec; seqNo:3965 2024-11-06T03:17:42Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:42Z- handleDepth()- Depth_Keller msg: 1730863062.00 epoch sec; seqNo:4213; depth 49.95 2024-11-06T03:17:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863062.01 epoch sec; seqNo:4213; 4061460.85 north; 594824.88 east; 36.69 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:17:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863060.41 2024-11-06T03:17:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:42Z- waiting for 1730863064.41, time is 1730863062.01 2024-11-06T03:17:42Z- AHRS_M2 msg: 1730863062.40 epoch sec; seqNo:3966 2024-11-06T03:17:42Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:42Z- handleDepth()- Depth_Keller msg: 1730863062.41 epoch sec; seqNo:4214; depth 49.95 2024-11-06T03:17:42Z- handleDVL() - DVL msg epochMillisec: 1730863062.41 2024-11-06T03:17:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:42Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:42Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:42Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:42Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:42Z- handleDvl() - RDI_Pathfinder msg: 1730863062.41 sec; seqNo:1404; #beams:0 2024-11-06T03:17:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.09 , 0.02 2024-11-06T03:17:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863062.42 epoch sec; seqNo:4214; 4061460.47 north; 594824.89 east; 36.69 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:17:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863060.41 2024-11-06T03:17:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:42Z- waiting for 1730863064.41, time is 1730863062.42 2024-11-06T03:17:42Z- AHRS_M2 msg: 1730863062.81 epoch sec; seqNo:3967 2024-11-06T03:17:42Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:42Z- handleDepth()- Depth_Keller msg: 1730863062.81 epoch sec; seqNo:4215; depth 49.94 2024-11-06T03:17:42Z- handleDVL() - DVL msg epochMillisec: 1730863062.83 2024-11-06T03:17:42Z- handleDvl() - RDI_Pathfinder msg: 1730863062.83 sec; seqNo:1405; #beams:4 2024-11-06T03:17:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 38.12 , 43.02 , 41.18 2024-11-06T03:17:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.07 , 0.03 2024-11-06T03:17:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863062.85 epoch sec; seqNo:4215; 4061460.08 north; 594823.67 east; 36.69 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:17:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863060.41 2024-11-06T03:17:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:42Z- waiting for 1730863064.41, time is 1730863062.85 2024-11-06T03:17:43Z- AHRS_M2 msg: 1730863063.24 epoch sec; seqNo:3968 2024-11-06T03:17:43Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:43Z- handleDepth()- Depth_Keller msg: 1730863063.24 epoch sec; seqNo:4216; depth 49.94 2024-11-06T03:17:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863063.26 epoch sec; seqNo:4216; 4061459.70 north; 594823.68 east; 36.69 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:17:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863060.41 2024-11-06T03:17:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:43Z- waiting for 1730863064.41, time is 1730863063.26 2024-11-06T03:17:43Z- AHRS_M2 msg: 1730863063.62 epoch sec; seqNo:3969 2024-11-06T03:17:43Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:43Z- handleDepth()- Depth_Keller msg: 1730863063.63 epoch sec; seqNo:4217; depth 49.93 2024-11-06T03:17:43Z- handleDVL() - DVL msg epochMillisec: 1730863063.64 2024-11-06T03:17:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:43Z- handleDvl() - RDI_Pathfinder msg: 1730863063.64 sec; seqNo:1406; #beams:0 2024-11-06T03:17:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.07 , 0.03 2024-11-06T03:17:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863063.65 epoch sec; seqNo:4217; 4061459.32 north; 594823.68 east; 36.69 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:17:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863060.41 2024-11-06T03:17:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:43Z- waiting for 1730863064.41, time is 1730863063.65 2024-11-06T03:17:44Z- AHRS_M2 msg: 1730863064.02 epoch sec; seqNo:3970 2024-11-06T03:17:44Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:44Z- handleDepth()- Depth_Keller msg: 1730863064.03 epoch sec; seqNo:4218; depth 49.93 2024-11-06T03:17:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863064.04 epoch sec; seqNo:4218; 4061459.32 north; 594823.68 east; 36.69 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:17:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863060.41 2024-11-06T03:17:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:44Z- waiting for 1730863064.41, time is 1730863064.04 2024-11-06T03:17:44Z- AHRS_M2 msg: 1730863064.43 epoch sec; seqNo:3971 2024-11-06T03:17:44Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:44Z- handleDepth()- Depth_Keller msg: 1730863064.44 epoch sec; seqNo:4219; depth 49.93 2024-11-06T03:17:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863064.45 epoch sec; seqNo:4219; 4061458.94 north; 594823.68 east; 36.69 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:17:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863060.41 2024-11-06T03:17:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:44Z- waiting for 1730863064.41, time is 1730863064.45 2024-11-06T03:17:44Z- LcmTrn::updateTrn() >>>> pose time 1730863064.45, meas time 1730863063.64 <<<< 2024-11-06T03:17:44Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863064.45 2024-11-06T03:17:44Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:17:44Z- TerrainNav::motionUpdate() - currEstimate = (4061459, 594823.68, 49.93) 2024-11-06T03:17:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:17:44Z- TerrainNav:: SubcloudNIS is 0.76, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:17:44Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:17:44Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:17:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:17:44Z- variances([0], [2], and psi_berg) = 0.257923 0.163238 0.000000 2024-11-06T03:17:44Z- publishEstimatesDecoder() - at 1730863064.45 sec 2024-11-06T03:17:44Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:17:44Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061490.01 594806.32 56.28 2024-11-06T03:17:44Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061489.17 594806.69 54.45 lat/lon: 36.69 -121.94 2024-11-06T03:17:44Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.16 1.93 0.00 2024-11-06T03:17:44Z- AHRS_M2 msg: 1730863064.83 epoch sec; seqNo:3972 2024-11-06T03:17:44Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:44Z- handleDepth()- Depth_Keller msg: 1730863064.83 epoch sec; seqNo:4220; depth 49.93 2024-11-06T03:17:44Z- handleDVL() - DVL msg epochMillisec: 1730863064.86 2024-11-06T03:17:44Z- handleDvl() - RDI_Pathfinder msg: 1730863064.86 sec; seqNo:1407; #beams:4 2024-11-06T03:17:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 38.33 , 43.02 , 41.18 2024-11-06T03:17:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.03 2024-11-06T03:17:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863064.88 epoch sec; seqNo:4220; 4061458.56 north; 594823.69 east; 36.69 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:17:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863064.45 2024-11-06T03:17:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:44Z- waiting for 1730863068.45, time is 1730863064.88 2024-11-06T03:17:45Z- AHRS_M2 msg: 1730863065.23 epoch sec; seqNo:3973 2024-11-06T03:17:45Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:45Z- handleDepth()- Depth_Keller msg: 1730863065.24 epoch sec; seqNo:4221; depth 49.93 2024-11-06T03:17:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863065.25 epoch sec; seqNo:4221; 4061458.18 north; 594823.69 east; 36.69 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:17:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863064.45 2024-11-06T03:17:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:45Z- waiting for 1730863068.45, time is 1730863065.25 2024-11-06T03:17:45Z- AHRS_M2 msg: 1730863065.64 epoch sec; seqNo:3974 2024-11-06T03:17:45Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:45Z- handleDepth()- Depth_Keller msg: 1730863065.65 epoch sec; seqNo:4222; depth 49.92 2024-11-06T03:17:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863065.66 epoch sec; seqNo:4222; 4061457.80 north; 594823.70 east; 36.69 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:17:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863064.45 2024-11-06T03:17:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:45Z- waiting for 1730863068.45, time is 1730863065.66 2024-11-06T03:17:46Z- AHRS_M2 msg: 1730863066.04 epoch sec; seqNo:3975 2024-11-06T03:17:46Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:46Z- handleDepth()- Depth_Keller msg: 1730863066.04 epoch sec; seqNo:4223; depth 49.91 2024-11-06T03:17:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863066.05 epoch sec; seqNo:4223; 4061457.42 north; 594823.70 east; 36.69 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:17:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863064.45 2024-11-06T03:17:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:46Z- waiting for 1730863068.45, time is 1730863066.05 2024-11-06T03:17:46Z- AHRS_M2 msg: 1730863066.44 epoch sec; seqNo:3976 2024-11-06T03:17:46Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:46Z- handleDepth()- Depth_Keller msg: 1730863066.45 epoch sec; seqNo:4224; depth 49.91 2024-11-06T03:17:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863066.46 epoch sec; seqNo:4224; 4061457.05 north; 594823.70 east; 36.69 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:17:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863064.45 2024-11-06T03:17:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:46Z- waiting for 1730863068.45, time is 1730863066.46 2024-11-06T03:17:46Z- AHRS_M2 msg: 1730863066.85 epoch sec; seqNo:3977 2024-11-06T03:17:46Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:46Z- handleDepth()- Depth_Keller msg: 1730863066.85 epoch sec; seqNo:4225; depth 49.90 2024-11-06T03:17:46Z- handleDVL() - DVL msg epochMillisec: 1730863066.87 2024-11-06T03:17:46Z- handleDvl() - RDI_Pathfinder msg: 1730863066.87 sec; seqNo:1408; #beams:4 2024-11-06T03:17:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 38.53 , 43.02 , 41.38 2024-11-06T03:17:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , 0.01 2024-11-06T03:17:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863066.89 epoch sec; seqNo:4225; 4061456.67 north; 594823.71 east; 36.69 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:17:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863064.45 2024-11-06T03:17:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:47Z- waiting for 1730863068.45, time is 1730863066.89 2024-11-06T03:17:47Z- handleDepth()- Depth_Keller msg: 1730863067.27 epoch sec; seqNo:4226; depth 49.90 2024-11-06T03:17:47Z- handleDVL() - DVL msg epochMillisec: 1730863067.27 2024-11-06T03:17:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:47Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:47Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:47Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:47Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:47Z- handleDvl() - RDI_Pathfinder msg: 1730863067.27 sec; seqNo:1409; #beams:0 2024-11-06T03:17:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , 0.01 2024-11-06T03:17:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863067.28 epoch sec; seqNo:4226; 4061456.29 north; 594823.71 east; 36.69 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:17:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863064.45 2024-11-06T03:17:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:47Z- waiting for 1730863068.45, time is 1730863067.28 2024-11-06T03:17:47Z- AHRS_M2 msg: 1730863067.66 epoch sec; seqNo:3978 2024-11-06T03:17:47Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:47Z- handleDepth()- Depth_Keller msg: 1730863067.66 epoch sec; seqNo:4227; depth 49.87 2024-11-06T03:17:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863067.68 epoch sec; seqNo:4227; 4061455.90 north; 594822.50 east; 36.69 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:17:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863064.45 2024-11-06T03:17:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:47Z- waiting for 1730863068.45, time is 1730863067.68 2024-11-06T03:17:48Z- AHRS_M2 msg: 1730863068.06 epoch sec; seqNo:3979 2024-11-06T03:17:48Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:48Z- handleDepth()- Depth_Keller msg: 1730863068.06 epoch sec; seqNo:4228; depth 49.87 2024-11-06T03:17:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863068.08 epoch sec; seqNo:4228; 4061455.52 north; 594822.50 east; 36.69 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:17:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863064.45 2024-11-06T03:17:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:48Z- waiting for 1730863068.45, time is 1730863068.08 2024-11-06T03:17:48Z- AHRS_M2 msg: 1730863068.46 epoch sec; seqNo:3980 2024-11-06T03:17:48Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:48Z- handleDepth()- Depth_Keller msg: 1730863068.47 epoch sec; seqNo:4229; depth 49.86 2024-11-06T03:17:48Z- handleDVL() - DVL msg epochMillisec: 1730863068.48 2024-11-06T03:17:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:48Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:48Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:48Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:48Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:48Z- handleDvl() - RDI_Pathfinder msg: 1730863068.48 sec; seqNo:1410; #beams:0 2024-11-06T03:17:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , 0.01 2024-11-06T03:17:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863068.48 epoch sec; seqNo:4229; 4061455.14 north; 594822.51 east; 36.69 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:17:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863064.45 2024-11-06T03:17:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:48Z- waiting for 1730863068.45, time is 1730863068.48 2024-11-06T03:17:48Z- LcmTrn::updateTrn() >>>> pose time 1730863068.48, meas time 1730863068.48 <<<< 2024-11-06T03:17:48Z- LcmTrn::updateTrn() equating pose 1730863068.48 and meas times 1730863068.48 2024-11-06T03:17:48Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863068.48 2024-11-06T03:17:48Z- TerrainNav::motionUpdate() - currEstimate = (4061455, 594822.51, 49.86) 2024-11-06T03:17:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:17:48Z- TerrainNav:: SubcloudNIS is 0.76, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:17:48Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:17:48Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:17:48Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:17:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:17:48Z- variances([0], [2], and psi_berg) = 0.260622 0.165668 0.000000 2024-11-06T03:17:48Z- publishEstimatesDecoder() - at 1730863068.48 sec 2024-11-06T03:17:48Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:17:48Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061486.24 594805.16 56.86 2024-11-06T03:17:48Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061485.37 594805.51 54.38 lat/lon: 36.69 -121.94 2024-11-06T03:17:48Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.17 2.21 0.00 2024-11-06T03:17:48Z- AHRS_M2 msg: 1730863068.87 epoch sec; seqNo:3981 2024-11-06T03:17:48Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:48Z- handleDepth()- Depth_Keller msg: 1730863068.87 epoch sec; seqNo:4230; depth 49.83 2024-11-06T03:17:48Z- handleDVL() - DVL msg epochMillisec: 1730863068.90 2024-11-06T03:17:48Z- handleDvl() - RDI_Pathfinder msg: 1730863068.90 sec; seqNo:1411; #beams:4 2024-11-06T03:17:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.69 , 38.74 , 42.81 , 41.38 2024-11-06T03:17:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , -0.03 2024-11-06T03:17:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863068.92 epoch sec; seqNo:4230; 4061454.76 north; 594822.51 east; 36.69 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:17:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863068.48 2024-11-06T03:17:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:49Z- waiting for 1730863072.48, time is 1730863068.92 2024-11-06T03:17:49Z- AHRS_M2 msg: 1730863069.27 epoch sec; seqNo:3982 2024-11-06T03:17:49Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:49Z- handleDepth()- Depth_Keller msg: 1730863069.28 epoch sec; seqNo:4231; depth 49.83 2024-11-06T03:17:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863069.29 epoch sec; seqNo:4231; 4061454.38 north; 594822.51 east; 36.69 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:17:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863068.48 2024-11-06T03:17:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:49Z- waiting for 1730863072.48, time is 1730863069.29 2024-11-06T03:17:49Z- AHRS_M2 msg: 1730863069.68 epoch sec; seqNo:3983 2024-11-06T03:17:49Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:49Z- handleDepth()- Depth_Keller msg: 1730863069.69 epoch sec; seqNo:4232; depth 49.84 2024-11-06T03:17:49Z- handleDVL() - DVL msg epochMillisec: 1730863069.69 2024-11-06T03:17:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:49Z- handleDvl() - RDI_Pathfinder msg: 1730863069.69 sec; seqNo:1412; #beams:0 2024-11-06T03:17:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , -0.03 2024-11-06T03:17:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863069.70 epoch sec; seqNo:4232; 4061454.00 north; 594822.52 east; 36.69 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:17:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863068.48 2024-11-06T03:17:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:49Z- waiting for 1730863072.48, time is 1730863069.70 2024-11-06T03:17:50Z- AHRS_M2 msg: 1730863070.08 epoch sec; seqNo:3984 2024-11-06T03:17:50Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:50Z- handleDepth()- Depth_Keller msg: 1730863070.12 epoch sec; seqNo:4233; depth 49.81 2024-11-06T03:17:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863070.13 epoch sec; seqNo:4233; 4061453.62 north; 594822.52 east; 36.69 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:17:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863068.48 2024-11-06T03:17:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:50Z- waiting for 1730863072.48, time is 1730863070.13 2024-11-06T03:17:50Z- AHRS_M2 msg: 1730863070.48 epoch sec; seqNo:3985 2024-11-06T03:17:50Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:50Z- handleDepth()- Depth_Keller msg: 1730863070.49 epoch sec; seqNo:4234; depth 49.77 2024-11-06T03:17:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863070.50 epoch sec; seqNo:4234; 4061453.62 north; 594822.52 east; 36.69 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:17:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863068.48 2024-11-06T03:17:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:50Z- waiting for 1730863072.48, time is 1730863070.50 2024-11-06T03:17:50Z- AHRS_M2 msg: 1730863070.89 epoch sec; seqNo:3986 2024-11-06T03:17:50Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:50Z- handleDepth()- Depth_Keller msg: 1730863070.89 epoch sec; seqNo:4235; depth 49.78 2024-11-06T03:17:50Z- handleDVL() - DVL msg epochMillisec: 1730863070.91 2024-11-06T03:17:50Z- handleDvl() - RDI_Pathfinder msg: 1730863070.91 sec; seqNo:1413; #beams:4 2024-11-06T03:17:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 38.53 , 42.61 , 41.38 2024-11-06T03:17:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.05 2024-11-06T03:17:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863070.95 epoch sec; seqNo:4235; 4061453.24 north; 594822.53 east; 36.69 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:17:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863068.48 2024-11-06T03:17:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:51Z- waiting for 1730863072.48, time is 1730863070.95 2024-11-06T03:17:51Z- AHRS_M2 msg: 1730863071.29 epoch sec; seqNo:3987 2024-11-06T03:17:51Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:51Z- handleDepth()- Depth_Keller msg: 1730863071.30 epoch sec; seqNo:4236; depth 49.76 2024-11-06T03:17:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863071.31 epoch sec; seqNo:4236; 4061452.86 north; 594822.53 east; 36.69 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:17:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863068.48 2024-11-06T03:17:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:51Z- waiting for 1730863072.48, time is 1730863071.31 2024-11-06T03:17:51Z- AHRS_M2 msg: 1730863071.69 epoch sec; seqNo:3988 2024-11-06T03:17:51Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:51Z- handleDepth()- Depth_Keller msg: 1730863071.71 epoch sec; seqNo:4237; depth 49.76 2024-11-06T03:17:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863071.72 epoch sec; seqNo:4237; 4061452.49 north; 594822.53 east; 36.69 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:17:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863068.48 2024-11-06T03:17:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:51Z- waiting for 1730863072.48, time is 1730863071.72 2024-11-06T03:17:52Z- AHRS_M2 msg: 1730863072.10 epoch sec; seqNo:3989 2024-11-06T03:17:52Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:52Z- handleDepth()- Depth_Keller msg: 1730863072.11 epoch sec; seqNo:4238; depth 49.76 2024-11-06T03:17:52Z- handleDVL() - DVL msg epochMillisec: 1730863072.11 2024-11-06T03:17:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:52Z- handleDvl() - RDI_Pathfinder msg: 1730863072.11 sec; seqNo:1414; #beams:0 2024-11-06T03:17:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.05 2024-11-06T03:17:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863072.13 epoch sec; seqNo:4238; 4061452.11 north; 594822.54 east; 36.69 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:17:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863068.48 2024-11-06T03:17:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:52Z- waiting for 1730863072.48, time is 1730863072.13 2024-11-06T03:17:52Z- AHRS_M2 msg: 1730863072.50 epoch sec; seqNo:3990 2024-11-06T03:17:52Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.40 psi 2024-11-06T03:17:52Z- handleDepth()- Depth_Keller msg: 1730863072.51 epoch sec; seqNo:4239; depth 49.74 2024-11-06T03:17:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863072.52 epoch sec; seqNo:4239; 4061451.71 north; 594821.32 east; 36.69 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:17:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863068.48 2024-11-06T03:17:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:52Z- waiting for 1730863072.48, time is 1730863072.52 2024-11-06T03:17:52Z- LcmTrn::updateTrn() >>>> pose time 1730863072.52, meas time 1730863072.11 <<<< 2024-11-06T03:17:52Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863072.52 2024-11-06T03:17:52Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:17:52Z- TerrainNav::motionUpdate() - currEstimate = (4061452, 594821.32, 49.74) 2024-11-06T03:17:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:17:52Z- TerrainNav:: SubcloudNIS is 0.76, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:17:52Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:17:52Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:17:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:17:52Z- variances([0], [2], and psi_berg) = 0.262821 0.168010 0.000000 2024-11-06T03:17:52Z- publishEstimatesDecoder() - at 1730863072.52 sec 2024-11-06T03:17:52Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:17:52Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061482.78 594803.90 56.03 2024-11-06T03:17:52Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061481.95 594804.33 54.27 lat/lon: 36.69 -121.94 2024-11-06T03:17:52Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.17 2.44 0.00 2024-11-06T03:17:52Z- AHRS_M2 msg: 1730863072.91 epoch sec; seqNo:3991 2024-11-06T03:17:52Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:52Z- handleDepth()- Depth_Keller msg: 1730863072.91 epoch sec; seqNo:4240; depth 49.72 2024-11-06T03:17:52Z- handleDVL() - DVL msg epochMillisec: 1730863072.93 2024-11-06T03:17:52Z- handleDvl() - RDI_Pathfinder msg: 1730863072.93 sec; seqNo:1415; #beams:4 2024-11-06T03:17:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 38.53 , 42.61 , 41.59 2024-11-06T03:17:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.13 , -0.02 2024-11-06T03:17:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863072.95 epoch sec; seqNo:4240; 4061451.34 north; 594821.33 east; 36.69 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:17:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863072.52 2024-11-06T03:17:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:53Z- waiting for 1730863076.52, time is 1730863072.95 2024-11-06T03:17:53Z- AHRS_M2 msg: 1730863073.31 epoch sec; seqNo:3992 2024-11-06T03:17:53Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:53Z- handleDepth()- Depth_Keller msg: 1730863073.32 epoch sec; seqNo:4241; depth 49.71 2024-11-06T03:17:53Z- handleDVL() - DVL msg epochMillisec: 1730863073.33 2024-11-06T03:17:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:53Z- handleDvl() - RDI_Pathfinder msg: 1730863073.33 sec; seqNo:1416; #beams:0 2024-11-06T03:17:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.13 , -0.02 2024-11-06T03:17:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863073.34 epoch sec; seqNo:4241; 4061450.96 north; 594821.33 east; 36.69 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:17:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863072.52 2024-11-06T03:17:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:53Z- waiting for 1730863076.52, time is 1730863073.34 2024-11-06T03:17:53Z- AHRS_M2 msg: 1730863073.72 epoch sec; seqNo:3993 2024-11-06T03:17:53Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:53Z- handleDepth()- Depth_Keller msg: 1730863073.72 epoch sec; seqNo:4242; depth 49.72 2024-11-06T03:17:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863073.73 epoch sec; seqNo:4242; 4061450.58 north; 594821.34 east; 36.69 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:17:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863072.52 2024-11-06T03:17:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:53Z- waiting for 1730863076.52, time is 1730863073.73 2024-11-06T03:17:54Z- AHRS_M2 msg: 1730863074.12 epoch sec; seqNo:3994 2024-11-06T03:17:54Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:54Z- handleDepth()- Depth_Keller msg: 1730863074.12 epoch sec; seqNo:4243; depth 49.71 2024-11-06T03:17:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863074.13 epoch sec; seqNo:4243; 4061450.20 north; 594821.34 east; 36.69 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:17:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863072.52 2024-11-06T03:17:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:54Z- waiting for 1730863076.52, time is 1730863074.13 2024-11-06T03:17:54Z- AHRS_M2 msg: 1730863074.52 epoch sec; seqNo:3995 2024-11-06T03:17:54Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:54Z- handleDepth()- Depth_Keller msg: 1730863074.53 epoch sec; seqNo:4244; depth 49.72 2024-11-06T03:17:54Z- handleDVL() - DVL msg epochMillisec: 1730863074.53 2024-11-06T03:17:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:54Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:54Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:54Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:54Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:54Z- handleDvl() - RDI_Pathfinder msg: 1730863074.53 sec; seqNo:1417; #beams:0 2024-11-06T03:17:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.13 , -0.02 2024-11-06T03:17:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863074.54 epoch sec; seqNo:4244; 4061449.82 north; 594821.34 east; 36.69 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:17:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863072.52 2024-11-06T03:17:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:54Z- waiting for 1730863076.52, time is 1730863074.54 2024-11-06T03:17:54Z- AHRS_M2 msg: 1730863074.93 epoch sec; seqNo:3996 2024-11-06T03:17:54Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:17:54Z- handleDepth()- Depth_Keller msg: 1730863074.94 epoch sec; seqNo:4245; depth 49.72 2024-11-06T03:17:54Z- handleDVL() - DVL msg epochMillisec: 1730863074.97 2024-11-06T03:17:54Z- handleDvl() - RDI_Pathfinder msg: 1730863074.97 sec; seqNo:1418; #beams:4 2024-11-06T03:17:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.09 , 38.53 , 42.81 , 41.59 2024-11-06T03:17:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.14 , 0.02 2024-11-06T03:17:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863074.99 epoch sec; seqNo:4245; 4061449.44 north; 594821.35 east; 36.69 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:17:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863072.52 2024-11-06T03:17:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:55Z- waiting for 1730863076.52, time is 1730863074.99 2024-11-06T03:17:55Z- handleDepth()- Depth_Keller msg: 1730863075.35 epoch sec; seqNo:4246; depth 49.72 2024-11-06T03:17:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863075.37 epoch sec; seqNo:4246; 4061449.44 north; 594821.35 east; 36.69 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:17:55Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:17:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863072.52 2024-11-06T03:17:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:55Z- waiting for 1730863076.52, time is 1730863075.37 2024-11-06T03:17:55Z- handleDepth()- Depth_Keller msg: 1730863075.76 epoch sec; seqNo:4247; depth 49.72 2024-11-06T03:17:55Z- handleDVL() - DVL msg epochMillisec: 1730863075.76 2024-11-06T03:17:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:55Z- handleDvl() - RDI_Pathfinder msg: 1730863075.76 sec; seqNo:1419; #beams:0 2024-11-06T03:17:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.14 , 0.02 2024-11-06T03:17:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863075.78 epoch sec; seqNo:4247; 4061449.05 north; 594820.13 east; 36.69 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:17:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863072.52 2024-11-06T03:17:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:55Z- waiting for 1730863076.52, time is 1730863075.78 2024-11-06T03:17:56Z- AHRS_M2 msg: 1730863076.14 epoch sec; seqNo:3997 2024-11-06T03:17:56Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.38 psi 2024-11-06T03:17:56Z- handleDepth()- Depth_Keller msg: 1730863076.15 epoch sec; seqNo:4248; depth 49.72 2024-11-06T03:17:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863076.17 epoch sec; seqNo:4248; 4061448.67 north; 594820.14 east; 36.69 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:17:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863072.52 2024-11-06T03:17:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:56Z- waiting for 1730863076.52, time is 1730863076.17 2024-11-06T03:17:56Z- AHRS_M2 msg: 1730863076.54 epoch sec; seqNo:3998 2024-11-06T03:17:56Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.38 psi 2024-11-06T03:17:56Z- handleDepth()- Depth_Keller msg: 1730863076.55 epoch sec; seqNo:4249; depth 49.71 2024-11-06T03:17:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863076.56 epoch sec; seqNo:4249; 4061448.29 north; 594820.14 east; 36.69 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:17:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863072.52 2024-11-06T03:17:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:56Z- waiting for 1730863076.52, time is 1730863076.56 2024-11-06T03:17:56Z- LcmTrn::updateTrn() >>>> pose time 1730863076.56, meas time 1730863075.76 <<<< 2024-11-06T03:17:56Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863076.56 2024-11-06T03:17:56Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:17:56Z- TerrainNav::motionUpdate() - currEstimate = (4061448, 594820.14, 49.71) 2024-11-06T03:17:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:17:56Z- TerrainNav:: SubcloudNIS is 0.76, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:17:56Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:17:56Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:17:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:17:56Z- variances([0], [2], and psi_berg) = 0.264529 0.169472 0.000000 2024-11-06T03:17:56Z- publishEstimatesDecoder() - at 1730863076.56 sec 2024-11-06T03:17:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:17:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061479.38 594802.75 57.10 2024-11-06T03:17:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061478.52 594803.15 54.23 lat/lon: 36.69 -121.94 2024-11-06T03:17:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.17 2.70 0.00 2024-11-06T03:17:56Z- AHRS_M2 msg: 1730863076.95 epoch sec; seqNo:3999 2024-11-06T03:17:56Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.38 psi 2024-11-06T03:17:56Z- handleDepth()- Depth_Keller msg: 1730863076.96 epoch sec; seqNo:4250; depth 49.71 2024-11-06T03:17:56Z- handleDVL() - DVL msg epochMillisec: 1730863076.98 2024-11-06T03:17:56Z- handleDvl() - RDI_Pathfinder msg: 1730863076.98 sec; seqNo:1420; #beams:4 2024-11-06T03:17:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.69 , 38.94 , 42.61 , 41.79 2024-11-06T03:17:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.09 , 0.06 2024-11-06T03:17:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863077.00 epoch sec; seqNo:4250; 4061447.91 north; 594820.14 east; 36.69 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:17:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863076.56 2024-11-06T03:17:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:57Z- waiting for 1730863080.56, time is 1730863077.00 2024-11-06T03:17:57Z- AHRS_M2 msg: 1730863077.36 epoch sec; seqNo:4000 2024-11-06T03:17:57Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.38 psi 2024-11-06T03:17:57Z- handleDepth()- Depth_Keller msg: 1730863077.36 epoch sec; seqNo:4251; depth 49.73 2024-11-06T03:17:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863077.38 epoch sec; seqNo:4251; 4061447.53 north; 594820.15 east; 36.69 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:17:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863076.56 2024-11-06T03:17:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:57Z- waiting for 1730863080.56, time is 1730863077.38 2024-11-06T03:17:57Z- AHRS_M2 msg: 1730863077.76 epoch sec; seqNo:4001 2024-11-06T03:17:57Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.38 psi 2024-11-06T03:17:57Z- handleDepth()- Depth_Keller msg: 1730863077.77 epoch sec; seqNo:4252; depth 49.73 2024-11-06T03:17:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863077.78 epoch sec; seqNo:4252; 4061447.15 north; 594820.15 east; 36.69 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:17:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863076.56 2024-11-06T03:17:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:57Z- waiting for 1730863080.56, time is 1730863077.78 2024-11-06T03:17:58Z- AHRS_M2 msg: 1730863078.16 epoch sec; seqNo:4002 2024-11-06T03:17:58Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.38 psi 2024-11-06T03:17:58Z- handleDepth()- Depth_Keller msg: 1730863078.17 epoch sec; seqNo:4253; depth 49.73 2024-11-06T03:17:58Z- handleDVL() - DVL msg epochMillisec: 1730863078.17 2024-11-06T03:17:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:58Z- handleDvl() - RDI_Pathfinder msg: 1730863078.17 sec; seqNo:1421; #beams:0 2024-11-06T03:17:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.09 , 0.06 2024-11-06T03:17:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863078.18 epoch sec; seqNo:4253; 4061446.78 north; 594820.16 east; 36.69 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:17:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863076.56 2024-11-06T03:17:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:58Z- waiting for 1730863080.56, time is 1730863078.18 2024-11-06T03:17:58Z- AHRS_M2 msg: 1730863078.56 epoch sec; seqNo:4003 2024-11-06T03:17:58Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.38 psi 2024-11-06T03:17:58Z- handleDepth()- Depth_Keller msg: 1730863078.57 epoch sec; seqNo:4254; depth 49.72 2024-11-06T03:17:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863078.59 epoch sec; seqNo:4254; 4061446.40 north; 594820.16 east; 36.69 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:17:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863076.56 2024-11-06T03:17:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:58Z- waiting for 1730863080.56, time is 1730863078.59 2024-11-06T03:17:58Z- AHRS_M2 msg: 1730863078.96 epoch sec; seqNo:4004 2024-11-06T03:17:58Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.38 psi 2024-11-06T03:17:58Z- handleDepth()- Depth_Keller msg: 1730863078.98 epoch sec; seqNo:4255; depth 49.72 2024-11-06T03:17:59Z- handleDVL() - DVL msg epochMillisec: 1730863079.01 2024-11-06T03:17:59Z- handleDvl() - RDI_Pathfinder msg: 1730863079.01 sec; seqNo:1422; #beams:4 2024-11-06T03:17:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.69 , 38.94 , 42.82 , 41.80 2024-11-06T03:17:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.06 2024-11-06T03:17:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863079.02 epoch sec; seqNo:4255; 4061446.02 north; 594820.16 east; 36.69 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:17:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863076.56 2024-11-06T03:17:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:59Z- waiting for 1730863080.56, time is 1730863079.02 2024-11-06T03:17:59Z- AHRS_M2 msg: 1730863079.37 epoch sec; seqNo:4005 2024-11-06T03:17:59Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.39 psi 2024-11-06T03:17:59Z- handleDepth()- Depth_Keller msg: 1730863079.38 epoch sec; seqNo:4256; depth 49.71 2024-11-06T03:17:59Z- handleDVL() - DVL msg epochMillisec: 1730863079.38 2024-11-06T03:17:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:17:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:17:59Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:17:59Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:17:59Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:17:59Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:17:59Z- handleDvl() - RDI_Pathfinder msg: 1730863079.38 sec; seqNo:1423; #beams:0 2024-11-06T03:17:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:17:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.06 2024-11-06T03:17:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863079.39 epoch sec; seqNo:4256; 4061445.64 north; 594820.17 east; 36.69 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:17:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:17:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863076.56 2024-11-06T03:17:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:59Z- waiting for 1730863080.56, time is 1730863079.39 2024-11-06T03:17:59Z- AHRS_M2 msg: 1730863079.78 epoch sec; seqNo:4006 2024-11-06T03:17:59Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.39 psi 2024-11-06T03:17:59Z- handleDepth()- Depth_Keller msg: 1730863079.78 epoch sec; seqNo:4257; depth 49.71 2024-11-06T03:17:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863079.81 epoch sec; seqNo:4257; 4061445.26 north; 594820.17 east; 36.69 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:17:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:17:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863076.56 2024-11-06T03:17:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:17:59Z- waiting for 1730863080.56, time is 1730863079.81 2024-11-06T03:18:00Z- AHRS_M2 msg: 1730863080.18 epoch sec; seqNo:4007 2024-11-06T03:18:00Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.39 psi 2024-11-06T03:18:00Z- handleDepth()- Depth_Keller msg: 1730863080.19 epoch sec; seqNo:4258; depth 49.71 2024-11-06T03:18:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863080.22 epoch sec; seqNo:4258; 4061444.87 north; 594818.96 east; 36.69 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:18:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863076.56 2024-11-06T03:18:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:00Z- waiting for 1730863080.56, time is 1730863080.22 2024-11-06T03:18:00Z- AHRS_M2 msg: 1730863080.58 epoch sec; seqNo:4008 2024-11-06T03:18:00Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:18:00Z- handleDepth()- Depth_Keller msg: 1730863080.59 epoch sec; seqNo:4259; depth 49.71 2024-11-06T03:18:00Z- handleDVL() - DVL msg epochMillisec: 1730863080.59 2024-11-06T03:18:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:18:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:18:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:18:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:18:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:18:00Z- handleDvl() - RDI_Pathfinder msg: 1730863080.59 sec; seqNo:1424; #beams:0 2024-11-06T03:18:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:18:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.06 2024-11-06T03:18:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863080.60 epoch sec; seqNo:4259; 4061444.49 north; 594818.96 east; 36.69 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:18:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863076.56 2024-11-06T03:18:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:00Z- waiting for 1730863080.56, time is 1730863080.60 2024-11-06T03:18:00Z- LcmTrn::updateTrn() >>>> pose time 1730863080.60, meas time 1730863080.59 <<<< 2024-11-06T03:18:00Z- LcmTrn::updateTrn() equating pose 1730863080.59 and meas times 1730863080.59 2024-11-06T03:18:00Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863080.59 2024-11-06T03:18:00Z- TerrainNav::motionUpdate() - currEstimate = (4061444, 594818.96, 49.71) 2024-11-06T03:18:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:18:00Z- TerrainNav:: SubcloudNIS is 0.76, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:18:00Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:18:00Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:18:00Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:18:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:18:00Z- variances([0], [2], and psi_berg) = 0.266700 0.172006 0.000000 2024-11-06T03:18:00Z- publishEstimatesDecoder() - at 1730863080.59 sec 2024-11-06T03:18:00Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:18:00Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061475.53 594801.53 58.06 2024-11-06T03:18:00Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061474.72 594801.97 54.22 lat/lon: 36.69 -121.94 2024-11-06T03:18:00Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.17 2.94 0.00 2024-11-06T03:18:00Z- AHRS_M2 msg: 1730863080.99 epoch sec; seqNo:4009 2024-11-06T03:18:00Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.39 psi 2024-11-06T03:18:00Z- handleDepth()- Depth_Keller msg: 1730863080.99 epoch sec; seqNo:4260; depth 49.70 2024-11-06T03:18:01Z- handleDVL() - DVL msg epochMillisec: 1730863081.02 2024-11-06T03:18:01Z- handleDvl() - RDI_Pathfinder msg: 1730863081.02 sec; seqNo:1425; #beams:4 2024-11-06T03:18:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 39.15 , 43.02 , 42.00 2024-11-06T03:18:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , 0.00 2024-11-06T03:18:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863081.04 epoch sec; seqNo:4260; 4061444.11 north; 594818.97 east; 36.69 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:18:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863080.59 2024-11-06T03:18:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:01Z- waiting for 1730863084.59, time is 1730863081.04 2024-11-06T03:18:01Z- AHRS_M2 msg: 1730863081.39 epoch sec; seqNo:4010 2024-11-06T03:18:01Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.39 psi 2024-11-06T03:18:01Z- handleDepth()- Depth_Keller msg: 1730863081.39 epoch sec; seqNo:4261; depth 49.70 2024-11-06T03:18:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863081.41 epoch sec; seqNo:4261; 4061443.73 north; 594818.97 east; 36.69 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:18:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863080.59 2024-11-06T03:18:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:01Z- waiting for 1730863084.59, time is 1730863081.41 2024-11-06T03:18:01Z- AHRS_M2 msg: 1730863081.80 epoch sec; seqNo:4011 2024-11-06T03:18:01Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.39 psi 2024-11-06T03:18:01Z- handleDepth()- Depth_Keller msg: 1730863081.81 epoch sec; seqNo:4262; depth 49.68 2024-11-06T03:18:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863081.83 epoch sec; seqNo:4262; 4061443.35 north; 594818.97 east; 36.69 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:18:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863080.59 2024-11-06T03:18:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:01Z- waiting for 1730863084.59, time is 1730863081.83 2024-11-06T03:18:02Z- handleDepth()- Depth_Keller msg: 1730863082.22 epoch sec; seqNo:4263; depth 49.67 2024-11-06T03:18:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863082.23 epoch sec; seqNo:4263; 4061442.97 north; 594818.98 east; 36.69 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:18:02Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:18:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863080.59 2024-11-06T03:18:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:02Z- waiting for 1730863084.59, time is 1730863082.23 2024-11-06T03:18:02Z- AHRS_M2 msg: 1730863082.60 epoch sec; seqNo:4012 2024-11-06T03:18:02Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.40 psi 2024-11-06T03:18:02Z- handleDepth()- Depth_Keller msg: 1730863082.61 epoch sec; seqNo:4264; depth 49.67 2024-11-06T03:18:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863082.63 epoch sec; seqNo:4264; 4061442.60 north; 594818.98 east; 36.69 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:18:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863080.59 2024-11-06T03:18:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:02Z- waiting for 1730863084.59, time is 1730863082.63 2024-11-06T03:18:03Z- AHRS_M2 msg: 1730863083.01 epoch sec; seqNo:4013 2024-11-06T03:18:03Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.40 psi 2024-11-06T03:18:03Z- handleDepth()- Depth_Keller msg: 1730863083.01 epoch sec; seqNo:4265; depth 49.66 2024-11-06T03:18:03Z- handleDVL() - DVL msg epochMillisec: 1730863083.03 2024-11-06T03:18:03Z- handleDvl() - RDI_Pathfinder msg: 1730863083.03 sec; seqNo:1426; #beams:4 2024-11-06T03:18:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 38.94 , 43.02 , 42.00 2024-11-06T03:18:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , -0.07 2024-11-06T03:18:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863083.06 epoch sec; seqNo:4265; 4061442.22 north; 594818.99 east; 36.69 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:18:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863080.59 2024-11-06T03:18:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:03Z- waiting for 1730863084.59, time is 1730863083.06 2024-11-06T03:18:03Z- AHRS_M2 msg: 1730863083.41 epoch sec; seqNo:4014 2024-11-06T03:18:03Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.40 psi 2024-11-06T03:18:03Z- handleDepth()- Depth_Keller msg: 1730863083.42 epoch sec; seqNo:4266; depth 49.64 2024-11-06T03:18:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863083.44 epoch sec; seqNo:4266; 4061442.22 north; 594818.99 east; 36.69 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:18:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863080.59 2024-11-06T03:18:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:03Z- waiting for 1730863084.59, time is 1730863083.44 2024-11-06T03:18:03Z- AHRS_M2 msg: 1730863083.82 epoch sec; seqNo:4015 2024-11-06T03:18:03Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.40 psi 2024-11-06T03:18:03Z- handleDepth()- Depth_Keller msg: 1730863083.83 epoch sec; seqNo:4267; depth 49.62 2024-11-06T03:18:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863083.84 epoch sec; seqNo:4267; 4061441.84 north; 594818.99 east; 36.69 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:18:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863080.59 2024-11-06T03:18:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:03Z- waiting for 1730863084.59, time is 1730863083.84 2024-11-06T03:18:04Z- AHRS_M2 msg: 1730863084.22 epoch sec; seqNo:4016 2024-11-06T03:18:04Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.40 psi 2024-11-06T03:18:04Z- handleDepth()- Depth_Keller msg: 1730863084.22 epoch sec; seqNo:4268; depth 49.60 2024-11-06T03:18:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863084.23 epoch sec; seqNo:4268; 4061441.46 north; 594818.99 east; 36.69 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:18:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863080.59 2024-11-06T03:18:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:04Z- waiting for 1730863084.59, time is 1730863084.23 2024-11-06T03:18:04Z- AHRS_M2 msg: 1730863084.63 epoch sec; seqNo:4017 2024-11-06T03:18:04Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.40 psi 2024-11-06T03:18:04Z- handleDepth()- Depth_Keller msg: 1730863084.64 epoch sec; seqNo:4269; depth 49.60 2024-11-06T03:18:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863084.65 epoch sec; seqNo:4269; 4061441.08 north; 594819.00 east; 36.69 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:18:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863080.59 2024-11-06T03:18:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:04Z- waiting for 1730863084.59, time is 1730863084.65 2024-11-06T03:18:04Z- LcmTrn::updateTrn() >>>> pose time 1730863084.65, meas time 1730863083.03 <<<< 2024-11-06T03:18:04Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863084.65 2024-11-06T03:18:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863083.03 sec, ping # 0; waiting for INS data more recent than 1730863080.59... 2024-11-06T03:18:04Z- TerrainNav::motionUpdate() - currEstimate = (4061441, 594819.00, 49.60) 2024-11-06T03:18:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:18:04Z- TerrainNav:: SubcloudNIS is 0.76, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:18:04Z- TNavPF::Projecting Measurements 2024-11-06T03:18:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:18:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:18:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:18:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:18:04Z- TNavFilter::4 good beams. 2024-11-06T03:18:04Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:18:04Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:18:04Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:18:04Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:18:04Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:18:04Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:18:04Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:18:04Z- TerrainMapOctree: (vehN, vehE ) = (4061442.44, 594818.98). 2024-11-06T03:18:04Z- TerrainMapOctree: Min Distance = 204.43. 2024-11-06T03:18:04Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:18:04Z- Forcing Subcloud Comparison 2024-11-06T03:18:04Z- Weighting particles with subcloud comparison 2024-11-06T03:18:04Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:04Z- meanExpectedMeasDiff: -3.378556 delta_rms_squared: 5.143141 alpha: 0.996126 2024-11-06T03:18:04Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:04Z- meanExpectedMeasDiff: 3.826512 delta_rms_squared: 3.766726 alpha: 0.994718 2024-11-06T03:18:04Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:04Z- meanExpectedMeasDiff: -1.179913 delta_rms_squared: 4.372826 alpha: 0.995447 2024-11-06T03:18:04Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:04Z- meanExpectedMeasDiff: -0.401743 delta_rms_squared: 4.247111 alpha: 0.995313 2024-11-06T03:18:04Z- Subcloud NIS: 0.795257 2024-11-06T03:18:04Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:18:04Z- TNavPF:: sumWeights = 0.023424 2024-11-06T03:18:04Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:18:04Z- TNavPF::First Term of Map Covariance Matrix is 0.25 2024-11-06T03:18:04Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:18:04Z- TerrainNav::Resampling particle filter... 2024-11-06T03:18:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863083.03 sec. 2024-11-06T03:18:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:18:04Z- variances([0], [2], and psi_berg) = 0.333905 0.179874 0.000000 2024-11-06T03:18:04Z- publishEstimatesDecoder() - at 1730863084.65 sec 2024-11-06T03:18:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:18:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061471.85 594801.69 53.55 2024-11-06T03:18:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061471.36 594802.02 54.26 lat/lon: 36.69 -121.94 2024-11-06T03:18:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.18 0.93 0.00 2024-11-06T03:18:05Z- AHRS_M2 msg: 1730863085.04 epoch sec; seqNo:4018 2024-11-06T03:18:05Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.40 psi 2024-11-06T03:18:05Z- handleDepth()- Depth_Keller msg: 1730863085.04 epoch sec; seqNo:4270; depth 49.59 2024-11-06T03:18:05Z- handleDVL() - DVL msg epochMillisec: 1730863085.08 2024-11-06T03:18:05Z- handleDvl() - RDI_Pathfinder msg: 1730863085.08 sec; seqNo:1427; #beams:4 2024-11-06T03:18:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 38.74 , 42.61 , 42.00 2024-11-06T03:18:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.10 , -0.08 2024-11-06T03:18:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863085.10 epoch sec; seqNo:4270; 4061440.70 north; 594819.00 east; 36.69 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:18:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863084.65 2024-11-06T03:18:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:05Z- waiting for 1730863088.65, time is 1730863085.10 2024-11-06T03:18:05Z- AHRS_M2 msg: 1730863085.43 epoch sec; seqNo:4019 2024-11-06T03:18:05Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.40 psi 2024-11-06T03:18:05Z- handleDepth()- Depth_Keller msg: 1730863085.44 epoch sec; seqNo:4271; depth 49.58 2024-11-06T03:18:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863085.45 epoch sec; seqNo:4271; 4061440.31 north; 594817.79 east; 36.69 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:18:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863084.65 2024-11-06T03:18:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:05Z- waiting for 1730863088.65, time is 1730863085.45 2024-11-06T03:18:05Z- AHRS_M2 msg: 1730863085.85 epoch sec; seqNo:4020 2024-11-06T03:18:05Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.39 psi 2024-11-06T03:18:05Z- handleDepth()- Depth_Keller msg: 1730863085.86 epoch sec; seqNo:4272; depth 49.57 2024-11-06T03:18:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863085.87 epoch sec; seqNo:4272; 4061439.93 north; 594817.79 east; 36.69 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:18:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863084.65 2024-11-06T03:18:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:06Z- waiting for 1730863088.65, time is 1730863085.87 2024-11-06T03:18:06Z- AHRS_M2 msg: 1730863086.26 epoch sec; seqNo:4021 2024-11-06T03:18:06Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.39 psi 2024-11-06T03:18:06Z- handleDepth()- Depth_Keller msg: 1730863086.27 epoch sec; seqNo:4273; depth 49.56 2024-11-06T03:18:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863086.28 epoch sec; seqNo:4273; 4061439.55 north; 594817.80 east; 36.69 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:18:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863084.65 2024-11-06T03:18:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:06Z- waiting for 1730863088.65, time is 1730863086.28 2024-11-06T03:18:06Z- AHRS_M2 msg: 1730863086.66 epoch sec; seqNo:4022 2024-11-06T03:18:06Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.39 psi 2024-11-06T03:18:06Z- handleDepth()- Depth_Keller msg: 1730863086.67 epoch sec; seqNo:4274; depth 49.56 2024-11-06T03:18:06Z- handleDVL() - DVL msg epochMillisec: 1730863086.67 2024-11-06T03:18:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:18:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:18:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:18:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:18:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:18:06Z- handleDvl() - RDI_Pathfinder msg: 1730863086.67 sec; seqNo:1428; #beams:0 2024-11-06T03:18:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:18:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.10 , -0.08 2024-11-06T03:18:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863086.68 epoch sec; seqNo:4274; 4061439.17 north; 594817.80 east; 36.69 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:18:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863084.65 2024-11-06T03:18:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:06Z- waiting for 1730863088.65, time is 1730863086.68 2024-11-06T03:18:07Z- AHRS_M2 msg: 1730863087.06 epoch sec; seqNo:4023 2024-11-06T03:18:07Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.39 psi 2024-11-06T03:18:07Z- handleDepth()- Depth_Keller msg: 1730863087.07 epoch sec; seqNo:4275; depth 49.56 2024-11-06T03:18:07Z- handleDVL() - DVL msg epochMillisec: 1730863087.10 2024-11-06T03:18:07Z- handleDvl() - RDI_Pathfinder msg: 1730863087.10 sec; seqNo:1429; #beams:4 2024-11-06T03:18:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.69 , 38.74 , 42.61 , 42.00 2024-11-06T03:18:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.17 , -0.00 2024-11-06T03:18:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863087.11 epoch sec; seqNo:4275; 4061438.79 north; 594817.80 east; 36.69 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:18:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863084.65 2024-11-06T03:18:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:07Z- waiting for 1730863088.65, time is 1730863087.11 2024-11-06T03:18:07Z- AHRS_M2 msg: 1730863087.46 epoch sec; seqNo:4024 2024-11-06T03:18:07Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.39 psi 2024-11-06T03:18:07Z- handleDepth()- Depth_Keller msg: 1730863087.47 epoch sec; seqNo:4276; depth 49.54 2024-11-06T03:18:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863087.49 epoch sec; seqNo:4276; 4061438.79 north; 594817.80 east; 36.69 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T03:18:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863084.65 2024-11-06T03:18:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:07Z- waiting for 1730863088.65, time is 1730863087.49 2024-11-06T03:18:07Z- AHRS_M2 msg: 1730863087.87 epoch sec; seqNo:4025 2024-11-06T03:18:07Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.38 psi 2024-11-06T03:18:07Z- handleDepth()- Depth_Keller msg: 1730863087.87 epoch sec; seqNo:4277; depth 49.56 2024-11-06T03:18:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863087.88 epoch sec; seqNo:4277; 4061438.41 north; 594817.81 east; 36.69 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:18:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863084.65 2024-11-06T03:18:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:08Z- waiting for 1730863088.65, time is 1730863087.88 2024-11-06T03:18:08Z- AHRS_M2 msg: 1730863088.28 epoch sec; seqNo:4026 2024-11-06T03:18:08Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.38 psi 2024-11-06T03:18:08Z- handleDepth()- Depth_Keller msg: 1730863088.28 epoch sec; seqNo:4278; depth 49.54 2024-11-06T03:18:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863088.29 epoch sec; seqNo:4278; 4061438.04 north; 594817.81 east; 36.69 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T03:18:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863084.65 2024-11-06T03:18:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:08Z- waiting for 1730863088.65, time is 1730863088.29 2024-11-06T03:18:08Z- AHRS_M2 msg: 1730863088.70 epoch sec; seqNo:4027 2024-11-06T03:18:08Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.38 psi 2024-11-06T03:18:08Z- handleDepth()- Depth_Keller msg: 1730863088.72 epoch sec; seqNo:4279; depth 49.56 2024-11-06T03:18:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863088.77 epoch sec; seqNo:4279; 4061437.64 north; 594816.60 east; 36.69 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:18:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863084.65 2024-11-06T03:18:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:08Z- waiting for 1730863088.65, time is 1730863088.77 2024-11-06T03:18:08Z- LcmTrn::updateTrn() >>>> pose time 1730863088.77, meas time 1730863087.10 <<<< 2024-11-06T03:18:08Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863088.77 2024-11-06T03:18:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863087.10 sec, ping # 0; waiting for INS data more recent than 1730863084.65... 2024-11-06T03:18:08Z- TerrainNav::motionUpdate() - currEstimate = (4061438, 594816.60, 49.56) 2024-11-06T03:18:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:18:08Z- TerrainNav:: SubcloudNIS is 0.80, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:18:08Z- TNavPF::Projecting Measurements 2024-11-06T03:18:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:18:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:18:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:18:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:18:08Z- TNavFilter::4 good beams. 2024-11-06T03:18:08Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:18:08Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:18:08Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:18:08Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:18:08Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:18:08Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:18:08Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:18:08Z- TerrainMapOctree: (vehN, vehE ) = (4061439.03, 594817.57). 2024-11-06T03:18:08Z- TerrainMapOctree: Min Distance = 200.75. 2024-11-06T03:18:08Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:18:08Z- Forcing Subcloud Comparison 2024-11-06T03:18:08Z- Weighting particles with subcloud comparison 2024-11-06T03:18:08Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:09Z- meanExpectedMeasDiff: -3.735878 delta_rms_squared: 1.870496 alpha: 0.989421 2024-11-06T03:18:09Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:09Z- meanExpectedMeasDiff: 3.785048 delta_rms_squared: 1.324583 alpha: 0.985125 2024-11-06T03:18:09Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:09Z- meanExpectedMeasDiff: -1.366231 delta_rms_squared: 1.576971 alpha: 0.987476 2024-11-06T03:18:09Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:09Z- meanExpectedMeasDiff: -0.417998 delta_rms_squared: 1.548737 alpha: 0.987251 2024-11-06T03:18:09Z- Subcloud NIS: 0.805487 2024-11-06T03:18:09Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:18:09Z- TNavPF:: sumWeights = 0.026239 2024-11-06T03:18:09Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:18:09Z- TNavPF::First Term of Map Covariance Matrix is 0.33 2024-11-06T03:18:09Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:18:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863087.10 sec. 2024-11-06T03:18:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:18:09Z- variances([0], [2], and psi_berg) = 0.337382 0.184518 0.000000 2024-11-06T03:18:09Z- publishEstimatesDecoder() - at 1730863088.77 sec 2024-11-06T03:18:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:18:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061467.92 594800.07 54.27 2024-11-06T03:18:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061467.92 594799.62 54.41 lat/lon: 36.69 -121.94 2024-11-06T03:18:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.34 0.18 0.75 0.00 2024-11-06T03:18:09Z- AHRS_M2 msg: 1730863089.09 epoch sec; seqNo:4028 2024-11-06T03:18:09Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.38 psi 2024-11-06T03:18:09Z- handleDepth()- Depth_Keller msg: 1730863089.11 epoch sec; seqNo:4280; depth 49.56 2024-11-06T03:18:09Z- handleDVL() - DVL msg epochMillisec: 1730863089.14 2024-11-06T03:18:09Z- handleDvl() - RDI_Pathfinder msg: 1730863089.14 sec; seqNo:1430; #beams:4 2024-11-06T03:18:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 38.74 , 42.41 , 42.00 2024-11-06T03:18:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.13 , 0.08 2024-11-06T03:18:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863089.16 epoch sec; seqNo:4280; 4061437.26 north; 594816.60 east; 36.69 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:18:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863088.77 2024-11-06T03:18:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:09Z- waiting for 1730863092.77, time is 1730863089.16 2024-11-06T03:18:09Z- AHRS_M2 msg: 1730863089.48 epoch sec; seqNo:4029 2024-11-06T03:18:09Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.38 psi 2024-11-06T03:18:09Z- handleDepth()- Depth_Keller msg: 1730863089.49 epoch sec; seqNo:4281; depth 49.56 2024-11-06T03:18:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863089.50 epoch sec; seqNo:4281; 4061436.89 north; 594816.60 east; 36.69 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:18:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863088.77 2024-11-06T03:18:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:09Z- waiting for 1730863092.77, time is 1730863089.50 2024-11-06T03:18:09Z- handleDepth()- Depth_Keller msg: 1730863089.91 epoch sec; seqNo:4282; depth 49.55 2024-11-06T03:18:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863089.92 epoch sec; seqNo:4282; 4061436.51 north; 594816.61 east; 36.69 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:18:10Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:18:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863088.77 2024-11-06T03:18:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:10Z- waiting for 1730863092.77, time is 1730863089.92 2024-11-06T03:18:10Z- AHRS_M2 msg: 1730863090.29 epoch sec; seqNo:4030 2024-11-06T03:18:10Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.38 psi 2024-11-06T03:18:10Z- handleDepth()- Depth_Keller msg: 1730863090.30 epoch sec; seqNo:4283; depth 49.56 2024-11-06T03:18:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863090.32 epoch sec; seqNo:4283; 4061436.13 north; 594816.61 east; 36.69 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:18:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863088.77 2024-11-06T03:18:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:10Z- waiting for 1730863092.77, time is 1730863090.32 2024-11-06T03:18:10Z- AHRS_M2 msg: 1730863090.71 epoch sec; seqNo:4031 2024-11-06T03:18:10Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.38 psi 2024-11-06T03:18:10Z- handleDepth()- Depth_Keller msg: 1730863090.75 epoch sec; seqNo:4284; depth 49.57 2024-11-06T03:18:10Z- handleDVL() - DVL msg epochMillisec: 1730863090.80 2024-11-06T03:18:10Z- handleDvl() - RDI_Pathfinder msg: 1730863090.80 sec; seqNo:1431; #beams:4 2024-11-06T03:18:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 39.15 , 42.61 , 42.41 2024-11-06T03:18:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.03 , 0.08 2024-11-06T03:18:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863090.81 epoch sec; seqNo:4284; 4061435.75 north; 594816.62 east; 36.69 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:18:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863088.77 2024-11-06T03:18:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:10Z- waiting for 1730863092.77, time is 1730863090.81 2024-11-06T03:18:11Z- AHRS_M2 msg: 1730863091.10 epoch sec; seqNo:4032 2024-11-06T03:18:11Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.38 psi 2024-11-06T03:18:11Z- handleDepth()- Depth_Keller msg: 1730863091.11 epoch sec; seqNo:4285; depth 49.57 2024-11-06T03:18:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863091.12 epoch sec; seqNo:4285; 4061435.37 north; 594816.62 east; 36.69 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:18:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863088.77 2024-11-06T03:18:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:11Z- waiting for 1730863092.77, time is 1730863091.12 2024-11-06T03:18:11Z- AHRS_M2 msg: 1730863091.50 epoch sec; seqNo:4033 2024-11-06T03:18:11Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.38 psi 2024-11-06T03:18:11Z- handleDepth()- Depth_Keller msg: 1730863091.51 epoch sec; seqNo:4286; depth 49.57 2024-11-06T03:18:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863091.52 epoch sec; seqNo:4286; 4061434.99 north; 594816.63 east; 36.69 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:18:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863088.77 2024-11-06T03:18:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:11Z- waiting for 1730863092.77, time is 1730863091.52 2024-11-06T03:18:11Z- AHRS_M2 msg: 1730863091.91 epoch sec; seqNo:4034 2024-11-06T03:18:11Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.38 psi 2024-11-06T03:18:11Z- handleDepth()- Depth_Keller msg: 1730863091.92 epoch sec; seqNo:4287; depth 49.58 2024-11-06T03:18:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863091.93 epoch sec; seqNo:4287; 4061434.61 north; 594816.63 east; 36.69 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:18:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863088.77 2024-11-06T03:18:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:12Z- waiting for 1730863092.77, time is 1730863091.93 2024-11-06T03:18:12Z- AHRS_M2 msg: 1730863092.31 epoch sec; seqNo:4035 2024-11-06T03:18:12Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 3.38 psi 2024-11-06T03:18:12Z- handleDepth()- Depth_Keller msg: 1730863092.32 epoch sec; seqNo:4288; depth 49.57 2024-11-06T03:18:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863092.33 epoch sec; seqNo:4288; 4061434.23 north; 594816.63 east; 36.69 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:18:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863088.77 2024-11-06T03:18:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:12Z- waiting for 1730863092.77, time is 1730863092.33 2024-11-06T03:18:12Z- AHRS_M2 msg: 1730863092.76 epoch sec; seqNo:4036 2024-11-06T03:18:12Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 3.38 psi 2024-11-06T03:18:12Z- handleDepth()- Depth_Keller msg: 1730863092.78 epoch sec; seqNo:4289; depth 49.56 2024-11-06T03:18:12Z- handleDVL() - DVL msg epochMillisec: 1730863092.80 2024-11-06T03:18:12Z- handleDvl() - RDI_Pathfinder msg: 1730863092.80 sec; seqNo:1432; #beams:4 2024-11-06T03:18:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 38.94 , 42.61 , 42.41 2024-11-06T03:18:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.02 , 0.02 2024-11-06T03:18:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863092.82 epoch sec; seqNo:4289; 4061433.85 north; 594816.64 east; 36.69 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:18:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863088.77 2024-11-06T03:18:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:12Z- waiting for 1730863092.77, time is 1730863092.82 2024-11-06T03:18:12Z- LcmTrn::updateTrn() >>>> pose time 1730863092.82, meas time 1730863092.80 <<<< 2024-11-06T03:18:12Z- LcmTrn::updateTrn() equating pose 1730863092.80 and meas times 1730863092.80 2024-11-06T03:18:12Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863092.80 2024-11-06T03:18:12Z- TerrainNav::motionUpdate() - currEstimate = (4061434, 594816.64, 49.56) 2024-11-06T03:18:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:18:12Z- TerrainNav:: SubcloudNIS is 0.81, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:18:12Z- TNavPF::Projecting Measurements 2024-11-06T03:18:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:18:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:18:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:18:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:18:12Z- TNavFilter::4 good beams. 2024-11-06T03:18:12Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:18:12Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:18:12Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:18:12Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:18:12Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:18:12Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:18:12Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:18:12Z- TerrainMapOctree: (vehN, vehE ) = (4061433.85, 594816.64). 2024-11-06T03:18:12Z- TerrainMapOctree: Min Distance = 195.56. 2024-11-06T03:18:12Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:18:13Z- Forcing Subcloud Comparison 2024-11-06T03:18:13Z- Weighting particles with subcloud comparison 2024-11-06T03:18:13Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:13Z- meanExpectedMeasDiff: -3.168655 delta_rms_squared: 1.622124 alpha: 0.987821 2024-11-06T03:18:13Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:13Z- meanExpectedMeasDiff: 3.277211 delta_rms_squared: 1.726379 alpha: 0.988548 2024-11-06T03:18:13Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:13Z- meanExpectedMeasDiff: -1.056488 delta_rms_squared: 1.368454 alpha: 0.985595 2024-11-06T03:18:13Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:13Z- meanExpectedMeasDiff: 0.075123 delta_rms_squared: 1.309610 alpha: 0.984958 2024-11-06T03:18:13Z- Subcloud NIS: 0.820760 2024-11-06T03:18:13Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:18:13Z- TNavPF:: sumWeights = 0.068130 2024-11-06T03:18:13Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:18:13Z- TNavPF::First Term of Map Covariance Matrix is 0.63 2024-11-06T03:18:13Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.01 2024-11-06T03:18:13Z- TerrainNav::Resampling particle filter... 2024-11-06T03:18:13Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863092.80 sec, ping # 0. 2024-11-06T03:18:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:18:13Z- variances([0], [2], and psi_berg) = 0.304220 0.157591 0.000000 2024-11-06T03:18:13Z- publishEstimatesDecoder() - at 1730863092.80 sec 2024-11-06T03:18:13Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:18:13Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061464.62 594799.35 54.32 2024-11-06T03:18:13Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061464.08 594799.58 54.44 lat/lon: 36.69 -121.94 2024-11-06T03:18:13Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.30 0.16 0.44 0.00 2024-11-06T03:18:13Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:18:13Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:18:13Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:18:13Z- AHRS_M2 msg: 1730863093.11 epoch sec; seqNo:4037 2024-11-06T03:18:13Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 3.38 psi 2024-11-06T03:18:13Z- handleDepth()- Depth_Keller msg: 1730863093.15 epoch sec; seqNo:4290; depth 49.56 2024-11-06T03:18:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863093.18 epoch sec; seqNo:4290; 4061433.46 north; 594815.42 east; 36.69 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:18:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863092.80 2024-11-06T03:18:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:13Z- waiting for 1730863096.80, time is 1730863093.18 2024-11-06T03:18:13Z- AHRS_M2 msg: 1730863093.52 epoch sec; seqNo:4038 2024-11-06T03:18:13Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 3.38 psi 2024-11-06T03:18:13Z- handleDepth()- Depth_Keller msg: 1730863093.54 epoch sec; seqNo:4291; depth 49.56 2024-11-06T03:18:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863093.55 epoch sec; seqNo:4291; 4061433.08 north; 594815.43 east; 36.69 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:18:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863092.80 2024-11-06T03:18:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:13Z- waiting for 1730863096.80, time is 1730863093.55 2024-11-06T03:18:13Z- AHRS_M2 msg: 1730863093.93 epoch sec; seqNo:4039 2024-11-06T03:18:13Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 3.39 psi 2024-11-06T03:18:13Z- handleDepth()- Depth_Keller msg: 1730863093.94 epoch sec; seqNo:4292; depth 49.56 2024-11-06T03:18:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863093.95 epoch sec; seqNo:4292; 4061432.70 north; 594815.43 east; 36.69 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:18:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863092.80 2024-11-06T03:18:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:14Z- waiting for 1730863096.80, time is 1730863093.95 2024-11-06T03:18:14Z- handleDepth()- Depth_Keller msg: 1730863094.35 epoch sec; seqNo:4293; depth 49.55 2024-11-06T03:18:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863094.37 epoch sec; seqNo:4293; 4061432.33 north; 594815.43 east; 36.69 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:18:14Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:18:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863092.80 2024-11-06T03:18:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:14Z- waiting for 1730863096.80, time is 1730863094.37 2024-11-06T03:18:14Z- AHRS_M2 msg: 1730863094.77 epoch sec; seqNo:4040 2024-11-06T03:18:14Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 3.39 psi 2024-11-06T03:18:14Z- handleDepth()- Depth_Keller msg: 1730863094.77 epoch sec; seqNo:4294; depth 49.54 2024-11-06T03:18:14Z- handleDVL() - DVL msg epochMillisec: 1730863094.80 2024-11-06T03:18:14Z- handleDvl() - RDI_Pathfinder msg: 1730863094.80 sec; seqNo:1433; #beams:4 2024-11-06T03:18:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 39.15 , 42.61 , 42.41 2024-11-06T03:18:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , -0.03 2024-11-06T03:18:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863094.81 epoch sec; seqNo:4294; 4061431.95 north; 594815.44 east; 36.69 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T03:18:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863092.80 2024-11-06T03:18:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:14Z- waiting for 1730863096.80, time is 1730863094.81 2024-11-06T03:18:15Z- AHRS_M2 msg: 1730863095.14 epoch sec; seqNo:4041 2024-11-06T03:18:15Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 3.39 psi 2024-11-06T03:18:15Z- handleDepth()- Depth_Keller msg: 1730863095.16 epoch sec; seqNo:4295; depth 49.52 2024-11-06T03:18:15Z- handleDVL() - DVL msg epochMillisec: 1730863095.16 2024-11-06T03:18:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:18:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:18:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:18:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:18:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:18:15Z- handleDvl() - RDI_Pathfinder msg: 1730863095.16 sec; seqNo:1434; #beams:0 2024-11-06T03:18:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:18:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , -0.03 2024-11-06T03:18:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863095.17 epoch sec; seqNo:4295; 4061431.57 north; 594815.44 east; 36.69 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T03:18:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863092.80 2024-11-06T03:18:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:15Z- waiting for 1730863096.80, time is 1730863095.17 2024-11-06T03:18:15Z- AHRS_M2 msg: 1730863095.54 epoch sec; seqNo:4042 2024-11-06T03:18:15Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 3.39 psi 2024-11-06T03:18:15Z- handleDepth()- Depth_Keller msg: 1730863095.55 epoch sec; seqNo:4296; depth 49.53 2024-11-06T03:18:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863095.56 epoch sec; seqNo:4296; 4061431.57 north; 594815.44 east; 36.69 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T03:18:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863092.80 2024-11-06T03:18:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:15Z- waiting for 1730863096.80, time is 1730863095.56 2024-11-06T03:18:15Z- AHRS_M2 msg: 1730863095.95 epoch sec; seqNo:4043 2024-11-06T03:18:15Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 3.39 psi 2024-11-06T03:18:15Z- handleDepth()- Depth_Keller msg: 1730863095.95 epoch sec; seqNo:4297; depth 49.51 2024-11-06T03:18:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863095.96 epoch sec; seqNo:4297; 4061431.19 north; 594815.45 east; 36.69 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T03:18:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863092.80 2024-11-06T03:18:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:16Z- waiting for 1730863096.80, time is 1730863095.96 2024-11-06T03:18:16Z- AHRS_M2 msg: 1730863096.36 epoch sec; seqNo:4044 2024-11-06T03:18:16Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.39 psi 2024-11-06T03:18:16Z- handleDepth()- Depth_Keller msg: 1730863096.37 epoch sec; seqNo:4298; depth 49.50 2024-11-06T03:18:16Z- handleDVL() - DVL msg epochMillisec: 1730863096.37 2024-11-06T03:18:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:18:16Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:18:16Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:18:16Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:18:16Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:18:16Z- handleDvl() - RDI_Pathfinder msg: 1730863096.37 sec; seqNo:1435; #beams:0 2024-11-06T03:18:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:18:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , -0.03 2024-11-06T03:18:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863096.38 epoch sec; seqNo:4298; 4061430.81 north; 594815.45 east; 36.69 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:18:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863092.80 2024-11-06T03:18:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:16Z- waiting for 1730863096.80, time is 1730863096.38 2024-11-06T03:18:16Z- AHRS_M2 msg: 1730863096.77 epoch sec; seqNo:4045 2024-11-06T03:18:16Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.39 psi 2024-11-06T03:18:16Z- handleDepth()- Depth_Keller msg: 1730863096.77 epoch sec; seqNo:4299; depth 49.49 2024-11-06T03:18:16Z- handleDVL() - DVL msg epochMillisec: 1730863096.79 2024-11-06T03:18:16Z- handleDvl() - RDI_Pathfinder msg: 1730863096.79 sec; seqNo:1436; #beams:4 2024-11-06T03:18:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 38.94 , 42.41 , 42.61 2024-11-06T03:18:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , -0.04 2024-11-06T03:18:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863096.81 epoch sec; seqNo:4299; 4061430.43 north; 594815.46 east; 36.69 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:18:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863092.80 2024-11-06T03:18:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:16Z- waiting for 1730863096.80, time is 1730863096.81 2024-11-06T03:18:16Z- LcmTrn::updateTrn() >>>> pose time 1730863096.81, meas time 1730863096.79 <<<< 2024-11-06T03:18:16Z- LcmTrn::updateTrn() equating pose 1730863096.79 and meas times 1730863096.79 2024-11-06T03:18:16Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863096.79 2024-11-06T03:18:16Z- TerrainNav::motionUpdate() - currEstimate = (4061430, 594815.46, 49.49) 2024-11-06T03:18:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:18:16Z- TerrainNav:: SubcloudNIS is 0.82, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:18:16Z- TNavPF::Projecting Measurements 2024-11-06T03:18:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:18:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:18:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:18:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:18:16Z- TNavFilter::4 good beams. 2024-11-06T03:18:16Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:18:16Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:18:16Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:18:16Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:18:16Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:18:16Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:18:16Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:18:16Z- TerrainMapOctree: (vehN, vehE ) = (4061430.43, 594815.46). 2024-11-06T03:18:16Z- TerrainMapOctree: Min Distance = 191.94. 2024-11-06T03:18:16Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:18:17Z- Forcing Subcloud Comparison 2024-11-06T03:18:17Z- Weighting particles with subcloud comparison 2024-11-06T03:18:17Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:17Z- meanExpectedMeasDiff: -3.116066 delta_rms_squared: 1.123911 alpha: 0.982516 2024-11-06T03:18:17Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:17Z- meanExpectedMeasDiff: 2.075764 delta_rms_squared: 0.911008 alpha: 0.978518 2024-11-06T03:18:17Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:17Z- meanExpectedMeasDiff: -1.135809 delta_rms_squared: 0.936388 alpha: 0.979088 2024-11-06T03:18:17Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:17Z- meanExpectedMeasDiff: -0.763596 delta_rms_squared: 1.310664 alpha: 0.984970 2024-11-06T03:18:17Z- Subcloud NIS: 0.814754 2024-11-06T03:18:17Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:18:17Z- TNavPF:: sumWeights = 0.158725 2024-11-06T03:18:17Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:18:17Z- TNavPF::First Term of Map Covariance Matrix is 0.96 2024-11-06T03:18:17Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T03:18:17Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863096.79 sec, ping # 0. 2024-11-06T03:18:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:18:17Z- variances([0], [2], and psi_berg) = 0.267609 0.155819 0.000000 2024-11-06T03:18:17Z- publishEstimatesDecoder() - at 1730863096.79 sec 2024-11-06T03:18:17Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:18:17Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061458.93 594798.07 54.62 2024-11-06T03:18:17Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061460.60 594798.37 54.65 lat/lon: 36.69 -121.94 2024-11-06T03:18:17Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.16 0.38 0.00 2024-11-06T03:18:17Z- AHRS_M2 msg: 1730863097.16 epoch sec; seqNo:4046 2024-11-06T03:18:17Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.39 psi 2024-11-06T03:18:17Z- handleDepth()- Depth_Keller msg: 1730863097.19 epoch sec; seqNo:4300; depth 49.47 2024-11-06T03:18:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863097.22 epoch sec; seqNo:4300; 4061430.05 north; 594815.46 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:18:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863096.79 2024-11-06T03:18:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:17Z- waiting for 1730863100.79, time is 1730863097.22 2024-11-06T03:18:17Z- AHRS_M2 msg: 1730863097.57 epoch sec; seqNo:4047 2024-11-06T03:18:17Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.39 psi 2024-11-06T03:18:17Z- handleDepth()- Depth_Keller msg: 1730863097.58 epoch sec; seqNo:4301; depth 49.47 2024-11-06T03:18:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863097.58 epoch sec; seqNo:4301; 4061429.67 north; 594815.46 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:18:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863096.79 2024-11-06T03:18:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:17Z- waiting for 1730863100.79, time is 1730863097.58 2024-11-06T03:18:17Z- AHRS_M2 msg: 1730863097.97 epoch sec; seqNo:4048 2024-11-06T03:18:17Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.38 psi 2024-11-06T03:18:17Z- handleDepth()- Depth_Keller msg: 1730863097.97 epoch sec; seqNo:4302; depth 49.46 2024-11-06T03:18:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863097.98 epoch sec; seqNo:4302; 4061429.29 north; 594815.47 east; 36.69 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T03:18:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863096.79 2024-11-06T03:18:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:18Z- waiting for 1730863100.79, time is 1730863097.98 2024-11-06T03:18:18Z- AHRS_M2 msg: 1730863098.37 epoch sec; seqNo:4049 2024-11-06T03:18:18Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.39 psi 2024-11-06T03:18:18Z- handleDepth()- Depth_Keller msg: 1730863098.38 epoch sec; seqNo:4303; depth 49.45 2024-11-06T03:18:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863098.39 epoch sec; seqNo:4303; 4061428.92 north; 594815.47 east; 36.69 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:18:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863096.79 2024-11-06T03:18:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:18Z- waiting for 1730863100.79, time is 1730863098.39 2024-11-06T03:18:18Z- AHRS_M2 msg: 1730863098.78 epoch sec; seqNo:4050 2024-11-06T03:18:18Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.38 psi 2024-11-06T03:18:18Z- handleDepth()- Depth_Keller msg: 1730863098.79 epoch sec; seqNo:4304; depth 49.45 2024-11-06T03:18:18Z- handleDVL() - DVL msg epochMillisec: 1730863098.81 2024-11-06T03:18:18Z- handleDvl() - RDI_Pathfinder msg: 1730863098.81 sec; seqNo:1437; #beams:4 2024-11-06T03:18:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 38.74 , 42.41 , 42.61 2024-11-06T03:18:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.10 , -0.02 2024-11-06T03:18:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863098.84 epoch sec; seqNo:4304; 4061428.52 north; 594814.26 east; 36.69 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:18:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863096.79 2024-11-06T03:18:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:18Z- waiting for 1730863100.79, time is 1730863098.84 2024-11-06T03:18:19Z- AHRS_M2 msg: 1730863099.18 epoch sec; seqNo:4051 2024-11-06T03:18:19Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.38 psi 2024-11-06T03:18:19Z- handleDepth()- Depth_Keller msg: 1730863099.19 epoch sec; seqNo:4305; depth 49.44 2024-11-06T03:18:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863099.20 epoch sec; seqNo:4305; 4061428.14 north; 594814.26 east; 36.69 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:18:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863096.79 2024-11-06T03:18:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:19Z- waiting for 1730863100.79, time is 1730863099.20 2024-11-06T03:18:19Z- AHRS_M2 msg: 1730863099.58 epoch sec; seqNo:4052 2024-11-06T03:18:19Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.38 psi 2024-11-06T03:18:19Z- handleDepth()- Depth_Keller msg: 1730863099.59 epoch sec; seqNo:4306; depth 49.44 2024-11-06T03:18:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863099.60 epoch sec; seqNo:4306; 4061427.77 north; 594814.26 east; 36.69 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:18:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863096.79 2024-11-06T03:18:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:19Z- waiting for 1730863100.79, time is 1730863099.60 2024-11-06T03:18:19Z- AHRS_M2 msg: 1730863099.99 epoch sec; seqNo:4053 2024-11-06T03:18:19Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.38 psi 2024-11-06T03:18:19Z- handleDepth()- Depth_Keller msg: 1730863099.99 epoch sec; seqNo:4307; depth 49.42 2024-11-06T03:18:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863100.00 epoch sec; seqNo:4307; 4061427.39 north; 594814.27 east; 36.69 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T03:18:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863096.79 2024-11-06T03:18:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:20Z- waiting for 1730863100.79, time is 1730863100.00 2024-11-06T03:18:20Z- AHRS_M2 msg: 1730863100.39 epoch sec; seqNo:4054 2024-11-06T03:18:20Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.38 psi 2024-11-06T03:18:20Z- handleDepth()- Depth_Keller msg: 1730863100.40 epoch sec; seqNo:4308; depth 49.43 2024-11-06T03:18:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863100.41 epoch sec; seqNo:4308; 4061427.01 north; 594814.27 east; 36.69 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T03:18:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863096.79 2024-11-06T03:18:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:20Z- waiting for 1730863100.79, time is 1730863100.41 2024-11-06T03:18:20Z- AHRS_M2 msg: 1730863100.80 epoch sec; seqNo:4055 2024-11-06T03:18:20Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.38 psi 2024-11-06T03:18:20Z- handleDepth()- Depth_Keller msg: 1730863100.81 epoch sec; seqNo:4309; depth 49.42 2024-11-06T03:18:20Z- handleDVL() - DVL msg epochMillisec: 1730863100.83 2024-11-06T03:18:20Z- handleDvl() - RDI_Pathfinder msg: 1730863100.83 sec; seqNo:1438; #beams:4 2024-11-06T03:18:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 38.74 , 42.21 , 42.41 2024-11-06T03:18:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.14 , 0.01 2024-11-06T03:18:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863100.84 epoch sec; seqNo:4309; 4061426.63 north; 594814.28 east; 36.69 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T03:18:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863096.79 2024-11-06T03:18:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:20Z- waiting for 1730863100.79, time is 1730863100.84 2024-11-06T03:18:20Z- LcmTrn::updateTrn() >>>> pose time 1730863100.84, meas time 1730863100.83 <<<< 2024-11-06T03:18:20Z- LcmTrn::updateTrn() equating pose 1730863100.83 and meas times 1730863100.83 2024-11-06T03:18:20Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863100.83 2024-11-06T03:18:20Z- TerrainNav::motionUpdate() - currEstimate = (4061427, 594814.28, 49.42) 2024-11-06T03:18:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:18:20Z- TerrainNav:: SubcloudNIS is 0.81, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:18:20Z- TNavPF::Projecting Measurements 2024-11-06T03:18:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:18:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:18:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:18:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:18:20Z- TNavFilter::4 good beams. 2024-11-06T03:18:20Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:18:20Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:18:20Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:18:20Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:18:20Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:18:20Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:18:20Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:18:20Z- TerrainMapOctree: (vehN, vehE ) = (4061426.63, 594814.28). 2024-11-06T03:18:20Z- TerrainMapOctree: Min Distance = 187.96. 2024-11-06T03:18:20Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:18:21Z- Forcing Subcloud Comparison 2024-11-06T03:18:21Z- Weighting particles with subcloud comparison 2024-11-06T03:18:21Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:21Z- meanExpectedMeasDiff: -3.082589 delta_rms_squared: 1.014757 alpha: 0.980672 2024-11-06T03:18:21Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:21Z- meanExpectedMeasDiff: 0.904143 delta_rms_squared: 0.719645 alpha: 0.972960 2024-11-06T03:18:21Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:21Z- meanExpectedMeasDiff: -1.076076 delta_rms_squared: 0.843495 alpha: 0.976838 2024-11-06T03:18:21Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:21Z- meanExpectedMeasDiff: -1.075506 delta_rms_squared: 0.890619 alpha: 0.978037 2024-11-06T03:18:21Z- Subcloud NIS: 0.814108 2024-11-06T03:18:21Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:18:21Z- TNavPF:: sumWeights = 0.270181 2024-11-06T03:18:21Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:18:21Z- TNavPF::First Term of Map Covariance Matrix is 1.13 2024-11-06T03:18:21Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T03:18:21Z- TerrainNav::Resampling particle filter... 2024-11-06T03:18:21Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863100.83 sec, ping # 0. 2024-11-06T03:18:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:18:21Z- variances([0], [2], and psi_berg) = 0.257977 0.157288 0.000000 2024-11-06T03:18:21Z- publishEstimatesDecoder() - at 1730863100.83 sec 2024-11-06T03:18:21Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:18:21Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061457.38 594796.95 54.49 2024-11-06T03:18:21Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061456.78 594797.19 54.95 lat/lon: 36.69 -121.94 2024-11-06T03:18:21Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.16 0.37 0.00 2024-11-06T03:18:21Z- AHRS_M2 msg: 1730863101.20 epoch sec; seqNo:4056 2024-11-06T03:18:21Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.38 psi 2024-11-06T03:18:21Z- handleDepth()- Depth_Keller msg: 1730863101.22 epoch sec; seqNo:4310; depth 49.42 2024-11-06T03:18:21Z- handleDVL() - DVL msg epochMillisec: 1730863101.22 2024-11-06T03:18:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:18:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:18:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:18:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:18:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:18:21Z- handleDvl() - RDI_Pathfinder msg: 1730863101.22 sec; seqNo:1439; #beams:0 2024-11-06T03:18:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:18:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.14 , 0.01 2024-11-06T03:18:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863101.25 epoch sec; seqNo:4310; 4061426.63 north; 594814.28 east; 36.69 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T03:18:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863100.83 2024-11-06T03:18:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:21Z- waiting for 1730863104.83, time is 1730863101.25 2024-11-06T03:18:21Z- AHRS_M2 msg: 1730863101.64 epoch sec; seqNo:4057 2024-11-06T03:18:21Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.38 psi 2024-11-06T03:18:21Z- handleDepth()- Depth_Keller msg: 1730863101.70 epoch sec; seqNo:4311; depth 49.40 2024-11-06T03:18:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863101.73 epoch sec; seqNo:4311; 4061425.87 north; 594814.29 east; 36.69 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T03:18:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863100.83 2024-11-06T03:18:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:21Z- waiting for 1730863104.83, time is 1730863101.73 2024-11-06T03:18:22Z- AHRS_M2 msg: 1730863102.01 epoch sec; seqNo:4058 2024-11-06T03:18:22Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.38 psi 2024-11-06T03:18:22Z- handleDepth()- Depth_Keller msg: 1730863102.01 epoch sec; seqNo:4312; depth 49.43 2024-11-06T03:18:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863102.02 epoch sec; seqNo:4312; 4061425.87 north; 594814.29 east; 36.69 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T03:18:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863100.83 2024-11-06T03:18:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:22Z- waiting for 1730863104.83, time is 1730863102.02 2024-11-06T03:18:22Z- AHRS_M2 msg: 1730863102.41 epoch sec; seqNo:4059 2024-11-06T03:18:22Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.38 psi 2024-11-06T03:18:22Z- handleDepth()- Depth_Keller msg: 1730863102.42 epoch sec; seqNo:4313; depth 49.42 2024-11-06T03:18:22Z- handleDVL() - DVL msg epochMillisec: 1730863102.43 2024-11-06T03:18:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:18:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:18:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:18:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:18:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:18:22Z- handleDvl() - RDI_Pathfinder msg: 1730863102.43 sec; seqNo:1440; #beams:0 2024-11-06T03:18:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:18:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.14 , 0.01 2024-11-06T03:18:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863102.44 epoch sec; seqNo:4313; 4061425.48 north; 594813.07 east; 36.69 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T03:18:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863100.83 2024-11-06T03:18:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:22Z- waiting for 1730863104.83, time is 1730863102.44 2024-11-06T03:18:22Z- AHRS_M2 msg: 1730863102.82 epoch sec; seqNo:4060 2024-11-06T03:18:22Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.37 psi 2024-11-06T03:18:22Z- handleDepth()- Depth_Keller msg: 1730863102.83 epoch sec; seqNo:4314; depth 49.43 2024-11-06T03:18:22Z- handleDVL() - DVL msg epochMillisec: 1730863102.85 2024-11-06T03:18:22Z- handleDvl() - RDI_Pathfinder msg: 1730863102.85 sec; seqNo:1441; #beams:4 2024-11-06T03:18:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 38.94 , 42.00 , 42.61 2024-11-06T03:18:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.12 , 0.07 2024-11-06T03:18:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863102.87 epoch sec; seqNo:4314; 4061425.10 north; 594813.07 east; 36.69 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T03:18:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863100.83 2024-11-06T03:18:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:22Z- waiting for 1730863104.83, time is 1730863102.87 2024-11-06T03:18:23Z- AHRS_M2 msg: 1730863103.22 epoch sec; seqNo:4061 2024-11-06T03:18:23Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.37 psi 2024-11-06T03:18:23Z- handleDepth()- Depth_Keller msg: 1730863103.23 epoch sec; seqNo:4315; depth 49.42 2024-11-06T03:18:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863103.27 epoch sec; seqNo:4315; 4061424.72 north; 594813.08 east; 36.69 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T03:18:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863100.83 2024-11-06T03:18:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:23Z- waiting for 1730863104.83, time is 1730863103.27 2024-11-06T03:18:23Z- AHRS_M2 msg: 1730863103.63 epoch sec; seqNo:4062 2024-11-06T03:18:23Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.37 psi 2024-11-06T03:18:23Z- handleDepth()- Depth_Keller msg: 1730863103.64 epoch sec; seqNo:4316; depth 49.43 2024-11-06T03:18:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863103.66 epoch sec; seqNo:4316; 4061424.34 north; 594813.08 east; 36.69 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T03:18:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863100.83 2024-11-06T03:18:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:23Z- waiting for 1730863104.83, time is 1730863103.66 2024-11-06T03:18:24Z- AHRS_M2 msg: 1730863104.03 epoch sec; seqNo:4063 2024-11-06T03:18:24Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.37 psi 2024-11-06T03:18:24Z- handleDepth()- Depth_Keller msg: 1730863104.04 epoch sec; seqNo:4317; depth 49.43 2024-11-06T03:18:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863104.05 epoch sec; seqNo:4317; 4061423.96 north; 594813.09 east; 36.69 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T03:18:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863100.83 2024-11-06T03:18:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:24Z- waiting for 1730863104.83, time is 1730863104.05 2024-11-06T03:18:24Z- handleDepth()- Depth_Keller msg: 1730863104.45 epoch sec; seqNo:4318; depth 49.44 2024-11-06T03:18:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863104.46 epoch sec; seqNo:4318; 4061423.59 north; 594813.09 east; 36.69 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:18:24Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:18:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863100.83 2024-11-06T03:18:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:24Z- waiting for 1730863104.83, time is 1730863104.46 2024-11-06T03:18:24Z- AHRS_M2 msg: 1730863104.83 epoch sec; seqNo:4064 2024-11-06T03:18:24Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.38 psi 2024-11-06T03:18:24Z- handleDepth()- Depth_Keller msg: 1730863104.85 epoch sec; seqNo:4319; depth 49.44 2024-11-06T03:18:24Z- handleDVL() - DVL msg epochMillisec: 1730863104.87 2024-11-06T03:18:24Z- handleDvl() - RDI_Pathfinder msg: 1730863104.87 sec; seqNo:1442; #beams:4 2024-11-06T03:18:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.69 , 39.15 , 42.00 , 43.02 2024-11-06T03:18:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , 0.09 2024-11-06T03:18:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863104.88 epoch sec; seqNo:4319; 4061423.21 north; 594813.09 east; 36.69 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:18:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863100.83 2024-11-06T03:18:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:25Z- waiting for 1730863104.83, time is 1730863104.88 2024-11-06T03:18:25Z- LcmTrn::updateTrn() >>>> pose time 1730863104.88, meas time 1730863104.87 <<<< 2024-11-06T03:18:25Z- LcmTrn::updateTrn() equating pose 1730863104.87 and meas times 1730863104.87 2024-11-06T03:18:25Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863104.87 2024-11-06T03:18:25Z- TerrainNav::motionUpdate() - currEstimate = (4061423, 594813.09, 49.44) 2024-11-06T03:18:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:18:25Z- TerrainNav:: SubcloudNIS is 0.81, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:18:25Z- TNavPF::Projecting Measurements 2024-11-06T03:18:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:18:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:18:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:18:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:18:25Z- TNavFilter::4 good beams. 2024-11-06T03:18:25Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:18:25Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:18:25Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:18:25Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:18:25Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:18:25Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:18:25Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:18:25Z- TerrainMapOctree: (vehN, vehE ) = (4061423.21, 594813.09). 2024-11-06T03:18:25Z- TerrainMapOctree: Min Distance = 184.34. 2024-11-06T03:18:25Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:18:25Z- Forcing Subcloud Comparison 2024-11-06T03:18:25Z- Weighting particles with subcloud comparison 2024-11-06T03:18:25Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:25Z- meanExpectedMeasDiff: -2.980909 delta_rms_squared: 1.021270 alpha: 0.980793 2024-11-06T03:18:25Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:25Z- meanExpectedMeasDiff: 1.804579 delta_rms_squared: 0.701664 alpha: 0.972286 2024-11-06T03:18:25Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:25Z- meanExpectedMeasDiff: -0.465837 delta_rms_squared: 0.813038 alpha: 0.975991 2024-11-06T03:18:25Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:25Z- meanExpectedMeasDiff: -0.433097 delta_rms_squared: 0.924439 alpha: 0.978823 2024-11-06T03:18:25Z- Subcloud NIS: 0.812044 2024-11-06T03:18:25Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:18:25Z- TNavPF:: sumWeights = 0.226339 2024-11-06T03:18:25Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:18:25Z- TNavPF::First Term of Map Covariance Matrix is 1.16 2024-11-06T03:18:25Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T03:18:25Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863104.87 sec, ping # 0. 2024-11-06T03:18:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:18:25Z- variances([0], [2], and psi_berg) = 0.258295 0.156610 0.000000 2024-11-06T03:18:25Z- publishEstimatesDecoder() - at 1730863104.87 sec 2024-11-06T03:18:25Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:18:25Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061454.07 594796.49 55.40 2024-11-06T03:18:25Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061453.35 594796.01 55.14 lat/lon: 36.69 -121.94 2024-11-06T03:18:25Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.16 0.35 0.00 2024-11-06T03:18:25Z- AHRS_M2 msg: 1730863105.24 epoch sec; seqNo:4065 2024-11-06T03:18:25Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 3.38 psi 2024-11-06T03:18:25Z- handleDepth()- Depth_Keller msg: 1730863105.25 epoch sec; seqNo:4320; depth 49.46 2024-11-06T03:18:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863105.27 epoch sec; seqNo:4320; 4061422.83 north; 594813.10 east; 36.69 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T03:18:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863104.87 2024-11-06T03:18:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:25Z- waiting for 1730863108.87, time is 1730863105.27 2024-11-06T03:18:25Z- handleDepth()- Depth_Keller msg: 1730863105.67 epoch sec; seqNo:4321; depth 49.46 2024-11-06T03:18:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863105.68 epoch sec; seqNo:4321; 4061422.45 north; 594813.10 east; 36.69 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T03:18:25Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:18:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863104.87 2024-11-06T03:18:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:25Z- waiting for 1730863108.87, time is 1730863105.68 2024-11-06T03:18:26Z- AHRS_M2 msg: 1730863106.05 epoch sec; seqNo:4066 2024-11-06T03:18:26Z- AHRS_M2 msg: 0.20 phi; -0.03 theta; 3.41 psi 2024-11-06T03:18:26Z- handleDepth()- Depth_Keller msg: 1730863106.06 epoch sec; seqNo:4322; depth 49.46 2024-11-06T03:18:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863106.07 epoch sec; seqNo:4322; 4061422.07 north; 594813.11 east; 36.69 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T03:18:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863104.87 2024-11-06T03:18:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:26Z- waiting for 1730863108.87, time is 1730863106.07 2024-11-06T03:18:26Z- AHRS_M2 msg: 1730863106.45 epoch sec; seqNo:4067 2024-11-06T03:18:26Z- AHRS_M2 msg: 0.22 phi; -0.04 theta; 3.43 psi 2024-11-06T03:18:26Z- handleDepth()- Depth_Keller msg: 1730863106.47 epoch sec; seqNo:4323; depth 49.47 2024-11-06T03:18:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863106.48 epoch sec; seqNo:4323; 4061421.68 north; 594811.89 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:18:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863104.87 2024-11-06T03:18:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:26Z- waiting for 1730863108.87, time is 1730863106.48 2024-11-06T03:18:26Z- AHRS_M2 msg: 1730863106.86 epoch sec; seqNo:4068 2024-11-06T03:18:26Z- AHRS_M2 msg: 0.25 phi; -0.04 theta; 3.45 psi 2024-11-06T03:18:26Z- handleDepth()- Depth_Keller msg: 1730863106.87 epoch sec; seqNo:4324; depth 49.47 2024-11-06T03:18:26Z- handleDVL() - DVL msg epochMillisec: 1730863106.89 2024-11-06T03:18:26Z- handleDvl() - RDI_Pathfinder msg: 1730863106.89 sec; seqNo:1443; #beams:4 2024-11-06T03:18:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.93 , 38.53 , 42.00 , 44.04 2024-11-06T03:18:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.03 , 0.04 2024-11-06T03:18:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863106.90 epoch sec; seqNo:4324; 4061421.30 north; 594811.90 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:18:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863104.87 2024-11-06T03:18:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:27Z- waiting for 1730863108.87, time is 1730863106.90 2024-11-06T03:18:27Z- AHRS_M2 msg: 1730863107.27 epoch sec; seqNo:4069 2024-11-06T03:18:27Z- AHRS_M2 msg: 0.29 phi; -0.05 theta; 3.48 psi 2024-11-06T03:18:27Z- handleDepth()- Depth_Keller msg: 1730863107.28 epoch sec; seqNo:4325; depth 49.47 2024-11-06T03:18:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863107.29 epoch sec; seqNo:4325; 4061420.92 north; 594811.90 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:18:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863104.87 2024-11-06T03:18:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:27Z- waiting for 1730863108.87, time is 1730863107.29 2024-11-06T03:18:27Z- AHRS_M2 msg: 1730863107.66 epoch sec; seqNo:4070 2024-11-06T03:18:27Z- AHRS_M2 msg: 0.33 phi; -0.06 theta; 3.51 psi 2024-11-06T03:18:27Z- handleDepth()- Depth_Keller msg: 1730863107.67 epoch sec; seqNo:4326; depth 49.48 2024-11-06T03:18:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863107.68 epoch sec; seqNo:4326; 4061420.92 north; 594811.90 east; 36.69 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:18:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863104.87 2024-11-06T03:18:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:27Z- waiting for 1730863108.87, time is 1730863107.68 2024-11-06T03:18:28Z- AHRS_M2 msg: 1730863108.07 epoch sec; seqNo:4071 2024-11-06T03:18:28Z- AHRS_M2 msg: 0.37 phi; -0.07 theta; 3.54 psi 2024-11-06T03:18:28Z- handleDepth()- Depth_Keller msg: 1730863108.08 epoch sec; seqNo:4327; depth 49.49 2024-11-06T03:18:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863108.10 epoch sec; seqNo:4327; 4061420.54 north; 594811.90 east; 36.69 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:18:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863104.87 2024-11-06T03:18:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:28Z- waiting for 1730863108.87, time is 1730863108.10 2024-11-06T03:18:28Z- AHRS_M2 msg: 1730863108.47 epoch sec; seqNo:4072 2024-11-06T03:18:28Z- AHRS_M2 msg: 0.40 phi; -0.08 theta; 3.58 psi 2024-11-06T03:18:28Z- handleDepth()- Depth_Keller msg: 1730863108.48 epoch sec; seqNo:4328; depth 49.49 2024-11-06T03:18:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863108.49 epoch sec; seqNo:4328; 4061420.16 north; 594811.91 east; 36.69 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:18:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863104.87 2024-11-06T03:18:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:28Z- waiting for 1730863108.87, time is 1730863108.49 2024-11-06T03:18:28Z- AHRS_M2 msg: 1730863108.88 epoch sec; seqNo:4073 2024-11-06T03:18:28Z- AHRS_M2 msg: 0.42 phi; -0.10 theta; 3.61 psi 2024-11-06T03:18:28Z- handleDepth()- Depth_Keller msg: 1730863108.88 epoch sec; seqNo:4329; depth 49.51 2024-11-06T03:18:28Z- handleDVL() - DVL msg epochMillisec: 1730863108.90 2024-11-06T03:18:28Z- handleDvl() - RDI_Pathfinder msg: 1730863108.90 sec; seqNo:1444; #beams:4 2024-11-06T03:18:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 59.53 , 37.31 , 43.43 , 48.12 2024-11-06T03:18:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , -0.05 2024-11-06T03:18:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863108.91 epoch sec; seqNo:4329; 4061419.78 north; 594811.91 east; 36.69 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T03:18:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863104.87 2024-11-06T03:18:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:29Z- waiting for 1730863108.87, time is 1730863108.91 2024-11-06T03:18:29Z- LcmTrn::updateTrn() >>>> pose time 1730863108.91, meas time 1730863108.90 <<<< 2024-11-06T03:18:29Z- LcmTrn::updateTrn() equating pose 1730863108.90 and meas times 1730863108.90 2024-11-06T03:18:29Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863108.90 2024-11-06T03:18:29Z- TerrainNav::motionUpdate() - currEstimate = (4061420, 594811.91, 49.51) 2024-11-06T03:18:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:18:29Z- TerrainNav:: SubcloudNIS is 0.81, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:18:29Z- TNavPF::Projecting Measurements 2024-11-06T03:18:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:18:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:18:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:18:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:18:29Z- TNavFilter::4 good beams. 2024-11-06T03:18:29Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:18:29Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:18:29Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:18:29Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:18:29Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:18:29Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:18:29Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:18:29Z- TerrainMapOctree: (vehN, vehE ) = (4061419.78, 594811.91). 2024-11-06T03:18:29Z- TerrainMapOctree: Min Distance = 180.72. 2024-11-06T03:18:29Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:18:29Z- Forcing Subcloud Comparison 2024-11-06T03:18:29Z- Weighting particles with subcloud comparison 2024-11-06T03:18:29Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:29Z- meanExpectedMeasDiff: -8.290318 delta_rms_squared: 1.793540 alpha: 0.988972 2024-11-06T03:18:29Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:29Z- meanExpectedMeasDiff: 1.423357 delta_rms_squared: 0.618920 alpha: 0.968697 2024-11-06T03:18:29Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:29Z- meanExpectedMeasDiff: -1.531957 delta_rms_squared: 0.869189 alpha: 0.977508 2024-11-06T03:18:29Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:29Z- meanExpectedMeasDiff: -3.137365 delta_rms_squared: 2.315112 alpha: 0.991435 2024-11-06T03:18:29Z- Subcloud NIS: 0.824532 2024-11-06T03:18:29Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:18:29Z- TNavPF:: sumWeights = 0.007404 2024-11-06T03:18:29Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:18:29Z- TNavPF::First Term of Map Covariance Matrix is 0.34 2024-11-06T03:18:29Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:18:29Z- TerrainNav::Resampling particle filter... 2024-11-06T03:18:29Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863108.90 sec, ping # 0. 2024-11-06T03:18:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:18:29Z- variances([0], [2], and psi_berg) = 0.260859 0.147249 0.000000 2024-11-06T03:18:29Z- publishEstimatesDecoder() - at 1730863108.90 sec 2024-11-06T03:18:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:18:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061450.71 594794.89 54.47 2024-11-06T03:18:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061449.92 594794.82 56.13 lat/lon: 36.69 -121.94 2024-11-06T03:18:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.15 0.32 0.00 2024-11-06T03:18:29Z- handleDepth()- Depth_Keller msg: 1730863109.35 epoch sec; seqNo:4330; depth 49.50 2024-11-06T03:18:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863109.36 epoch sec; seqNo:4330; 4061419.40 north; 594811.92 east; 36.69 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:18:29Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:18:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863108.90 2024-11-06T03:18:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:29Z- waiting for 1730863112.90, time is 1730863109.36 2024-11-06T03:18:29Z- handleDepth()- Depth_Keller msg: 1730863109.72 epoch sec; seqNo:4331; depth 49.53 2024-11-06T03:18:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863109.73 epoch sec; seqNo:4331; 4061419.03 north; 594811.92 east; 36.69 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T03:18:29Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:18:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863108.90 2024-11-06T03:18:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:29Z- waiting for 1730863112.90, time is 1730863109.73 2024-11-06T03:18:30Z- AHRS_M2 msg: 1730863110.10 epoch sec; seqNo:4074 2024-11-06T03:18:30Z- AHRS_M2 msg: 0.45 phi; -0.13 theta; 3.72 psi 2024-11-06T03:18:30Z- handleDepth()- Depth_Keller msg: 1730863110.11 epoch sec; seqNo:4332; depth 49.55 2024-11-06T03:18:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863110.13 epoch sec; seqNo:4332; 4061418.63 north; 594810.70 east; 36.69 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:18:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863108.90 2024-11-06T03:18:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:30Z- waiting for 1730863112.90, time is 1730863110.13 2024-11-06T03:18:30Z- AHRS_M2 msg: 1730863110.51 epoch sec; seqNo:4075 2024-11-06T03:18:30Z- AHRS_M2 msg: 0.44 phi; -0.15 theta; 3.78 psi 2024-11-06T03:18:30Z- handleDepth()- Depth_Keller msg: 1730863110.52 epoch sec; seqNo:4333; depth 49.55 2024-11-06T03:18:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863110.53 epoch sec; seqNo:4333; 4061418.63 north; 594810.70 east; 36.69 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:18:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863108.90 2024-11-06T03:18:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:30Z- waiting for 1730863112.90, time is 1730863110.53 2024-11-06T03:18:30Z- AHRS_M2 msg: 1730863110.91 epoch sec; seqNo:4076 2024-11-06T03:18:30Z- AHRS_M2 msg: 0.44 phi; -0.16 theta; 3.81 psi 2024-11-06T03:18:30Z- handleDepth()- Depth_Keller msg: 1730863110.92 epoch sec; seqNo:4334; depth 49.60 2024-11-06T03:18:30Z- handleDVL() - DVL msg epochMillisec: 1730863110.94 2024-11-06T03:18:30Z- handleDvl() - RDI_Pathfinder msg: 1730863110.94 sec; seqNo:1445; #beams:4 2024-11-06T03:18:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.25 , 37.11 , 43.29 , 52.13 2024-11-06T03:18:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.09 , -0.04 2024-11-06T03:18:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863110.98 epoch sec; seqNo:4334; 4061418.25 north; 594810.71 east; 36.69 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:18:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863108.90 2024-11-06T03:18:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:31Z- waiting for 1730863112.90, time is 1730863110.98 2024-11-06T03:18:31Z- AHRS_M2 msg: 1730863111.31 epoch sec; seqNo:4077 2024-11-06T03:18:31Z- AHRS_M2 msg: 0.43 phi; -0.16 theta; 3.85 psi 2024-11-06T03:18:31Z- handleDepth()- Depth_Keller msg: 1730863111.32 epoch sec; seqNo:4335; depth 49.63 2024-11-06T03:18:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863111.35 epoch sec; seqNo:4335; 4061417.88 north; 594810.71 east; 36.69 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:18:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863108.90 2024-11-06T03:18:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:31Z- waiting for 1730863112.90, time is 1730863111.35 2024-11-06T03:18:31Z- AHRS_M2 msg: 1730863111.72 epoch sec; seqNo:4078 2024-11-06T03:18:31Z- AHRS_M2 msg: 0.42 phi; -0.17 theta; 3.88 psi 2024-11-06T03:18:31Z- handleDepth()- Depth_Keller msg: 1730863111.72 epoch sec; seqNo:4336; depth 49.66 2024-11-06T03:18:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863111.73 epoch sec; seqNo:4336; 4061417.88 north; 594810.71 east; 36.69 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:18:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863108.90 2024-11-06T03:18:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:31Z- waiting for 1730863112.90, time is 1730863111.73 2024-11-06T03:18:32Z- AHRS_M2 msg: 1730863112.13 epoch sec; seqNo:4079 2024-11-06T03:18:32Z- AHRS_M2 msg: 0.42 phi; -0.17 theta; 3.91 psi 2024-11-06T03:18:32Z- handleDepth()- Depth_Keller msg: 1730863112.13 epoch sec; seqNo:4337; depth 49.70 2024-11-06T03:18:32Z- handleDVL() - DVL msg epochMillisec: 1730863112.14 2024-11-06T03:18:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:18:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:18:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:18:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:18:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:18:32Z- handleDvl() - RDI_Pathfinder msg: 1730863112.14 sec; seqNo:1446; #beams:0 2024-11-06T03:18:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:18:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.09 , -0.04 2024-11-06T03:18:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863112.14 epoch sec; seqNo:4337; 4061417.48 north; 594809.50 east; 36.69 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:18:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863108.90 2024-11-06T03:18:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:32Z- waiting for 1730863112.90, time is 1730863112.14 2024-11-06T03:18:32Z- AHRS_M2 msg: 1730863112.53 epoch sec; seqNo:4080 2024-11-06T03:18:32Z- AHRS_M2 msg: 0.41 phi; -0.18 theta; 3.95 psi 2024-11-06T03:18:32Z- handleDepth()- Depth_Keller msg: 1730863112.54 epoch sec; seqNo:4338; depth 49.75 2024-11-06T03:18:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863112.55 epoch sec; seqNo:4338; 4061417.10 north; 594809.50 east; 36.69 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:18:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863108.90 2024-11-06T03:18:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:32Z- waiting for 1730863112.90, time is 1730863112.55 2024-11-06T03:18:32Z- AHRS_M2 msg: 1730863112.93 epoch sec; seqNo:4081 2024-11-06T03:18:32Z- AHRS_M2 msg: 0.41 phi; -0.18 theta; 3.98 psi 2024-11-06T03:18:32Z- handleDepth()- Depth_Keller msg: 1730863112.94 epoch sec; seqNo:4339; depth 49.80 2024-11-06T03:18:32Z- handleDVL() - DVL msg epochMillisec: 1730863112.97 2024-11-06T03:18:32Z- handleDvl() - RDI_Pathfinder msg: 1730863112.97 sec; seqNo:1447; #beams:4 2024-11-06T03:18:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.94 , 36.87 , 41.87 , 52.57 2024-11-06T03:18:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.12 , 0.04 2024-11-06T03:18:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863112.98 epoch sec; seqNo:4339; 4061417.10 north; 594809.50 east; 36.69 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:18:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863108.90 2024-11-06T03:18:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:33Z- waiting for 1730863112.90, time is 1730863112.98 2024-11-06T03:18:33Z- LcmTrn::updateTrn() >>>> pose time 1730863112.98, meas time 1730863112.97 <<<< 2024-11-06T03:18:33Z- LcmTrn::updateTrn() equating pose 1730863112.97 and meas times 1730863112.97 2024-11-06T03:18:33Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863112.97 2024-11-06T03:18:33Z- TerrainNav::motionUpdate() - currEstimate = (4061417, 594809.50, 49.80) 2024-11-06T03:18:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:18:33Z- TerrainNav:: SubcloudNIS is 0.82, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:18:33Z- TNavPF::Projecting Measurements 2024-11-06T03:18:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:18:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:18:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:18:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:18:33Z- TNavFilter::4 good beams. 2024-11-06T03:18:33Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:18:33Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:18:33Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:18:33Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:18:33Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:18:33Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:18:33Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:18:33Z- TerrainMapOctree: (vehN, vehE ) = (4061417.10, 594809.50). 2024-11-06T03:18:33Z- TerrainMapOctree: Min Distance = 177.38. 2024-11-06T03:18:33Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:18:33Z- Forcing Subcloud Comparison 2024-11-06T03:18:33Z- Weighting particles with subcloud comparison 2024-11-06T03:18:33Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:33Z- meanExpectedMeasDiff: 1.182730 delta_rms_squared: 1.866955 alpha: 0.989401 2024-11-06T03:18:33Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:33Z- meanExpectedMeasDiff: 1.766030 delta_rms_squared: 0.589741 alpha: 0.967199 2024-11-06T03:18:33Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:33Z- meanExpectedMeasDiff: 1.295466 delta_rms_squared: 0.746341 alpha: 0.973902 2024-11-06T03:18:33Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:33Z- meanExpectedMeasDiff: -1.306215 delta_rms_squared: 1.184685 alpha: 0.983398 2024-11-06T03:18:33Z- Subcloud NIS: 0.789885 2024-11-06T03:18:33Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:18:33Z- TNavPF:: sumWeights = 0.380712 2024-11-06T03:18:33Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:18:33Z- TNavPF::First Term of Map Covariance Matrix is 0.48 2024-11-06T03:18:33Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T03:18:33Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863112.97 sec, ping # 0. 2024-11-06T03:18:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:18:33Z- variances([0], [2], and psi_berg) = 0.261801 0.147136 0.000000 2024-11-06T03:18:33Z- publishEstimatesDecoder() - at 1730863112.97 sec 2024-11-06T03:18:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:18:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061448.80 594793.22 55.88 2024-11-06T03:18:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061447.24 594792.41 56.18 lat/lon: 36.69 -121.94 2024-11-06T03:18:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.15 0.32 0.00 2024-11-06T03:18:33Z- AHRS_M2 msg: 1730863113.34 epoch sec; seqNo:4082 2024-11-06T03:18:33Z- AHRS_M2 msg: 0.41 phi; -0.18 theta; 4.01 psi 2024-11-06T03:18:33Z- handleDepth()- Depth_Keller msg: 1730863113.35 epoch sec; seqNo:4340; depth 49.86 2024-11-06T03:18:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863113.38 epoch sec; seqNo:4340; 4061416.73 north; 594809.50 east; 36.69 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:18:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863112.97 2024-11-06T03:18:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:33Z- waiting for 1730863116.97, time is 1730863113.38 2024-11-06T03:18:33Z- AHRS_M2 msg: 1730863113.75 epoch sec; seqNo:4083 2024-11-06T03:18:33Z- AHRS_M2 msg: 0.40 phi; -0.18 theta; 4.05 psi 2024-11-06T03:18:33Z- handleDepth()- Depth_Keller msg: 1730863113.75 epoch sec; seqNo:4341; depth 49.89 2024-11-06T03:18:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863113.76 epoch sec; seqNo:4341; 4061416.71 north; 594808.28 east; 36.69 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:18:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863112.97 2024-11-06T03:18:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:33Z- waiting for 1730863116.97, time is 1730863113.76 2024-11-06T03:18:34Z- AHRS_M2 msg: 1730863114.15 epoch sec; seqNo:4084 2024-11-06T03:18:34Z- AHRS_M2 msg: 0.40 phi; -0.18 theta; 4.08 psi 2024-11-06T03:18:34Z- handleDepth()- Depth_Keller msg: 1730863114.15 epoch sec; seqNo:4342; depth 49.96 2024-11-06T03:18:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863114.16 epoch sec; seqNo:4342; 4061416.33 north; 594808.29 east; 36.69 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:18:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863112.97 2024-11-06T03:18:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:34Z- waiting for 1730863116.97, time is 1730863114.16 2024-11-06T03:18:34Z- AHRS_M2 msg: 1730863114.55 epoch sec; seqNo:4085 2024-11-06T03:18:34Z- AHRS_M2 msg: 0.40 phi; -0.18 theta; 4.12 psi 2024-11-06T03:18:34Z- handleDepth()- Depth_Keller msg: 1730863114.56 epoch sec; seqNo:4343; depth 50.03 2024-11-06T03:18:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863114.57 epoch sec; seqNo:4343; 4061416.33 north; 594808.29 east; 36.69 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:18:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863112.97 2024-11-06T03:18:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:34Z- waiting for 1730863116.97, time is 1730863114.57 2024-11-06T03:18:34Z- AHRS_M2 msg: 1730863114.95 epoch sec; seqNo:4086 2024-11-06T03:18:34Z- AHRS_M2 msg: 0.40 phi; -0.17 theta; 4.15 psi 2024-11-06T03:18:34Z- handleDepth()- Depth_Keller msg: 1730863114.96 epoch sec; seqNo:4344; depth 50.08 2024-11-06T03:18:34Z- handleDVL() - DVL msg epochMillisec: 1730863114.98 2024-11-06T03:18:34Z- handleDvl() - RDI_Pathfinder msg: 1730863114.98 sec; seqNo:1448; #beams:4 2024-11-06T03:18:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.32 , 36.87 , 40.91 , 52.09 2024-11-06T03:18:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , 0.12 2024-11-06T03:18:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863115.00 epoch sec; seqNo:4344; 4061415.95 north; 594808.29 east; 36.69 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:18:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863112.97 2024-11-06T03:18:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:35Z- waiting for 1730863116.97, time is 1730863115.00 2024-11-06T03:18:35Z- AHRS_M2 msg: 1730863115.36 epoch sec; seqNo:4087 2024-11-06T03:18:35Z- AHRS_M2 msg: 0.40 phi; -0.17 theta; 4.19 psi 2024-11-06T03:18:35Z- handleDepth()- Depth_Keller msg: 1730863115.37 epoch sec; seqNo:4345; depth 50.16 2024-11-06T03:18:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863115.38 epoch sec; seqNo:4345; 4061415.94 north; 594807.07 east; 36.69 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:18:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863112.97 2024-11-06T03:18:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:35Z- waiting for 1730863116.97, time is 1730863115.38 2024-11-06T03:18:35Z- AHRS_M2 msg: 1730863115.76 epoch sec; seqNo:4088 2024-11-06T03:18:35Z- AHRS_M2 msg: 0.40 phi; -0.16 theta; 4.23 psi 2024-11-06T03:18:35Z- handleDepth()- Depth_Keller msg: 1730863115.78 epoch sec; seqNo:4346; depth 50.23 2024-11-06T03:18:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863115.79 epoch sec; seqNo:4346; 4061415.56 north; 594807.08 east; 36.69 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:18:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863112.97 2024-11-06T03:18:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:35Z- waiting for 1730863116.97, time is 1730863115.79 2024-11-06T03:18:36Z- AHRS_M2 msg: 1730863116.17 epoch sec; seqNo:4089 2024-11-06T03:18:36Z- AHRS_M2 msg: 0.40 phi; -0.16 theta; 4.26 psi 2024-11-06T03:18:36Z- handleDepth()- Depth_Keller msg: 1730863116.17 epoch sec; seqNo:4347; depth 50.28 2024-11-06T03:18:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863116.18 epoch sec; seqNo:4347; 4061415.56 north; 594807.08 east; 36.69 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:18:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863112.97 2024-11-06T03:18:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:36Z- waiting for 1730863116.97, time is 1730863116.18 2024-11-06T03:18:36Z- handleDepth()- Depth_Keller msg: 1730863116.59 epoch sec; seqNo:4348; depth 50.34 2024-11-06T03:18:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863116.61 epoch sec; seqNo:4348; 4061415.56 north; 594807.08 east; 36.69 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:18:36Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:18:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863112.97 2024-11-06T03:18:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:36Z- waiting for 1730863116.97, time is 1730863116.61 2024-11-06T03:18:36Z- AHRS_M2 msg: 1730863116.98 epoch sec; seqNo:4090 2024-11-06T03:18:36Z- AHRS_M2 msg: 0.39 phi; -0.15 theta; 4.34 psi 2024-11-06T03:18:36Z- handleDepth()- Depth_Keller msg: 1730863116.99 epoch sec; seqNo:4349; depth 50.41 2024-11-06T03:18:37Z- handleDVL() - DVL msg epochMillisec: 1730863117.01 2024-11-06T03:18:37Z- handleDvl() - RDI_Pathfinder msg: 1730863117.01 sec; seqNo:1449; #beams:4 2024-11-06T03:18:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.96 , 36.67 , 40.86 , 50.58 2024-11-06T03:18:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.09 2024-11-06T03:18:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863117.03 epoch sec; seqNo:4349; 4061415.17 north; 594805.86 east; 36.69 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:18:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863112.97 2024-11-06T03:18:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:37Z- waiting for 1730863116.97, time is 1730863117.03 2024-11-06T03:18:37Z- LcmTrn::updateTrn() >>>> pose time 1730863117.03, meas time 1730863117.01 <<<< 2024-11-06T03:18:37Z- LcmTrn::updateTrn() equating pose 1730863117.01 and meas times 1730863117.01 2024-11-06T03:18:37Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863117.01 2024-11-06T03:18:37Z- TerrainNav::motionUpdate() - currEstimate = (4061415, 594805.86, 50.41) 2024-11-06T03:18:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:18:37Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:18:37Z- TNavPF::Projecting Measurements 2024-11-06T03:18:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:18:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:18:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:18:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:18:37Z- TNavFilter::4 good beams. 2024-11-06T03:18:37Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:18:37Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:18:37Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:18:37Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:18:37Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:18:37Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:18:37Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:18:37Z- TerrainMapOctree: (vehN, vehE ) = (4061415.17, 594805.86). 2024-11-06T03:18:37Z- TerrainMapOctree: Min Distance = 174.36. 2024-11-06T03:18:37Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:18:37Z- Forcing Subcloud Comparison 2024-11-06T03:18:37Z- Weighting particles with subcloud comparison 2024-11-06T03:18:37Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:37Z- meanExpectedMeasDiff: -0.600119 delta_rms_squared: 1.555516 alpha: 0.987306 2024-11-06T03:18:37Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:37Z- meanExpectedMeasDiff: 2.039103 delta_rms_squared: 0.576673 alpha: 0.966481 2024-11-06T03:18:37Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:37Z- meanExpectedMeasDiff: 0.454113 delta_rms_squared: 0.743859 alpha: 0.973817 2024-11-06T03:18:37Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:37Z- meanExpectedMeasDiff: -1.044077 delta_rms_squared: 1.093291 alpha: 0.982035 2024-11-06T03:18:37Z- Subcloud NIS: 0.749298 2024-11-06T03:18:37Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:18:37Z- TNavPF:: sumWeights = 0.403568 2024-11-06T03:18:37Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:18:37Z- TNavPF::First Term of Map Covariance Matrix is 0.45 2024-11-06T03:18:37Z- TNavPF::Calculated NIS Value : 0.27 numBeams normalized NIS: 0.07 2024-11-06T03:18:37Z- TerrainNav::Resampling particle filter... 2024-11-06T03:18:37Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863117.01 sec, ping # 0. 2024-11-06T03:18:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:18:37Z- variances([0], [2], and psi_berg) = 0.266897 0.149174 0.000000 2024-11-06T03:18:37Z- publishEstimatesDecoder() - at 1730863117.01 sec 2024-11-06T03:18:37Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:18:37Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061446.13 594788.91 56.15 2024-11-06T03:18:37Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061445.30 594788.77 56.70 lat/lon: 36.69 -121.94 2024-11-06T03:18:37Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.15 0.33 0.00 2024-11-06T03:18:37Z- AHRS_M2 msg: 1730863117.38 epoch sec; seqNo:4091 2024-11-06T03:18:37Z- AHRS_M2 msg: 0.39 phi; -0.14 theta; 4.38 psi 2024-11-06T03:18:37Z- handleDepth()- Depth_Keller msg: 1730863117.41 epoch sec; seqNo:4350; depth 50.46 2024-11-06T03:18:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863117.45 epoch sec; seqNo:4350; 4061415.17 north; 594805.86 east; 36.69 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:18:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863117.01 2024-11-06T03:18:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:37Z- waiting for 1730863121.01, time is 1730863117.45 2024-11-06T03:18:37Z- AHRS_M2 msg: 1730863117.78 epoch sec; seqNo:4092 2024-11-06T03:18:37Z- AHRS_M2 msg: 0.39 phi; -0.14 theta; 4.42 psi 2024-11-06T03:18:37Z- handleDepth()- Depth_Keller msg: 1730863117.79 epoch sec; seqNo:4351; depth 50.52 2024-11-06T03:18:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863117.80 epoch sec; seqNo:4351; 4061414.79 north; 594805.86 east; 36.69 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:18:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863117.01 2024-11-06T03:18:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:37Z- waiting for 1730863121.01, time is 1730863117.80 2024-11-06T03:18:38Z- AHRS_M2 msg: 1730863118.19 epoch sec; seqNo:4093 2024-11-06T03:18:38Z- AHRS_M2 msg: 0.39 phi; -0.13 theta; 4.47 psi 2024-11-06T03:18:38Z- handleDepth()- Depth_Keller msg: 1730863118.19 epoch sec; seqNo:4352; depth 50.56 2024-11-06T03:18:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863118.21 epoch sec; seqNo:4352; 4061414.79 north; 594805.86 east; 36.69 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T03:18:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863117.01 2024-11-06T03:18:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:38Z- waiting for 1730863121.01, time is 1730863118.21 2024-11-06T03:18:38Z- AHRS_M2 msg: 1730863118.59 epoch sec; seqNo:4094 2024-11-06T03:18:38Z- AHRS_M2 msg: 0.40 phi; -0.13 theta; 4.51 psi 2024-11-06T03:18:38Z- handleDepth()- Depth_Keller msg: 1730863118.60 epoch sec; seqNo:4353; depth 50.61 2024-11-06T03:18:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863118.61 epoch sec; seqNo:4353; 4061414.78 north; 594804.64 east; 36.69 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:18:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863117.01 2024-11-06T03:18:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:38Z- waiting for 1730863121.01, time is 1730863118.61 2024-11-06T03:18:39Z- AHRS_M2 msg: 1730863119.00 epoch sec; seqNo:4095 2024-11-06T03:18:39Z- AHRS_M2 msg: 0.40 phi; -0.12 theta; 4.55 psi 2024-11-06T03:18:39Z- handleDepth()- Depth_Keller msg: 1730863119.01 epoch sec; seqNo:4354; depth 50.67 2024-11-06T03:18:39Z- handleDVL() - DVL msg epochMillisec: 1730863119.04 2024-11-06T03:18:39Z- handleDvl() - RDI_Pathfinder msg: 1730863119.04 sec; seqNo:1450; #beams:4 2024-11-06T03:18:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.52 , 36.22 , 41.08 , 49.03 2024-11-06T03:18:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.08 , 0.00 2024-11-06T03:18:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863119.05 epoch sec; seqNo:4354; 4061414.78 north; 594804.64 east; 36.69 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T03:18:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863117.01 2024-11-06T03:18:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:39Z- waiting for 1730863121.01, time is 1730863119.05 2024-11-06T03:18:39Z- AHRS_M2 msg: 1730863119.40 epoch sec; seqNo:4096 2024-11-06T03:18:39Z- AHRS_M2 msg: 0.40 phi; -0.11 theta; 4.59 psi 2024-11-06T03:18:39Z- handleDepth()- Depth_Keller msg: 1730863119.41 epoch sec; seqNo:4355; depth 50.71 2024-11-06T03:18:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863119.42 epoch sec; seqNo:4355; 4061414.78 north; 594804.64 east; 36.69 lat; -121.94 lon; 50.71 depth; utmZone: 10 2024-11-06T03:18:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863117.01 2024-11-06T03:18:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:39Z- waiting for 1730863121.01, time is 1730863119.42 2024-11-06T03:18:39Z- handleDepth()- Depth_Keller msg: 1730863119.82 epoch sec; seqNo:4356; depth 50.75 2024-11-06T03:18:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863119.83 epoch sec; seqNo:4356; 4061414.38 north; 594803.43 east; 36.69 lat; -121.94 lon; 50.75 depth; utmZone: 10 2024-11-06T03:18:39Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:18:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863117.01 2024-11-06T03:18:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:39Z- waiting for 1730863121.01, time is 1730863119.83 2024-11-06T03:18:40Z- AHRS_M2 msg: 1730863120.21 epoch sec; seqNo:4097 2024-11-06T03:18:40Z- AHRS_M2 msg: 0.40 phi; -0.10 theta; 4.68 psi 2024-11-06T03:18:40Z- handleDepth()- Depth_Keller msg: 1730863120.21 epoch sec; seqNo:4357; depth 50.78 2024-11-06T03:18:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863120.22 epoch sec; seqNo:4357; 4061414.38 north; 594803.43 east; 36.69 lat; -121.94 lon; 50.78 depth; utmZone: 10 2024-11-06T03:18:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863117.01 2024-11-06T03:18:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:40Z- waiting for 1730863121.01, time is 1730863120.22 2024-11-06T03:18:40Z- AHRS_M2 msg: 1730863120.62 epoch sec; seqNo:4098 2024-11-06T03:18:40Z- AHRS_M2 msg: 0.40 phi; -0.10 theta; 4.72 psi 2024-11-06T03:18:40Z- handleDepth()- Depth_Keller msg: 1730863120.62 epoch sec; seqNo:4358; depth 50.80 2024-11-06T03:18:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863120.64 epoch sec; seqNo:4358; 4061414.38 north; 594803.43 east; 36.69 lat; -121.94 lon; 50.80 depth; utmZone: 10 2024-11-06T03:18:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863117.01 2024-11-06T03:18:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:40Z- waiting for 1730863121.01, time is 1730863120.64 2024-11-06T03:18:41Z- AHRS_M2 msg: 1730863121.02 epoch sec; seqNo:4099 2024-11-06T03:18:41Z- AHRS_M2 msg: 0.40 phi; -0.09 theta; 4.76 psi 2024-11-06T03:18:41Z- handleDepth()- Depth_Keller msg: 1730863121.03 epoch sec; seqNo:4359; depth 50.83 2024-11-06T03:18:41Z- handleDVL() - DVL msg epochMillisec: 1730863121.04 2024-11-06T03:18:41Z- handleDvl() - RDI_Pathfinder msg: 1730863121.04 sec; seqNo:1451; #beams:4 2024-11-06T03:18:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.18 , 36.22 , 41.53 , 48.15 2024-11-06T03:18:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , -0.07 2024-11-06T03:18:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863121.06 epoch sec; seqNo:4359; 4061414.38 north; 594803.43 east; 36.69 lat; -121.94 lon; 50.83 depth; utmZone: 10 2024-11-06T03:18:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863117.01 2024-11-06T03:18:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:41Z- waiting for 1730863121.01, time is 1730863121.06 2024-11-06T03:18:41Z- LcmTrn::updateTrn() >>>> pose time 1730863121.06, meas time 1730863121.04 <<<< 2024-11-06T03:18:41Z- LcmTrn::updateTrn() equating pose 1730863121.04 and meas times 1730863121.04 2024-11-06T03:18:41Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863121.04 2024-11-06T03:18:41Z- TerrainNav::motionUpdate() - currEstimate = (4061414, 594803.43, 50.83) 2024-11-06T03:18:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:18:41Z- TerrainNav:: SubcloudNIS is 0.75, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:18:41Z- TNavPF::Projecting Measurements 2024-11-06T03:18:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:18:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:18:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:18:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:18:41Z- TNavFilter::4 good beams. 2024-11-06T03:18:41Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:18:41Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:18:41Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:18:41Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:18:41Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:18:41Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:18:41Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:18:41Z- TerrainMapOctree: (vehN, vehE ) = (4061414.38, 594803.43). 2024-11-06T03:18:41Z- TerrainMapOctree: Min Distance = 172.85. 2024-11-06T03:18:41Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:18:41Z- Forcing Subcloud Comparison 2024-11-06T03:18:41Z- Weighting particles with subcloud comparison 2024-11-06T03:18:41Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:41Z- meanExpectedMeasDiff: 1.883915 delta_rms_squared: 1.635109 alpha: 0.987916 2024-11-06T03:18:41Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:41Z- meanExpectedMeasDiff: 2.206609 delta_rms_squared: 0.597713 alpha: 0.967623 2024-11-06T03:18:41Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:41Z- meanExpectedMeasDiff: 0.351103 delta_rms_squared: 0.870953 alpha: 0.977552 2024-11-06T03:18:41Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:41Z- meanExpectedMeasDiff: 0.590260 delta_rms_squared: 1.048269 alpha: 0.981278 2024-11-06T03:18:41Z- Subcloud NIS: 0.704339 2024-11-06T03:18:41Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:18:41Z- TNavPF:: sumWeights = 0.397778 2024-11-06T03:18:41Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:18:41Z- TNavPF::First Term of Map Covariance Matrix is 0.58 2024-11-06T03:18:41Z- TNavPF::Calculated NIS Value : 0.21 numBeams normalized NIS: 0.05 2024-11-06T03:18:41Z- TerrainNav::Resampling particle filter... 2024-11-06T03:18:41Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863121.04 sec, ping # 0. 2024-11-06T03:18:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:18:41Z- variances([0], [2], and psi_berg) = 0.266286 0.149597 0.000000 2024-11-06T03:18:41Z- publishEstimatesDecoder() - at 1730863121.04 sec 2024-11-06T03:18:41Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:18:41Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061445.35 594786.39 56.29 2024-11-06T03:18:41Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061444.51 594786.33 56.72 lat/lon: 36.69 -121.94 2024-11-06T03:18:41Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.15 0.34 0.00 2024-11-06T03:18:41Z- AHRS_M2 msg: 1730863121.42 epoch sec; seqNo:4100 2024-11-06T03:18:41Z- AHRS_M2 msg: 0.40 phi; -0.09 theta; 4.80 psi 2024-11-06T03:18:41Z- handleDepth()- Depth_Keller msg: 1730863121.43 epoch sec; seqNo:4360; depth 50.85 2024-11-06T03:18:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863121.45 epoch sec; seqNo:4360; 4061414.37 north; 594802.21 east; 36.69 lat; -121.94 lon; 50.85 depth; utmZone: 10 2024-11-06T03:18:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863121.04 2024-11-06T03:18:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:41Z- waiting for 1730863125.04, time is 1730863121.45 2024-11-06T03:18:41Z- AHRS_M2 msg: 1730863121.83 epoch sec; seqNo:4101 2024-11-06T03:18:41Z- AHRS_M2 msg: 0.40 phi; -0.08 theta; 4.84 psi 2024-11-06T03:18:41Z- handleDepth()- Depth_Keller msg: 1730863121.83 epoch sec; seqNo:4361; depth 50.89 2024-11-06T03:18:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863121.85 epoch sec; seqNo:4361; 4061414.37 north; 594802.21 east; 36.69 lat; -121.94 lon; 50.89 depth; utmZone: 10 2024-11-06T03:18:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863121.04 2024-11-06T03:18:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:41Z- waiting for 1730863125.04, time is 1730863121.85 2024-11-06T03:18:42Z- AHRS_M2 msg: 1730863122.23 epoch sec; seqNo:4102 2024-11-06T03:18:42Z- AHRS_M2 msg: 0.40 phi; -0.08 theta; 4.88 psi 2024-11-06T03:18:42Z- handleDepth()- Depth_Keller msg: 1730863122.23 epoch sec; seqNo:4362; depth 50.89 2024-11-06T03:18:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863122.24 epoch sec; seqNo:4362; 4061414.37 north; 594802.21 east; 36.69 lat; -121.94 lon; 50.89 depth; utmZone: 10 2024-11-06T03:18:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863121.04 2024-11-06T03:18:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:42Z- waiting for 1730863125.04, time is 1730863122.24 2024-11-06T03:18:42Z- handleDepth()- Depth_Keller msg: 1730863122.66 epoch sec; seqNo:4363; depth 50.93 2024-11-06T03:18:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863122.67 epoch sec; seqNo:4363; 4061414.75 north; 594802.20 east; 36.69 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T03:18:42Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:18:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863121.04 2024-11-06T03:18:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:42Z- waiting for 1730863125.04, time is 1730863122.67 2024-11-06T03:18:43Z- handleDepth()- Depth_Keller msg: 1730863123.07 epoch sec; seqNo:4364; depth 50.94 2024-11-06T03:18:43Z- handleDVL() - DVL msg epochMillisec: 1730863123.09 2024-11-06T03:18:43Z- handleDvl() - RDI_Pathfinder msg: 1730863123.09 sec; seqNo:1452; #beams:4 2024-11-06T03:18:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 59.86 , 35.79 , 41.75 , 46.83 2024-11-06T03:18:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , -0.07 2024-11-06T03:18:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863123.11 epoch sec; seqNo:4364; 4061414.74 north; 594800.98 east; 36.69 lat; -121.94 lon; 50.94 depth; utmZone: 10 2024-11-06T03:18:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863121.04 2024-11-06T03:18:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:43Z- waiting for 1730863125.04, time is 1730863123.11 2024-11-06T03:18:43Z- AHRS_M2 msg: 1730863123.44 epoch sec; seqNo:4103 2024-11-06T03:18:43Z- AHRS_M2 msg: 0.39 phi; -0.07 theta; 4.99 psi 2024-11-06T03:18:43Z- handleDepth()- Depth_Keller msg: 1730863123.45 epoch sec; seqNo:4365; depth 50.96 2024-11-06T03:18:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863123.47 epoch sec; seqNo:4365; 4061414.74 north; 594800.98 east; 36.69 lat; -121.94 lon; 50.96 depth; utmZone: 10 2024-11-06T03:18:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863121.04 2024-11-06T03:18:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:43Z- waiting for 1730863125.04, time is 1730863123.47 2024-11-06T03:18:43Z- AHRS_M2 msg: 1730863123.84 epoch sec; seqNo:4104 2024-11-06T03:18:43Z- AHRS_M2 msg: 0.39 phi; -0.06 theta; 5.03 psi 2024-11-06T03:18:43Z- handleDepth()- Depth_Keller msg: 1730863123.85 epoch sec; seqNo:4366; depth 50.97 2024-11-06T03:18:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863123.86 epoch sec; seqNo:4366; 4061414.74 north; 594800.98 east; 36.69 lat; -121.94 lon; 50.97 depth; utmZone: 10 2024-11-06T03:18:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863121.04 2024-11-06T03:18:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:44Z- waiting for 1730863125.04, time is 1730863123.86 2024-11-06T03:18:44Z- AHRS_M2 msg: 1730863124.25 epoch sec; seqNo:4105 2024-11-06T03:18:44Z- AHRS_M2 msg: 0.39 phi; -0.06 theta; 5.07 psi 2024-11-06T03:18:44Z- handleDepth()- Depth_Keller msg: 1730863124.26 epoch sec; seqNo:4367; depth 50.99 2024-11-06T03:18:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863124.27 epoch sec; seqNo:4367; 4061415.12 north; 594800.98 east; 36.69 lat; -121.94 lon; 50.99 depth; utmZone: 10 2024-11-06T03:18:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863121.04 2024-11-06T03:18:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:44Z- waiting for 1730863125.04, time is 1730863124.27 2024-11-06T03:18:44Z- AHRS_M2 msg: 1730863124.65 epoch sec; seqNo:4106 2024-11-06T03:18:44Z- AHRS_M2 msg: 0.39 phi; -0.06 theta; 5.10 psi 2024-11-06T03:18:44Z- handleDepth()- Depth_Keller msg: 1730863124.66 epoch sec; seqNo:4368; depth 51.02 2024-11-06T03:18:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863124.68 epoch sec; seqNo:4368; 4061415.10 north; 594799.76 east; 36.69 lat; -121.94 lon; 51.02 depth; utmZone: 10 2024-11-06T03:18:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863121.04 2024-11-06T03:18:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:44Z- waiting for 1730863125.04, time is 1730863124.68 2024-11-06T03:18:45Z- AHRS_M2 msg: 1730863125.08 epoch sec; seqNo:4107 2024-11-06T03:18:45Z- AHRS_M2 msg: 0.39 phi; -0.05 theta; 5.14 psi 2024-11-06T03:18:45Z- handleDepth()- Depth_Keller msg: 1730863125.08 epoch sec; seqNo:4369; depth 51.04 2024-11-06T03:18:45Z- handleDVL() - DVL msg epochMillisec: 1730863125.10 2024-11-06T03:18:45Z- handleDvl() - RDI_Pathfinder msg: 1730863125.10 sec; seqNo:1453; #beams:4 2024-11-06T03:18:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 58.98 , 35.56 , 41.31 , 45.28 2024-11-06T03:18:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , -0.04 2024-11-06T03:18:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863125.12 epoch sec; seqNo:4369; 4061415.48 north; 594799.75 east; 36.69 lat; -121.94 lon; 51.04 depth; utmZone: 10 2024-11-06T03:18:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863121.04 2024-11-06T03:18:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:45Z- waiting for 1730863125.04, time is 1730863125.12 2024-11-06T03:18:45Z- LcmTrn::updateTrn() >>>> pose time 1730863125.12, meas time 1730863125.10 <<<< 2024-11-06T03:18:45Z- LcmTrn::updateTrn() equating pose 1730863125.10 and meas times 1730863125.10 2024-11-06T03:18:45Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863125.10 2024-11-06T03:18:45Z- TerrainNav::motionUpdate() - currEstimate = (4061415, 594799.75, 51.04) 2024-11-06T03:18:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:18:45Z- TerrainNav:: SubcloudNIS is 0.70, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:18:45Z- TNavPF::Projecting Measurements 2024-11-06T03:18:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:18:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:18:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:18:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:18:45Z- TNavFilter::4 good beams. 2024-11-06T03:18:45Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:18:45Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:18:45Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:18:45Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:18:45Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:18:45Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:18:45Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:18:45Z- TerrainMapOctree: (vehN, vehE ) = (4061415.48, 594799.75). 2024-11-06T03:18:45Z- TerrainMapOctree: Min Distance = 172.80. 2024-11-06T03:18:45Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:18:45Z- Forcing Subcloud Comparison 2024-11-06T03:18:45Z- Weighting particles with subcloud comparison 2024-11-06T03:18:45Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:45Z- meanExpectedMeasDiff: 2.206348 delta_rms_squared: 1.564420 alpha: 0.987377 2024-11-06T03:18:45Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:45Z- meanExpectedMeasDiff: 1.369044 delta_rms_squared: 0.595460 alpha: 0.967504 2024-11-06T03:18:45Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:45Z- meanExpectedMeasDiff: 0.475631 delta_rms_squared: 0.853267 alpha: 0.977098 2024-11-06T03:18:45Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:45Z- meanExpectedMeasDiff: -0.991754 delta_rms_squared: 0.978420 alpha: 0.979968 2024-11-06T03:18:45Z- Subcloud NIS: 0.665636 2024-11-06T03:18:45Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:18:45Z- TNavPF:: sumWeights = 0.413701 2024-11-06T03:18:45Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:18:45Z- TNavPF::First Term of Map Covariance Matrix is 0.83 2024-11-06T03:18:45Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T03:18:45Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863125.10 sec, ping # 0. 2024-11-06T03:18:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:18:45Z- variances([0], [2], and psi_berg) = 0.267639 0.151068 0.000000 2024-11-06T03:18:45Z- publishEstimatesDecoder() - at 1730863125.10 sec 2024-11-06T03:18:45Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:18:45Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061445.61 594782.86 56.36 2024-11-06T03:18:45Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061445.61 594782.66 56.68 lat/lon: 36.69 -121.94 2024-11-06T03:18:45Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.15 0.33 0.00 2024-11-06T03:18:45Z- AHRS_M2 msg: 1730863125.48 epoch sec; seqNo:4108 2024-11-06T03:18:45Z- AHRS_M2 msg: 0.39 phi; -0.05 theta; 5.17 psi 2024-11-06T03:18:45Z- handleDepth()- Depth_Keller msg: 1730863125.48 epoch sec; seqNo:4370; depth 51.04 2024-11-06T03:18:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863125.52 epoch sec; seqNo:4370; 4061415.48 north; 594799.75 east; 36.69 lat; -121.94 lon; 51.04 depth; utmZone: 10 2024-11-06T03:18:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863125.10 2024-11-06T03:18:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:45Z- waiting for 1730863129.10, time is 1730863125.52 2024-11-06T03:18:45Z- handleDepth()- Depth_Keller msg: 1730863125.89 epoch sec; seqNo:4371; depth 51.07 2024-11-06T03:18:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863125.90 epoch sec; seqNo:4371; 4061415.47 north; 594798.53 east; 36.69 lat; -121.94 lon; 51.07 depth; utmZone: 10 2024-11-06T03:18:46Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:18:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863125.10 2024-11-06T03:18:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:46Z- waiting for 1730863129.10, time is 1730863125.90 2024-11-06T03:18:46Z- AHRS_M2 msg: 1730863126.27 epoch sec; seqNo:4109 2024-11-06T03:18:46Z- AHRS_M2 msg: 0.39 phi; -0.05 theta; 5.24 psi 2024-11-06T03:18:46Z- handleDepth()- Depth_Keller msg: 1730863126.28 epoch sec; seqNo:4372; depth 51.09 2024-11-06T03:18:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863126.30 epoch sec; seqNo:4372; 4061415.85 north; 594798.53 east; 36.69 lat; -121.94 lon; 51.09 depth; utmZone: 10 2024-11-06T03:18:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863125.10 2024-11-06T03:18:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:46Z- waiting for 1730863129.10, time is 1730863126.30 2024-11-06T03:18:46Z- handleDepth()- Depth_Keller msg: 1730863126.71 epoch sec; seqNo:4373; depth 51.11 2024-11-06T03:18:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863126.74 epoch sec; seqNo:4373; 4061415.85 north; 594798.53 east; 36.69 lat; -121.94 lon; 51.11 depth; utmZone: 10 2024-11-06T03:18:46Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:18:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863125.10 2024-11-06T03:18:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:46Z- waiting for 1730863129.10, time is 1730863126.74 2024-11-06T03:18:47Z- AHRS_M2 msg: 1730863127.08 epoch sec; seqNo:4110 2024-11-06T03:18:47Z- AHRS_M2 msg: 0.39 phi; -0.04 theta; 5.32 psi 2024-11-06T03:18:47Z- handleDepth()- Depth_Keller msg: 1730863127.09 epoch sec; seqNo:4374; depth 51.15 2024-11-06T03:18:47Z- handleDVL() - DVL msg epochMillisec: 1730863127.11 2024-11-06T03:18:47Z- handleDvl() - RDI_Pathfinder msg: 1730863127.11 sec; seqNo:1454; #beams:4 2024-11-06T03:18:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 57.30 , 34.66 , 41.80 , 44.86 2024-11-06T03:18:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.08 , 0.00 2024-11-06T03:18:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863127.13 epoch sec; seqNo:4374; 4061416.23 north; 594798.53 east; 36.69 lat; -121.94 lon; 51.15 depth; utmZone: 10 2024-11-06T03:18:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863125.10 2024-11-06T03:18:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:47Z- waiting for 1730863129.10, time is 1730863127.13 2024-11-06T03:18:47Z- AHRS_M2 msg: 1730863127.49 epoch sec; seqNo:4111 2024-11-06T03:18:47Z- AHRS_M2 msg: 0.39 phi; -0.04 theta; 5.39 psi 2024-11-06T03:18:47Z- handleDepth()- Depth_Keller msg: 1730863127.49 epoch sec; seqNo:4375; depth 51.16 2024-11-06T03:18:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863127.50 epoch sec; seqNo:4375; 4061416.21 north; 594797.31 east; 36.69 lat; -121.94 lon; 51.16 depth; utmZone: 10 2024-11-06T03:18:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863125.10 2024-11-06T03:18:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:47Z- waiting for 1730863129.10, time is 1730863127.50 2024-11-06T03:18:47Z- AHRS_M2 msg: 1730863127.89 epoch sec; seqNo:4112 2024-11-06T03:18:47Z- AHRS_M2 msg: 0.39 phi; -0.04 theta; 5.42 psi 2024-11-06T03:18:47Z- handleDepth()- Depth_Keller msg: 1730863127.90 epoch sec; seqNo:4376; depth 51.19 2024-11-06T03:18:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863127.91 epoch sec; seqNo:4376; 4061416.59 north; 594797.30 east; 36.69 lat; -121.94 lon; 51.19 depth; utmZone: 10 2024-11-06T03:18:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863125.10 2024-11-06T03:18:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:48Z- waiting for 1730863129.10, time is 1730863127.91 2024-11-06T03:18:48Z- AHRS_M2 msg: 1730863128.30 epoch sec; seqNo:4113 2024-11-06T03:18:48Z- AHRS_M2 msg: 0.39 phi; -0.04 theta; 5.46 psi 2024-11-06T03:18:48Z- handleDepth()- Depth_Keller msg: 1730863128.30 epoch sec; seqNo:4377; depth 51.20 2024-11-06T03:18:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863128.32 epoch sec; seqNo:4377; 4061416.97 north; 594797.30 east; 36.69 lat; -121.94 lon; 51.20 depth; utmZone: 10 2024-11-06T03:18:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863125.10 2024-11-06T03:18:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:48Z- waiting for 1730863129.10, time is 1730863128.32 2024-11-06T03:18:48Z- AHRS_M2 msg: 1730863128.72 epoch sec; seqNo:4114 2024-11-06T03:18:48Z- AHRS_M2 msg: 0.39 phi; -0.03 theta; 5.50 psi 2024-11-06T03:18:48Z- handleDepth()- Depth_Keller msg: 1730863128.75 epoch sec; seqNo:4378; depth 51.24 2024-11-06T03:18:48Z- handleDVL() - DVL msg epochMillisec: 1730863128.78 2024-11-06T03:18:48Z- handleDvl() - RDI_Pathfinder msg: 1730863128.78 sec; seqNo:1455; #beams:4 2024-11-06T03:18:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 56.89 , 35.48 , 42.01 , 44.04 2024-11-06T03:18:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.11 , 0.03 2024-11-06T03:18:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863128.79 epoch sec; seqNo:4378; 4061416.97 north; 594797.30 east; 36.69 lat; -121.94 lon; 51.24 depth; utmZone: 10 2024-11-06T03:18:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863125.10 2024-11-06T03:18:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:48Z- waiting for 1730863129.10, time is 1730863128.79 2024-11-06T03:18:49Z- AHRS_M2 msg: 1730863129.11 epoch sec; seqNo:4115 2024-11-06T03:18:49Z- AHRS_M2 msg: 0.39 phi; -0.03 theta; 5.53 psi 2024-11-06T03:18:49Z- handleDepth()- Depth_Keller msg: 1730863129.11 epoch sec; seqNo:4379; depth 51.27 2024-11-06T03:18:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863129.12 epoch sec; seqNo:4379; 4061417.33 north; 594796.07 east; 36.69 lat; -121.94 lon; 51.27 depth; utmZone: 10 2024-11-06T03:18:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863125.10 2024-11-06T03:18:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:49Z- waiting for 1730863129.10, time is 1730863129.12 2024-11-06T03:18:49Z- LcmTrn::updateTrn() >>>> pose time 1730863129.12, meas time 1730863128.78 <<<< 2024-11-06T03:18:49Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863129.12 2024-11-06T03:18:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863128.78 sec, ping # 0; waiting for INS data more recent than 1730863125.10... 2024-11-06T03:18:49Z- TerrainNav::motionUpdate() - currEstimate = (4061417, 594796.07, 51.27) 2024-11-06T03:18:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:18:49Z- TerrainNav:: SubcloudNIS is 0.67, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:18:49Z- TNavPF::Projecting Measurements 2024-11-06T03:18:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:18:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:18:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:18:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:18:49Z- TNavFilter::4 good beams. 2024-11-06T03:18:49Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:18:49Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:18:49Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:18:49Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:18:49Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:18:49Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:18:49Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:18:49Z- TerrainMapOctree: (vehN, vehE ) = (4061417.17, 594796.39). 2024-11-06T03:18:49Z- TerrainMapOctree: Min Distance = 173.49. 2024-11-06T03:18:49Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:18:49Z- Forcing Subcloud Comparison 2024-11-06T03:18:49Z- Weighting particles with subcloud comparison 2024-11-06T03:18:49Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:49Z- meanExpectedMeasDiff: 0.974539 delta_rms_squared: 1.910741 alpha: 0.989641 2024-11-06T03:18:49Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:49Z- meanExpectedMeasDiff: 1.301180 delta_rms_squared: 0.583719 alpha: 0.966872 2024-11-06T03:18:49Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:49Z- meanExpectedMeasDiff: 0.593875 delta_rms_squared: 0.861566 alpha: 0.977313 2024-11-06T03:18:49Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:49Z- meanExpectedMeasDiff: -0.449773 delta_rms_squared: 0.939250 alpha: 0.979150 2024-11-06T03:18:49Z- Subcloud NIS: 0.630161 2024-11-06T03:18:49Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:18:49Z- TNavPF:: sumWeights = 0.554342 2024-11-06T03:18:49Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:18:49Z- TNavPF::First Term of Map Covariance Matrix is 0.70 2024-11-06T03:18:49Z- TNavPF::Calculated NIS Value : 0.24 numBeams normalized NIS: 0.06 2024-11-06T03:18:49Z- TerrainNav::Resampling particle filter... 2024-11-06T03:18:49Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863128.78 sec. 2024-11-06T03:18:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:18:49Z- variances([0], [2], and psi_berg) = 0.267017 0.154788 0.000000 2024-11-06T03:18:49Z- publishEstimatesDecoder() - at 1730863129.12 sec 2024-11-06T03:18:49Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:18:49Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061448.25 594779.03 56.40 2024-11-06T03:18:49Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061447.48 594778.98 56.72 lat/lon: 36.69 -121.94 2024-11-06T03:18:49Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.15 0.58 0.00 2024-11-06T03:18:49Z- AHRS_M2 msg: 1730863129.50 epoch sec; seqNo:4116 2024-11-06T03:18:49Z- AHRS_M2 msg: 0.39 phi; -0.03 theta; 5.57 psi 2024-11-06T03:18:49Z- handleDepth()- Depth_Keller msg: 1730863129.51 epoch sec; seqNo:4380; depth 51.28 2024-11-06T03:18:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863129.53 epoch sec; seqNo:4380; 4061417.33 north; 594796.07 east; 36.69 lat; -121.94 lon; 51.28 depth; utmZone: 10 2024-11-06T03:18:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863129.12 2024-11-06T03:18:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:49Z- waiting for 1730863133.12, time is 1730863129.53 2024-11-06T03:18:49Z- AHRS_M2 msg: 1730863129.91 epoch sec; seqNo:4117 2024-11-06T03:18:49Z- AHRS_M2 msg: 0.39 phi; -0.03 theta; 5.61 psi 2024-11-06T03:18:49Z- handleDepth()- Depth_Keller msg: 1730863129.91 epoch sec; seqNo:4381; depth 51.29 2024-11-06T03:18:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863129.92 epoch sec; seqNo:4381; 4061417.71 north; 594796.07 east; 36.69 lat; -121.94 lon; 51.29 depth; utmZone: 10 2024-11-06T03:18:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863129.12 2024-11-06T03:18:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:50Z- waiting for 1730863133.12, time is 1730863129.92 2024-11-06T03:18:50Z- AHRS_M2 msg: 1730863130.31 epoch sec; seqNo:4118 2024-11-06T03:18:50Z- AHRS_M2 msg: 0.39 phi; -0.03 theta; 5.64 psi 2024-11-06T03:18:50Z- handleDepth()- Depth_Keller msg: 1730863130.32 epoch sec; seqNo:4382; depth 51.32 2024-11-06T03:18:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863130.33 epoch sec; seqNo:4382; 4061418.09 north; 594796.06 east; 36.69 lat; -121.94 lon; 51.32 depth; utmZone: 10 2024-11-06T03:18:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863129.12 2024-11-06T03:18:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:50Z- waiting for 1730863133.12, time is 1730863130.33 2024-11-06T03:18:50Z- AHRS_M2 msg: 1730863130.75 epoch sec; seqNo:4119 2024-11-06T03:18:50Z- AHRS_M2 msg: 0.40 phi; -0.03 theta; 5.68 psi 2024-11-06T03:18:50Z- handleDepth()- Depth_Keller msg: 1730863130.76 epoch sec; seqNo:4383; depth 51.34 2024-11-06T03:18:50Z- handleDVL() - DVL msg epochMillisec: 1730863130.79 2024-11-06T03:18:50Z- handleDvl() - RDI_Pathfinder msg: 1730863130.79 sec; seqNo:1456; #beams:4 2024-11-06T03:18:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 56.48 , 35.48 , 42.41 , 44.25 2024-11-06T03:18:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.06 , 0.03 2024-11-06T03:18:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863130.81 epoch sec; seqNo:4383; 4061418.09 north; 594796.06 east; 36.69 lat; -121.94 lon; 51.34 depth; utmZone: 10 2024-11-06T03:18:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863129.12 2024-11-06T03:18:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:50Z- waiting for 1730863133.12, time is 1730863130.81 2024-11-06T03:18:51Z- AHRS_M2 msg: 1730863131.12 epoch sec; seqNo:4120 2024-11-06T03:18:51Z- AHRS_M2 msg: 0.40 phi; -0.03 theta; 5.71 psi 2024-11-06T03:18:51Z- handleDepth()- Depth_Keller msg: 1730863131.13 epoch sec; seqNo:4384; depth 51.37 2024-11-06T03:18:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863131.14 epoch sec; seqNo:4384; 4061418.46 north; 594794.84 east; 36.69 lat; -121.94 lon; 51.37 depth; utmZone: 10 2024-11-06T03:18:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863129.12 2024-11-06T03:18:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:51Z- waiting for 1730863133.12, time is 1730863131.14 2024-11-06T03:18:51Z- AHRS_M2 msg: 1730863131.53 epoch sec; seqNo:4121 2024-11-06T03:18:51Z- AHRS_M2 msg: 0.40 phi; -0.03 theta; 5.75 psi 2024-11-06T03:18:51Z- handleDepth()- Depth_Keller msg: 1730863131.53 epoch sec; seqNo:4385; depth 51.35 2024-11-06T03:18:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863131.54 epoch sec; seqNo:4385; 4061418.84 north; 594794.84 east; 36.69 lat; -121.94 lon; 51.35 depth; utmZone: 10 2024-11-06T03:18:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863129.12 2024-11-06T03:18:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:51Z- waiting for 1730863133.12, time is 1730863131.54 2024-11-06T03:18:51Z- AHRS_M2 msg: 1730863131.93 epoch sec; seqNo:4122 2024-11-06T03:18:51Z- AHRS_M2 msg: 0.40 phi; -0.02 theta; 5.79 psi 2024-11-06T03:18:51Z- handleDepth()- Depth_Keller msg: 1730863131.94 epoch sec; seqNo:4386; depth 51.37 2024-11-06T03:18:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863131.95 epoch sec; seqNo:4386; 4061418.84 north; 594794.84 east; 36.69 lat; -121.94 lon; 51.37 depth; utmZone: 10 2024-11-06T03:18:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863129.12 2024-11-06T03:18:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:52Z- waiting for 1730863133.12, time is 1730863131.95 2024-11-06T03:18:52Z- AHRS_M2 msg: 1730863132.33 epoch sec; seqNo:4123 2024-11-06T03:18:52Z- AHRS_M2 msg: 0.40 phi; -0.02 theta; 5.82 psi 2024-11-06T03:18:52Z- handleDepth()- Depth_Keller msg: 1730863132.34 epoch sec; seqNo:4387; depth 51.38 2024-11-06T03:18:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863132.35 epoch sec; seqNo:4387; 4061419.22 north; 594794.83 east; 36.69 lat; -121.94 lon; 51.38 depth; utmZone: 10 2024-11-06T03:18:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863129.12 2024-11-06T03:18:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:52Z- waiting for 1730863133.12, time is 1730863132.35 2024-11-06T03:18:52Z- AHRS_M2 msg: 1730863132.76 epoch sec; seqNo:4124 2024-11-06T03:18:52Z- AHRS_M2 msg: 0.39 phi; -0.02 theta; 5.86 psi 2024-11-06T03:18:52Z- handleDepth()- Depth_Keller msg: 1730863132.77 epoch sec; seqNo:4388; depth 51.39 2024-11-06T03:18:52Z- handleDVL() - DVL msg epochMillisec: 1730863132.79 2024-11-06T03:18:52Z- handleDvl() - RDI_Pathfinder msg: 1730863132.79 sec; seqNo:1457; #beams:4 2024-11-06T03:18:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 55.67 , 35.48 , 42.41 , 44.25 2024-11-06T03:18:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.02 , -0.03 2024-11-06T03:18:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863132.82 epoch sec; seqNo:4388; 4061419.59 north; 594794.83 east; 36.69 lat; -121.94 lon; 51.39 depth; utmZone: 10 2024-11-06T03:18:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863129.12 2024-11-06T03:18:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:52Z- waiting for 1730863133.12, time is 1730863132.82 2024-11-06T03:18:53Z- AHRS_M2 msg: 1730863133.14 epoch sec; seqNo:4125 2024-11-06T03:18:53Z- AHRS_M2 msg: 0.39 phi; -0.02 theta; 5.89 psi 2024-11-06T03:18:53Z- handleDepth()- Depth_Keller msg: 1730863133.14 epoch sec; seqNo:4389; depth 51.39 2024-11-06T03:18:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863133.16 epoch sec; seqNo:4389; 4061419.59 north; 594794.83 east; 36.69 lat; -121.94 lon; 51.39 depth; utmZone: 10 2024-11-06T03:18:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863129.12 2024-11-06T03:18:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:53Z- waiting for 1730863133.12, time is 1730863133.16 2024-11-06T03:18:53Z- LcmTrn::updateTrn() >>>> pose time 1730863133.16, meas time 1730863132.79 <<<< 2024-11-06T03:18:53Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863133.16 2024-11-06T03:18:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863132.79 sec, ping # 0; waiting for INS data more recent than 1730863129.12... 2024-11-06T03:18:53Z- TerrainNav::motionUpdate() - currEstimate = (4061420, 594794.83, 51.39) 2024-11-06T03:18:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:18:53Z- TerrainNav:: SubcloudNIS is 0.63, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:18:53Z- TNavPF::Projecting Measurements 2024-11-06T03:18:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:18:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:18:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:18:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:18:53Z- TNavFilter::4 good beams. 2024-11-06T03:18:53Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:18:53Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:18:53Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:18:53Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:18:53Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:18:53Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:18:53Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:18:53Z- TerrainMapOctree: (vehN, vehE ) = (4061419.39, 594794.94). 2024-11-06T03:18:53Z- TerrainMapOctree: Min Distance = 175.25. 2024-11-06T03:18:53Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:18:53Z- Forcing Subcloud Comparison 2024-11-06T03:18:53Z- Weighting particles with subcloud comparison 2024-11-06T03:18:53Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:53Z- meanExpectedMeasDiff: -0.118246 delta_rms_squared: 2.224972 alpha: 0.991091 2024-11-06T03:18:53Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:53Z- meanExpectedMeasDiff: 1.274611 delta_rms_squared: 0.830278 alpha: 0.976478 2024-11-06T03:18:53Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:53Z- meanExpectedMeasDiff: 1.733543 delta_rms_squared: 1.251341 alpha: 0.984269 2024-11-06T03:18:53Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:53Z- meanExpectedMeasDiff: 0.070829 delta_rms_squared: 1.312470 alpha: 0.984990 2024-11-06T03:18:53Z- Subcloud NIS: 0.597902 2024-11-06T03:18:53Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:18:53Z- TNavPF:: sumWeights = 0.460469 2024-11-06T03:18:53Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:18:53Z- TNavPF::First Term of Map Covariance Matrix is 0.54 2024-11-06T03:18:53Z- TNavPF::Calculated NIS Value : 0.21 numBeams normalized NIS: 0.05 2024-11-06T03:18:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863132.79 sec. 2024-11-06T03:18:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:18:53Z- variances([0], [2], and psi_berg) = 0.268233 0.153982 0.000000 2024-11-06T03:18:53Z- publishEstimatesDecoder() - at 1730863133.16 sec 2024-11-06T03:18:53Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:18:53Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061449.48 594777.72 55.72 2024-11-06T03:18:53Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061449.74 594777.73 56.55 lat/lon: 36.69 -121.94 2024-11-06T03:18:53Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.15 0.64 0.00 2024-11-06T03:18:53Z- AHRS_M2 msg: 1730863133.54 epoch sec; seqNo:4126 2024-11-06T03:18:53Z- AHRS_M2 msg: 0.39 phi; -0.02 theta; 5.93 psi 2024-11-06T03:18:53Z- handleDepth()- Depth_Keller msg: 1730863133.55 epoch sec; seqNo:4390; depth 51.38 2024-11-06T03:18:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863133.57 epoch sec; seqNo:4390; 4061419.97 north; 594794.82 east; 36.69 lat; -121.94 lon; 51.38 depth; utmZone: 10 2024-11-06T03:18:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863133.16 2024-11-06T03:18:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:53Z- waiting for 1730863137.16, time is 1730863133.57 2024-11-06T03:18:53Z- AHRS_M2 msg: 1730863133.95 epoch sec; seqNo:4127 2024-11-06T03:18:53Z- AHRS_M2 msg: 0.39 phi; -0.01 theta; 5.96 psi 2024-11-06T03:18:53Z- handleDepth()- Depth_Keller msg: 1730863133.95 epoch sec; seqNo:4391; depth 51.39 2024-11-06T03:18:53Z- handleDVL() - DVL msg epochMillisec: 1730863133.96 2024-11-06T03:18:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:18:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:18:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:18:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:18:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:18:53Z- handleDvl() - RDI_Pathfinder msg: 1730863133.96 sec; seqNo:1458; #beams:0 2024-11-06T03:18:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:18:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.02 , -0.03 2024-11-06T03:18:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863133.97 epoch sec; seqNo:4391; 4061420.35 north; 594794.82 east; 36.69 lat; -121.94 lon; 51.39 depth; utmZone: 10 2024-11-06T03:18:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863133.16 2024-11-06T03:18:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:54Z- waiting for 1730863137.16, time is 1730863133.97 2024-11-06T03:18:54Z- AHRS_M2 msg: 1730863134.35 epoch sec; seqNo:4128 2024-11-06T03:18:54Z- AHRS_M2 msg: 0.39 phi; -0.01 theta; 5.99 psi 2024-11-06T03:18:54Z- handleDepth()- Depth_Keller msg: 1730863134.36 epoch sec; seqNo:4392; depth 51.37 2024-11-06T03:18:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863134.37 epoch sec; seqNo:4392; 4061420.73 north; 594794.81 east; 36.69 lat; -121.94 lon; 51.37 depth; utmZone: 10 2024-11-06T03:18:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863133.16 2024-11-06T03:18:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:54Z- waiting for 1730863137.16, time is 1730863134.37 2024-11-06T03:18:54Z- AHRS_M2 msg: 1730863134.77 epoch sec; seqNo:4129 2024-11-06T03:18:54Z- AHRS_M2 msg: 0.39 phi; -0.01 theta; 6.03 psi 2024-11-06T03:18:54Z- handleDepth()- Depth_Keller msg: 1730863134.77 epoch sec; seqNo:4393; depth 51.38 2024-11-06T03:18:54Z- handleDVL() - DVL msg epochMillisec: 1730863134.79 2024-11-06T03:18:54Z- handleDvl() - RDI_Pathfinder msg: 1730863134.79 sec; seqNo:1459; #beams:4 2024-11-06T03:18:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 54.23 , 35.48 , 42.41 , 43.43 2024-11-06T03:18:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.02 , -0.08 2024-11-06T03:18:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863134.82 epoch sec; seqNo:4393; 4061421.11 north; 594794.81 east; 36.69 lat; -121.94 lon; 51.38 depth; utmZone: 10 2024-11-06T03:18:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863133.16 2024-11-06T03:18:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:54Z- waiting for 1730863137.16, time is 1730863134.82 2024-11-06T03:18:55Z- AHRS_M2 msg: 1730863135.16 epoch sec; seqNo:4130 2024-11-06T03:18:55Z- AHRS_M2 msg: 0.38 phi; -0.01 theta; 6.06 psi 2024-11-06T03:18:55Z- handleDepth()- Depth_Keller msg: 1730863135.16 epoch sec; seqNo:4394; depth 51.37 2024-11-06T03:18:55Z- handleDVL() - DVL msg epochMillisec: 1730863135.17 2024-11-06T03:18:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:18:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:18:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:18:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:18:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:18:55Z- handleDvl() - RDI_Pathfinder msg: 1730863135.17 sec; seqNo:1460; #beams:0 2024-11-06T03:18:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:18:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.02 , -0.08 2024-11-06T03:18:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863135.19 epoch sec; seqNo:4394; 4061421.11 north; 594794.81 east; 36.69 lat; -121.94 lon; 51.37 depth; utmZone: 10 2024-11-06T03:18:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863133.16 2024-11-06T03:18:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:55Z- waiting for 1730863137.16, time is 1730863135.19 2024-11-06T03:18:55Z- AHRS_M2 msg: 1730863135.56 epoch sec; seqNo:4131 2024-11-06T03:18:55Z- AHRS_M2 msg: 0.38 phi; -0.00 theta; 6.09 psi 2024-11-06T03:18:55Z- handleDepth()- Depth_Keller msg: 1730863135.57 epoch sec; seqNo:4395; depth 51.36 2024-11-06T03:18:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863135.58 epoch sec; seqNo:4395; 4061421.47 north; 594793.59 east; 36.69 lat; -121.94 lon; 51.36 depth; utmZone: 10 2024-11-06T03:18:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863133.16 2024-11-06T03:18:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:55Z- waiting for 1730863137.16, time is 1730863135.58 2024-11-06T03:18:55Z- AHRS_M2 msg: 1730863135.97 epoch sec; seqNo:4132 2024-11-06T03:18:55Z- AHRS_M2 msg: 0.38 phi; -0.00 theta; 6.12 psi 2024-11-06T03:18:55Z- handleDepth()- Depth_Keller msg: 1730863135.97 epoch sec; seqNo:4396; depth 51.36 2024-11-06T03:18:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863135.99 epoch sec; seqNo:4396; 4061421.85 north; 594793.58 east; 36.69 lat; -121.94 lon; 51.36 depth; utmZone: 10 2024-11-06T03:18:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863133.16 2024-11-06T03:18:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:56Z- waiting for 1730863137.16, time is 1730863135.99 2024-11-06T03:18:56Z- AHRS_M2 msg: 1730863136.37 epoch sec; seqNo:4133 2024-11-06T03:18:56Z- AHRS_M2 msg: 0.38 phi; -0.00 theta; 6.16 psi 2024-11-06T03:18:56Z- handleDepth()- Depth_Keller msg: 1730863136.38 epoch sec; seqNo:4397; depth 51.37 2024-11-06T03:18:56Z- handleDVL() - DVL msg epochMillisec: 1730863136.38 2024-11-06T03:18:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:18:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:18:56Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:18:56Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:18:56Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:18:56Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:18:56Z- handleDvl() - RDI_Pathfinder msg: 1730863136.38 sec; seqNo:1461; #beams:0 2024-11-06T03:18:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:18:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.02 , -0.08 2024-11-06T03:18:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863136.39 epoch sec; seqNo:4397; 4061422.23 north; 594793.58 east; 36.69 lat; -121.94 lon; 51.37 depth; utmZone: 10 2024-11-06T03:18:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863133.16 2024-11-06T03:18:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:56Z- waiting for 1730863137.16, time is 1730863136.39 2024-11-06T03:18:56Z- AHRS_M2 msg: 1730863136.78 epoch sec; seqNo:4134 2024-11-06T03:18:56Z- AHRS_M2 msg: 0.38 phi; 0.00 theta; 6.19 psi 2024-11-06T03:18:56Z- handleDepth()- Depth_Keller msg: 1730863136.79 epoch sec; seqNo:4398; depth 51.36 2024-11-06T03:18:56Z- handleDVL() - DVL msg epochMillisec: 1730863136.81 2024-11-06T03:18:56Z- handleDvl() - RDI_Pathfinder msg: 1730863136.81 sec; seqNo:1462; #beams:4 2024-11-06T03:18:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 53.63 , 35.28 , 42.21 , 42.82 2024-11-06T03:18:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.09 , -0.09 2024-11-06T03:18:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863136.83 epoch sec; seqNo:4398; 4061422.61 north; 594793.57 east; 36.69 lat; -121.94 lon; 51.36 depth; utmZone: 10 2024-11-06T03:18:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863133.16 2024-11-06T03:18:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:56Z- waiting for 1730863137.16, time is 1730863136.83 2024-11-06T03:18:57Z- AHRS_M2 msg: 1730863137.18 epoch sec; seqNo:4135 2024-11-06T03:18:57Z- AHRS_M2 msg: 0.38 phi; 0.00 theta; 6.22 psi 2024-11-06T03:18:57Z- handleDepth()- Depth_Keller msg: 1730863137.19 epoch sec; seqNo:4399; depth 51.36 2024-11-06T03:18:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863137.20 epoch sec; seqNo:4399; 4061422.61 north; 594793.57 east; 36.69 lat; -121.94 lon; 51.36 depth; utmZone: 10 2024-11-06T03:18:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863133.16 2024-11-06T03:18:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:57Z- waiting for 1730863137.16, time is 1730863137.20 2024-11-06T03:18:57Z- LcmTrn::updateTrn() >>>> pose time 1730863137.20, meas time 1730863136.81 <<<< 2024-11-06T03:18:57Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863137.20 2024-11-06T03:18:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863136.81 sec, ping # 0; waiting for INS data more recent than 1730863133.16... 2024-11-06T03:18:57Z- TerrainNav::motionUpdate() - currEstimate = (4061423, 594793.57, 51.36) 2024-11-06T03:18:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:18:57Z- TerrainNav:: SubcloudNIS is 0.60, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:18:57Z- TNavPF::Projecting Measurements 2024-11-06T03:18:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:18:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:18:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:18:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:18:57Z- TNavFilter::4 good beams. 2024-11-06T03:18:57Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:18:57Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:18:57Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:18:57Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:18:57Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:18:57Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:18:57Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:18:57Z- TerrainMapOctree: (vehN, vehE ) = (4061422.32, 594793.69). 2024-11-06T03:18:57Z- TerrainMapOctree: Min Distance = 177.78. 2024-11-06T03:18:57Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:18:57Z- Forcing Subcloud Comparison 2024-11-06T03:18:57Z- Weighting particles with subcloud comparison 2024-11-06T03:18:57Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:57Z- meanExpectedMeasDiff: -1.816533 delta_rms_squared: 2.317569 alpha: 0.991444 2024-11-06T03:18:57Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:57Z- meanExpectedMeasDiff: 0.722405 delta_rms_squared: 0.901636 alpha: 0.978299 2024-11-06T03:18:57Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:57Z- meanExpectedMeasDiff: 4.432787 delta_rms_squared: 1.008833 alpha: 0.980560 2024-11-06T03:18:57Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:18:57Z- meanExpectedMeasDiff: -0.428709 delta_rms_squared: 2.300289 alpha: 0.991380 2024-11-06T03:18:57Z- Subcloud NIS: 0.575730 2024-11-06T03:18:57Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:18:57Z- TNavPF:: sumWeights = 0.060095 2024-11-06T03:18:57Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:18:57Z- TNavPF::First Term of Map Covariance Matrix is 0.32 2024-11-06T03:18:57Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T03:18:57Z- TerrainNav::Resampling particle filter... 2024-11-06T03:18:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863136.81 sec. 2024-11-06T03:18:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:18:57Z- variances([0], [2], and psi_berg) = 0.289992 0.159967 0.000000 2024-11-06T03:18:57Z- publishEstimatesDecoder() - at 1730863137.20 sec 2024-11-06T03:18:57Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:18:57Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061453.51 594776.55 54.63 2024-11-06T03:18:57Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061452.69 594776.46 56.31 lat/lon: 36.69 -121.94 2024-11-06T03:18:57Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.29 0.16 0.69 0.00 2024-11-06T03:18:57Z- AHRS_M2 msg: 1730863137.58 epoch sec; seqNo:4136 2024-11-06T03:18:57Z- AHRS_M2 msg: 0.38 phi; 0.01 theta; 6.25 psi 2024-11-06T03:18:57Z- handleDepth()- Depth_Keller msg: 1730863137.59 epoch sec; seqNo:4400; depth 51.35 2024-11-06T03:18:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863137.60 epoch sec; seqNo:4400; 4061422.99 north; 594793.57 east; 36.69 lat; -121.94 lon; 51.35 depth; utmZone: 10 2024-11-06T03:18:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863137.20 2024-11-06T03:18:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:57Z- waiting for 1730863141.20, time is 1730863137.60 2024-11-06T03:18:57Z- AHRS_M2 msg: 1730863137.99 epoch sec; seqNo:4137 2024-11-06T03:18:57Z- AHRS_M2 msg: 0.38 phi; 0.01 theta; 6.28 psi 2024-11-06T03:18:57Z- handleDepth()- Depth_Keller msg: 1730863137.99 epoch sec; seqNo:4401; depth 51.35 2024-11-06T03:18:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863138.00 epoch sec; seqNo:4401; 4061423.37 north; 594793.56 east; 36.69 lat; -121.94 lon; 51.35 depth; utmZone: 10 2024-11-06T03:18:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863137.20 2024-11-06T03:18:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:58Z- waiting for 1730863141.20, time is 1730863138.00 2024-11-06T03:18:58Z- handleDepth()- Depth_Keller msg: 1730863138.41 epoch sec; seqNo:4402; depth 51.36 2024-11-06T03:18:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863138.42 epoch sec; seqNo:4402; 4061423.75 north; 594793.56 east; 36.69 lat; -121.94 lon; 51.36 depth; utmZone: 10 2024-11-06T03:18:58Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:18:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863137.20 2024-11-06T03:18:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:58Z- waiting for 1730863141.20, time is 1730863138.42 2024-11-06T03:18:58Z- AHRS_M2 msg: 1730863138.79 epoch sec; seqNo:4138 2024-11-06T03:18:58Z- AHRS_M2 msg: 0.38 phi; 0.01 theta; 0.06 psi 2024-11-06T03:18:58Z- handleDepth()- Depth_Keller msg: 1730863138.81 epoch sec; seqNo:4403; depth 51.36 2024-11-06T03:18:58Z- handleDVL() - DVL msg epochMillisec: 1730863138.86 2024-11-06T03:18:58Z- handleDvl() - RDI_Pathfinder msg: 1730863138.86 sec; seqNo:1463; #beams:4 2024-11-06T03:18:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 53.62 , 35.27 , 42.61 , 42.41 2024-11-06T03:18:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.13 , -0.06 2024-11-06T03:18:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863138.87 epoch sec; seqNo:4403; 4061424.13 north; 594793.56 east; 36.69 lat; -121.94 lon; 51.36 depth; utmZone: 10 2024-11-06T03:18:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:18:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863137.20 2024-11-06T03:18:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:58Z- waiting for 1730863141.20, time is 1730863138.87 2024-11-06T03:18:59Z- AHRS_M2 msg: 1730863139.20 epoch sec; seqNo:4139 2024-11-06T03:18:59Z- AHRS_M2 msg: 0.38 phi; 0.01 theta; 0.09 psi 2024-11-06T03:18:59Z- handleDepth()- Depth_Keller msg: 1730863139.21 epoch sec; seqNo:4404; depth 51.35 2024-11-06T03:18:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863139.22 epoch sec; seqNo:4404; 4061424.51 north; 594793.55 east; 36.69 lat; -121.94 lon; 51.35 depth; utmZone: 10 2024-11-06T03:18:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863137.20 2024-11-06T03:18:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:59Z- waiting for 1730863141.20, time is 1730863139.22 2024-11-06T03:18:59Z- AHRS_M2 msg: 1730863139.60 epoch sec; seqNo:4140 2024-11-06T03:18:59Z- AHRS_M2 msg: 0.38 phi; 0.01 theta; 0.12 psi 2024-11-06T03:18:59Z- handleDepth()- Depth_Keller msg: 1730863139.61 epoch sec; seqNo:4405; depth 51.35 2024-11-06T03:18:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863139.62 epoch sec; seqNo:4405; 4061424.51 north; 594793.55 east; 36.69 lat; -121.94 lon; 51.35 depth; utmZone: 10 2024-11-06T03:18:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:18:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863137.20 2024-11-06T03:18:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:18:59Z- waiting for 1730863141.20, time is 1730863139.62 2024-11-06T03:19:00Z- AHRS_M2 msg: 1730863140.01 epoch sec; seqNo:4141 2024-11-06T03:19:00Z- AHRS_M2 msg: 0.38 phi; 0.02 theta; 0.15 psi 2024-11-06T03:19:00Z- handleDepth()- Depth_Keller msg: 1730863140.01 epoch sec; seqNo:4406; depth 51.36 2024-11-06T03:19:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863140.02 epoch sec; seqNo:4406; 4061424.88 north; 594793.55 east; 36.69 lat; -121.94 lon; 51.36 depth; utmZone: 10 2024-11-06T03:19:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863137.20 2024-11-06T03:19:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:00Z- waiting for 1730863141.20, time is 1730863140.02 2024-11-06T03:19:00Z- AHRS_M2 msg: 1730863140.41 epoch sec; seqNo:4142 2024-11-06T03:19:00Z- AHRS_M2 msg: 0.37 phi; 0.02 theta; 0.18 psi 2024-11-06T03:19:00Z- handleDepth()- Depth_Keller msg: 1730863140.41 epoch sec; seqNo:4407; depth 51.36 2024-11-06T03:19:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863140.42 epoch sec; seqNo:4407; 4061425.26 north; 594793.54 east; 36.69 lat; -121.94 lon; 51.36 depth; utmZone: 10 2024-11-06T03:19:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863137.20 2024-11-06T03:19:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:00Z- waiting for 1730863141.20, time is 1730863140.42 2024-11-06T03:19:00Z- AHRS_M2 msg: 1730863140.82 epoch sec; seqNo:4143 2024-11-06T03:19:00Z- AHRS_M2 msg: 0.36 phi; 0.02 theta; 0.21 psi 2024-11-06T03:19:00Z- handleDepth()- Depth_Keller msg: 1730863140.82 epoch sec; seqNo:4408; depth 51.37 2024-11-06T03:19:00Z- handleDVL() - DVL msg epochMillisec: 1730863140.85 2024-11-06T03:19:00Z- handleDvl() - RDI_Pathfinder msg: 1730863140.85 sec; seqNo:1464; #beams:4 2024-11-06T03:19:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 54.23 , 35.27 , 42.82 , 42.00 2024-11-06T03:19:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.13 , -0.04 2024-11-06T03:19:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863140.86 epoch sec; seqNo:4408; 4061425.64 north; 594793.54 east; 36.69 lat; -121.94 lon; 51.37 depth; utmZone: 10 2024-11-06T03:19:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863137.20 2024-11-06T03:19:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:00Z- waiting for 1730863141.20, time is 1730863140.86 2024-11-06T03:19:01Z- AHRS_M2 msg: 1730863141.22 epoch sec; seqNo:4144 2024-11-06T03:19:01Z- AHRS_M2 msg: 0.36 phi; 0.03 theta; 0.23 psi 2024-11-06T03:19:01Z- handleDepth()- Depth_Keller msg: 1730863141.22 epoch sec; seqNo:4409; depth 51.37 2024-11-06T03:19:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863141.25 epoch sec; seqNo:4409; 4061426.02 north; 594793.54 east; 36.69 lat; -121.94 lon; 51.37 depth; utmZone: 10 2024-11-06T03:19:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863137.20 2024-11-06T03:19:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:01Z- waiting for 1730863141.20, time is 1730863141.25 2024-11-06T03:19:01Z- LcmTrn::updateTrn() >>>> pose time 1730863141.25, meas time 1730863140.85 <<<< 2024-11-06T03:19:01Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863141.25 2024-11-06T03:19:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863140.85 sec, ping # 0; waiting for INS data more recent than 1730863137.20... 2024-11-06T03:19:01Z- TerrainNav::motionUpdate() - currEstimate = (4061426, 594793.54, 51.37) 2024-11-06T03:19:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:19:01Z- TerrainNav:: SubcloudNIS is 0.58, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:19:01Z- TNavPF::Projecting Measurements 2024-11-06T03:19:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:19:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:19:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:19:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:19:01Z- TNavFilter::4 good beams. 2024-11-06T03:19:01Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:19:01Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:19:01Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:19:01Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:19:01Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:19:01Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:19:01Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:19:01Z- TerrainMapOctree: (vehN, vehE ) = (4061425.68, 594793.54). 2024-11-06T03:19:01Z- TerrainMapOctree: Min Distance = 180.99. 2024-11-06T03:19:01Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:19:01Z- Forcing Subcloud Comparison 2024-11-06T03:19:01Z- Weighting particles with subcloud comparison 2024-11-06T03:19:01Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:01Z- meanExpectedMeasDiff: 12.234255 delta_rms_squared: 1.477556 alpha: 0.986645 2024-11-06T03:19:01Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:01Z- meanExpectedMeasDiff: 0.378077 delta_rms_squared: 0.954717 alpha: 0.979481 2024-11-06T03:19:01Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:01Z- meanExpectedMeasDiff: 4.679930 delta_rms_squared: 1.452795 alpha: 0.986420 2024-11-06T03:19:01Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:01Z- meanExpectedMeasDiff: 0.143541 delta_rms_squared: 1.364746 alpha: 0.985557 2024-11-06T03:19:01Z- Subcloud NIS: 0.587843 2024-11-06T03:19:01Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:19:01Z- TNavPF:: sumWeights = 0.000023 2024-11-06T03:19:01Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:19:01Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:19:01Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:19:01Z- TerrainNav::Resampling particle filter... 2024-11-06T03:19:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863140.85 sec. 2024-11-06T03:19:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:19:01Z- variances([0], [2], and psi_berg) = 0.307695 0.161429 0.000000 2024-11-06T03:19:01Z- publishEstimatesDecoder() - at 1730863141.25 sec 2024-11-06T03:19:01Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:19:01Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061456.82 594776.51 54.03 2024-11-06T03:19:01Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061456.17 594776.41 54.48 lat/lon: 36.69 -121.94 2024-11-06T03:19:01Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.31 0.16 0.66 0.00 2024-11-06T03:19:01Z- AHRS_M2 msg: 1730863141.63 epoch sec; seqNo:4145 2024-11-06T03:19:01Z- AHRS_M2 msg: 0.35 phi; 0.03 theta; 0.25 psi 2024-11-06T03:19:01Z- handleDepth()- Depth_Keller msg: 1730863141.64 epoch sec; seqNo:4410; depth 51.39 2024-11-06T03:19:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863141.70 epoch sec; seqNo:4410; 4061426.40 north; 594793.53 east; 36.69 lat; -121.94 lon; 51.39 depth; utmZone: 10 2024-11-06T03:19:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863141.25 2024-11-06T03:19:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:01Z- waiting for 1730863145.25, time is 1730863141.70 2024-11-06T03:19:02Z- AHRS_M2 msg: 1730863142.03 epoch sec; seqNo:4146 2024-11-06T03:19:02Z- AHRS_M2 msg: 0.35 phi; 0.04 theta; 0.27 psi 2024-11-06T03:19:02Z- handleDepth()- Depth_Keller msg: 1730863142.04 epoch sec; seqNo:4411; depth 51.38 2024-11-06T03:19:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863142.05 epoch sec; seqNo:4411; 4061426.40 north; 594793.53 east; 36.69 lat; -121.94 lon; 51.38 depth; utmZone: 10 2024-11-06T03:19:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863141.25 2024-11-06T03:19:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:02Z- waiting for 1730863145.25, time is 1730863142.05 2024-11-06T03:19:02Z- AHRS_M2 msg: 1730863142.43 epoch sec; seqNo:4147 2024-11-06T03:19:02Z- AHRS_M2 msg: 0.34 phi; 0.04 theta; 0.29 psi 2024-11-06T03:19:02Z- handleDepth()- Depth_Keller msg: 1730863142.44 epoch sec; seqNo:4412; depth 51.39 2024-11-06T03:19:02Z- handleDVL() - DVL msg epochMillisec: 1730863142.44 2024-11-06T03:19:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:19:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:19:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:19:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:19:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:19:02Z- handleDvl() - RDI_Pathfinder msg: 1730863142.44 sec; seqNo:1465; #beams:0 2024-11-06T03:19:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:19:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.13 , -0.04 2024-11-06T03:19:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863142.45 epoch sec; seqNo:4412; 4061426.78 north; 594793.53 east; 36.69 lat; -121.94 lon; 51.39 depth; utmZone: 10 2024-11-06T03:19:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863141.25 2024-11-06T03:19:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:02Z- waiting for 1730863145.25, time is 1730863142.45 2024-11-06T03:19:02Z- handleDepth()- Depth_Keller msg: 1730863142.85 epoch sec; seqNo:4413; depth 51.40 2024-11-06T03:19:02Z- handleDVL() - DVL msg epochMillisec: 1730863142.89 2024-11-06T03:19:02Z- handleDvl() - RDI_Pathfinder msg: 1730863142.89 sec; seqNo:1466; #beams:4 2024-11-06T03:19:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 51.59 , 35.48 , 42.62 , 40.99 2024-11-06T03:19:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.09 , -0.00 2024-11-06T03:19:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863142.91 epoch sec; seqNo:4413; 4061427.16 north; 594793.52 east; 36.69 lat; -121.94 lon; 51.40 depth; utmZone: 10 2024-11-06T03:19:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863141.25 2024-11-06T03:19:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:03Z- waiting for 1730863145.25, time is 1730863142.91 2024-11-06T03:19:03Z- AHRS_M2 msg: 1730863143.24 epoch sec; seqNo:4148 2024-11-06T03:19:03Z- AHRS_M2 msg: 0.32 phi; 0.05 theta; 0.33 psi 2024-11-06T03:19:03Z- handleDepth()- Depth_Keller msg: 1730863143.25 epoch sec; seqNo:4414; depth 51.37 2024-11-06T03:19:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863143.28 epoch sec; seqNo:4414; 4061427.54 north; 594793.52 east; 36.69 lat; -121.94 lon; 51.37 depth; utmZone: 10 2024-11-06T03:19:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863141.25 2024-11-06T03:19:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:03Z- waiting for 1730863145.25, time is 1730863143.28 2024-11-06T03:19:03Z- AHRS_M2 msg: 1730863143.65 epoch sec; seqNo:4149 2024-11-06T03:19:03Z- AHRS_M2 msg: 0.32 phi; 0.06 theta; 0.34 psi 2024-11-06T03:19:03Z- handleDepth()- Depth_Keller msg: 1730863143.65 epoch sec; seqNo:4415; depth 51.38 2024-11-06T03:19:03Z- handleDVL() - DVL msg epochMillisec: 1730863143.66 2024-11-06T03:19:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:19:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:19:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:19:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:19:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:19:03Z- handleDvl() - RDI_Pathfinder msg: 1730863143.66 sec; seqNo:1467; #beams:0 2024-11-06T03:19:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:19:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.09 , -0.00 2024-11-06T03:19:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863143.67 epoch sec; seqNo:4415; 4061427.93 north; 594794.73 east; 36.69 lat; -121.94 lon; 51.38 depth; utmZone: 10 2024-11-06T03:19:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863141.25 2024-11-06T03:19:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:03Z- waiting for 1730863145.25, time is 1730863143.67 2024-11-06T03:19:04Z- AHRS_M2 msg: 1730863144.05 epoch sec; seqNo:4150 2024-11-06T03:19:04Z- AHRS_M2 msg: 0.31 phi; 0.06 theta; 0.36 psi 2024-11-06T03:19:04Z- handleDepth()- Depth_Keller msg: 1730863144.06 epoch sec; seqNo:4416; depth 51.38 2024-11-06T03:19:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863144.07 epoch sec; seqNo:4416; 4061427.93 north; 594794.73 east; 36.69 lat; -121.94 lon; 51.38 depth; utmZone: 10 2024-11-06T03:19:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863141.25 2024-11-06T03:19:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:04Z- waiting for 1730863145.25, time is 1730863144.07 2024-11-06T03:19:04Z- AHRS_M2 msg: 1730863144.46 epoch sec; seqNo:4151 2024-11-06T03:19:04Z- AHRS_M2 msg: 0.29 phi; 0.07 theta; 0.38 psi 2024-11-06T03:19:04Z- handleDepth()- Depth_Keller msg: 1730863144.47 epoch sec; seqNo:4417; depth 51.37 2024-11-06T03:19:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863144.49 epoch sec; seqNo:4417; 4061428.31 north; 594794.73 east; 36.69 lat; -121.94 lon; 51.37 depth; utmZone: 10 2024-11-06T03:19:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863141.25 2024-11-06T03:19:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:04Z- waiting for 1730863145.25, time is 1730863144.49 2024-11-06T03:19:04Z- AHRS_M2 msg: 1730863144.86 epoch sec; seqNo:4152 2024-11-06T03:19:04Z- AHRS_M2 msg: 0.28 phi; 0.08 theta; 0.39 psi 2024-11-06T03:19:04Z- handleDepth()- Depth_Keller msg: 1730863144.86 epoch sec; seqNo:4418; depth 51.38 2024-11-06T03:19:04Z- handleDVL() - DVL msg epochMillisec: 1730863144.88 2024-11-06T03:19:04Z- handleDvl() - RDI_Pathfinder msg: 1730863144.88 sec; seqNo:1468; #beams:4 2024-11-06T03:19:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.16 , 35.88 , 43.02 , 40.17 2024-11-06T03:19:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.03 , 0.02 2024-11-06T03:19:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863144.92 epoch sec; seqNo:4418; 4061428.69 north; 594794.73 east; 36.69 lat; -121.94 lon; 51.38 depth; utmZone: 10 2024-11-06T03:19:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863141.25 2024-11-06T03:19:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:05Z- waiting for 1730863145.25, time is 1730863144.92 2024-11-06T03:19:05Z- AHRS_M2 msg: 1730863145.26 epoch sec; seqNo:4153 2024-11-06T03:19:05Z- AHRS_M2 msg: 0.27 phi; 0.09 theta; 0.40 psi 2024-11-06T03:19:05Z- handleDepth()- Depth_Keller msg: 1730863145.27 epoch sec; seqNo:4419; depth 51.36 2024-11-06T03:19:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863145.29 epoch sec; seqNo:4419; 4061429.07 north; 594794.72 east; 36.69 lat; -121.94 lon; 51.36 depth; utmZone: 10 2024-11-06T03:19:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863141.25 2024-11-06T03:19:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:05Z- waiting for 1730863145.25, time is 1730863145.29 2024-11-06T03:19:05Z- LcmTrn::updateTrn() >>>> pose time 1730863145.29, meas time 1730863144.88 <<<< 2024-11-06T03:19:05Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863145.29 2024-11-06T03:19:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863144.88 sec, ping # 0; waiting for INS data more recent than 1730863141.25... 2024-11-06T03:19:05Z- TerrainNav::motionUpdate() - currEstimate = (4061429, 594794.72, 51.36) 2024-11-06T03:19:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:19:05Z- TerrainNav:: SubcloudNIS is 0.59, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:19:05Z- TNavPF::Projecting Measurements 2024-11-06T03:19:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:19:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:19:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:19:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:19:05Z- TNavFilter::4 good beams. 2024-11-06T03:19:05Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:19:05Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:19:05Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:19:05Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:19:05Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:19:05Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:19:05Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:19:05Z- TerrainMapOctree: (vehN, vehE ) = (4061428.76, 594794.60). 2024-11-06T03:19:05Z- TerrainMapOctree: Min Distance = 184.24. 2024-11-06T03:19:05Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:19:05Z- Forcing Subcloud Comparison 2024-11-06T03:19:05Z- Weighting particles with subcloud comparison 2024-11-06T03:19:05Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:05Z- meanExpectedMeasDiff: 4.331549 delta_rms_squared: 1.583809 alpha: 0.987530 2024-11-06T03:19:05Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:05Z- meanExpectedMeasDiff: -0.555665 delta_rms_squared: 0.963054 alpha: 0.979655 2024-11-06T03:19:05Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:05Z- meanExpectedMeasDiff: 3.962866 delta_rms_squared: 2.679567 alpha: 0.992591 2024-11-06T03:19:05Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:05Z- meanExpectedMeasDiff: -1.566130 delta_rms_squared: 1.195647 alpha: 0.983548 2024-11-06T03:19:05Z- Subcloud NIS: 0.578387 2024-11-06T03:19:05Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:19:05Z- TNavPF:: sumWeights = 0.032163 2024-11-06T03:19:05Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:19:05Z- TNavPF::First Term of Map Covariance Matrix is 0.40 2024-11-06T03:19:05Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:19:05Z- TerrainNav::Resampling particle filter... 2024-11-06T03:19:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863144.88 sec. 2024-11-06T03:19:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:19:05Z- variances([0], [2], and psi_berg) = 0.326392 0.164686 0.000000 2024-11-06T03:19:05Z- publishEstimatesDecoder() - at 1730863145.29 sec 2024-11-06T03:19:05Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:19:05Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061459.81 594777.72 54.03 2024-11-06T03:19:05Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061459.20 594777.62 53.78 lat/lon: 36.69 -121.94 2024-11-06T03:19:05Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.16 0.68 0.00 2024-11-06T03:19:05Z- AHRS_M2 msg: 1730863145.66 epoch sec; seqNo:4154 2024-11-06T03:19:05Z- AHRS_M2 msg: 0.26 phi; 0.09 theta; 0.41 psi 2024-11-06T03:19:05Z- handleDepth()- Depth_Keller msg: 1730863145.68 epoch sec; seqNo:4420; depth 51.34 2024-11-06T03:19:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863145.71 epoch sec; seqNo:4420; 4061429.44 north; 594794.72 east; 36.69 lat; -121.94 lon; 51.34 depth; utmZone: 10 2024-11-06T03:19:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863145.29 2024-11-06T03:19:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:05Z- waiting for 1730863149.29, time is 1730863145.71 2024-11-06T03:19:06Z- handleDepth()- Depth_Keller msg: 1730863146.08 epoch sec; seqNo:4421; depth 51.31 2024-11-06T03:19:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863146.10 epoch sec; seqNo:4421; 4061429.44 north; 594794.72 east; 36.69 lat; -121.94 lon; 51.31 depth; utmZone: 10 2024-11-06T03:19:06Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:19:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863145.29 2024-11-06T03:19:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:06Z- waiting for 1730863149.29, time is 1730863146.10 2024-11-06T03:19:06Z- AHRS_M2 msg: 1730863146.47 epoch sec; seqNo:4155 2024-11-06T03:19:06Z- AHRS_M2 msg: 0.24 phi; 0.11 theta; 0.43 psi 2024-11-06T03:19:06Z- handleDepth()- Depth_Keller msg: 1730863146.48 epoch sec; seqNo:4422; depth 51.30 2024-11-06T03:19:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863146.49 epoch sec; seqNo:4422; 4061429.82 north; 594794.71 east; 36.69 lat; -121.94 lon; 51.30 depth; utmZone: 10 2024-11-06T03:19:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863145.29 2024-11-06T03:19:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:06Z- waiting for 1730863149.29, time is 1730863146.49 2024-11-06T03:19:06Z- AHRS_M2 msg: 1730863146.88 epoch sec; seqNo:4156 2024-11-06T03:19:06Z- AHRS_M2 msg: 0.23 phi; 0.12 theta; 0.43 psi 2024-11-06T03:19:06Z- handleDepth()- Depth_Keller msg: 1730863146.88 epoch sec; seqNo:4423; depth 51.27 2024-11-06T03:19:06Z- handleDVL() - DVL msg epochMillisec: 1730863146.91 2024-11-06T03:19:06Z- handleDvl() - RDI_Pathfinder msg: 1730863146.91 sec; seqNo:1469; #beams:4 2024-11-06T03:19:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.32 , 36.29 , 43.63 , 39.15 2024-11-06T03:19:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.01 , -0.00 2024-11-06T03:19:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863146.93 epoch sec; seqNo:4423; 4061430.20 north; 594794.71 east; 36.69 lat; -121.94 lon; 51.27 depth; utmZone: 10 2024-11-06T03:19:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863145.29 2024-11-06T03:19:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:07Z- waiting for 1730863149.29, time is 1730863146.93 2024-11-06T03:19:07Z- AHRS_M2 msg: 1730863147.28 epoch sec; seqNo:4157 2024-11-06T03:19:07Z- AHRS_M2 msg: 0.23 phi; 0.12 theta; 0.44 psi 2024-11-06T03:19:07Z- handleDepth()- Depth_Keller msg: 1730863147.29 epoch sec; seqNo:4424; depth 51.24 2024-11-06T03:19:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863147.30 epoch sec; seqNo:4424; 4061430.58 north; 594794.71 east; 36.69 lat; -121.94 lon; 51.24 depth; utmZone: 10 2024-11-06T03:19:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863145.29 2024-11-06T03:19:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:07Z- waiting for 1730863149.29, time is 1730863147.30 2024-11-06T03:19:07Z- AHRS_M2 msg: 1730863147.68 epoch sec; seqNo:4158 2024-11-06T03:19:07Z- AHRS_M2 msg: 0.22 phi; 0.13 theta; 0.45 psi 2024-11-06T03:19:07Z- handleDepth()- Depth_Keller msg: 1730863147.69 epoch sec; seqNo:4425; depth 51.20 2024-11-06T03:19:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863147.70 epoch sec; seqNo:4425; 4061430.58 north; 594794.71 east; 36.69 lat; -121.94 lon; 51.20 depth; utmZone: 10 2024-11-06T03:19:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863145.29 2024-11-06T03:19:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:07Z- waiting for 1730863149.29, time is 1730863147.70 2024-11-06T03:19:08Z- handleDepth()- Depth_Keller msg: 1730863148.11 epoch sec; seqNo:4426; depth 51.17 2024-11-06T03:19:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863148.12 epoch sec; seqNo:4426; 4061430.97 north; 594795.92 east; 36.69 lat; -121.94 lon; 51.17 depth; utmZone: 10 2024-11-06T03:19:08Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:19:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863145.29 2024-11-06T03:19:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:08Z- waiting for 1730863149.29, time is 1730863148.12 2024-11-06T03:19:08Z- AHRS_M2 msg: 1730863148.49 epoch sec; seqNo:4159 2024-11-06T03:19:08Z- AHRS_M2 msg: 0.21 phi; 0.15 theta; 0.46 psi 2024-11-06T03:19:08Z- handleDepth()- Depth_Keller msg: 1730863148.50 epoch sec; seqNo:4427; depth 51.13 2024-11-06T03:19:08Z- handleDVL() - DVL msg epochMillisec: 1730863148.51 2024-11-06T03:19:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:19:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:19:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:19:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:19:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:19:08Z- handleDvl() - RDI_Pathfinder msg: 1730863148.51 sec; seqNo:1470; #beams:0 2024-11-06T03:19:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:19:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.01 , -0.00 2024-11-06T03:19:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863148.52 epoch sec; seqNo:4427; 4061431.35 north; 594795.92 east; 36.69 lat; -121.94 lon; 51.13 depth; utmZone: 10 2024-11-06T03:19:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863145.29 2024-11-06T03:19:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:08Z- waiting for 1730863149.29, time is 1730863148.52 2024-11-06T03:19:08Z- AHRS_M2 msg: 1730863148.89 epoch sec; seqNo:4160 2024-11-06T03:19:08Z- AHRS_M2 msg: 0.21 phi; 0.15 theta; 0.47 psi 2024-11-06T03:19:08Z- handleDepth()- Depth_Keller msg: 1730863148.91 epoch sec; seqNo:4428; depth 51.09 2024-11-06T03:19:08Z- handleDVL() - DVL msg epochMillisec: 1730863148.94 2024-11-06T03:19:08Z- handleDvl() - RDI_Pathfinder msg: 1730863148.94 sec; seqNo:1471; #beams:4 2024-11-06T03:19:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 37.11 , 45.06 , 38.33 2024-11-06T03:19:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , -0.05 2024-11-06T03:19:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863148.96 epoch sec; seqNo:4428; 4061431.73 north; 594795.91 east; 36.69 lat; -121.94 lon; 51.09 depth; utmZone: 10 2024-11-06T03:19:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863145.29 2024-11-06T03:19:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:09Z- waiting for 1730863149.29, time is 1730863148.96 2024-11-06T03:19:09Z- AHRS_M2 msg: 1730863149.31 epoch sec; seqNo:4161 2024-11-06T03:19:09Z- AHRS_M2 msg: 0.21 phi; 0.16 theta; 0.47 psi 2024-11-06T03:19:09Z- handleDepth()- Depth_Keller msg: 1730863149.31 epoch sec; seqNo:4429; depth 51.03 2024-11-06T03:19:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863149.33 epoch sec; seqNo:4429; 4061431.73 north; 594795.91 east; 36.69 lat; -121.94 lon; 51.03 depth; utmZone: 10 2024-11-06T03:19:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863145.29 2024-11-06T03:19:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:09Z- waiting for 1730863149.29, time is 1730863149.33 2024-11-06T03:19:09Z- LcmTrn::updateTrn() >>>> pose time 1730863149.33, meas time 1730863148.94 <<<< 2024-11-06T03:19:09Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863149.33 2024-11-06T03:19:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863148.94 sec, ping # 0; waiting for INS data more recent than 1730863145.29... 2024-11-06T03:19:09Z- TerrainNav::motionUpdate() - currEstimate = (4061432, 594795.91, 51.03) 2024-11-06T03:19:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:19:09Z- TerrainNav:: SubcloudNIS is 0.58, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:19:09Z- TNavPF::Projecting Measurements 2024-11-06T03:19:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:19:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:19:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:19:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:19:09Z- TNavFilter::4 good beams. 2024-11-06T03:19:09Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:19:09Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:19:09Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:19:09Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:19:09Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:19:09Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:19:09Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:19:09Z- TerrainMapOctree: (vehN, vehE ) = (4061431.48, 594795.80). 2024-11-06T03:19:09Z- TerrainMapOctree: Min Distance = 187.17. 2024-11-06T03:19:09Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:19:09Z- Forcing Subcloud Comparison 2024-11-06T03:19:09Z- Weighting particles with subcloud comparison 2024-11-06T03:19:09Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:09Z- meanExpectedMeasDiff: 5.446141 delta_rms_squared: 1.578867 alpha: 0.987491 2024-11-06T03:19:09Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:09Z- meanExpectedMeasDiff: -0.330872 delta_rms_squared: 1.128444 alpha: 0.982585 2024-11-06T03:19:09Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:09Z- meanExpectedMeasDiff: 1.296633 delta_rms_squared: 1.586663 alpha: 0.987552 2024-11-06T03:19:09Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:09Z- meanExpectedMeasDiff: -2.354627 delta_rms_squared: 1.520104 alpha: 0.987014 2024-11-06T03:19:09Z- Subcloud NIS: 0.562822 2024-11-06T03:19:09Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:19:09Z- TNavPF:: sumWeights = 0.020969 2024-11-06T03:19:09Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:19:09Z- TNavPF::First Term of Map Covariance Matrix is 0.50 2024-11-06T03:19:09Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:19:09Z- TerrainNav::Resampling particle filter... 2024-11-06T03:19:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863148.94 sec. 2024-11-06T03:19:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:19:09Z- variances([0], [2], and psi_berg) = 0.315935 0.164116 0.000000 2024-11-06T03:19:09Z- publishEstimatesDecoder() - at 1730863149.33 sec 2024-11-06T03:19:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:19:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061462.48 594778.90 52.99 2024-11-06T03:19:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061461.91 594778.80 53.07 lat/lon: 36.69 -121.94 2024-11-06T03:19:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.32 0.16 0.67 0.00 2024-11-06T03:19:09Z- AHRS_M2 msg: 1730863149.70 epoch sec; seqNo:4162 2024-11-06T03:19:09Z- AHRS_M2 msg: 0.21 phi; 0.16 theta; 0.48 psi 2024-11-06T03:19:09Z- handleDepth()- Depth_Keller msg: 1730863149.71 epoch sec; seqNo:4430; depth 50.99 2024-11-06T03:19:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863149.74 epoch sec; seqNo:4430; 4061432.11 north; 594795.91 east; 36.69 lat; -121.94 lon; 50.99 depth; utmZone: 10 2024-11-06T03:19:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863149.33 2024-11-06T03:19:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:09Z- waiting for 1730863153.33, time is 1730863149.74 2024-11-06T03:19:10Z- AHRS_M2 msg: 1730863150.11 epoch sec; seqNo:4163 2024-11-06T03:19:10Z- AHRS_M2 msg: 0.20 phi; 0.17 theta; 0.48 psi 2024-11-06T03:19:10Z- handleDepth()- Depth_Keller msg: 1730863150.12 epoch sec; seqNo:4431; depth 50.93 2024-11-06T03:19:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863150.14 epoch sec; seqNo:4431; 4061432.49 north; 594795.90 east; 36.69 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T03:19:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863149.33 2024-11-06T03:19:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:10Z- waiting for 1730863153.33, time is 1730863150.14 2024-11-06T03:19:10Z- AHRS_M2 msg: 1730863150.51 epoch sec; seqNo:4164 2024-11-06T03:19:10Z- AHRS_M2 msg: 0.19 phi; 0.17 theta; 0.49 psi 2024-11-06T03:19:10Z- handleDepth()- Depth_Keller msg: 1730863150.52 epoch sec; seqNo:4432; depth 50.88 2024-11-06T03:19:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863150.53 epoch sec; seqNo:4432; 4061432.87 north; 594795.90 east; 36.69 lat; -121.94 lon; 50.88 depth; utmZone: 10 2024-11-06T03:19:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863149.33 2024-11-06T03:19:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:10Z- waiting for 1730863153.33, time is 1730863150.53 2024-11-06T03:19:10Z- AHRS_M2 msg: 1730863150.92 epoch sec; seqNo:4165 2024-11-06T03:19:10Z- AHRS_M2 msg: 0.19 phi; 0.18 theta; 0.49 psi 2024-11-06T03:19:10Z- handleDepth()- Depth_Keller msg: 1730863150.92 epoch sec; seqNo:4433; depth 50.83 2024-11-06T03:19:10Z- handleDVL() - DVL msg epochMillisec: 1730863150.95 2024-11-06T03:19:10Z- handleDvl() - RDI_Pathfinder msg: 1730863150.95 sec; seqNo:1472; #beams:4 2024-11-06T03:19:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.49 , 37.92 , 45.88 , 37.72 2024-11-06T03:19:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.06 , -0.08 2024-11-06T03:19:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863150.96 epoch sec; seqNo:4433; 4061432.87 north; 594795.90 east; 36.69 lat; -121.94 lon; 50.83 depth; utmZone: 10 2024-11-06T03:19:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863149.33 2024-11-06T03:19:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:11Z- waiting for 1730863153.33, time is 1730863150.96 2024-11-06T03:19:11Z- AHRS_M2 msg: 1730863151.32 epoch sec; seqNo:4166 2024-11-06T03:19:11Z- AHRS_M2 msg: 0.18 phi; 0.18 theta; 0.49 psi 2024-11-06T03:19:11Z- handleDepth()- Depth_Keller msg: 1730863151.33 epoch sec; seqNo:4434; depth 50.76 2024-11-06T03:19:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863151.35 epoch sec; seqNo:4434; 4061433.26 north; 594797.12 east; 36.69 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T03:19:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863149.33 2024-11-06T03:19:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:11Z- waiting for 1730863153.33, time is 1730863151.35 2024-11-06T03:19:11Z- handleDepth()- Depth_Keller msg: 1730863151.74 epoch sec; seqNo:4435; depth 50.71 2024-11-06T03:19:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863151.76 epoch sec; seqNo:4435; 4061433.64 north; 594797.11 east; 36.69 lat; -121.94 lon; 50.71 depth; utmZone: 10 2024-11-06T03:19:11Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:19:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863149.33 2024-11-06T03:19:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:11Z- waiting for 1730863153.33, time is 1730863151.76 2024-11-06T03:19:12Z- AHRS_M2 msg: 1730863152.13 epoch sec; seqNo:4167 2024-11-06T03:19:12Z- AHRS_M2 msg: 0.17 phi; 0.18 theta; 0.49 psi 2024-11-06T03:19:12Z- handleDepth()- Depth_Keller msg: 1730863152.13 epoch sec; seqNo:4436; depth 50.65 2024-11-06T03:19:12Z- handleDVL() - DVL msg epochMillisec: 1730863152.14 2024-11-06T03:19:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:19:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:19:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:19:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:19:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:19:12Z- handleDvl() - RDI_Pathfinder msg: 1730863152.14 sec; seqNo:1473; #beams:0 2024-11-06T03:19:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:19:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.06 , -0.08 2024-11-06T03:19:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863152.14 epoch sec; seqNo:4436; 4061434.02 north; 594797.11 east; 36.69 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:19:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863149.33 2024-11-06T03:19:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:12Z- waiting for 1730863153.33, time is 1730863152.14 2024-11-06T03:19:12Z- AHRS_M2 msg: 1730863152.53 epoch sec; seqNo:4168 2024-11-06T03:19:12Z- AHRS_M2 msg: 0.16 phi; 0.19 theta; 0.49 psi 2024-11-06T03:19:12Z- handleDepth()- Depth_Keller msg: 1730863152.54 epoch sec; seqNo:4437; depth 50.57 2024-11-06T03:19:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863152.55 epoch sec; seqNo:4437; 4061434.02 north; 594797.11 east; 36.69 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T03:19:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863149.33 2024-11-06T03:19:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:12Z- waiting for 1730863153.33, time is 1730863152.55 2024-11-06T03:19:12Z- handleDepth()- Depth_Keller msg: 1730863152.95 epoch sec; seqNo:4438; depth 50.54 2024-11-06T03:19:12Z- handleDVL() - DVL msg epochMillisec: 1730863152.97 2024-11-06T03:19:12Z- handleDvl() - RDI_Pathfinder msg: 1730863152.97 sec; seqNo:1474; #beams:4 2024-11-06T03:19:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.26 , 38.53 , 46.28 , 37.51 2024-11-06T03:19:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.14 , -0.06 2024-11-06T03:19:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863152.99 epoch sec; seqNo:4438; 4061434.40 north; 594797.10 east; 36.69 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:19:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863149.33 2024-11-06T03:19:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:13Z- waiting for 1730863153.33, time is 1730863152.99 2024-11-06T03:19:13Z- AHRS_M2 msg: 1730863153.34 epoch sec; seqNo:4169 2024-11-06T03:19:13Z- AHRS_M2 msg: 0.16 phi; 0.19 theta; 0.50 psi 2024-11-06T03:19:13Z- handleDepth()- Depth_Keller msg: 1730863153.34 epoch sec; seqNo:4439; depth 50.46 2024-11-06T03:19:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863153.36 epoch sec; seqNo:4439; 4061434.78 north; 594797.10 east; 36.69 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:19:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863149.33 2024-11-06T03:19:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:13Z- waiting for 1730863153.33, time is 1730863153.36 2024-11-06T03:19:13Z- LcmTrn::updateTrn() >>>> pose time 1730863153.36, meas time 1730863152.97 <<<< 2024-11-06T03:19:13Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863153.36 2024-11-06T03:19:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863152.97 sec, ping # 0; waiting for INS data more recent than 1730863149.33... 2024-11-06T03:19:13Z- TerrainNav::motionUpdate() - currEstimate = (4061435, 594797.10, 50.46) 2024-11-06T03:19:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:19:13Z- TerrainNav:: SubcloudNIS is 0.56, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:19:13Z- TNavPF::Projecting Measurements 2024-11-06T03:19:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:19:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:19:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:19:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:19:13Z- TNavFilter::4 good beams. 2024-11-06T03:19:13Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:19:13Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:19:13Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:19:13Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:19:13Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:19:13Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:19:13Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:19:13Z- TerrainMapOctree: (vehN, vehE ) = (4061434.48, 594796.99). 2024-11-06T03:19:13Z- TerrainMapOctree: Min Distance = 190.37. 2024-11-06T03:19:13Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:19:13Z- Forcing Subcloud Comparison 2024-11-06T03:19:13Z- Weighting particles with subcloud comparison 2024-11-06T03:19:13Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:13Z- meanExpectedMeasDiff: 5.449293 delta_rms_squared: 1.359360 alpha: 0.985501 2024-11-06T03:19:13Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:13Z- meanExpectedMeasDiff: -1.034785 delta_rms_squared: 2.129405 alpha: 0.990695 2024-11-06T03:19:13Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:13Z- meanExpectedMeasDiff: -0.781527 delta_rms_squared: 1.654456 alpha: 0.988056 2024-11-06T03:19:13Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:13Z- meanExpectedMeasDiff: -2.394941 delta_rms_squared: 1.048407 alpha: 0.981281 2024-11-06T03:19:13Z- Subcloud NIS: 0.566988 2024-11-06T03:19:13Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:19:13Z- TNavPF:: sumWeights = 0.013956 2024-11-06T03:19:13Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:19:13Z- TNavPF::First Term of Map Covariance Matrix is 0.41 2024-11-06T03:19:13Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:19:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863152.97 sec. 2024-11-06T03:19:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:19:13Z- variances([0], [2], and psi_berg) = 0.351939 0.169283 0.000000 2024-11-06T03:19:13Z- publishEstimatesDecoder() - at 1730863153.36 sec 2024-11-06T03:19:13Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:19:13Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061463.12 594781.11 52.10 2024-11-06T03:19:13Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061464.95 594779.96 52.46 lat/lon: 36.69 -121.94 2024-11-06T03:19:13Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.35 0.17 0.69 0.00 2024-11-06T03:19:13Z- AHRS_M2 msg: 1730863153.77 epoch sec; seqNo:4170 2024-11-06T03:19:13Z- AHRS_M2 msg: 0.16 phi; 0.19 theta; 0.50 psi 2024-11-06T03:19:13Z- handleDepth()- Depth_Keller msg: 1730863153.79 epoch sec; seqNo:4440; depth 50.41 2024-11-06T03:19:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863153.81 epoch sec; seqNo:4440; 4061435.15 north; 594797.10 east; 36.69 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:19:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863153.36 2024-11-06T03:19:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:13Z- waiting for 1730863157.36, time is 1730863153.81 2024-11-06T03:19:14Z- AHRS_M2 msg: 1730863154.16 epoch sec; seqNo:4171 2024-11-06T03:19:14Z- AHRS_M2 msg: 0.16 phi; 0.19 theta; 0.50 psi 2024-11-06T03:19:14Z- handleDepth()- Depth_Keller msg: 1730863154.16 epoch sec; seqNo:4441; depth 50.35 2024-11-06T03:19:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863154.17 epoch sec; seqNo:4441; 4061435.15 north; 594797.10 east; 36.69 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:19:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863153.36 2024-11-06T03:19:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:14Z- waiting for 1730863157.36, time is 1730863154.17 2024-11-06T03:19:14Z- AHRS_M2 msg: 1730863154.56 epoch sec; seqNo:4172 2024-11-06T03:19:14Z- AHRS_M2 msg: 0.16 phi; 0.19 theta; 0.50 psi 2024-11-06T03:19:14Z- handleDepth()- Depth_Keller msg: 1730863154.57 epoch sec; seqNo:4442; depth 50.31 2024-11-06T03:19:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863154.58 epoch sec; seqNo:4442; 4061435.53 north; 594797.09 east; 36.69 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:19:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863153.36 2024-11-06T03:19:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:14Z- waiting for 1730863157.36, time is 1730863154.58 2024-11-06T03:19:14Z- AHRS_M2 msg: 1730863154.97 epoch sec; seqNo:4173 2024-11-06T03:19:14Z- AHRS_M2 msg: 0.15 phi; 0.19 theta; 0.50 psi 2024-11-06T03:19:14Z- handleDepth()- Depth_Keller msg: 1730863154.98 epoch sec; seqNo:4443; depth 50.25 2024-11-06T03:19:15Z- handleDVL() - DVL msg epochMillisec: 1730863155.01 2024-11-06T03:19:15Z- handleDvl() - RDI_Pathfinder msg: 1730863155.01 sec; seqNo:1475; #beams:4 2024-11-06T03:19:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.47 , 39.15 , 47.30 , 37.51 2024-11-06T03:19:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.16 , -0.02 2024-11-06T03:19:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863155.02 epoch sec; seqNo:4443; 4061435.91 north; 594797.09 east; 36.69 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:19:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863153.36 2024-11-06T03:19:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:15Z- waiting for 1730863157.36, time is 1730863155.02 2024-11-06T03:19:15Z- AHRS_M2 msg: 1730863155.37 epoch sec; seqNo:4174 2024-11-06T03:19:15Z- AHRS_M2 msg: 0.15 phi; 0.18 theta; 0.49 psi 2024-11-06T03:19:15Z- handleDepth()- Depth_Keller msg: 1730863155.38 epoch sec; seqNo:4444; depth 50.18 2024-11-06T03:19:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863155.39 epoch sec; seqNo:4444; 4061436.29 north; 594797.08 east; 36.69 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:19:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863153.36 2024-11-06T03:19:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:15Z- waiting for 1730863157.36, time is 1730863155.39 2024-11-06T03:19:15Z- AHRS_M2 msg: 1730863155.78 epoch sec; seqNo:4175 2024-11-06T03:19:15Z- AHRS_M2 msg: 0.15 phi; 0.18 theta; 0.49 psi 2024-11-06T03:19:15Z- handleDepth()- Depth_Keller msg: 1730863155.78 epoch sec; seqNo:4445; depth 50.12 2024-11-06T03:19:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863155.79 epoch sec; seqNo:4445; 4061436.68 north; 594798.30 east; 36.69 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:19:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863153.36 2024-11-06T03:19:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:15Z- waiting for 1730863157.36, time is 1730863155.79 2024-11-06T03:19:16Z- AHRS_M2 msg: 1730863156.18 epoch sec; seqNo:4176 2024-11-06T03:19:16Z- AHRS_M2 msg: 0.15 phi; 0.18 theta; 0.49 psi 2024-11-06T03:19:16Z- handleDepth()- Depth_Keller msg: 1730863156.19 epoch sec; seqNo:4446; depth 50.06 2024-11-06T03:19:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863156.20 epoch sec; seqNo:4446; 4061437.06 north; 594798.29 east; 36.69 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:19:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863153.36 2024-11-06T03:19:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:16Z- waiting for 1730863157.36, time is 1730863156.20 2024-11-06T03:19:16Z- AHRS_M2 msg: 1730863156.58 epoch sec; seqNo:4177 2024-11-06T03:19:16Z- AHRS_M2 msg: 0.15 phi; 0.18 theta; 0.49 psi 2024-11-06T03:19:16Z- handleDepth()- Depth_Keller msg: 1730863156.59 epoch sec; seqNo:4447; depth 50.01 2024-11-06T03:19:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863156.60 epoch sec; seqNo:4447; 4061437.06 north; 594798.29 east; 36.69 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:19:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863153.36 2024-11-06T03:19:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:16Z- waiting for 1730863157.36, time is 1730863156.60 2024-11-06T03:19:16Z- AHRS_M2 msg: 1730863156.99 epoch sec; seqNo:4178 2024-11-06T03:19:16Z- AHRS_M2 msg: 0.15 phi; 0.18 theta; 0.49 psi 2024-11-06T03:19:16Z- handleDepth()- Depth_Keller msg: 1730863156.99 epoch sec; seqNo:4448; depth 49.96 2024-11-06T03:19:17Z- handleDVL() - DVL msg epochMillisec: 1730863157.01 2024-11-06T03:19:17Z- handleDvl() - RDI_Pathfinder msg: 1730863157.01 sec; seqNo:1476; #beams:4 2024-11-06T03:19:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.47 , 39.35 , 46.89 , 38.13 2024-11-06T03:19:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.12 , 0.04 2024-11-06T03:19:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863157.03 epoch sec; seqNo:4448; 4061437.44 north; 594798.29 east; 36.69 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:19:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863153.36 2024-11-06T03:19:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:17Z- waiting for 1730863157.36, time is 1730863157.03 2024-11-06T03:19:17Z- AHRS_M2 msg: 1730863157.39 epoch sec; seqNo:4179 2024-11-06T03:19:17Z- AHRS_M2 msg: 0.15 phi; 0.18 theta; 0.49 psi 2024-11-06T03:19:17Z- handleDepth()- Depth_Keller msg: 1730863157.40 epoch sec; seqNo:4449; depth 49.90 2024-11-06T03:19:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863157.41 epoch sec; seqNo:4449; 4061437.82 north; 594798.29 east; 36.69 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:19:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863153.36 2024-11-06T03:19:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:17Z- waiting for 1730863157.36, time is 1730863157.41 2024-11-06T03:19:17Z- LcmTrn::updateTrn() >>>> pose time 1730863157.41, meas time 1730863157.01 <<<< 2024-11-06T03:19:17Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863157.41 2024-11-06T03:19:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863157.01 sec, ping # 0; waiting for INS data more recent than 1730863153.36... 2024-11-06T03:19:17Z- TerrainNav::motionUpdate() - currEstimate = (4061438, 594798.29, 49.90) 2024-11-06T03:19:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:19:17Z- TerrainNav:: SubcloudNIS is 0.57, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:19:17Z- TNavPF::Projecting Measurements 2024-11-06T03:19:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:19:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:19:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:19:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:19:17Z- TNavFilter::4 good beams. 2024-11-06T03:19:17Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:19:17Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:19:17Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:19:17Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:19:17Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:19:17Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:19:17Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:19:17Z- TerrainMapOctree: (vehN, vehE ) = (4061437.52, 594798.17). 2024-11-06T03:19:17Z- TerrainMapOctree: Min Distance = 193.61. 2024-11-06T03:19:17Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:19:17Z- Forcing Subcloud Comparison 2024-11-06T03:19:17Z- Weighting particles with subcloud comparison 2024-11-06T03:19:17Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:17Z- meanExpectedMeasDiff: 5.797187 delta_rms_squared: 1.089319 alpha: 0.981971 2024-11-06T03:19:17Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:17Z- meanExpectedMeasDiff: -1.495383 delta_rms_squared: 1.999033 alpha: 0.990094 2024-11-06T03:19:17Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:17Z- meanExpectedMeasDiff: -0.518348 delta_rms_squared: 1.638695 alpha: 0.987942 2024-11-06T03:19:17Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:17Z- meanExpectedMeasDiff: -2.485834 delta_rms_squared: 1.087282 alpha: 0.981938 2024-11-06T03:19:17Z- Subcloud NIS: 0.572591 2024-11-06T03:19:17Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:19:17Z- TNavPF:: sumWeights = 0.010024 2024-11-06T03:19:17Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:19:17Z- TNavPF::First Term of Map Covariance Matrix is 0.32 2024-11-06T03:19:17Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:19:17Z- TerrainNav::Resampling particle filter... 2024-11-06T03:19:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863157.01 sec. 2024-11-06T03:19:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:19:17Z- variances([0], [2], and psi_berg) = 0.278858 0.168207 0.000000 2024-11-06T03:19:17Z- publishEstimatesDecoder() - at 1730863157.41 sec 2024-11-06T03:19:17Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:19:17Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061468.54 594781.22 51.56 2024-11-06T03:19:17Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061467.82 594781.13 51.96 lat/lon: 36.69 -121.94 2024-11-06T03:19:17Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.28 0.17 0.64 0.00 2024-11-06T03:19:17Z- AHRS_M2 msg: 1730863157.79 epoch sec; seqNo:4180 2024-11-06T03:19:17Z- AHRS_M2 msg: 0.15 phi; 0.17 theta; 0.49 psi 2024-11-06T03:19:17Z- handleDepth()- Depth_Keller msg: 1730863157.80 epoch sec; seqNo:4450; depth 49.84 2024-11-06T03:19:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863157.82 epoch sec; seqNo:4450; 4061438.20 north; 594798.28 east; 36.69 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:19:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863157.41 2024-11-06T03:19:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:17Z- waiting for 1730863161.41, time is 1730863157.82 2024-11-06T03:19:18Z- handleDepth()- Depth_Keller msg: 1730863158.22 epoch sec; seqNo:4451; depth 49.79 2024-11-06T03:19:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863158.23 epoch sec; seqNo:4451; 4061438.58 north; 594798.28 east; 36.69 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:19:18Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:19:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863157.41 2024-11-06T03:19:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:18Z- waiting for 1730863161.41, time is 1730863158.23 2024-11-06T03:19:18Z- AHRS_M2 msg: 1730863158.60 epoch sec; seqNo:4181 2024-11-06T03:19:18Z- AHRS_M2 msg: 0.16 phi; 0.17 theta; 0.50 psi 2024-11-06T03:19:18Z- handleDepth()- Depth_Keller msg: 1730863158.60 epoch sec; seqNo:4452; depth 49.74 2024-11-06T03:19:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863158.61 epoch sec; seqNo:4452; 4061438.96 north; 594798.27 east; 36.69 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:19:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863157.41 2024-11-06T03:19:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:18Z- waiting for 1730863161.41, time is 1730863158.61 2024-11-06T03:19:19Z- AHRS_M2 msg: 1730863159.01 epoch sec; seqNo:4182 2024-11-06T03:19:19Z- AHRS_M2 msg: 0.16 phi; 0.17 theta; 0.50 psi 2024-11-06T03:19:19Z- handleDepth()- Depth_Keller msg: 1730863159.01 epoch sec; seqNo:4453; depth 49.70 2024-11-06T03:19:19Z- handleDVL() - DVL msg epochMillisec: 1730863159.03 2024-11-06T03:19:19Z- handleDvl() - RDI_Pathfinder msg: 1730863159.03 sec; seqNo:1477; #beams:4 2024-11-06T03:19:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.88 , 39.55 , 46.89 , 38.53 2024-11-06T03:19:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , 0.06 2024-11-06T03:19:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863159.04 epoch sec; seqNo:4453; 4061438.96 north; 594798.27 east; 36.69 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:19:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863157.41 2024-11-06T03:19:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:19Z- waiting for 1730863161.41, time is 1730863159.04 2024-11-06T03:19:19Z- AHRS_M2 msg: 1730863159.40 epoch sec; seqNo:4183 2024-11-06T03:19:19Z- AHRS_M2 msg: 0.16 phi; 0.16 theta; 0.50 psi 2024-11-06T03:19:19Z- handleDepth()- Depth_Keller msg: 1730863159.41 epoch sec; seqNo:4454; depth 49.61 2024-11-06T03:19:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863159.43 epoch sec; seqNo:4454; 4061439.33 north; 594798.27 east; 36.69 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:19:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863157.41 2024-11-06T03:19:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:19Z- waiting for 1730863161.41, time is 1730863159.43 2024-11-06T03:19:19Z- AHRS_M2 msg: 1730863159.81 epoch sec; seqNo:4184 2024-11-06T03:19:19Z- AHRS_M2 msg: 0.16 phi; 0.16 theta; 0.50 psi 2024-11-06T03:19:19Z- handleDepth()- Depth_Keller msg: 1730863159.81 epoch sec; seqNo:4455; depth 49.57 2024-11-06T03:19:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863159.82 epoch sec; seqNo:4455; 4061439.73 north; 594799.49 east; 36.69 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:19:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863157.41 2024-11-06T03:19:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:19Z- waiting for 1730863161.41, time is 1730863159.82 2024-11-06T03:19:20Z- AHRS_M2 msg: 1730863160.21 epoch sec; seqNo:4185 2024-11-06T03:19:20Z- AHRS_M2 msg: 0.16 phi; 0.15 theta; 0.51 psi 2024-11-06T03:19:20Z- handleDepth()- Depth_Keller msg: 1730863160.21 epoch sec; seqNo:4456; depth 49.52 2024-11-06T03:19:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863160.23 epoch sec; seqNo:4456; 4061440.11 north; 594799.48 east; 36.69 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T03:19:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863157.41 2024-11-06T03:19:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:20Z- waiting for 1730863161.41, time is 1730863160.23 2024-11-06T03:19:20Z- AHRS_M2 msg: 1730863160.62 epoch sec; seqNo:4186 2024-11-06T03:19:20Z- AHRS_M2 msg: 0.16 phi; 0.15 theta; 0.51 psi 2024-11-06T03:19:20Z- handleDepth()- Depth_Keller msg: 1730863160.63 epoch sec; seqNo:4457; depth 49.45 2024-11-06T03:19:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863160.65 epoch sec; seqNo:4457; 4061440.48 north; 594799.48 east; 36.69 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:19:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863157.41 2024-11-06T03:19:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:20Z- waiting for 1730863161.41, time is 1730863160.65 2024-11-06T03:19:21Z- handleDepth()- Depth_Keller msg: 1730863161.04 epoch sec; seqNo:4458; depth 49.41 2024-11-06T03:19:21Z- handleDVL() - DVL msg epochMillisec: 1730863161.06 2024-11-06T03:19:21Z- handleDvl() - RDI_Pathfinder msg: 1730863161.06 sec; seqNo:1478; #beams:4 2024-11-06T03:19:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.49 , 39.96 , 46.89 , 39.35 2024-11-06T03:19:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.03 , 0.00 2024-11-06T03:19:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863161.07 epoch sec; seqNo:4458; 4061440.48 north; 594799.48 east; 36.69 lat; -121.94 lon; 49.41 depth; utmZone: 10 2024-11-06T03:19:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863157.41 2024-11-06T03:19:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:21Z- waiting for 1730863161.41, time is 1730863161.07 2024-11-06T03:19:21Z- AHRS_M2 msg: 1730863161.43 epoch sec; seqNo:4187 2024-11-06T03:19:21Z- AHRS_M2 msg: 0.16 phi; 0.14 theta; 0.52 psi 2024-11-06T03:19:21Z- handleDepth()- Depth_Keller msg: 1730863161.43 epoch sec; seqNo:4459; depth 49.33 2024-11-06T03:19:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863161.45 epoch sec; seqNo:4459; 4061440.86 north; 594799.47 east; 36.69 lat; -121.94 lon; 49.33 depth; utmZone: 10 2024-11-06T03:19:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863157.41 2024-11-06T03:19:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:21Z- waiting for 1730863161.41, time is 1730863161.45 2024-11-06T03:19:21Z- LcmTrn::updateTrn() >>>> pose time 1730863161.45, meas time 1730863161.06 <<<< 2024-11-06T03:19:21Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863161.45 2024-11-06T03:19:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863161.06 sec, ping # 0; waiting for INS data more recent than 1730863157.41... 2024-11-06T03:19:21Z- TerrainNav::motionUpdate() - currEstimate = (4061441, 594799.47, 49.33) 2024-11-06T03:19:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:19:21Z- TerrainNav:: SubcloudNIS is 0.57, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:19:21Z- TNavPF::Projecting Measurements 2024-11-06T03:19:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:19:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:19:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:19:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:19:21Z- TNavFilter::4 good beams. 2024-11-06T03:19:21Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:19:21Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:19:21Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:19:21Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:19:21Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:19:21Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:19:21Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:19:21Z- TerrainMapOctree: (vehN, vehE ) = (4061440.57, 594799.36). 2024-11-06T03:19:21Z- TerrainMapOctree: Min Distance = 196.86. 2024-11-06T03:19:21Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:19:21Z- Forcing Subcloud Comparison 2024-11-06T03:19:21Z- Weighting particles with subcloud comparison 2024-11-06T03:19:21Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:21Z- meanExpectedMeasDiff: 7.032057 delta_rms_squared: 1.497791 alpha: 0.986823 2024-11-06T03:19:21Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:21Z- meanExpectedMeasDiff: -0.848777 delta_rms_squared: 1.894884 alpha: 0.989556 2024-11-06T03:19:21Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:21Z- meanExpectedMeasDiff: 0.109175 delta_rms_squared: 1.392548 alpha: 0.985841 2024-11-06T03:19:21Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:21Z- meanExpectedMeasDiff: -2.173174 delta_rms_squared: 1.487781 alpha: 0.986735 2024-11-06T03:19:21Z- Subcloud NIS: 0.576834 2024-11-06T03:19:21Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:19:21Z- TNavPF:: sumWeights = 0.003661 2024-11-06T03:19:21Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:19:21Z- TNavPF::First Term of Map Covariance Matrix is 0.16 2024-11-06T03:19:21Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:19:21Z- TerrainNav::Resampling particle filter... 2024-11-06T03:19:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863161.06 sec. 2024-11-06T03:19:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:19:21Z- variances([0], [2], and psi_berg) = 0.351894 0.170751 0.000000 2024-11-06T03:19:21Z- publishEstimatesDecoder() - at 1730863161.45 sec 2024-11-06T03:19:21Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:19:21Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061471.67 594782.53 50.84 2024-11-06T03:19:21Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061470.98 594782.26 51.01 lat/lon: 36.69 -121.94 2024-11-06T03:19:21Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.35 0.17 0.68 0.00 2024-11-06T03:19:21Z- AHRS_M2 msg: 1730863161.86 epoch sec; seqNo:4188 2024-11-06T03:19:21Z- AHRS_M2 msg: 0.16 phi; 0.13 theta; 0.52 psi 2024-11-06T03:19:21Z- handleDepth()- Depth_Keller msg: 1730863161.87 epoch sec; seqNo:4460; depth 49.27 2024-11-06T03:19:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863161.91 epoch sec; seqNo:4460; 4061441.24 north; 594799.47 east; 36.69 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T03:19:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863161.45 2024-11-06T03:19:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:22Z- waiting for 1730863165.45, time is 1730863161.91 2024-11-06T03:19:22Z- AHRS_M2 msg: 1730863162.23 epoch sec; seqNo:4189 2024-11-06T03:19:22Z- AHRS_M2 msg: 0.15 phi; 0.13 theta; 0.52 psi 2024-11-06T03:19:22Z- handleDepth()- Depth_Keller msg: 1730863162.24 epoch sec; seqNo:4461; depth 49.22 2024-11-06T03:19:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863162.25 epoch sec; seqNo:4461; 4061441.62 north; 594799.46 east; 36.69 lat; -121.94 lon; 49.22 depth; utmZone: 10 2024-11-06T03:19:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863161.45 2024-11-06T03:19:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:22Z- waiting for 1730863165.45, time is 1730863162.25 2024-11-06T03:19:22Z- AHRS_M2 msg: 1730863162.64 epoch sec; seqNo:4190 2024-11-06T03:19:22Z- AHRS_M2 msg: 0.14 phi; 0.13 theta; 0.52 psi 2024-11-06T03:19:22Z- handleDepth()- Depth_Keller msg: 1730863162.65 epoch sec; seqNo:4462; depth 49.16 2024-11-06T03:19:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863162.66 epoch sec; seqNo:4462; 4061442.01 north; 594800.68 east; 36.69 lat; -121.94 lon; 49.16 depth; utmZone: 10 2024-11-06T03:19:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863161.45 2024-11-06T03:19:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:22Z- waiting for 1730863165.45, time is 1730863162.66 2024-11-06T03:19:23Z- AHRS_M2 msg: 1730863163.04 epoch sec; seqNo:4191 2024-11-06T03:19:23Z- AHRS_M2 msg: 0.13 phi; 0.13 theta; 0.51 psi 2024-11-06T03:19:23Z- handleDepth()- Depth_Keller msg: 1730863163.04 epoch sec; seqNo:4463; depth 49.10 2024-11-06T03:19:23Z- handleDVL() - DVL msg epochMillisec: 1730863163.06 2024-11-06T03:19:23Z- handleDvl() - RDI_Pathfinder msg: 1730863163.06 sec; seqNo:1479; #beams:4 2024-11-06T03:19:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 40.17 , 46.08 , 39.76 2024-11-06T03:19:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , -0.06 2024-11-06T03:19:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863163.09 epoch sec; seqNo:4463; 4061442.01 north; 594800.68 east; 36.69 lat; -121.94 lon; 49.10 depth; utmZone: 10 2024-11-06T03:19:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863161.45 2024-11-06T03:19:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:23Z- waiting for 1730863165.45, time is 1730863163.09 2024-11-06T03:19:23Z- AHRS_M2 msg: 1730863163.44 epoch sec; seqNo:4192 2024-11-06T03:19:23Z- AHRS_M2 msg: 0.13 phi; 0.12 theta; 0.51 psi 2024-11-06T03:19:23Z- handleDepth()- Depth_Keller msg: 1730863163.45 epoch sec; seqNo:4464; depth 49.04 2024-11-06T03:19:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863163.47 epoch sec; seqNo:4464; 4061442.39 north; 594800.68 east; 36.69 lat; -121.94 lon; 49.04 depth; utmZone: 10 2024-11-06T03:19:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863161.45 2024-11-06T03:19:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:23Z- waiting for 1730863165.45, time is 1730863163.47 2024-11-06T03:19:23Z- AHRS_M2 msg: 1730863163.85 epoch sec; seqNo:4193 2024-11-06T03:19:23Z- AHRS_M2 msg: 0.12 phi; 0.12 theta; 0.51 psi 2024-11-06T03:19:23Z- handleDepth()- Depth_Keller msg: 1730863163.85 epoch sec; seqNo:4465; depth 48.99 2024-11-06T03:19:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863163.87 epoch sec; seqNo:4465; 4061442.77 north; 594800.67 east; 36.69 lat; -121.94 lon; 48.99 depth; utmZone: 10 2024-11-06T03:19:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863161.45 2024-11-06T03:19:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:24Z- waiting for 1730863165.45, time is 1730863163.87 2024-11-06T03:19:24Z- AHRS_M2 msg: 1730863164.26 epoch sec; seqNo:4194 2024-11-06T03:19:24Z- AHRS_M2 msg: 0.11 phi; 0.11 theta; 0.50 psi 2024-11-06T03:19:24Z- handleDepth()- Depth_Keller msg: 1730863164.26 epoch sec; seqNo:4466; depth 48.92 2024-11-06T03:19:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863164.27 epoch sec; seqNo:4466; 4061443.15 north; 594800.67 east; 36.69 lat; -121.94 lon; 48.92 depth; utmZone: 10 2024-11-06T03:19:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863161.45 2024-11-06T03:19:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:24Z- waiting for 1730863165.45, time is 1730863164.27 2024-11-06T03:19:24Z- AHRS_M2 msg: 1730863164.66 epoch sec; seqNo:4195 2024-11-06T03:19:24Z- AHRS_M2 msg: 0.10 phi; 0.11 theta; 0.49 psi 2024-11-06T03:19:24Z- handleDepth()- Depth_Keller msg: 1730863164.66 epoch sec; seqNo:4467; depth 48.87 2024-11-06T03:19:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863164.68 epoch sec; seqNo:4467; 4061443.15 north; 594800.67 east; 36.69 lat; -121.94 lon; 48.87 depth; utmZone: 10 2024-11-06T03:19:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863161.45 2024-11-06T03:19:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:24Z- waiting for 1730863165.45, time is 1730863164.68 2024-11-06T03:19:25Z- AHRS_M2 msg: 1730863165.06 epoch sec; seqNo:4196 2024-11-06T03:19:25Z- AHRS_M2 msg: 0.09 phi; 0.11 theta; 0.49 psi 2024-11-06T03:19:25Z- handleDepth()- Depth_Keller msg: 1730863165.06 epoch sec; seqNo:4468; depth 48.81 2024-11-06T03:19:25Z- handleDVL() - DVL msg epochMillisec: 1730863165.09 2024-11-06T03:19:25Z- handleDvl() - RDI_Pathfinder msg: 1730863165.09 sec; seqNo:1480; #beams:4 2024-11-06T03:19:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.85 , 40.78 , 45.67 , 39.96 2024-11-06T03:19:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.09 , -0.07 2024-11-06T03:19:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863165.10 epoch sec; seqNo:4468; 4061443.53 north; 594800.66 east; 36.69 lat; -121.94 lon; 48.81 depth; utmZone: 10 2024-11-06T03:19:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863161.45 2024-11-06T03:19:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:25Z- waiting for 1730863165.45, time is 1730863165.10 2024-11-06T03:19:25Z- AHRS_M2 msg: 1730863165.46 epoch sec; seqNo:4197 2024-11-06T03:19:25Z- AHRS_M2 msg: 0.09 phi; 0.10 theta; 0.48 psi 2024-11-06T03:19:25Z- handleDepth()- Depth_Keller msg: 1730863165.47 epoch sec; seqNo:4469; depth 48.75 2024-11-06T03:19:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863165.49 epoch sec; seqNo:4469; 4061443.91 north; 594800.66 east; 36.69 lat; -121.94 lon; 48.75 depth; utmZone: 10 2024-11-06T03:19:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863161.45 2024-11-06T03:19:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:25Z- waiting for 1730863165.45, time is 1730863165.49 2024-11-06T03:19:25Z- LcmTrn::updateTrn() >>>> pose time 1730863165.49, meas time 1730863165.09 <<<< 2024-11-06T03:19:25Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863165.49 2024-11-06T03:19:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863165.09 sec, ping # 0; waiting for INS data more recent than 1730863161.45... 2024-11-06T03:19:25Z- TerrainNav::motionUpdate() - currEstimate = (4061444, 594800.66, 48.75) 2024-11-06T03:19:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:19:25Z- TerrainNav:: SubcloudNIS is 0.58, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:19:25Z- TNavPF::Projecting Measurements 2024-11-06T03:19:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:19:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:19:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:19:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:19:25Z- TNavFilter::4 good beams. 2024-11-06T03:19:25Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:19:25Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:19:25Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:19:25Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:19:25Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:19:25Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:19:25Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:19:25Z- TerrainMapOctree: (vehN, vehE ) = (4061443.61, 594800.54). 2024-11-06T03:19:25Z- TerrainMapOctree: Min Distance = 200.10. 2024-11-06T03:19:25Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:19:25Z- Forcing Subcloud Comparison 2024-11-06T03:19:25Z- Weighting particles with subcloud comparison 2024-11-06T03:19:25Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:25Z- meanExpectedMeasDiff: 6.317437 delta_rms_squared: 1.399709 alpha: 0.985913 2024-11-06T03:19:25Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:25Z- meanExpectedMeasDiff: -3.171912 delta_rms_squared: 1.983117 alpha: 0.990016 2024-11-06T03:19:25Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:25Z- meanExpectedMeasDiff: -0.507614 delta_rms_squared: 1.185128 alpha: 0.983404 2024-11-06T03:19:25Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:25Z- meanExpectedMeasDiff: -3.167774 delta_rms_squared: 1.150520 alpha: 0.982914 2024-11-06T03:19:25Z- Subcloud NIS: 0.604359 2024-11-06T03:19:25Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:19:25Z- TNavPF:: sumWeights = 0.001366 2024-11-06T03:19:25Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:19:25Z- TNavPF::First Term of Map Covariance Matrix is 0.06 2024-11-06T03:19:25Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:19:25Z- TerrainNav::Resampling particle filter... 2024-11-06T03:19:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863165.09 sec. 2024-11-06T03:19:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:19:25Z- variances([0], [2], and psi_berg) = 0.357302 0.153751 0.000000 2024-11-06T03:19:25Z- publishEstimatesDecoder() - at 1730863165.49 sec 2024-11-06T03:19:25Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:19:25Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061474.71 594783.69 49.57 2024-11-06T03:19:25Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061474.15 594783.35 50.56 lat/lon: 36.69 -121.94 2024-11-06T03:19:25Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.36 0.15 0.68 0.00 2024-11-06T03:19:25Z- AHRS_M2 msg: 1730863165.87 epoch sec; seqNo:4198 2024-11-06T03:19:25Z- AHRS_M2 msg: 0.08 phi; 0.10 theta; 0.47 psi 2024-11-06T03:19:25Z- handleDepth()- Depth_Keller msg: 1730863165.88 epoch sec; seqNo:4470; depth 48.70 2024-11-06T03:19:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863165.90 epoch sec; seqNo:4470; 4061444.29 north; 594800.66 east; 36.69 lat; -121.94 lon; 48.70 depth; utmZone: 10 2024-11-06T03:19:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863165.49 2024-11-06T03:19:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:26Z- waiting for 1730863169.49, time is 1730863165.90 2024-11-06T03:19:26Z- AHRS_M2 msg: 1730863166.27 epoch sec; seqNo:4199 2024-11-06T03:19:26Z- AHRS_M2 msg: 0.08 phi; 0.09 theta; 0.47 psi 2024-11-06T03:19:26Z- handleDepth()- Depth_Keller msg: 1730863166.28 epoch sec; seqNo:4471; depth 48.66 2024-11-06T03:19:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863166.31 epoch sec; seqNo:4471; 4061444.68 north; 594801.87 east; 36.69 lat; -121.94 lon; 48.66 depth; utmZone: 10 2024-11-06T03:19:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863165.49 2024-11-06T03:19:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:26Z- waiting for 1730863169.49, time is 1730863166.31 2024-11-06T03:19:26Z- AHRS_M2 msg: 1730863166.68 epoch sec; seqNo:4200 2024-11-06T03:19:26Z- AHRS_M2 msg: 0.08 phi; 0.09 theta; 0.46 psi 2024-11-06T03:19:26Z- handleDepth()- Depth_Keller msg: 1730863166.68 epoch sec; seqNo:4472; depth 48.60 2024-11-06T03:19:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863166.73 epoch sec; seqNo:4472; 4061445.06 north; 594801.87 east; 36.69 lat; -121.94 lon; 48.60 depth; utmZone: 10 2024-11-06T03:19:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863165.49 2024-11-06T03:19:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:26Z- waiting for 1730863169.49, time is 1730863166.73 2024-11-06T03:19:27Z- AHRS_M2 msg: 1730863167.08 epoch sec; seqNo:4201 2024-11-06T03:19:27Z- AHRS_M2 msg: 0.07 phi; 0.08 theta; 0.45 psi 2024-11-06T03:19:27Z- handleDepth()- Depth_Keller msg: 1730863167.09 epoch sec; seqNo:4473; depth 48.57 2024-11-06T03:19:27Z- handleDVL() - DVL msg epochMillisec: 1730863167.11 2024-11-06T03:19:27Z- handleDvl() - RDI_Pathfinder msg: 1730863167.11 sec; seqNo:1481; #beams:4 2024-11-06T03:19:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.63 , 41.59 , 44.65 , 40.37 2024-11-06T03:19:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.16 , -0.03 2024-11-06T03:19:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863167.12 epoch sec; seqNo:4473; 4061445.44 north; 594801.86 east; 36.69 lat; -121.94 lon; 48.57 depth; utmZone: 10 2024-11-06T03:19:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863165.49 2024-11-06T03:19:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:27Z- waiting for 1730863169.49, time is 1730863167.12 2024-11-06T03:19:27Z- AHRS_M2 msg: 1730863167.48 epoch sec; seqNo:4202 2024-11-06T03:19:27Z- AHRS_M2 msg: 0.07 phi; 0.08 theta; 0.45 psi 2024-11-06T03:19:27Z- handleDepth()- Depth_Keller msg: 1730863167.49 epoch sec; seqNo:4474; depth 48.51 2024-11-06T03:19:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863167.50 epoch sec; seqNo:4474; 4061445.44 north; 594801.86 east; 36.69 lat; -121.94 lon; 48.51 depth; utmZone: 10 2024-11-06T03:19:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863165.49 2024-11-06T03:19:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:27Z- waiting for 1730863169.49, time is 1730863167.50 2024-11-06T03:19:27Z- AHRS_M2 msg: 1730863167.89 epoch sec; seqNo:4203 2024-11-06T03:19:27Z- AHRS_M2 msg: 0.06 phi; 0.08 theta; 0.44 psi 2024-11-06T03:19:27Z- handleDepth()- Depth_Keller msg: 1730863167.89 epoch sec; seqNo:4475; depth 48.49 2024-11-06T03:19:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863167.90 epoch sec; seqNo:4475; 4061445.82 north; 594801.86 east; 36.69 lat; -121.94 lon; 48.49 depth; utmZone: 10 2024-11-06T03:19:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863165.49 2024-11-06T03:19:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:28Z- waiting for 1730863169.49, time is 1730863167.90 2024-11-06T03:19:28Z- AHRS_M2 msg: 1730863168.33 epoch sec; seqNo:4204 2024-11-06T03:19:28Z- AHRS_M2 msg: 0.06 phi; 0.07 theta; 0.43 psi 2024-11-06T03:19:28Z- handleDepth()- Depth_Keller msg: 1730863168.33 epoch sec; seqNo:4476; depth 48.46 2024-11-06T03:19:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863168.35 epoch sec; seqNo:4476; 4061446.19 north; 594801.86 east; 36.69 lat; -121.94 lon; 48.46 depth; utmZone: 10 2024-11-06T03:19:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863165.49 2024-11-06T03:19:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:28Z- waiting for 1730863169.49, time is 1730863168.35 2024-11-06T03:19:28Z- AHRS_M2 msg: 1730863168.72 epoch sec; seqNo:4205 2024-11-06T03:19:28Z- AHRS_M2 msg: 0.05 phi; 0.07 theta; 0.42 psi 2024-11-06T03:19:28Z- handleDepth()- Depth_Keller msg: 1730863168.74 epoch sec; seqNo:4477; depth 48.44 2024-11-06T03:19:28Z- handleDVL() - DVL msg epochMillisec: 1730863168.78 2024-11-06T03:19:28Z- handleDvl() - RDI_Pathfinder msg: 1730863168.78 sec; seqNo:1482; #beams:4 2024-11-06T03:19:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.43 , 42.00 , 44.24 , 40.57 2024-11-06T03:19:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.17 , 0.01 2024-11-06T03:19:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863168.79 epoch sec; seqNo:4477; 4061446.57 north; 594801.85 east; 36.69 lat; -121.94 lon; 48.44 depth; utmZone: 10 2024-11-06T03:19:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863165.49 2024-11-06T03:19:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:28Z- waiting for 1730863169.49, time is 1730863168.79 2024-11-06T03:19:29Z- AHRS_M2 msg: 1730863169.10 epoch sec; seqNo:4206 2024-11-06T03:19:29Z- AHRS_M2 msg: 0.05 phi; 0.06 theta; 0.41 psi 2024-11-06T03:19:29Z- handleDepth()- Depth_Keller msg: 1730863169.10 epoch sec; seqNo:4478; depth 48.40 2024-11-06T03:19:29Z- handleDVL() - DVL msg epochMillisec: 1730863169.11 2024-11-06T03:19:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:19:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:19:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:19:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:19:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:19:29Z- handleDvl() - RDI_Pathfinder msg: 1730863169.11 sec; seqNo:1483; #beams:0 2024-11-06T03:19:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:19:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.17 , 0.01 2024-11-06T03:19:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863169.12 epoch sec; seqNo:4478; 4061446.95 north; 594801.85 east; 36.69 lat; -121.94 lon; 48.40 depth; utmZone: 10 2024-11-06T03:19:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863165.49 2024-11-06T03:19:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:29Z- waiting for 1730863169.49, time is 1730863169.12 2024-11-06T03:19:29Z- AHRS_M2 msg: 1730863169.50 epoch sec; seqNo:4207 2024-11-06T03:19:29Z- AHRS_M2 msg: 0.05 phi; 0.06 theta; 0.40 psi 2024-11-06T03:19:29Z- handleDepth()- Depth_Keller msg: 1730863169.51 epoch sec; seqNo:4479; depth 48.40 2024-11-06T03:19:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863169.52 epoch sec; seqNo:4479; 4061447.33 north; 594801.84 east; 36.69 lat; -121.94 lon; 48.40 depth; utmZone: 10 2024-11-06T03:19:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863165.49 2024-11-06T03:19:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:29Z- waiting for 1730863169.49, time is 1730863169.52 2024-11-06T03:19:29Z- LcmTrn::updateTrn() >>>> pose time 1730863169.52, meas time 1730863169.11 <<<< 2024-11-06T03:19:29Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863169.52 2024-11-06T03:19:29Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:19:29Z- TerrainNav::motionUpdate() - currEstimate = (4061447, 594801.84, 48.40) 2024-11-06T03:19:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:19:29Z- TerrainNav:: SubcloudNIS is 0.60, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:19:29Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:19:29Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:19:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:19:29Z- variances([0], [2], and psi_berg) = 0.360269 0.155650 0.000000 2024-11-06T03:19:29Z- publishEstimatesDecoder() - at 1730863169.52 sec 2024-11-06T03:19:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:19:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061478.05 594784.85 49.19 2024-11-06T03:19:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061477.57 594784.54 50.20 lat/lon: 36.69 -121.94 2024-11-06T03:19:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.36 0.16 0.93 0.00 2024-11-06T03:19:29Z- AHRS_M2 msg: 1730863169.91 epoch sec; seqNo:4208 2024-11-06T03:19:29Z- AHRS_M2 msg: 0.05 phi; 0.06 theta; 0.39 psi 2024-11-06T03:19:29Z- handleDepth()- Depth_Keller msg: 1730863169.91 epoch sec; seqNo:4480; depth 48.37 2024-11-06T03:19:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863169.93 epoch sec; seqNo:4480; 4061447.71 north; 594801.84 east; 36.69 lat; -121.94 lon; 48.37 depth; utmZone: 10 2024-11-06T03:19:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863169.52 2024-11-06T03:19:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:30Z- waiting for 1730863173.52, time is 1730863169.93 2024-11-06T03:19:30Z- AHRS_M2 msg: 1730863170.31 epoch sec; seqNo:4209 2024-11-06T03:19:30Z- AHRS_M2 msg: 0.05 phi; 0.05 theta; 0.38 psi 2024-11-06T03:19:30Z- handleDepth()- Depth_Keller msg: 1730863170.31 epoch sec; seqNo:4481; depth 48.33 2024-11-06T03:19:30Z- handleDVL() - DVL msg epochMillisec: 1730863170.32 2024-11-06T03:19:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:19:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:19:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:19:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:19:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:19:30Z- handleDvl() - RDI_Pathfinder msg: 1730863170.32 sec; seqNo:1484; #beams:0 2024-11-06T03:19:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:19:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.17 , 0.01 2024-11-06T03:19:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863170.32 epoch sec; seqNo:4481; 4061448.09 north; 594801.83 east; 36.69 lat; -121.94 lon; 48.33 depth; utmZone: 10 2024-11-06T03:19:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863169.52 2024-11-06T03:19:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:30Z- waiting for 1730863173.52, time is 1730863170.32 2024-11-06T03:19:30Z- AHRS_M2 msg: 1730863170.76 epoch sec; seqNo:4210 2024-11-06T03:19:30Z- AHRS_M2 msg: 0.04 phi; 0.05 theta; 0.37 psi 2024-11-06T03:19:30Z- handleDepth()- Depth_Keller msg: 1730863170.77 epoch sec; seqNo:4482; depth 48.32 2024-11-06T03:19:30Z- handleDVL() - DVL msg epochMillisec: 1730863170.79 2024-11-06T03:19:30Z- handleDvl() - RDI_Pathfinder msg: 1730863170.79 sec; seqNo:1485; #beams:4 2024-11-06T03:19:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.22 , 42.41 , 43.63 , 41.19 2024-11-06T03:19:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.14 , 0.05 2024-11-06T03:19:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863170.80 epoch sec; seqNo:4482; 4061448.47 north; 594801.83 east; 36.69 lat; -121.94 lon; 48.32 depth; utmZone: 10 2024-11-06T03:19:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863169.52 2024-11-06T03:19:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:30Z- waiting for 1730863173.52, time is 1730863170.80 2024-11-06T03:19:31Z- AHRS_M2 msg: 1730863171.12 epoch sec; seqNo:4211 2024-11-06T03:19:31Z- AHRS_M2 msg: 0.04 phi; 0.05 theta; 0.36 psi 2024-11-06T03:19:31Z- handleDepth()- Depth_Keller msg: 1730863171.12 epoch sec; seqNo:4483; depth 48.31 2024-11-06T03:19:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863171.14 epoch sec; seqNo:4483; 4061448.47 north; 594801.83 east; 36.69 lat; -121.94 lon; 48.31 depth; utmZone: 10 2024-11-06T03:19:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863169.52 2024-11-06T03:19:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:31Z- waiting for 1730863173.52, time is 1730863171.14 2024-11-06T03:19:31Z- AHRS_M2 msg: 1730863171.53 epoch sec; seqNo:4212 2024-11-06T03:19:31Z- AHRS_M2 msg: 0.04 phi; 0.04 theta; 0.35 psi 2024-11-06T03:19:31Z- handleDepth()- Depth_Keller msg: 1730863171.54 epoch sec; seqNo:4484; depth 48.29 2024-11-06T03:19:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863171.56 epoch sec; seqNo:4484; 4061448.85 north; 594801.83 east; 36.69 lat; -121.94 lon; 48.29 depth; utmZone: 10 2024-11-06T03:19:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863169.52 2024-11-06T03:19:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:31Z- waiting for 1730863173.52, time is 1730863171.56 2024-11-06T03:19:31Z- AHRS_M2 msg: 1730863171.93 epoch sec; seqNo:4213 2024-11-06T03:19:31Z- AHRS_M2 msg: 0.04 phi; 0.04 theta; 0.34 psi 2024-11-06T03:19:31Z- handleDepth()- Depth_Keller msg: 1730863171.94 epoch sec; seqNo:4485; depth 48.28 2024-11-06T03:19:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863171.94 epoch sec; seqNo:4485; 4061449.23 north; 594801.82 east; 36.69 lat; -121.94 lon; 48.28 depth; utmZone: 10 2024-11-06T03:19:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863169.52 2024-11-06T03:19:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:32Z- waiting for 1730863173.52, time is 1730863171.94 2024-11-06T03:19:32Z- AHRS_M2 msg: 1730863172.33 epoch sec; seqNo:4214 2024-11-06T03:19:32Z- AHRS_M2 msg: 0.04 phi; 0.03 theta; 0.33 psi 2024-11-06T03:19:32Z- handleDepth()- Depth_Keller msg: 1730863172.35 epoch sec; seqNo:4486; depth 48.27 2024-11-06T03:19:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863172.35 epoch sec; seqNo:4486; 4061449.62 north; 594803.04 east; 36.69 lat; -121.94 lon; 48.27 depth; utmZone: 10 2024-11-06T03:19:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863169.52 2024-11-06T03:19:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:32Z- waiting for 1730863173.52, time is 1730863172.35 2024-11-06T03:19:32Z- AHRS_M2 msg: 1730863172.76 epoch sec; seqNo:4215 2024-11-06T03:19:32Z- AHRS_M2 msg: 0.04 phi; 0.03 theta; 0.32 psi 2024-11-06T03:19:32Z- handleDepth()- Depth_Keller msg: 1730863172.77 epoch sec; seqNo:4487; depth 48.27 2024-11-06T03:19:32Z- handleDVL() - DVL msg epochMillisec: 1730863172.79 2024-11-06T03:19:32Z- handleDvl() - RDI_Pathfinder msg: 1730863172.79 sec; seqNo:1486; #beams:4 2024-11-06T03:19:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.22 , 42.61 , 43.43 , 41.80 2024-11-06T03:19:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , 0.07 2024-11-06T03:19:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863172.81 epoch sec; seqNo:4487; 4061450.00 north; 594803.03 east; 36.69 lat; -121.94 lon; 48.27 depth; utmZone: 10 2024-11-06T03:19:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863169.52 2024-11-06T03:19:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:32Z- waiting for 1730863173.52, time is 1730863172.81 2024-11-06T03:19:33Z- AHRS_M2 msg: 1730863173.14 epoch sec; seqNo:4216 2024-11-06T03:19:33Z- AHRS_M2 msg: 0.04 phi; 0.02 theta; 0.32 psi 2024-11-06T03:19:33Z- handleDepth()- Depth_Keller msg: 1730863173.14 epoch sec; seqNo:4488; depth 48.26 2024-11-06T03:19:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863173.16 epoch sec; seqNo:4488; 4061450.38 north; 594803.03 east; 36.69 lat; -121.94 lon; 48.26 depth; utmZone: 10 2024-11-06T03:19:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863169.52 2024-11-06T03:19:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:33Z- waiting for 1730863173.52, time is 1730863173.16 2024-11-06T03:19:33Z- AHRS_M2 msg: 1730863173.54 epoch sec; seqNo:4217 2024-11-06T03:19:33Z- AHRS_M2 msg: 0.04 phi; 0.02 theta; 0.31 psi 2024-11-06T03:19:33Z- handleDepth()- Depth_Keller msg: 1730863173.55 epoch sec; seqNo:4489; depth 48.25 2024-11-06T03:19:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863173.56 epoch sec; seqNo:4489; 4061450.75 north; 594803.03 east; 36.69 lat; -121.94 lon; 48.25 depth; utmZone: 10 2024-11-06T03:19:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863169.52 2024-11-06T03:19:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:33Z- waiting for 1730863173.52, time is 1730863173.56 2024-11-06T03:19:33Z- LcmTrn::updateTrn() >>>> pose time 1730863173.56, meas time 1730863172.79 <<<< 2024-11-06T03:19:33Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863173.56 2024-11-06T03:19:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863172.79 sec, ping # 0; waiting for INS data more recent than 1730863169.52... 2024-11-06T03:19:33Z- TerrainNav::motionUpdate() - currEstimate = (4061451, 594803.03, 48.25) 2024-11-06T03:19:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:19:33Z- TerrainNav:: SubcloudNIS is 0.60, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:19:33Z- TNavPF::Projecting Measurements 2024-11-06T03:19:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:19:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:19:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:19:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:19:33Z- TNavFilter::4 good beams. 2024-11-06T03:19:33Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:19:33Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:19:33Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:19:33Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:19:33Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:19:33Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:19:33Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:19:33Z- TerrainMapOctree: (vehN, vehE ) = (4061450.10, 594802.80). 2024-11-06T03:19:33Z- TerrainMapOctree: Min Distance = 206.95. 2024-11-06T03:19:33Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:19:33Z- Forcing Subcloud Comparison 2024-11-06T03:19:33Z- Weighting particles with subcloud comparison 2024-11-06T03:19:33Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:33Z- meanExpectedMeasDiff: 5.673533 delta_rms_squared: 1.605343 alpha: 0.987695 2024-11-06T03:19:33Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:33Z- meanExpectedMeasDiff: -4.075079 delta_rms_squared: 2.419024 alpha: 0.991800 2024-11-06T03:19:33Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:33Z- meanExpectedMeasDiff: 0.412474 delta_rms_squared: 1.416874 alpha: 0.986081 2024-11-06T03:19:33Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:33Z- meanExpectedMeasDiff: -3.316076 delta_rms_squared: 1.688701 alpha: 0.988295 2024-11-06T03:19:33Z- Subcloud NIS: 0.625331 2024-11-06T03:19:33Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:19:33Z- TNavPF:: sumWeights = 0.001166 2024-11-06T03:19:33Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:19:33Z- TNavPF::First Term of Map Covariance Matrix is 0.04 2024-11-06T03:19:33Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:19:33Z- TerrainNav::Resampling particle filter... 2024-11-06T03:19:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863172.79 sec. 2024-11-06T03:19:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:19:33Z- variances([0], [2], and psi_berg) = 0.493239 0.184557 0.000000 2024-11-06T03:19:33Z- publishEstimatesDecoder() - at 1730863173.56 sec 2024-11-06T03:19:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:19:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061481.53 594786.15 50.84 2024-11-06T03:19:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061481.08 594785.66 50.35 lat/lon: 36.69 -121.94 2024-11-06T03:19:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.49 0.18 0.80 0.00 2024-11-06T03:19:33Z- AHRS_M2 msg: 1730863173.95 epoch sec; seqNo:4218 2024-11-06T03:19:33Z- AHRS_M2 msg: 0.04 phi; 0.01 theta; 0.30 psi 2024-11-06T03:19:33Z- handleDepth()- Depth_Keller msg: 1730863173.96 epoch sec; seqNo:4490; depth 48.24 2024-11-06T03:19:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863173.97 epoch sec; seqNo:4490; 4061451.13 north; 594803.02 east; 36.69 lat; -121.94 lon; 48.24 depth; utmZone: 10 2024-11-06T03:19:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863173.56 2024-11-06T03:19:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:34Z- waiting for 1730863177.56, time is 1730863173.97 2024-11-06T03:19:34Z- AHRS_M2 msg: 1730863174.35 epoch sec; seqNo:4219 2024-11-06T03:19:34Z- AHRS_M2 msg: 0.04 phi; 0.01 theta; 0.29 psi 2024-11-06T03:19:34Z- handleDepth()- Depth_Keller msg: 1730863174.36 epoch sec; seqNo:4491; depth 48.22 2024-11-06T03:19:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863174.38 epoch sec; seqNo:4491; 4061451.51 north; 594803.02 east; 36.69 lat; -121.94 lon; 48.22 depth; utmZone: 10 2024-11-06T03:19:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863173.56 2024-11-06T03:19:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:34Z- waiting for 1730863177.56, time is 1730863174.38 2024-11-06T03:19:34Z- AHRS_M2 msg: 1730863174.77 epoch sec; seqNo:4220 2024-11-06T03:19:34Z- AHRS_M2 msg: 0.04 phi; 0.01 theta; 0.29 psi 2024-11-06T03:19:34Z- handleDepth()- Depth_Keller msg: 1730863174.78 epoch sec; seqNo:4492; depth 48.21 2024-11-06T03:19:34Z- handleDVL() - DVL msg epochMillisec: 1730863174.80 2024-11-06T03:19:34Z- handleDvl() - RDI_Pathfinder msg: 1730863174.80 sec; seqNo:1487; #beams:4 2024-11-06T03:19:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.22 , 42.82 , 43.02 , 42.41 2024-11-06T03:19:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.00 , 0.03 2024-11-06T03:19:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863174.82 epoch sec; seqNo:4492; 4061451.89 north; 594803.01 east; 36.69 lat; -121.94 lon; 48.21 depth; utmZone: 10 2024-11-06T03:19:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863173.56 2024-11-06T03:19:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:34Z- waiting for 1730863177.56, time is 1730863174.82 2024-11-06T03:19:35Z- AHRS_M2 msg: 1730863175.16 epoch sec; seqNo:4221 2024-11-06T03:19:35Z- AHRS_M2 msg: 0.04 phi; 0.00 theta; 0.28 psi 2024-11-06T03:19:35Z- handleDepth()- Depth_Keller msg: 1730863175.16 epoch sec; seqNo:4493; depth 48.20 2024-11-06T03:19:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863175.18 epoch sec; seqNo:4493; 4061452.27 north; 594803.01 east; 36.69 lat; -121.94 lon; 48.20 depth; utmZone: 10 2024-11-06T03:19:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863173.56 2024-11-06T03:19:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:35Z- waiting for 1730863177.56, time is 1730863175.18 2024-11-06T03:19:35Z- AHRS_M2 msg: 1730863175.57 epoch sec; seqNo:4222 2024-11-06T03:19:35Z- AHRS_M2 msg: 0.04 phi; -0.00 theta; 0.27 psi 2024-11-06T03:19:35Z- handleDepth()- Depth_Keller msg: 1730863175.57 epoch sec; seqNo:4494; depth 48.20 2024-11-06T03:19:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863175.58 epoch sec; seqNo:4494; 4061452.27 north; 594803.01 east; 36.69 lat; -121.94 lon; 48.20 depth; utmZone: 10 2024-11-06T03:19:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863173.56 2024-11-06T03:19:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:35Z- waiting for 1730863177.56, time is 1730863175.58 2024-11-06T03:19:35Z- AHRS_M2 msg: 1730863175.97 epoch sec; seqNo:4223 2024-11-06T03:19:35Z- AHRS_M2 msg: 0.04 phi; -0.01 theta; 0.26 psi 2024-11-06T03:19:35Z- handleDepth()- Depth_Keller msg: 1730863175.97 epoch sec; seqNo:4495; depth 48.18 2024-11-06T03:19:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863175.99 epoch sec; seqNo:4495; 4061452.65 north; 594803.00 east; 36.69 lat; -121.94 lon; 48.18 depth; utmZone: 10 2024-11-06T03:19:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863173.56 2024-11-06T03:19:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:36Z- waiting for 1730863177.56, time is 1730863175.99 2024-11-06T03:19:36Z- AHRS_M2 msg: 1730863176.37 epoch sec; seqNo:4224 2024-11-06T03:19:36Z- AHRS_M2 msg: 0.04 phi; -0.01 theta; 0.26 psi 2024-11-06T03:19:36Z- handleDepth()- Depth_Keller msg: 1730863176.38 epoch sec; seqNo:4496; depth 48.17 2024-11-06T03:19:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863176.39 epoch sec; seqNo:4496; 4061453.03 north; 594803.00 east; 36.69 lat; -121.94 lon; 48.17 depth; utmZone: 10 2024-11-06T03:19:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863173.56 2024-11-06T03:19:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:36Z- waiting for 1730863177.56, time is 1730863176.39 2024-11-06T03:19:36Z- AHRS_M2 msg: 1730863176.80 epoch sec; seqNo:4225 2024-11-06T03:19:36Z- AHRS_M2 msg: 0.04 phi; -0.01 theta; 0.25 psi 2024-11-06T03:19:36Z- handleDepth()- Depth_Keller msg: 1730863176.86 epoch sec; seqNo:4497; depth 48.16 2024-11-06T03:19:36Z- handleDVL() - DVL msg epochMillisec: 1730863176.89 2024-11-06T03:19:36Z- handleDvl() - RDI_Pathfinder msg: 1730863176.89 sec; seqNo:1488; #beams:4 2024-11-06T03:19:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.22 , 42.82 , 42.41 , 43.02 2024-11-06T03:19:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , -0.02 2024-11-06T03:19:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863176.90 epoch sec; seqNo:4497; 4061453.41 north; 594803.00 east; 36.69 lat; -121.94 lon; 48.16 depth; utmZone: 10 2024-11-06T03:19:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863173.56 2024-11-06T03:19:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:36Z- waiting for 1730863177.56, time is 1730863176.90 2024-11-06T03:19:37Z- AHRS_M2 msg: 1730863177.19 epoch sec; seqNo:4226 2024-11-06T03:19:37Z- AHRS_M2 msg: 0.04 phi; -0.02 theta; 0.24 psi 2024-11-06T03:19:37Z- handleDepth()- Depth_Keller msg: 1730863177.20 epoch sec; seqNo:4498; depth 48.15 2024-11-06T03:19:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863177.22 epoch sec; seqNo:4498; 4061453.78 north; 594802.99 east; 36.69 lat; -121.94 lon; 48.15 depth; utmZone: 10 2024-11-06T03:19:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863173.56 2024-11-06T03:19:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:37Z- waiting for 1730863177.56, time is 1730863177.22 2024-11-06T03:19:37Z- handleDepth()- Depth_Keller msg: 1730863177.61 epoch sec; seqNo:4499; depth 48.14 2024-11-06T03:19:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863177.63 epoch sec; seqNo:4499; 4061454.16 north; 594802.99 east; 36.69 lat; -121.94 lon; 48.14 depth; utmZone: 10 2024-11-06T03:19:37Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:19:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863173.56 2024-11-06T03:19:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:37Z- waiting for 1730863177.56, time is 1730863177.63 2024-11-06T03:19:37Z- LcmTrn::updateTrn() >>>> pose time 1730863177.63, meas time 1730863176.89 <<<< 2024-11-06T03:19:37Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863177.63 2024-11-06T03:19:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863176.89 sec, ping # 0; waiting for INS data more recent than 1730863173.56... 2024-11-06T03:19:37Z- TerrainNav::motionUpdate() - currEstimate = (4061454, 594802.99, 48.14) 2024-11-06T03:19:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:19:37Z- TerrainNav:: SubcloudNIS is 0.63, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:19:37Z- TNavPF::Projecting Measurements 2024-11-06T03:19:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:19:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:19:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:19:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:19:37Z- TNavFilter::4 good beams. 2024-11-06T03:19:37Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:19:37Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:19:37Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:19:37Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:19:37Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:19:37Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:19:37Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:19:37Z- TerrainMapOctree: (vehN, vehE ) = (4061453.54, 594802.99). 2024-11-06T03:19:37Z- TerrainMapOctree: Min Distance = 210.33. 2024-11-06T03:19:37Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:19:37Z- Forcing Subcloud Comparison 2024-11-06T03:19:37Z- Weighting particles with subcloud comparison 2024-11-06T03:19:37Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:37Z- meanExpectedMeasDiff: 5.898877 delta_rms_squared: 1.467674 alpha: 0.986556 2024-11-06T03:19:37Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:37Z- meanExpectedMeasDiff: -3.028037 delta_rms_squared: 1.938711 alpha: 0.989789 2024-11-06T03:19:37Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:37Z- meanExpectedMeasDiff: 1.658046 delta_rms_squared: 1.035068 alpha: 0.981044 2024-11-06T03:19:37Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:37Z- meanExpectedMeasDiff: -2.493158 delta_rms_squared: 2.309124 alpha: 0.991413 2024-11-06T03:19:37Z- Subcloud NIS: 0.640376 2024-11-06T03:19:37Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:19:37Z- TNavPF:: sumWeights = 0.002614 2024-11-06T03:19:37Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:19:37Z- TNavPF::First Term of Map Covariance Matrix is 0.09 2024-11-06T03:19:37Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:19:37Z- TerrainNav::Resampling particle filter... 2024-11-06T03:19:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863176.89 sec. 2024-11-06T03:19:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:19:37Z- variances([0], [2], and psi_berg) = 0.566766 0.185902 0.000000 2024-11-06T03:19:37Z- publishEstimatesDecoder() - at 1730863177.63 sec 2024-11-06T03:19:37Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:19:37Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061484.92 594786.12 51.24 2024-11-06T03:19:37Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061484.65 594785.56 50.12 lat/lon: 36.69 -121.94 2024-11-06T03:19:37Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.19 0.72 0.00 2024-11-06T03:19:38Z- AHRS_M2 msg: 1730863178.01 epoch sec; seqNo:4227 2024-11-06T03:19:38Z- AHRS_M2 msg: 0.04 phi; -0.03 theta; 0.21 psi 2024-11-06T03:19:38Z- handleDepth()- Depth_Keller msg: 1730863178.02 epoch sec; seqNo:4500; depth 48.13 2024-11-06T03:19:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863178.05 epoch sec; seqNo:4500; 4061454.56 north; 594804.20 east; 36.69 lat; -121.94 lon; 48.13 depth; utmZone: 10 2024-11-06T03:19:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863177.63 2024-11-06T03:19:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:38Z- waiting for 1730863181.63, time is 1730863178.05 2024-11-06T03:19:38Z- AHRS_M2 msg: 1730863178.40 epoch sec; seqNo:4228 2024-11-06T03:19:38Z- AHRS_M2 msg: 0.04 phi; -0.03 theta; 0.21 psi 2024-11-06T03:19:38Z- handleDepth()- Depth_Keller msg: 1730863178.40 epoch sec; seqNo:4501; depth 48.12 2024-11-06T03:19:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863178.41 epoch sec; seqNo:4501; 4061454.93 north; 594804.20 east; 36.69 lat; -121.94 lon; 48.12 depth; utmZone: 10 2024-11-06T03:19:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863177.63 2024-11-06T03:19:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:38Z- waiting for 1730863181.63, time is 1730863178.41 2024-11-06T03:19:38Z- AHRS_M2 msg: 1730863178.80 epoch sec; seqNo:4229 2024-11-06T03:19:38Z- AHRS_M2 msg: 0.05 phi; -0.03 theta; 0.20 psi 2024-11-06T03:19:38Z- handleDepth()- Depth_Keller msg: 1730863178.81 epoch sec; seqNo:4502; depth 48.13 2024-11-06T03:19:38Z- handleDVL() - DVL msg epochMillisec: 1730863178.84 2024-11-06T03:19:38Z- handleDvl() - RDI_Pathfinder msg: 1730863178.84 sec; seqNo:1489; #beams:4 2024-11-06T03:19:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.02 , 42.82 , 41.59 , 43.43 2024-11-06T03:19:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.07 , -0.06 2024-11-06T03:19:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863178.85 epoch sec; seqNo:4502; 4061455.31 north; 594804.20 east; 36.69 lat; -121.94 lon; 48.13 depth; utmZone: 10 2024-11-06T03:19:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863177.63 2024-11-06T03:19:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:38Z- waiting for 1730863181.63, time is 1730863178.85 2024-11-06T03:19:39Z- AHRS_M2 msg: 1730863179.20 epoch sec; seqNo:4230 2024-11-06T03:19:39Z- AHRS_M2 msg: 0.05 phi; -0.04 theta; 0.19 psi 2024-11-06T03:19:39Z- handleDepth()- Depth_Keller msg: 1730863179.20 epoch sec; seqNo:4503; depth 48.11 2024-11-06T03:19:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863179.22 epoch sec; seqNo:4503; 4061455.69 north; 594804.19 east; 36.69 lat; -121.94 lon; 48.11 depth; utmZone: 10 2024-11-06T03:19:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863177.63 2024-11-06T03:19:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:39Z- waiting for 1730863181.63, time is 1730863179.22 2024-11-06T03:19:39Z- AHRS_M2 msg: 1730863179.61 epoch sec; seqNo:4231 2024-11-06T03:19:39Z- AHRS_M2 msg: 0.05 phi; -0.04 theta; 0.18 psi 2024-11-06T03:19:39Z- handleDepth()- Depth_Keller msg: 1730863179.62 epoch sec; seqNo:4504; depth 48.11 2024-11-06T03:19:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863179.63 epoch sec; seqNo:4504; 4061456.07 north; 594804.19 east; 36.69 lat; -121.94 lon; 48.11 depth; utmZone: 10 2024-11-06T03:19:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863177.63 2024-11-06T03:19:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:39Z- waiting for 1730863181.63, time is 1730863179.63 2024-11-06T03:19:40Z- AHRS_M2 msg: 1730863180.01 epoch sec; seqNo:4232 2024-11-06T03:19:40Z- AHRS_M2 msg: 0.05 phi; -0.04 theta; 0.17 psi 2024-11-06T03:19:40Z- handleDepth()- Depth_Keller msg: 1730863180.02 epoch sec; seqNo:4505; depth 48.11 2024-11-06T03:19:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863180.03 epoch sec; seqNo:4505; 4061456.07 north; 594804.19 east; 36.69 lat; -121.94 lon; 48.11 depth; utmZone: 10 2024-11-06T03:19:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863177.63 2024-11-06T03:19:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:40Z- waiting for 1730863181.63, time is 1730863180.03 2024-11-06T03:19:40Z- AHRS_M2 msg: 1730863180.41 epoch sec; seqNo:4233 2024-11-06T03:19:40Z- AHRS_M2 msg: 0.05 phi; -0.05 theta; 0.16 psi 2024-11-06T03:19:40Z- handleDepth()- Depth_Keller msg: 1730863180.42 epoch sec; seqNo:4506; depth 48.11 2024-11-06T03:19:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863180.44 epoch sec; seqNo:4506; 4061456.45 north; 594804.18 east; 36.69 lat; -121.94 lon; 48.11 depth; utmZone: 10 2024-11-06T03:19:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863177.63 2024-11-06T03:19:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:40Z- waiting for 1730863181.63, time is 1730863180.44 2024-11-06T03:19:40Z- AHRS_M2 msg: 1730863180.82 epoch sec; seqNo:4234 2024-11-06T03:19:40Z- AHRS_M2 msg: 0.05 phi; -0.05 theta; 0.15 psi 2024-11-06T03:19:40Z- handleDepth()- Depth_Keller msg: 1730863180.82 epoch sec; seqNo:4507; depth 48.12 2024-11-06T03:19:40Z- handleDVL() - DVL msg epochMillisec: 1730863180.84 2024-11-06T03:19:40Z- handleDvl() - RDI_Pathfinder msg: 1730863180.84 sec; seqNo:1490; #beams:4 2024-11-06T03:19:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.02 , 42.61 , 41.19 , 43.63 2024-11-06T03:19:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.14 , -0.07 2024-11-06T03:19:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863180.86 epoch sec; seqNo:4507; 4061456.83 north; 594804.18 east; 36.69 lat; -121.94 lon; 48.12 depth; utmZone: 10 2024-11-06T03:19:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863177.63 2024-11-06T03:19:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:40Z- waiting for 1730863181.63, time is 1730863180.86 2024-11-06T03:19:41Z- AHRS_M2 msg: 1730863181.22 epoch sec; seqNo:4235 2024-11-06T03:19:41Z- AHRS_M2 msg: 0.05 phi; -0.05 theta; 0.14 psi 2024-11-06T03:19:41Z- handleDepth()- Depth_Keller msg: 1730863181.22 epoch sec; seqNo:4508; depth 48.11 2024-11-06T03:19:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863181.24 epoch sec; seqNo:4508; 4061457.21 north; 594804.17 east; 36.69 lat; -121.94 lon; 48.11 depth; utmZone: 10 2024-11-06T03:19:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863177.63 2024-11-06T03:19:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:41Z- waiting for 1730863181.63, time is 1730863181.24 2024-11-06T03:19:41Z- AHRS_M2 msg: 1730863181.63 epoch sec; seqNo:4236 2024-11-06T03:19:41Z- AHRS_M2 msg: 0.05 phi; -0.05 theta; 0.13 psi 2024-11-06T03:19:41Z- handleDepth()- Depth_Keller msg: 1730863181.64 epoch sec; seqNo:4509; depth 48.12 2024-11-06T03:19:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863181.66 epoch sec; seqNo:4509; 4061457.59 north; 594804.17 east; 36.69 lat; -121.94 lon; 48.12 depth; utmZone: 10 2024-11-06T03:19:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863177.63 2024-11-06T03:19:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:41Z- waiting for 1730863181.63, time is 1730863181.66 2024-11-06T03:19:41Z- LcmTrn::updateTrn() >>>> pose time 1730863181.66, meas time 1730863180.84 <<<< 2024-11-06T03:19:41Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863181.66 2024-11-06T03:19:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863180.84 sec, ping # 0; waiting for INS data more recent than 1730863177.63... 2024-11-06T03:19:41Z- TerrainNav::motionUpdate() - currEstimate = (4061458, 594804.17, 48.12) 2024-11-06T03:19:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:19:41Z- TerrainNav:: SubcloudNIS is 0.64, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:19:41Z- TNavPF::Projecting Measurements 2024-11-06T03:19:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:19:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:19:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:19:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:19:41Z- TNavFilter::4 good beams. 2024-11-06T03:19:41Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:19:41Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:19:41Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:19:41Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:19:41Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:19:41Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:19:41Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:19:41Z- TerrainMapOctree: (vehN, vehE ) = (4061456.89, 594803.93). 2024-11-06T03:19:41Z- TerrainMapOctree: Min Distance = 213.81. 2024-11-06T03:19:41Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:19:41Z- Forcing Subcloud Comparison 2024-11-06T03:19:41Z- Weighting particles with subcloud comparison 2024-11-06T03:19:41Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:41Z- meanExpectedMeasDiff: 5.385085 delta_rms_squared: 1.355592 alpha: 0.985461 2024-11-06T03:19:41Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:41Z- meanExpectedMeasDiff: -3.177603 delta_rms_squared: 1.250996 alpha: 0.984264 2024-11-06T03:19:41Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:41Z- meanExpectedMeasDiff: 1.919805 delta_rms_squared: 0.996211 alpha: 0.980319 2024-11-06T03:19:41Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:41Z- meanExpectedMeasDiff: -2.614003 delta_rms_squared: 2.204713 alpha: 0.991010 2024-11-06T03:19:41Z- Subcloud NIS: 0.664878 2024-11-06T03:19:41Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:19:41Z- TNavPF:: sumWeights = 0.003735 2024-11-06T03:19:41Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:19:41Z- TNavPF::First Term of Map Covariance Matrix is 0.10 2024-11-06T03:19:41Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:19:41Z- TerrainNav::Resampling particle filter... 2024-11-06T03:19:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863180.84 sec. 2024-11-06T03:19:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:19:41Z- variances([0], [2], and psi_berg) = 0.595208 0.259724 0.000000 2024-11-06T03:19:41Z- publishEstimatesDecoder() - at 1730863181.66 sec 2024-11-06T03:19:41Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:19:41Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061488.36 594787.28 49.91 2024-11-06T03:19:41Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061488.21 594786.75 50.01 lat/lon: 36.69 -121.94 2024-11-06T03:19:41Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.60 0.26 0.72 0.00 2024-11-06T03:19:42Z- AHRS_M2 msg: 1730863182.04 epoch sec; seqNo:4237 2024-11-06T03:19:42Z- AHRS_M2 msg: 0.05 phi; -0.06 theta; 0.12 psi 2024-11-06T03:19:42Z- handleDepth()- Depth_Keller msg: 1730863182.06 epoch sec; seqNo:4510; depth 48.13 2024-11-06T03:19:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863182.10 epoch sec; seqNo:4510; 4061457.97 north; 594804.17 east; 36.69 lat; -121.94 lon; 48.13 depth; utmZone: 10 2024-11-06T03:19:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863181.66 2024-11-06T03:19:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:42Z- waiting for 1730863185.66, time is 1730863182.10 2024-11-06T03:19:42Z- AHRS_M2 msg: 1730863182.43 epoch sec; seqNo:4238 2024-11-06T03:19:42Z- AHRS_M2 msg: 0.05 phi; -0.06 theta; 0.12 psi 2024-11-06T03:19:42Z- handleDepth()- Depth_Keller msg: 1730863182.43 epoch sec; seqNo:4511; depth 48.15 2024-11-06T03:19:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863182.44 epoch sec; seqNo:4511; 4061458.34 north; 594804.16 east; 36.69 lat; -121.94 lon; 48.15 depth; utmZone: 10 2024-11-06T03:19:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863181.66 2024-11-06T03:19:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:42Z- waiting for 1730863185.66, time is 1730863182.44 2024-11-06T03:19:42Z- handleDepth()- Depth_Keller msg: 1730863182.86 epoch sec; seqNo:4512; depth 48.17 2024-11-06T03:19:42Z- handleDVL() - DVL msg epochMillisec: 1730863182.88 2024-11-06T03:19:42Z- handleDvl() - RDI_Pathfinder msg: 1730863182.88 sec; seqNo:1491; #beams:4 2024-11-06T03:19:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.82 , 42.41 , 40.57 , 44.04 2024-11-06T03:19:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.20 , -0.04 2024-11-06T03:19:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863182.90 epoch sec; seqNo:4512; 4061458.72 north; 594804.16 east; 36.69 lat; -121.94 lon; 48.17 depth; utmZone: 10 2024-11-06T03:19:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863181.66 2024-11-06T03:19:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:43Z- waiting for 1730863185.66, time is 1730863182.90 2024-11-06T03:19:43Z- AHRS_M2 msg: 1730863183.24 epoch sec; seqNo:4239 2024-11-06T03:19:43Z- AHRS_M2 msg: 0.05 phi; -0.06 theta; 0.10 psi 2024-11-06T03:19:43Z- handleDepth()- Depth_Keller msg: 1730863183.24 epoch sec; seqNo:4513; depth 48.20 2024-11-06T03:19:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863183.26 epoch sec; seqNo:4513; 4061459.10 north; 594804.15 east; 36.69 lat; -121.94 lon; 48.20 depth; utmZone: 10 2024-11-06T03:19:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863181.66 2024-11-06T03:19:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:43Z- waiting for 1730863185.66, time is 1730863183.26 2024-11-06T03:19:43Z- handleDepth()- Depth_Keller msg: 1730863183.67 epoch sec; seqNo:4514; depth 48.21 2024-11-06T03:19:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863183.68 epoch sec; seqNo:4514; 4061459.48 north; 594804.15 east; 36.69 lat; -121.94 lon; 48.21 depth; utmZone: 10 2024-11-06T03:19:43Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:19:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863181.66 2024-11-06T03:19:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:43Z- waiting for 1730863185.66, time is 1730863183.68 2024-11-06T03:19:44Z- AHRS_M2 msg: 1730863184.05 epoch sec; seqNo:4240 2024-11-06T03:19:44Z- AHRS_M2 msg: 0.05 phi; -0.07 theta; 0.08 psi 2024-11-06T03:19:44Z- handleDepth()- Depth_Keller msg: 1730863184.06 epoch sec; seqNo:4515; depth 48.24 2024-11-06T03:19:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863184.07 epoch sec; seqNo:4515; 4061459.86 north; 594804.15 east; 36.69 lat; -121.94 lon; 48.24 depth; utmZone: 10 2024-11-06T03:19:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863181.66 2024-11-06T03:19:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:44Z- waiting for 1730863185.66, time is 1730863184.07 2024-11-06T03:19:44Z- AHRS_M2 msg: 1730863184.45 epoch sec; seqNo:4241 2024-11-06T03:19:44Z- AHRS_M2 msg: 0.05 phi; -0.07 theta; 0.07 psi 2024-11-06T03:19:44Z- handleDepth()- Depth_Keller msg: 1730863184.46 epoch sec; seqNo:4516; depth 48.26 2024-11-06T03:19:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863184.47 epoch sec; seqNo:4516; 4061460.24 north; 594804.14 east; 36.69 lat; -121.94 lon; 48.26 depth; utmZone: 10 2024-11-06T03:19:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863181.66 2024-11-06T03:19:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:44Z- waiting for 1730863185.66, time is 1730863184.47 2024-11-06T03:19:44Z- AHRS_M2 msg: 1730863184.86 epoch sec; seqNo:4242 2024-11-06T03:19:44Z- AHRS_M2 msg: 0.05 phi; -0.07 theta; 0.06 psi 2024-11-06T03:19:44Z- handleDepth()- Depth_Keller msg: 1730863184.87 epoch sec; seqNo:4517; depth 48.29 2024-11-06T03:19:44Z- handleDVL() - DVL msg epochMillisec: 1730863184.89 2024-11-06T03:19:44Z- handleDvl() - RDI_Pathfinder msg: 1730863184.89 sec; seqNo:1492; #beams:4 2024-11-06T03:19:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.02 , 42.21 , 40.37 , 44.24 2024-11-06T03:19:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.20 , 0.04 2024-11-06T03:19:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863184.91 epoch sec; seqNo:4517; 4061460.60 north; 594802.92 east; 36.69 lat; -121.94 lon; 48.29 depth; utmZone: 10 2024-11-06T03:19:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863181.66 2024-11-06T03:19:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:45Z- waiting for 1730863185.66, time is 1730863184.91 2024-11-06T03:19:45Z- AHRS_M2 msg: 1730863185.26 epoch sec; seqNo:4243 2024-11-06T03:19:45Z- AHRS_M2 msg: 0.05 phi; -0.07 theta; 0.05 psi 2024-11-06T03:19:45Z- handleDepth()- Depth_Keller msg: 1730863185.26 epoch sec; seqNo:4518; depth 48.33 2024-11-06T03:19:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863185.28 epoch sec; seqNo:4518; 4061460.98 north; 594802.91 east; 36.69 lat; -121.94 lon; 48.33 depth; utmZone: 10 2024-11-06T03:19:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863181.66 2024-11-06T03:19:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:45Z- waiting for 1730863185.66, time is 1730863185.28 2024-11-06T03:19:45Z- AHRS_M2 msg: 1730863185.67 epoch sec; seqNo:4244 2024-11-06T03:19:45Z- AHRS_M2 msg: 0.05 phi; -0.07 theta; 0.04 psi 2024-11-06T03:19:45Z- handleDepth()- Depth_Keller msg: 1730863185.67 epoch sec; seqNo:4519; depth 48.36 2024-11-06T03:19:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863185.68 epoch sec; seqNo:4519; 4061461.36 north; 594802.91 east; 36.69 lat; -121.94 lon; 48.36 depth; utmZone: 10 2024-11-06T03:19:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863181.66 2024-11-06T03:19:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:45Z- waiting for 1730863185.66, time is 1730863185.68 2024-11-06T03:19:45Z- LcmTrn::updateTrn() >>>> pose time 1730863185.68, meas time 1730863184.89 <<<< 2024-11-06T03:19:45Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863185.68 2024-11-06T03:19:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863184.89 sec, ping # 0; waiting for INS data more recent than 1730863181.66... 2024-11-06T03:19:45Z- TerrainNav::motionUpdate() - currEstimate = (4061461, 594802.91, 48.36) 2024-11-06T03:19:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:19:45Z- TerrainNav:: SubcloudNIS is 0.66, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:19:45Z- TNavPF::Projecting Measurements 2024-11-06T03:19:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:19:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:19:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:19:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:19:45Z- TNavFilter::4 good beams. 2024-11-06T03:19:45Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:19:45Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:19:45Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:19:45Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:19:45Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:19:45Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:19:45Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:19:45Z- TerrainMapOctree: (vehN, vehE ) = (4061460.62, 594803.15). 2024-11-06T03:19:45Z- TerrainMapOctree: Min Distance = 217.23. 2024-11-06T03:19:45Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:19:45Z- Forcing Subcloud Comparison 2024-11-06T03:19:45Z- Weighting particles with subcloud comparison 2024-11-06T03:19:45Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:45Z- meanExpectedMeasDiff: 5.328037 delta_rms_squared: 1.358113 alpha: 0.985487 2024-11-06T03:19:45Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:45Z- meanExpectedMeasDiff: -2.376373 delta_rms_squared: 1.345428 alpha: 0.985353 2024-11-06T03:19:45Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:45Z- meanExpectedMeasDiff: 1.296513 delta_rms_squared: 1.035125 alpha: 0.981045 2024-11-06T03:19:45Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:45Z- meanExpectedMeasDiff: -1.491041 delta_rms_squared: 2.311157 alpha: 0.991421 2024-11-06T03:19:45Z- Subcloud NIS: 0.685247 2024-11-06T03:19:45Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:19:45Z- TNavPF:: sumWeights = 0.013283 2024-11-06T03:19:45Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:19:45Z- TNavPF::First Term of Map Covariance Matrix is 0.33 2024-11-06T03:19:45Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:19:45Z- TerrainNav::Resampling particle filter... 2024-11-06T03:19:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863184.89 sec. 2024-11-06T03:19:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:19:46Z- variances([0], [2], and psi_berg) = 0.608964 0.361856 0.000000 2024-11-06T03:19:46Z- publishEstimatesDecoder() - at 1730863185.68 sec 2024-11-06T03:19:46Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:19:46Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061492.23 594786.04 50.77 2024-11-06T03:19:46Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061492.17 594785.52 50.00 lat/lon: 36.69 -121.94 2024-11-06T03:19:46Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.61 0.36 0.71 0.00 2024-11-06T03:19:46Z- AHRS_M2 msg: 1730863186.07 epoch sec; seqNo:4245 2024-11-06T03:19:46Z- AHRS_M2 msg: 0.05 phi; -0.08 theta; 0.03 psi 2024-11-06T03:19:46Z- handleDepth()- Depth_Keller msg: 1730863186.09 epoch sec; seqNo:4520; depth 48.38 2024-11-06T03:19:46Z- handleDVL() - DVL msg epochMillisec: 1730863186.09 2024-11-06T03:19:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:19:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:19:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:19:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:19:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:19:46Z- handleDvl() - RDI_Pathfinder msg: 1730863186.09 sec; seqNo:1493; #beams:0 2024-11-06T03:19:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:19:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.20 , 0.04 2024-11-06T03:19:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863186.10 epoch sec; seqNo:4520; 4061461.74 north; 594802.90 east; 36.69 lat; -121.94 lon; 48.38 depth; utmZone: 10 2024-11-06T03:19:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863185.68 2024-11-06T03:19:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:46Z- waiting for 1730863189.68, time is 1730863186.10 2024-11-06T03:19:46Z- handleDepth()- Depth_Keller msg: 1730863186.49 epoch sec; seqNo:4521; depth 48.42 2024-11-06T03:19:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863186.49 epoch sec; seqNo:4521; 4061462.12 north; 594802.90 east; 36.69 lat; -121.94 lon; 48.42 depth; utmZone: 10 2024-11-06T03:19:46Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:19:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863185.68 2024-11-06T03:19:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:46Z- waiting for 1730863189.68, time is 1730863186.49 2024-11-06T03:19:46Z- AHRS_M2 msg: 1730863186.88 epoch sec; seqNo:4246 2024-11-06T03:19:46Z- AHRS_M2 msg: 0.06 phi; -0.08 theta; 0.02 psi 2024-11-06T03:19:46Z- handleDepth()- Depth_Keller msg: 1730863186.88 epoch sec; seqNo:4522; depth 48.45 2024-11-06T03:19:46Z- handleDVL() - DVL msg epochMillisec: 1730863186.90 2024-11-06T03:19:46Z- handleDvl() - RDI_Pathfinder msg: 1730863186.90 sec; seqNo:1494; #beams:4 2024-11-06T03:19:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.22 , 42.00 , 40.17 , 44.45 2024-11-06T03:19:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.10 , 0.11 2024-11-06T03:19:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863186.93 epoch sec; seqNo:4522; 4061462.50 north; 594802.90 east; 36.69 lat; -121.94 lon; 48.45 depth; utmZone: 10 2024-11-06T03:19:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863185.68 2024-11-06T03:19:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:47Z- waiting for 1730863189.68, time is 1730863186.93 2024-11-06T03:19:47Z- AHRS_M2 msg: 1730863187.28 epoch sec; seqNo:4247 2024-11-06T03:19:47Z- AHRS_M2 msg: 0.07 phi; -0.08 theta; 0.02 psi 2024-11-06T03:19:47Z- handleDepth()- Depth_Keller msg: 1730863187.28 epoch sec; seqNo:4523; depth 48.49 2024-11-06T03:19:47Z- handleDVL() - DVL msg epochMillisec: 1730863187.29 2024-11-06T03:19:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:19:47Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:19:47Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:19:47Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:19:47Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:19:47Z- handleDvl() - RDI_Pathfinder msg: 1730863187.29 sec; seqNo:1495; #beams:0 2024-11-06T03:19:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:19:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.10 , 0.11 2024-11-06T03:19:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863187.31 epoch sec; seqNo:4523; 4061462.88 north; 594802.89 east; 36.69 lat; -121.94 lon; 48.49 depth; utmZone: 10 2024-11-06T03:19:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863185.68 2024-11-06T03:19:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:47Z- waiting for 1730863189.68, time is 1730863187.31 2024-11-06T03:19:47Z- AHRS_M2 msg: 1730863187.68 epoch sec; seqNo:4248 2024-11-06T03:19:47Z- AHRS_M2 msg: 0.07 phi; -0.08 theta; 0.01 psi 2024-11-06T03:19:47Z- handleDepth()- Depth_Keller msg: 1730863187.69 epoch sec; seqNo:4524; depth 48.52 2024-11-06T03:19:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863187.71 epoch sec; seqNo:4524; 4061462.88 north; 594802.89 east; 36.69 lat; -121.94 lon; 48.52 depth; utmZone: 10 2024-11-06T03:19:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863185.68 2024-11-06T03:19:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:47Z- waiting for 1730863189.68, time is 1730863187.71 2024-11-06T03:19:48Z- AHRS_M2 msg: 1730863188.09 epoch sec; seqNo:4249 2024-11-06T03:19:48Z- AHRS_M2 msg: 0.07 phi; -0.09 theta; 0.01 psi 2024-11-06T03:19:48Z- handleDepth()- Depth_Keller msg: 1730863188.10 epoch sec; seqNo:4525; depth 48.55 2024-11-06T03:19:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863188.13 epoch sec; seqNo:4525; 4061463.26 north; 594802.89 east; 36.69 lat; -121.94 lon; 48.55 depth; utmZone: 10 2024-11-06T03:19:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863185.68 2024-11-06T03:19:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:48Z- waiting for 1730863189.68, time is 1730863188.13 2024-11-06T03:19:48Z- AHRS_M2 msg: 1730863188.49 epoch sec; seqNo:4250 2024-11-06T03:19:48Z- AHRS_M2 msg: 0.07 phi; -0.09 theta; 0.01 psi 2024-11-06T03:19:48Z- handleDepth()- Depth_Keller msg: 1730863188.50 epoch sec; seqNo:4526; depth 48.59 2024-11-06T03:19:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863188.51 epoch sec; seqNo:4526; 4061463.63 north; 594802.88 east; 36.69 lat; -121.94 lon; 48.59 depth; utmZone: 10 2024-11-06T03:19:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863185.68 2024-11-06T03:19:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:48Z- waiting for 1730863189.68, time is 1730863188.51 2024-11-06T03:19:48Z- AHRS_M2 msg: 1730863188.90 epoch sec; seqNo:4251 2024-11-06T03:19:48Z- AHRS_M2 msg: 0.07 phi; -0.09 theta; 0.00 psi 2024-11-06T03:19:48Z- handleDepth()- Depth_Keller msg: 1730863188.90 epoch sec; seqNo:4527; depth 48.61 2024-11-06T03:19:48Z- handleDVL() - DVL msg epochMillisec: 1730863188.92 2024-11-06T03:19:48Z- handleDvl() - RDI_Pathfinder msg: 1730863188.92 sec; seqNo:1496; #beams:4 2024-11-06T03:19:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.64 , 41.80 , 39.97 , 44.66 2024-11-06T03:19:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , 0.07 2024-11-06T03:19:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863188.93 epoch sec; seqNo:4527; 4061464.01 north; 594802.88 east; 36.69 lat; -121.94 lon; 48.61 depth; utmZone: 10 2024-11-06T03:19:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863185.68 2024-11-06T03:19:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:49Z- waiting for 1730863189.68, time is 1730863188.93 2024-11-06T03:19:49Z- AHRS_M2 msg: 1730863189.30 epoch sec; seqNo:4252 2024-11-06T03:19:49Z- AHRS_M2 msg: 0.08 phi; -0.09 theta; 6.28 psi 2024-11-06T03:19:49Z- handleDepth()- Depth_Keller msg: 1730863189.30 epoch sec; seqNo:4528; depth 48.63 2024-11-06T03:19:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863189.32 epoch sec; seqNo:4528; 4061464.39 north; 594802.87 east; 36.69 lat; -121.94 lon; 48.63 depth; utmZone: 10 2024-11-06T03:19:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863185.68 2024-11-06T03:19:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:49Z- waiting for 1730863189.68, time is 1730863189.32 2024-11-06T03:19:49Z- AHRS_M2 msg: 1730863189.70 epoch sec; seqNo:4253 2024-11-06T03:19:49Z- AHRS_M2 msg: 0.08 phi; -0.09 theta; 6.28 psi 2024-11-06T03:19:49Z- handleDepth()- Depth_Keller msg: 1730863189.71 epoch sec; seqNo:4529; depth 48.69 2024-11-06T03:19:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863189.72 epoch sec; seqNo:4529; 4061464.77 north; 594802.87 east; 36.69 lat; -121.94 lon; 48.69 depth; utmZone: 10 2024-11-06T03:19:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863185.68 2024-11-06T03:19:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:49Z- waiting for 1730863189.68, time is 1730863189.72 2024-11-06T03:19:49Z- LcmTrn::updateTrn() >>>> pose time 1730863189.72, meas time 1730863188.92 <<<< 2024-11-06T03:19:49Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863189.72 2024-11-06T03:19:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863188.92 sec, ping # 0; waiting for INS data more recent than 1730863185.68... 2024-11-06T03:19:49Z- TerrainNav::motionUpdate() - currEstimate = (4061465, 594802.87, 48.69) 2024-11-06T03:19:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:19:49Z- TerrainNav:: SubcloudNIS is 0.69, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:19:49Z- TNavPF::Projecting Measurements 2024-11-06T03:19:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:19:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:19:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:19:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:19:49Z- TNavFilter::4 good beams. 2024-11-06T03:19:49Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:19:49Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:19:49Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:19:49Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:19:49Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:19:49Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:19:49Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:19:49Z- TerrainMapOctree: (vehN, vehE ) = (4061464.10, 594802.88). 2024-11-06T03:19:49Z- TerrainMapOctree: Min Distance = 220.53. 2024-11-06T03:19:49Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:19:49Z- Forcing Subcloud Comparison 2024-11-06T03:19:49Z- Weighting particles with subcloud comparison 2024-11-06T03:19:49Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:49Z- meanExpectedMeasDiff: -1.362123 delta_rms_squared: 2.385114 alpha: 0.991684 2024-11-06T03:19:49Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:49Z- meanExpectedMeasDiff: 3.566410 delta_rms_squared: 1.342723 alpha: 0.985324 2024-11-06T03:19:49Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:49Z- meanExpectedMeasDiff: 1.862683 delta_rms_squared: 1.156565 alpha: 0.983001 2024-11-06T03:19:50Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:50Z- meanExpectedMeasDiff: -6.041816 delta_rms_squared: 2.022692 alpha: 0.990209 2024-11-06T03:19:50Z- Subcloud NIS: 0.707510 2024-11-06T03:19:50Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:19:50Z- TNavPF:: sumWeights = 0.002497 2024-11-06T03:19:50Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:19:50Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:19:50Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:19:50Z- TerrainNav::Resampling particle filter... 2024-11-06T03:19:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863188.92 sec. 2024-11-06T03:19:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:19:50Z- variances([0], [2], and psi_berg) = 0.682585 0.463457 0.000000 2024-11-06T03:19:50Z- publishEstimatesDecoder() - at 1730863189.72 sec 2024-11-06T03:19:50Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:19:50Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061495.64 594785.95 50.76 2024-11-06T03:19:50Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061495.63 594785.55 50.69 lat/lon: 36.69 -121.94 2024-11-06T03:19:50Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.68 0.46 0.71 0.00 2024-11-06T03:19:50Z- AHRS_M2 msg: 1730863190.12 epoch sec; seqNo:4254 2024-11-06T03:19:50Z- AHRS_M2 msg: 0.08 phi; -0.09 theta; 6.28 psi 2024-11-06T03:19:50Z- handleDepth()- Depth_Keller msg: 1730863190.13 epoch sec; seqNo:4530; depth 48.70 2024-11-06T03:19:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863190.14 epoch sec; seqNo:4530; 4061465.15 north; 594802.87 east; 36.69 lat; -121.94 lon; 48.70 depth; utmZone: 10 2024-11-06T03:19:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863189.72 2024-11-06T03:19:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:50Z- waiting for 1730863193.72, time is 1730863190.14 2024-11-06T03:19:50Z- AHRS_M2 msg: 1730863190.52 epoch sec; seqNo:4255 2024-11-06T03:19:50Z- AHRS_M2 msg: 0.08 phi; -0.10 theta; 6.27 psi 2024-11-06T03:19:50Z- handleDepth()- Depth_Keller msg: 1730863190.52 epoch sec; seqNo:4531; depth 48.72 2024-11-06T03:19:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863190.53 epoch sec; seqNo:4531; 4061465.53 north; 594802.86 east; 36.69 lat; -121.94 lon; 48.72 depth; utmZone: 10 2024-11-06T03:19:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863189.72 2024-11-06T03:19:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:50Z- waiting for 1730863193.72, time is 1730863190.53 2024-11-06T03:19:50Z- AHRS_M2 msg: 1730863190.92 epoch sec; seqNo:4256 2024-11-06T03:19:50Z- AHRS_M2 msg: 0.08 phi; -0.10 theta; 6.27 psi 2024-11-06T03:19:50Z- handleDepth()- Depth_Keller msg: 1730863190.93 epoch sec; seqNo:4532; depth 48.75 2024-11-06T03:19:50Z- handleDVL() - DVL msg epochMillisec: 1730863190.95 2024-11-06T03:19:50Z- handleDvl() - RDI_Pathfinder msg: 1730863190.95 sec; seqNo:1497; #beams:4 2024-11-06T03:19:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.84 , 41.39 , 39.76 , 45.06 2024-11-06T03:19:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.03 , -0.04 2024-11-06T03:19:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863190.97 epoch sec; seqNo:4532; 4061465.91 north; 594802.86 east; 36.69 lat; -121.94 lon; 48.75 depth; utmZone: 10 2024-11-06T03:19:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863189.72 2024-11-06T03:19:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:51Z- waiting for 1730863193.72, time is 1730863190.97 2024-11-06T03:19:51Z- AHRS_M2 msg: 1730863191.33 epoch sec; seqNo:4257 2024-11-06T03:19:51Z- AHRS_M2 msg: 0.08 phi; -0.10 theta; 6.26 psi 2024-11-06T03:19:51Z- handleDepth()- Depth_Keller msg: 1730863191.33 epoch sec; seqNo:4533; depth 48.76 2024-11-06T03:19:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863191.36 epoch sec; seqNo:4533; 4061466.29 north; 594802.85 east; 36.69 lat; -121.94 lon; 48.76 depth; utmZone: 10 2024-11-06T03:19:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863189.72 2024-11-06T03:19:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:51Z- waiting for 1730863193.72, time is 1730863191.36 2024-11-06T03:19:51Z- AHRS_M2 msg: 1730863191.73 epoch sec; seqNo:4258 2024-11-06T03:19:51Z- AHRS_M2 msg: 0.07 phi; -0.10 theta; 6.26 psi 2024-11-06T03:19:51Z- handleDepth()- Depth_Keller msg: 1730863191.73 epoch sec; seqNo:4534; depth 48.79 2024-11-06T03:19:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863191.75 epoch sec; seqNo:4534; 4061466.67 north; 594802.85 east; 36.69 lat; -121.94 lon; 48.79 depth; utmZone: 10 2024-11-06T03:19:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863189.72 2024-11-06T03:19:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:51Z- waiting for 1730863193.72, time is 1730863191.75 2024-11-06T03:19:52Z- AHRS_M2 msg: 1730863192.13 epoch sec; seqNo:4259 2024-11-06T03:19:52Z- AHRS_M2 msg: 0.07 phi; -0.10 theta; 6.25 psi 2024-11-06T03:19:52Z- handleDepth()- Depth_Keller msg: 1730863192.14 epoch sec; seqNo:4535; depth 48.81 2024-11-06T03:19:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863192.16 epoch sec; seqNo:4535; 4061466.67 north; 594802.85 east; 36.69 lat; -121.94 lon; 48.81 depth; utmZone: 10 2024-11-06T03:19:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863189.72 2024-11-06T03:19:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:52Z- waiting for 1730863193.72, time is 1730863192.16 2024-11-06T03:19:52Z- AHRS_M2 msg: 1730863192.54 epoch sec; seqNo:4260 2024-11-06T03:19:52Z- AHRS_M2 msg: 0.07 phi; -0.10 theta; 6.25 psi 2024-11-06T03:19:52Z- handleDepth()- Depth_Keller msg: 1730863192.55 epoch sec; seqNo:4536; depth 48.82 2024-11-06T03:19:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863192.56 epoch sec; seqNo:4536; 4061467.04 north; 594802.85 east; 36.69 lat; -121.94 lon; 48.82 depth; utmZone: 10 2024-11-06T03:19:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863189.72 2024-11-06T03:19:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:52Z- waiting for 1730863193.72, time is 1730863192.56 2024-11-06T03:19:52Z- AHRS_M2 msg: 1730863192.95 epoch sec; seqNo:4261 2024-11-06T03:19:52Z- AHRS_M2 msg: 0.07 phi; -0.10 theta; 6.24 psi 2024-11-06T03:19:52Z- handleDepth()- Depth_Keller msg: 1730863192.96 epoch sec; seqNo:4537; depth 48.85 2024-11-06T03:19:52Z- handleDVL() - DVL msg epochMillisec: 1730863192.98 2024-11-06T03:19:52Z- handleDvl() - RDI_Pathfinder msg: 1730863192.98 sec; seqNo:1498; #beams:4 2024-11-06T03:19:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.22 , 41.59 , 39.35 , 44.85 2024-11-06T03:19:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , -0.10 2024-11-06T03:19:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863193.02 epoch sec; seqNo:4537; 4061467.42 north; 594802.84 east; 36.69 lat; -121.94 lon; 48.85 depth; utmZone: 10 2024-11-06T03:19:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863189.72 2024-11-06T03:19:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:53Z- waiting for 1730863193.72, time is 1730863193.02 2024-11-06T03:19:53Z- AHRS_M2 msg: 1730863193.35 epoch sec; seqNo:4262 2024-11-06T03:19:53Z- AHRS_M2 msg: 0.07 phi; -0.10 theta; 6.23 psi 2024-11-06T03:19:53Z- handleDepth()- Depth_Keller msg: 1730863193.35 epoch sec; seqNo:4538; depth 48.87 2024-11-06T03:19:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863193.38 epoch sec; seqNo:4538; 4061467.80 north; 594802.84 east; 36.69 lat; -121.94 lon; 48.87 depth; utmZone: 10 2024-11-06T03:19:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863189.72 2024-11-06T03:19:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:53Z- waiting for 1730863193.72, time is 1730863193.38 2024-11-06T03:19:53Z- AHRS_M2 msg: 1730863193.75 epoch sec; seqNo:4263 2024-11-06T03:19:53Z- AHRS_M2 msg: 0.07 phi; -0.10 theta; 6.23 psi 2024-11-06T03:19:53Z- handleDepth()- Depth_Keller msg: 1730863193.76 epoch sec; seqNo:4539; depth 48.89 2024-11-06T03:19:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863193.76 epoch sec; seqNo:4539; 4061468.18 north; 594802.83 east; 36.69 lat; -121.94 lon; 48.89 depth; utmZone: 10 2024-11-06T03:19:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863189.72 2024-11-06T03:19:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:53Z- waiting for 1730863193.72, time is 1730863193.76 2024-11-06T03:19:53Z- LcmTrn::updateTrn() >>>> pose time 1730863193.76, meas time 1730863192.98 <<<< 2024-11-06T03:19:53Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863193.76 2024-11-06T03:19:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863192.98 sec, ping # 0; waiting for INS data more recent than 1730863189.72... 2024-11-06T03:19:53Z- TerrainNav::motionUpdate() - currEstimate = (4061468, 594802.83, 48.89) 2024-11-06T03:19:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:19:53Z- TerrainNav:: SubcloudNIS is 0.71, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:19:53Z- TNavPF::Projecting Measurements 2024-11-06T03:19:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:19:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:19:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:19:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:19:53Z- TNavFilter::4 good beams. 2024-11-06T03:19:53Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:19:53Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:19:53Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:19:53Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:19:53Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:19:53Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:19:53Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:19:53Z- TerrainMapOctree: (vehN, vehE ) = (4061467.52, 594802.84). 2024-11-06T03:19:53Z- TerrainMapOctree: Min Distance = 223.84. 2024-11-06T03:19:53Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:19:54Z- Forcing Subcloud Comparison 2024-11-06T03:19:54Z- Weighting particles with subcloud comparison 2024-11-06T03:19:54Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:54Z- meanExpectedMeasDiff: 4.640694 delta_rms_squared: 1.357598 alpha: 0.985482 2024-11-06T03:19:54Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:54Z- meanExpectedMeasDiff: -1.923906 delta_rms_squared: 1.208169 alpha: 0.983716 2024-11-06T03:19:54Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:54Z- meanExpectedMeasDiff: 1.977881 delta_rms_squared: 1.035534 alpha: 0.981052 2024-11-06T03:19:54Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:54Z- meanExpectedMeasDiff: 0.541314 delta_rms_squared: 1.639412 alpha: 0.987948 2024-11-06T03:19:54Z- Subcloud NIS: 0.700925 2024-11-06T03:19:54Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:19:54Z- TNavPF:: sumWeights = 0.044493 2024-11-06T03:19:54Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:19:54Z- TNavPF::First Term of Map Covariance Matrix is 0.63 2024-11-06T03:19:54Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:19:54Z- TerrainNav::Resampling particle filter... 2024-11-06T03:19:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863192.98 sec. 2024-11-06T03:19:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:19:54Z- variances([0], [2], and psi_berg) = 0.696321 0.703404 0.000000 2024-11-06T03:19:54Z- publishEstimatesDecoder() - at 1730863193.76 sec 2024-11-06T03:19:54Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:19:54Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061499.08 594785.96 50.73 2024-11-06T03:19:54Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061499.34 594785.71 50.34 lat/lon: 36.69 -121.94 2024-11-06T03:19:54Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.70 0.72 0.00 2024-11-06T03:19:54Z- AHRS_M2 msg: 1730863194.15 epoch sec; seqNo:4264 2024-11-06T03:19:54Z- AHRS_M2 msg: 0.06 phi; -0.10 theta; 6.22 psi 2024-11-06T03:19:54Z- handleDepth()- Depth_Keller msg: 1730863194.16 epoch sec; seqNo:4540; depth 48.90 2024-11-06T03:19:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863194.19 epoch sec; seqNo:4540; 4061468.56 north; 594802.83 east; 36.69 lat; -121.94 lon; 48.90 depth; utmZone: 10 2024-11-06T03:19:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863193.76 2024-11-06T03:19:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:54Z- waiting for 1730863197.76, time is 1730863194.19 2024-11-06T03:19:54Z- AHRS_M2 msg: 1730863194.56 epoch sec; seqNo:4265 2024-11-06T03:19:54Z- AHRS_M2 msg: 0.06 phi; -0.10 theta; 6.21 psi 2024-11-06T03:19:54Z- handleDepth()- Depth_Keller msg: 1730863194.57 epoch sec; seqNo:4541; depth 48.93 2024-11-06T03:19:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863194.58 epoch sec; seqNo:4541; 4061468.94 north; 594802.82 east; 36.69 lat; -121.94 lon; 48.93 depth; utmZone: 10 2024-11-06T03:19:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863193.76 2024-11-06T03:19:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:54Z- waiting for 1730863197.76, time is 1730863194.58 2024-11-06T03:19:54Z- AHRS_M2 msg: 1730863194.96 epoch sec; seqNo:4266 2024-11-06T03:19:54Z- AHRS_M2 msg: 0.06 phi; -0.10 theta; 6.21 psi 2024-11-06T03:19:54Z- handleDepth()- Depth_Keller msg: 1730863194.98 epoch sec; seqNo:4542; depth 48.97 2024-11-06T03:19:55Z- handleDVL() - DVL msg epochMillisec: 1730863195.00 2024-11-06T03:19:55Z- handleDvl() - RDI_Pathfinder msg: 1730863195.00 sec; seqNo:1499; #beams:4 2024-11-06T03:19:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.62 , 41.19 , 38.74 , 44.66 2024-11-06T03:19:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.16 , -0.06 2024-11-06T03:19:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863195.01 epoch sec; seqNo:4542; 4061469.32 north; 594802.82 east; 36.69 lat; -121.94 lon; 48.97 depth; utmZone: 10 2024-11-06T03:19:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863193.76 2024-11-06T03:19:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:55Z- waiting for 1730863197.76, time is 1730863195.01 2024-11-06T03:19:55Z- AHRS_M2 msg: 1730863195.37 epoch sec; seqNo:4267 2024-11-06T03:19:55Z- AHRS_M2 msg: 0.06 phi; -0.10 theta; 6.19 psi 2024-11-06T03:19:55Z- handleDepth()- Depth_Keller msg: 1730863195.38 epoch sec; seqNo:4543; depth 49.00 2024-11-06T03:19:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863195.40 epoch sec; seqNo:4543; 4061469.70 north; 594802.82 east; 36.69 lat; -121.94 lon; 49.00 depth; utmZone: 10 2024-11-06T03:19:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863193.76 2024-11-06T03:19:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:55Z- waiting for 1730863197.76, time is 1730863195.40 2024-11-06T03:19:55Z- AHRS_M2 msg: 1730863195.78 epoch sec; seqNo:4268 2024-11-06T03:19:55Z- AHRS_M2 msg: 0.06 phi; -0.10 theta; 6.18 psi 2024-11-06T03:19:55Z- handleDepth()- Depth_Keller msg: 1730863195.79 epoch sec; seqNo:4544; depth 49.01 2024-11-06T03:19:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863195.79 epoch sec; seqNo:4544; 4061470.08 north; 594802.81 east; 36.69 lat; -121.94 lon; 49.01 depth; utmZone: 10 2024-11-06T03:19:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863193.76 2024-11-06T03:19:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:55Z- waiting for 1730863197.76, time is 1730863195.79 2024-11-06T03:19:56Z- AHRS_M2 msg: 1730863196.18 epoch sec; seqNo:4269 2024-11-06T03:19:56Z- AHRS_M2 msg: 0.06 phi; -0.10 theta; 6.18 psi 2024-11-06T03:19:56Z- handleDepth()- Depth_Keller msg: 1730863196.19 epoch sec; seqNo:4545; depth 49.04 2024-11-06T03:19:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863196.19 epoch sec; seqNo:4545; 4061470.45 north; 594802.81 east; 36.69 lat; -121.94 lon; 49.04 depth; utmZone: 10 2024-11-06T03:19:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863193.76 2024-11-06T03:19:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:56Z- waiting for 1730863197.76, time is 1730863196.19 2024-11-06T03:19:56Z- AHRS_M2 msg: 1730863196.58 epoch sec; seqNo:4270 2024-11-06T03:19:56Z- AHRS_M2 msg: 0.06 phi; -0.10 theta; 6.17 psi 2024-11-06T03:19:56Z- handleDepth()- Depth_Keller msg: 1730863196.59 epoch sec; seqNo:4546; depth 49.07 2024-11-06T03:19:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863196.60 epoch sec; seqNo:4546; 4061470.83 north; 594802.80 east; 36.69 lat; -121.94 lon; 49.07 depth; utmZone: 10 2024-11-06T03:19:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863193.76 2024-11-06T03:19:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:56Z- waiting for 1730863197.76, time is 1730863196.60 2024-11-06T03:19:56Z- AHRS_M2 msg: 1730863196.98 epoch sec; seqNo:4271 2024-11-06T03:19:56Z- AHRS_M2 msg: 0.07 phi; -0.10 theta; 6.16 psi 2024-11-06T03:19:56Z- handleDepth()- Depth_Keller msg: 1730863196.99 epoch sec; seqNo:4547; depth 49.11 2024-11-06T03:19:57Z- handleDVL() - DVL msg epochMillisec: 1730863197.01 2024-11-06T03:19:57Z- handleDvl() - RDI_Pathfinder msg: 1730863197.01 sec; seqNo:1500; #beams:4 2024-11-06T03:19:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.41 , 41.19 , 38.74 , 44.66 2024-11-06T03:19:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.20 , 0.02 2024-11-06T03:19:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863197.03 epoch sec; seqNo:4547; 4061471.20 north; 594801.58 east; 36.69 lat; -121.94 lon; 49.11 depth; utmZone: 10 2024-11-06T03:19:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863193.76 2024-11-06T03:19:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:57Z- waiting for 1730863197.76, time is 1730863197.03 2024-11-06T03:19:57Z- AHRS_M2 msg: 1730863197.39 epoch sec; seqNo:4272 2024-11-06T03:19:57Z- AHRS_M2 msg: 0.07 phi; -0.10 theta; 6.16 psi 2024-11-06T03:19:57Z- handleDepth()- Depth_Keller msg: 1730863197.39 epoch sec; seqNo:4548; depth 49.17 2024-11-06T03:19:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863197.42 epoch sec; seqNo:4548; 4061471.58 north; 594801.57 east; 36.69 lat; -121.94 lon; 49.17 depth; utmZone: 10 2024-11-06T03:19:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863193.76 2024-11-06T03:19:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:57Z- waiting for 1730863197.76, time is 1730863197.42 2024-11-06T03:19:57Z- AHRS_M2 msg: 1730863197.80 epoch sec; seqNo:4273 2024-11-06T03:19:57Z- AHRS_M2 msg: 0.07 phi; -0.10 theta; 6.15 psi 2024-11-06T03:19:57Z- handleDepth()- Depth_Keller msg: 1730863197.81 epoch sec; seqNo:4549; depth 49.19 2024-11-06T03:19:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863197.82 epoch sec; seqNo:4549; 4061471.96 north; 594801.57 east; 36.69 lat; -121.94 lon; 49.19 depth; utmZone: 10 2024-11-06T03:19:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863193.76 2024-11-06T03:19:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:57Z- waiting for 1730863197.76, time is 1730863197.82 2024-11-06T03:19:57Z- LcmTrn::updateTrn() >>>> pose time 1730863197.82, meas time 1730863197.01 <<<< 2024-11-06T03:19:57Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863197.82 2024-11-06T03:19:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863197.01 sec, ping # 0; waiting for INS data more recent than 1730863193.76... 2024-11-06T03:19:57Z- TerrainNav::motionUpdate() - currEstimate = (4061472, 594801.57, 49.19) 2024-11-06T03:19:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:19:57Z- TerrainNav:: SubcloudNIS is 0.70, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:19:57Z- TNavPF::Projecting Measurements 2024-11-06T03:19:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:19:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:19:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:19:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:19:57Z- TNavFilter::4 good beams. 2024-11-06T03:19:57Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:19:57Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:19:57Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:19:57Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:19:57Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:19:57Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:19:57Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:19:57Z- TerrainMapOctree: (vehN, vehE ) = (4061471.21, 594801.82). 2024-11-06T03:19:57Z- TerrainMapOctree: Min Distance = 227.20. 2024-11-06T03:19:57Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:19:58Z- Forcing Subcloud Comparison 2024-11-06T03:19:58Z- Weighting particles with subcloud comparison 2024-11-06T03:19:58Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:58Z- meanExpectedMeasDiff: 3.187362 delta_rms_squared: 1.329859 alpha: 0.985184 2024-11-06T03:19:58Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:58Z- meanExpectedMeasDiff: -1.538938 delta_rms_squared: 1.240436 alpha: 0.984132 2024-11-06T03:19:58Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:58Z- meanExpectedMeasDiff: 1.254973 delta_rms_squared: 1.157917 alpha: 0.983021 2024-11-06T03:19:58Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:19:58Z- meanExpectedMeasDiff: -0.117125 delta_rms_squared: 1.457464 alpha: 0.986463 2024-11-06T03:19:58Z- Subcloud NIS: 0.674165 2024-11-06T03:19:58Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:19:58Z- TNavPF:: sumWeights = 0.165437 2024-11-06T03:19:58Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:19:58Z- TNavPF::First Term of Map Covariance Matrix is 0.99 2024-11-06T03:19:58Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T03:19:58Z- TerrainNav::Resampling particle filter... 2024-11-06T03:19:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863197.01 sec. 2024-11-06T03:19:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:19:58Z- variances([0], [2], and psi_berg) = 0.738832 0.893423 0.000000 2024-11-06T03:19:58Z- publishEstimatesDecoder() - at 1730863197.82 sec 2024-11-06T03:19:58Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:19:58Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061502.68 594784.62 49.86 2024-11-06T03:19:58Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061503.31 594784.66 50.35 lat/lon: 36.69 -121.94 2024-11-06T03:19:58Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.74 0.89 0.73 0.00 2024-11-06T03:19:58Z- AHRS_M2 msg: 1730863198.19 epoch sec; seqNo:4274 2024-11-06T03:19:58Z- AHRS_M2 msg: 0.07 phi; -0.11 theta; 6.15 psi 2024-11-06T03:19:58Z- handleDepth()- Depth_Keller msg: 1730863198.21 epoch sec; seqNo:4550; depth 49.24 2024-11-06T03:19:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863198.23 epoch sec; seqNo:4550; 4061472.33 north; 594801.57 east; 36.69 lat; -121.94 lon; 49.24 depth; utmZone: 10 2024-11-06T03:19:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863197.82 2024-11-06T03:19:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:58Z- waiting for 1730863201.82, time is 1730863198.23 2024-11-06T03:19:58Z- AHRS_M2 msg: 1730863198.60 epoch sec; seqNo:4275 2024-11-06T03:19:58Z- AHRS_M2 msg: 0.07 phi; -0.11 theta; 6.14 psi 2024-11-06T03:19:58Z- handleDepth()- Depth_Keller msg: 1730863198.61 epoch sec; seqNo:4551; depth 49.28 2024-11-06T03:19:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863198.63 epoch sec; seqNo:4551; 4061472.71 north; 594801.56 east; 36.69 lat; -121.94 lon; 49.28 depth; utmZone: 10 2024-11-06T03:19:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863197.82 2024-11-06T03:19:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:58Z- waiting for 1730863201.82, time is 1730863198.63 2024-11-06T03:19:59Z- AHRS_M2 msg: 1730863199.02 epoch sec; seqNo:4276 2024-11-06T03:19:59Z- AHRS_M2 msg: 0.08 phi; -0.11 theta; 6.14 psi 2024-11-06T03:19:59Z- handleDepth()- Depth_Keller msg: 1730863199.03 epoch sec; seqNo:4552; depth 49.34 2024-11-06T03:19:59Z- handleDVL() - DVL msg epochMillisec: 1730863199.06 2024-11-06T03:19:59Z- handleDvl() - RDI_Pathfinder msg: 1730863199.06 sec; seqNo:1501; #beams:4 2024-11-06T03:19:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.62 , 40.58 , 38.33 , 44.45 2024-11-06T03:19:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.14 , 0.04 2024-11-06T03:19:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863199.07 epoch sec; seqNo:4552; 4061473.09 north; 594801.56 east; 36.69 lat; -121.94 lon; 49.34 depth; utmZone: 10 2024-11-06T03:19:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863197.82 2024-11-06T03:19:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:59Z- waiting for 1730863201.82, time is 1730863199.07 2024-11-06T03:19:59Z- AHRS_M2 msg: 1730863199.41 epoch sec; seqNo:4277 2024-11-06T03:19:59Z- AHRS_M2 msg: 0.08 phi; -0.11 theta; 6.14 psi 2024-11-06T03:19:59Z- handleDepth()- Depth_Keller msg: 1730863199.41 epoch sec; seqNo:4553; depth 49.38 2024-11-06T03:19:59Z- handleDVL() - DVL msg epochMillisec: 1730863199.41 2024-11-06T03:19:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:19:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:19:59Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:19:59Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:19:59Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:19:59Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:19:59Z- handleDvl() - RDI_Pathfinder msg: 1730863199.41 sec; seqNo:1502; #beams:0 2024-11-06T03:19:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:19:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.14 , 0.04 2024-11-06T03:19:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863199.43 epoch sec; seqNo:4553; 4061473.47 north; 594801.55 east; 36.69 lat; -121.94 lon; 49.38 depth; utmZone: 10 2024-11-06T03:19:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:19:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863197.82 2024-11-06T03:19:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:59Z- waiting for 1730863201.82, time is 1730863199.43 2024-11-06T03:19:59Z- AHRS_M2 msg: 1730863199.81 epoch sec; seqNo:4278 2024-11-06T03:19:59Z- AHRS_M2 msg: 0.08 phi; -0.11 theta; 6.13 psi 2024-11-06T03:19:59Z- handleDepth()- Depth_Keller msg: 1730863199.81 epoch sec; seqNo:4554; depth 49.42 2024-11-06T03:19:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863199.83 epoch sec; seqNo:4554; 4061473.85 north; 594801.55 east; 36.69 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T03:19:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:19:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863197.82 2024-11-06T03:19:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:19:59Z- waiting for 1730863201.82, time is 1730863199.83 2024-11-06T03:20:00Z- AHRS_M2 msg: 1730863200.22 epoch sec; seqNo:4279 2024-11-06T03:20:00Z- AHRS_M2 msg: 0.09 phi; -0.11 theta; 6.13 psi 2024-11-06T03:20:00Z- handleDepth()- Depth_Keller msg: 1730863200.22 epoch sec; seqNo:4555; depth 49.45 2024-11-06T03:20:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863200.24 epoch sec; seqNo:4555; 4061474.23 north; 594801.55 east; 36.69 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:20:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863197.82 2024-11-06T03:20:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:00Z- waiting for 1730863201.82, time is 1730863200.24 2024-11-06T03:20:00Z- AHRS_M2 msg: 1730863200.62 epoch sec; seqNo:4280 2024-11-06T03:20:00Z- AHRS_M2 msg: 0.09 phi; -0.11 theta; 6.13 psi 2024-11-06T03:20:00Z- handleDepth()- Depth_Keller msg: 1730863200.63 epoch sec; seqNo:4556; depth 49.52 2024-11-06T03:20:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863200.65 epoch sec; seqNo:4556; 4061474.61 north; 594801.54 east; 36.69 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T03:20:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863197.82 2024-11-06T03:20:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:00Z- waiting for 1730863201.82, time is 1730863200.65 2024-11-06T03:20:01Z- AHRS_M2 msg: 1730863201.02 epoch sec; seqNo:4281 2024-11-06T03:20:01Z- AHRS_M2 msg: 0.09 phi; -0.11 theta; 6.13 psi 2024-11-06T03:20:01Z- handleDepth()- Depth_Keller msg: 1730863201.03 epoch sec; seqNo:4557; depth 49.56 2024-11-06T03:20:01Z- handleDVL() - DVL msg epochMillisec: 1730863201.06 2024-11-06T03:20:01Z- handleDvl() - RDI_Pathfinder msg: 1730863201.06 sec; seqNo:1503; #beams:4 2024-11-06T03:20:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.80 , 40.00 , 38.32 , 44.67 2024-11-06T03:20:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.10 , 0.04 2024-11-06T03:20:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863201.07 epoch sec; seqNo:4557; 4061474.97 north; 594800.32 east; 36.69 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:20:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863197.82 2024-11-06T03:20:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:01Z- waiting for 1730863201.82, time is 1730863201.07 2024-11-06T03:20:01Z- AHRS_M2 msg: 1730863201.43 epoch sec; seqNo:4282 2024-11-06T03:20:01Z- AHRS_M2 msg: 0.10 phi; -0.11 theta; 6.12 psi 2024-11-06T03:20:01Z- handleDepth()- Depth_Keller msg: 1730863201.44 epoch sec; seqNo:4558; depth 49.60 2024-11-06T03:20:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863201.45 epoch sec; seqNo:4558; 4061475.35 north; 594800.31 east; 36.69 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:20:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863197.82 2024-11-06T03:20:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:01Z- waiting for 1730863201.82, time is 1730863201.45 2024-11-06T03:20:01Z- handleDepth()- Depth_Keller msg: 1730863201.85 epoch sec; seqNo:4559; depth 49.65 2024-11-06T03:20:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863201.87 epoch sec; seqNo:4559; 4061475.73 north; 594800.31 east; 36.69 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:20:02Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:20:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863197.82 2024-11-06T03:20:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:02Z- waiting for 1730863201.82, time is 1730863201.87 2024-11-06T03:20:02Z- LcmTrn::updateTrn() >>>> pose time 1730863201.87, meas time 1730863201.06 <<<< 2024-11-06T03:20:02Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863201.87 2024-11-06T03:20:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863201.06 sec, ping # 0; waiting for INS data more recent than 1730863197.82... 2024-11-06T03:20:02Z- TerrainNav::motionUpdate() - currEstimate = (4061476, 594800.31, 49.65) 2024-11-06T03:20:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:20:02Z- TerrainNav:: SubcloudNIS is 0.67, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:20:02Z- TNavPF::Projecting Measurements 2024-11-06T03:20:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:20:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:20:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:20:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:20:02Z- TNavFilter::4 good beams. 2024-11-06T03:20:02Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:20:02Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:20:02Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:20:02Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:20:02Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:20:02Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:20:02Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:20:02Z- TerrainMapOctree: (vehN, vehE ) = (4061474.98, 594800.56). 2024-11-06T03:20:02Z- TerrainMapOctree: Min Distance = 230.59. 2024-11-06T03:20:02Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:20:02Z- Forcing Subcloud Comparison 2024-11-06T03:20:02Z- Weighting particles with subcloud comparison 2024-11-06T03:20:02Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:02Z- meanExpectedMeasDiff: 3.434969 delta_rms_squared: 1.009973 alpha: 0.980582 2024-11-06T03:20:02Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:02Z- meanExpectedMeasDiff: -0.252910 delta_rms_squared: 1.526051 alpha: 0.987064 2024-11-06T03:20:02Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:02Z- meanExpectedMeasDiff: 1.754900 delta_rms_squared: 1.236727 alpha: 0.984086 2024-11-06T03:20:02Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:02Z- meanExpectedMeasDiff: -0.449446 delta_rms_squared: 1.453018 alpha: 0.986422 2024-11-06T03:20:02Z- Subcloud NIS: 0.657784 2024-11-06T03:20:02Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:20:02Z- TNavPF:: sumWeights = 0.170841 2024-11-06T03:20:02Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:20:02Z- TNavPF::First Term of Map Covariance Matrix is 1.08 2024-11-06T03:20:02Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T03:20:02Z- TerrainNav::Resampling particle filter... 2024-11-06T03:20:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863201.06 sec. 2024-11-06T03:20:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:20:02Z- variances([0], [2], and psi_berg) = 0.710854 0.846550 0.000000 2024-11-06T03:20:02Z- publishEstimatesDecoder() - at 1730863201.87 sec 2024-11-06T03:20:02Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:20:02Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061506.49 594783.42 51.07 2024-11-06T03:20:02Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061507.05 594783.36 50.32 lat/lon: 36.69 -121.94 2024-11-06T03:20:02Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.71 0.85 0.74 0.00 2024-11-06T03:20:02Z- AHRS_M2 msg: 1730863202.23 epoch sec; seqNo:4283 2024-11-06T03:20:02Z- AHRS_M2 msg: 0.10 phi; -0.11 theta; 6.12 psi 2024-11-06T03:20:02Z- handleDepth()- Depth_Keller msg: 1730863202.24 epoch sec; seqNo:4560; depth 49.69 2024-11-06T03:20:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863202.28 epoch sec; seqNo:4560; 4061476.11 north; 594800.30 east; 36.69 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:20:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863201.87 2024-11-06T03:20:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:02Z- waiting for 1730863205.87, time is 1730863202.28 2024-11-06T03:20:02Z- AHRS_M2 msg: 1730863202.65 epoch sec; seqNo:4284 2024-11-06T03:20:02Z- AHRS_M2 msg: 0.10 phi; -0.10 theta; 6.12 psi 2024-11-06T03:20:02Z- handleDepth()- Depth_Keller msg: 1730863202.65 epoch sec; seqNo:4561; depth 49.72 2024-11-06T03:20:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863202.66 epoch sec; seqNo:4561; 4061476.11 north; 594800.30 east; 36.69 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:20:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863201.87 2024-11-06T03:20:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:02Z- waiting for 1730863205.87, time is 1730863202.66 2024-11-06T03:20:03Z- AHRS_M2 msg: 1730863203.04 epoch sec; seqNo:4285 2024-11-06T03:20:03Z- AHRS_M2 msg: 0.10 phi; -0.10 theta; 6.11 psi 2024-11-06T03:20:03Z- handleDepth()- Depth_Keller msg: 1730863203.05 epoch sec; seqNo:4562; depth 49.78 2024-11-06T03:20:03Z- handleDVL() - DVL msg epochMillisec: 1730863203.08 2024-11-06T03:20:03Z- handleDvl() - RDI_Pathfinder msg: 1730863203.08 sec; seqNo:1504; #beams:4 2024-11-06T03:20:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.99 , 39.63 , 38.32 , 44.11 2024-11-06T03:20:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , 0.02 2024-11-06T03:20:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863203.11 epoch sec; seqNo:4562; 4061476.87 north; 594800.30 east; 36.69 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:20:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863201.87 2024-11-06T03:20:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:03Z- waiting for 1730863205.87, time is 1730863203.11 2024-11-06T03:20:03Z- AHRS_M2 msg: 1730863203.45 epoch sec; seqNo:4286 2024-11-06T03:20:03Z- AHRS_M2 msg: 0.11 phi; -0.10 theta; 6.11 psi 2024-11-06T03:20:03Z- handleDepth()- Depth_Keller msg: 1730863203.45 epoch sec; seqNo:4563; depth 49.83 2024-11-06T03:20:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863203.47 epoch sec; seqNo:4563; 4061476.87 north; 594800.30 east; 36.69 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:20:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863201.87 2024-11-06T03:20:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:03Z- waiting for 1730863205.87, time is 1730863203.47 2024-11-06T03:20:03Z- AHRS_M2 msg: 1730863203.85 epoch sec; seqNo:4287 2024-11-06T03:20:03Z- AHRS_M2 msg: 0.11 phi; -0.10 theta; 6.11 psi 2024-11-06T03:20:03Z- handleDepth()- Depth_Keller msg: 1730863203.85 epoch sec; seqNo:4564; depth 49.87 2024-11-06T03:20:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863203.86 epoch sec; seqNo:4564; 4061477.25 north; 594800.29 east; 36.69 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:20:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863201.87 2024-11-06T03:20:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:04Z- waiting for 1730863205.87, time is 1730863203.86 2024-11-06T03:20:04Z- AHRS_M2 msg: 1730863204.26 epoch sec; seqNo:4288 2024-11-06T03:20:04Z- AHRS_M2 msg: 0.11 phi; -0.10 theta; 6.11 psi 2024-11-06T03:20:04Z- handleDepth()- Depth_Keller msg: 1730863204.27 epoch sec; seqNo:4565; depth 49.90 2024-11-06T03:20:04Z- handleDVL() - DVL msg epochMillisec: 1730863204.27 2024-11-06T03:20:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:20:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:20:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:20:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:20:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:20:04Z- handleDvl() - RDI_Pathfinder msg: 1730863204.27 sec; seqNo:1505; #beams:0 2024-11-06T03:20:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:20:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , 0.02 2024-11-06T03:20:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863204.28 epoch sec; seqNo:4565; 4061477.63 north; 594800.29 east; 36.69 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:20:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863201.87 2024-11-06T03:20:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:04Z- waiting for 1730863205.87, time is 1730863204.28 2024-11-06T03:20:04Z- AHRS_M2 msg: 1730863204.66 epoch sec; seqNo:4289 2024-11-06T03:20:04Z- AHRS_M2 msg: 0.11 phi; -0.10 theta; 6.11 psi 2024-11-06T03:20:04Z- handleDepth()- Depth_Keller msg: 1730863204.67 epoch sec; seqNo:4566; depth 49.94 2024-11-06T03:20:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863204.71 epoch sec; seqNo:4566; 4061478.00 north; 594800.28 east; 36.69 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:20:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863201.87 2024-11-06T03:20:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:04Z- waiting for 1730863205.87, time is 1730863204.71 2024-11-06T03:20:05Z- AHRS_M2 msg: 1730863205.06 epoch sec; seqNo:4290 2024-11-06T03:20:05Z- AHRS_M2 msg: 0.11 phi; -0.10 theta; 6.11 psi 2024-11-06T03:20:05Z- handleDepth()- Depth_Keller msg: 1730863205.07 epoch sec; seqNo:4567; depth 49.98 2024-11-06T03:20:05Z- handleDVL() - DVL msg epochMillisec: 1730863205.09 2024-11-06T03:20:05Z- handleDvl() - RDI_Pathfinder msg: 1730863205.09 sec; seqNo:1506; #beams:4 2024-11-06T03:20:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.18 , 39.06 , 38.13 , 44.49 2024-11-06T03:20:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.00 2024-11-06T03:20:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863205.11 epoch sec; seqNo:4567; 4061478.38 north; 594800.28 east; 36.69 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:20:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863201.87 2024-11-06T03:20:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:05Z- waiting for 1730863205.87, time is 1730863205.11 2024-11-06T03:20:05Z- AHRS_M2 msg: 1730863205.47 epoch sec; seqNo:4291 2024-11-06T03:20:05Z- AHRS_M2 msg: 0.12 phi; -0.09 theta; 6.11 psi 2024-11-06T03:20:05Z- handleDepth()- Depth_Keller msg: 1730863205.47 epoch sec; seqNo:4568; depth 50.01 2024-11-06T03:20:05Z- handleDVL() - DVL msg epochMillisec: 1730863205.47 2024-11-06T03:20:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:20:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:20:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:20:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:20:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:20:05Z- handleDvl() - RDI_Pathfinder msg: 1730863205.47 sec; seqNo:1507; #beams:0 2024-11-06T03:20:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:20:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.00 2024-11-06T03:20:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863205.49 epoch sec; seqNo:4568; 4061478.76 north; 594800.27 east; 36.69 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:20:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863201.87 2024-11-06T03:20:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:05Z- waiting for 1730863205.87, time is 1730863205.49 2024-11-06T03:20:05Z- AHRS_M2 msg: 1730863205.87 epoch sec; seqNo:4292 2024-11-06T03:20:05Z- AHRS_M2 msg: 0.12 phi; -0.09 theta; 6.11 psi 2024-11-06T03:20:05Z- handleDepth()- Depth_Keller msg: 1730863205.88 epoch sec; seqNo:4569; depth 50.03 2024-11-06T03:20:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863205.89 epoch sec; seqNo:4569; 4061479.14 north; 594800.27 east; 36.69 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:20:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863201.87 2024-11-06T03:20:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:06Z- waiting for 1730863205.87, time is 1730863205.89 2024-11-06T03:20:06Z- LcmTrn::updateTrn() >>>> pose time 1730863205.89, meas time 1730863205.47 <<<< 2024-11-06T03:20:06Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863205.89 2024-11-06T03:20:06Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:20:06Z- TerrainNav::motionUpdate() - currEstimate = (4061479, 594800.27, 50.03) 2024-11-06T03:20:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:20:06Z- TerrainNav:: SubcloudNIS is 0.66, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:20:06Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:20:06Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:20:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:20:06Z- variances([0], [2], and psi_berg) = 0.712991 0.848142 0.000000 2024-11-06T03:20:06Z- publishEstimatesDecoder() - at 1730863205.89 sec 2024-11-06T03:20:06Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:20:06Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061509.85 594783.37 51.52 2024-11-06T03:20:06Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061510.46 594783.33 50.69 lat/lon: 36.69 -121.94 2024-11-06T03:20:06Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.71 0.85 0.98 0.00 2024-11-06T03:20:06Z- AHRS_M2 msg: 1730863206.28 epoch sec; seqNo:4293 2024-11-06T03:20:06Z- AHRS_M2 msg: 0.11 phi; -0.09 theta; 6.11 psi 2024-11-06T03:20:06Z- handleDepth()- Depth_Keller msg: 1730863206.28 epoch sec; seqNo:4570; depth 50.05 2024-11-06T03:20:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863206.29 epoch sec; seqNo:4570; 4061479.52 north; 594800.27 east; 36.69 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:20:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863205.89 2024-11-06T03:20:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:06Z- waiting for 1730863209.89, time is 1730863206.29 2024-11-06T03:20:06Z- AHRS_M2 msg: 1730863206.68 epoch sec; seqNo:4294 2024-11-06T03:20:06Z- AHRS_M2 msg: 0.11 phi; -0.09 theta; 6.11 psi 2024-11-06T03:20:06Z- handleDepth()- Depth_Keller msg: 1730863206.69 epoch sec; seqNo:4571; depth 50.07 2024-11-06T03:20:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863206.76 epoch sec; seqNo:4571; 4061479.88 north; 594799.04 east; 36.69 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:20:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863205.89 2024-11-06T03:20:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:06Z- waiting for 1730863209.89, time is 1730863206.76 2024-11-06T03:20:07Z- AHRS_M2 msg: 1730863207.09 epoch sec; seqNo:4295 2024-11-06T03:20:07Z- AHRS_M2 msg: 0.11 phi; -0.08 theta; 6.11 psi 2024-11-06T03:20:07Z- handleDepth()- Depth_Keller msg: 1730863207.09 epoch sec; seqNo:4572; depth 50.11 2024-11-06T03:20:07Z- handleDVL() - DVL msg epochMillisec: 1730863207.11 2024-11-06T03:20:07Z- handleDvl() - RDI_Pathfinder msg: 1730863207.11 sec; seqNo:1508; #beams:4 2024-11-06T03:20:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.99 , 38.69 , 37.94 , 43.55 2024-11-06T03:20:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , -0.05 2024-11-06T03:20:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863207.12 epoch sec; seqNo:4572; 4061480.26 north; 594799.04 east; 36.69 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:20:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863205.89 2024-11-06T03:20:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:07Z- waiting for 1730863209.89, time is 1730863207.12 2024-11-06T03:20:07Z- AHRS_M2 msg: 1730863207.49 epoch sec; seqNo:4296 2024-11-06T03:20:07Z- AHRS_M2 msg: 0.11 phi; -0.08 theta; 6.10 psi 2024-11-06T03:20:07Z- handleDepth()- Depth_Keller msg: 1730863207.49 epoch sec; seqNo:4573; depth 50.13 2024-11-06T03:20:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863207.51 epoch sec; seqNo:4573; 4061480.64 north; 594799.03 east; 36.69 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:20:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863205.89 2024-11-06T03:20:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:07Z- waiting for 1730863209.89, time is 1730863207.51 2024-11-06T03:20:07Z- AHRS_M2 msg: 1730863207.89 epoch sec; seqNo:4297 2024-11-06T03:20:07Z- AHRS_M2 msg: 0.11 phi; -0.08 theta; 6.10 psi 2024-11-06T03:20:07Z- handleDepth()- Depth_Keller msg: 1730863207.90 epoch sec; seqNo:4574; depth 50.15 2024-11-06T03:20:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863207.91 epoch sec; seqNo:4574; 4061481.02 north; 594799.03 east; 36.69 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:20:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863205.89 2024-11-06T03:20:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:08Z- waiting for 1730863209.89, time is 1730863207.91 2024-11-06T03:20:08Z- AHRS_M2 msg: 1730863208.30 epoch sec; seqNo:4298 2024-11-06T03:20:08Z- AHRS_M2 msg: 0.12 phi; -0.07 theta; 6.10 psi 2024-11-06T03:20:08Z- handleDepth()- Depth_Keller msg: 1730863208.31 epoch sec; seqNo:4575; depth 50.18 2024-11-06T03:20:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863208.31 epoch sec; seqNo:4575; 4061481.02 north; 594799.03 east; 36.69 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:20:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863205.89 2024-11-06T03:20:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:08Z- waiting for 1730863209.89, time is 1730863208.31 2024-11-06T03:20:08Z- AHRS_M2 msg: 1730863208.76 epoch sec; seqNo:4299 2024-11-06T03:20:08Z- AHRS_M2 msg: 0.11 phi; -0.07 theta; 6.10 psi 2024-11-06T03:20:08Z- handleDepth()- Depth_Keller msg: 1730863208.76 epoch sec; seqNo:4576; depth 50.20 2024-11-06T03:20:08Z- handleDVL() - DVL msg epochMillisec: 1730863208.79 2024-11-06T03:20:08Z- handleDvl() - RDI_Pathfinder msg: 1730863208.79 sec; seqNo:1509; #beams:4 2024-11-06T03:20:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.62 , 38.13 , 37.94 , 42.80 2024-11-06T03:20:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , -0.07 2024-11-06T03:20:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863208.82 epoch sec; seqNo:4576; 4061481.78 north; 594799.02 east; 36.69 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:20:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863205.89 2024-11-06T03:20:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:08Z- waiting for 1730863209.89, time is 1730863208.82 2024-11-06T03:20:09Z- AHRS_M2 msg: 1730863209.11 epoch sec; seqNo:4300 2024-11-06T03:20:09Z- AHRS_M2 msg: 0.11 phi; -0.07 theta; 6.10 psi 2024-11-06T03:20:09Z- handleDepth()- Depth_Keller msg: 1730863209.12 epoch sec; seqNo:4577; depth 50.23 2024-11-06T03:20:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863209.13 epoch sec; seqNo:4577; 4061481.78 north; 594799.02 east; 36.69 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:20:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863205.89 2024-11-06T03:20:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:09Z- waiting for 1730863209.89, time is 1730863209.13 2024-11-06T03:20:09Z- AHRS_M2 msg: 1730863209.51 epoch sec; seqNo:4301 2024-11-06T03:20:09Z- AHRS_M2 msg: 0.11 phi; -0.07 theta; 6.09 psi 2024-11-06T03:20:09Z- handleDepth()- Depth_Keller msg: 1730863209.51 epoch sec; seqNo:4578; depth 50.25 2024-11-06T03:20:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863209.52 epoch sec; seqNo:4578; 4061482.16 north; 594799.02 east; 36.69 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:20:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863205.89 2024-11-06T03:20:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:09Z- waiting for 1730863209.89, time is 1730863209.52 2024-11-06T03:20:09Z- AHRS_M2 msg: 1730863209.91 epoch sec; seqNo:4302 2024-11-06T03:20:09Z- AHRS_M2 msg: 0.11 phi; -0.06 theta; 6.09 psi 2024-11-06T03:20:09Z- handleDepth()- Depth_Keller msg: 1730863209.92 epoch sec; seqNo:4579; depth 50.26 2024-11-06T03:20:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863209.93 epoch sec; seqNo:4579; 4061482.54 north; 594799.01 east; 36.69 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:20:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863205.89 2024-11-06T03:20:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:10Z- waiting for 1730863209.89, time is 1730863209.93 2024-11-06T03:20:10Z- LcmTrn::updateTrn() >>>> pose time 1730863209.93, meas time 1730863208.79 <<<< 2024-11-06T03:20:10Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863209.93 2024-11-06T03:20:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863208.79 sec, ping # 0; waiting for INS data more recent than 1730863205.89... 2024-11-06T03:20:10Z- TerrainNav::motionUpdate() - currEstimate = (4061483, 594799.01, 50.26) 2024-11-06T03:20:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:20:10Z- TerrainNav:: SubcloudNIS is 0.66, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:20:10Z- TNavPF::Projecting Measurements 2024-11-06T03:20:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:20:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:20:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:20:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:20:10Z- TNavFilter::4 good beams. 2024-11-06T03:20:10Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:20:10Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:20:10Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:20:10Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:20:10Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:20:10Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:20:10Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:20:10Z- TerrainMapOctree: (vehN, vehE ) = (4061481.58, 594799.37). 2024-11-06T03:20:10Z- TerrainMapOctree: Min Distance = 236.78. 2024-11-06T03:20:10Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:20:10Z- Forcing Subcloud Comparison 2024-11-06T03:20:10Z- Weighting particles with subcloud comparison 2024-11-06T03:20:10Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:10Z- meanExpectedMeasDiff: 3.079275 delta_rms_squared: 1.409315 alpha: 0.986007 2024-11-06T03:20:10Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:10Z- meanExpectedMeasDiff: 0.691100 delta_rms_squared: 1.989015 alpha: 0.990045 2024-11-06T03:20:10Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:10Z- meanExpectedMeasDiff: 1.144559 delta_rms_squared: 1.485052 alpha: 0.986711 2024-11-06T03:20:10Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:10Z- meanExpectedMeasDiff: -0.800477 delta_rms_squared: 1.804332 alpha: 0.989037 2024-11-06T03:20:10Z- Subcloud NIS: 0.682617 2024-11-06T03:20:10Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:20:10Z- TNavPF:: sumWeights = 0.220907 2024-11-06T03:20:10Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:20:10Z- TNavPF::First Term of Map Covariance Matrix is 0.97 2024-11-06T03:20:10Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.03 2024-11-06T03:20:10Z- TerrainNav::Resampling particle filter... 2024-11-06T03:20:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863208.79 sec. 2024-11-06T03:20:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:20:10Z- variances([0], [2], and psi_berg) = 0.639938 0.830885 0.000000 2024-11-06T03:20:10Z- publishEstimatesDecoder() - at 1730863209.93 sec 2024-11-06T03:20:10Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:20:10Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061513.23 594782.13 50.67 2024-11-06T03:20:10Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061513.92 594782.07 50.47 lat/lon: 36.69 -121.94 2024-11-06T03:20:10Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.64 0.83 0.71 0.00 2024-11-06T03:20:10Z- AHRS_M2 msg: 1730863210.32 epoch sec; seqNo:4303 2024-11-06T03:20:10Z- AHRS_M2 msg: 0.11 phi; -0.06 theta; 6.09 psi 2024-11-06T03:20:10Z- handleDepth()- Depth_Keller msg: 1730863210.32 epoch sec; seqNo:4580; depth 50.29 2024-11-06T03:20:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863210.34 epoch sec; seqNo:4580; 4061482.92 north; 594799.01 east; 36.69 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:20:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863209.93 2024-11-06T03:20:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:10Z- waiting for 1730863213.93, time is 1730863210.34 2024-11-06T03:20:10Z- AHRS_M2 msg: 1730863210.76 epoch sec; seqNo:4304 2024-11-06T03:20:10Z- AHRS_M2 msg: 0.11 phi; -0.05 theta; 6.08 psi 2024-11-06T03:20:10Z- handleDepth()- Depth_Keller msg: 1730863210.77 epoch sec; seqNo:4581; depth 50.31 2024-11-06T03:20:10Z- handleDVL() - DVL msg epochMillisec: 1730863210.79 2024-11-06T03:20:10Z- handleDvl() - RDI_Pathfinder msg: 1730863210.79 sec; seqNo:1510; #beams:4 2024-11-06T03:20:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.06 , 37.76 , 37.76 , 41.87 2024-11-06T03:20:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.14 , -0.06 2024-11-06T03:20:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863210.80 epoch sec; seqNo:4581; 4061483.29 north; 594799.00 east; 36.69 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:20:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863209.93 2024-11-06T03:20:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:10Z- waiting for 1730863213.93, time is 1730863210.80 2024-11-06T03:20:11Z- handleDepth()- Depth_Keller msg: 1730863211.15 epoch sec; seqNo:4582; depth 50.35 2024-11-06T03:20:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863211.17 epoch sec; seqNo:4582; 4061483.67 north; 594799.00 east; 36.69 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:20:11Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:20:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863209.93 2024-11-06T03:20:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:11Z- waiting for 1730863213.93, time is 1730863211.17 2024-11-06T03:20:11Z- AHRS_M2 msg: 1730863211.53 epoch sec; seqNo:4305 2024-11-06T03:20:11Z- AHRS_M2 msg: 0.11 phi; -0.05 theta; 6.08 psi 2024-11-06T03:20:11Z- handleDepth()- Depth_Keller msg: 1730863211.53 epoch sec; seqNo:4583; depth 50.36 2024-11-06T03:20:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863211.55 epoch sec; seqNo:4583; 4061484.05 north; 594799.00 east; 36.69 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:20:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863209.93 2024-11-06T03:20:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:11Z- waiting for 1730863213.93, time is 1730863211.55 2024-11-06T03:20:11Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:20:11Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:20:11Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:20:11Z- AHRS_M2 msg: 1730863211.92 epoch sec; seqNo:4306 2024-11-06T03:20:11Z- AHRS_M2 msg: 0.11 phi; -0.05 theta; 6.08 psi 2024-11-06T03:20:11Z- handleDepth()- Depth_Keller msg: 1730863211.95 epoch sec; seqNo:4584; depth 50.39 2024-11-06T03:20:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863211.97 epoch sec; seqNo:4584; 4061484.42 north; 594797.77 east; 36.69 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:20:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863209.93 2024-11-06T03:20:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:12Z- waiting for 1730863213.93, time is 1730863211.97 2024-11-06T03:20:12Z- AHRS_M2 msg: 1730863212.34 epoch sec; seqNo:4307 2024-11-06T03:20:12Z- AHRS_M2 msg: 0.11 phi; -0.04 theta; 6.07 psi 2024-11-06T03:20:12Z- handleDepth()- Depth_Keller msg: 1730863212.35 epoch sec; seqNo:4585; depth 50.43 2024-11-06T03:20:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863212.36 epoch sec; seqNo:4585; 4061484.80 north; 594797.77 east; 36.69 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:20:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863209.93 2024-11-06T03:20:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:12Z- waiting for 1730863213.93, time is 1730863212.36 2024-11-06T03:20:12Z- AHRS_M2 msg: 1730863212.77 epoch sec; seqNo:4308 2024-11-06T03:20:12Z- AHRS_M2 msg: 0.10 phi; -0.04 theta; 6.06 psi 2024-11-06T03:20:12Z- handleDepth()- Depth_Keller msg: 1730863212.77 epoch sec; seqNo:4586; depth 50.44 2024-11-06T03:20:12Z- handleDVL() - DVL msg epochMillisec: 1730863212.80 2024-11-06T03:20:12Z- handleDvl() - RDI_Pathfinder msg: 1730863212.80 sec; seqNo:1511; #beams:4 2024-11-06T03:20:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.49 , 37.76 , 37.76 , 41.12 2024-11-06T03:20:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.15 , -0.01 2024-11-06T03:20:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863212.81 epoch sec; seqNo:4586; 4061485.18 north; 594797.76 east; 36.69 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:20:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863209.93 2024-11-06T03:20:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:12Z- waiting for 1730863213.93, time is 1730863212.81 2024-11-06T03:20:13Z- AHRS_M2 msg: 1730863213.14 epoch sec; seqNo:4309 2024-11-06T03:20:13Z- AHRS_M2 msg: 0.10 phi; -0.04 theta; 6.06 psi 2024-11-06T03:20:13Z- handleDepth()- Depth_Keller msg: 1730863213.16 epoch sec; seqNo:4587; depth 50.46 2024-11-06T03:20:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863213.18 epoch sec; seqNo:4587; 4061485.55 north; 594797.76 east; 36.69 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:20:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863209.93 2024-11-06T03:20:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:13Z- waiting for 1730863213.93, time is 1730863213.18 2024-11-06T03:20:13Z- handleDepth()- Depth_Keller msg: 1730863213.57 epoch sec; seqNo:4588; depth 50.50 2024-11-06T03:20:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863213.58 epoch sec; seqNo:4588; 4061485.93 north; 594797.75 east; 36.69 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:20:13Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:20:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863209.93 2024-11-06T03:20:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:13Z- waiting for 1730863213.93, time is 1730863213.58 2024-11-06T03:20:13Z- AHRS_M2 msg: 1730863213.95 epoch sec; seqNo:4310 2024-11-06T03:20:13Z- AHRS_M2 msg: 0.10 phi; -0.03 theta; 6.06 psi 2024-11-06T03:20:13Z- handleDepth()- Depth_Keller msg: 1730863213.96 epoch sec; seqNo:4589; depth 50.53 2024-11-06T03:20:13Z- handleDVL() - DVL msg epochMillisec: 1730863213.96 2024-11-06T03:20:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:20:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:20:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:20:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:20:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:20:13Z- handleDvl() - RDI_Pathfinder msg: 1730863213.96 sec; seqNo:1512; #beams:0 2024-11-06T03:20:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:20:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.15 , -0.01 2024-11-06T03:20:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863213.97 epoch sec; seqNo:4589; 4061486.31 north; 594797.75 east; 36.69 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:20:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863209.93 2024-11-06T03:20:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:14Z- waiting for 1730863213.93, time is 1730863213.97 2024-11-06T03:20:14Z- LcmTrn::updateTrn() >>>> pose time 1730863213.97, meas time 1730863213.96 <<<< 2024-11-06T03:20:14Z- LcmTrn::updateTrn() equating pose 1730863213.96 and meas times 1730863213.96 2024-11-06T03:20:14Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863213.96 2024-11-06T03:20:14Z- TerrainNav::motionUpdate() - currEstimate = (4061486, 594797.75, 50.53) 2024-11-06T03:20:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:20:14Z- TerrainNav:: SubcloudNIS is 0.68, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:20:14Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:20:14Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:20:14Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:20:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:20:14Z- variances([0], [2], and psi_berg) = 0.642537 0.833723 0.000000 2024-11-06T03:20:14Z- publishEstimatesDecoder() - at 1730863213.96 sec 2024-11-06T03:20:14Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:20:14Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061516.96 594780.88 51.02 2024-11-06T03:20:14Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061517.69 594780.81 50.73 lat/lon: 36.69 -121.94 2024-11-06T03:20:14Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.64 0.83 0.95 0.00 2024-11-06T03:20:14Z- AHRS_M2 msg: 1730863214.37 epoch sec; seqNo:4311 2024-11-06T03:20:14Z- AHRS_M2 msg: 0.10 phi; -0.03 theta; 6.05 psi 2024-11-06T03:20:14Z- handleDepth()- Depth_Keller msg: 1730863214.38 epoch sec; seqNo:4590; depth 50.55 2024-11-06T03:20:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863214.39 epoch sec; seqNo:4590; 4061486.69 north; 594797.75 east; 36.69 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:20:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863213.96 2024-11-06T03:20:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:14Z- waiting for 1730863217.96, time is 1730863214.39 2024-11-06T03:20:14Z- AHRS_M2 msg: 1730863214.77 epoch sec; seqNo:4312 2024-11-06T03:20:14Z- AHRS_M2 msg: 0.11 phi; -0.03 theta; 6.05 psi 2024-11-06T03:20:14Z- handleDepth()- Depth_Keller msg: 1730863214.78 epoch sec; seqNo:4591; depth 50.57 2024-11-06T03:20:14Z- handleDVL() - DVL msg epochMillisec: 1730863214.81 2024-11-06T03:20:14Z- handleDvl() - RDI_Pathfinder msg: 1730863214.81 sec; seqNo:1513; #beams:4 2024-11-06T03:20:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.68 , 37.57 , 37.94 , 40.75 2024-11-06T03:20:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.12 , 0.04 2024-11-06T03:20:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863214.82 epoch sec; seqNo:4591; 4061487.07 north; 594797.74 east; 36.69 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T03:20:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863213.96 2024-11-06T03:20:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:14Z- waiting for 1730863217.96, time is 1730863214.82 2024-11-06T03:20:15Z- AHRS_M2 msg: 1730863215.16 epoch sec; seqNo:4313 2024-11-06T03:20:15Z- AHRS_M2 msg: 0.11 phi; -0.03 theta; 6.05 psi 2024-11-06T03:20:15Z- handleDepth()- Depth_Keller msg: 1730863215.17 epoch sec; seqNo:4592; depth 50.60 2024-11-06T03:20:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863215.18 epoch sec; seqNo:4592; 4061487.45 north; 594797.74 east; 36.69 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:20:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863213.96 2024-11-06T03:20:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:15Z- waiting for 1730863217.96, time is 1730863215.18 2024-11-06T03:20:15Z- AHRS_M2 msg: 1730863215.57 epoch sec; seqNo:4314 2024-11-06T03:20:15Z- AHRS_M2 msg: 0.11 phi; -0.02 theta; 6.05 psi 2024-11-06T03:20:15Z- handleDepth()- Depth_Keller msg: 1730863215.57 epoch sec; seqNo:4593; depth 50.64 2024-11-06T03:20:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863215.58 epoch sec; seqNo:4593; 4061487.81 north; 594796.51 east; 36.69 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:20:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863213.96 2024-11-06T03:20:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:15Z- waiting for 1730863217.96, time is 1730863215.58 2024-11-06T03:20:15Z- AHRS_M2 msg: 1730863215.97 epoch sec; seqNo:4315 2024-11-06T03:20:15Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 6.05 psi 2024-11-06T03:20:15Z- handleDepth()- Depth_Keller msg: 1730863215.97 epoch sec; seqNo:4594; depth 50.66 2024-11-06T03:20:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863215.99 epoch sec; seqNo:4594; 4061488.19 north; 594796.51 east; 36.69 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T03:20:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863213.96 2024-11-06T03:20:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:16Z- waiting for 1730863217.96, time is 1730863215.99 2024-11-06T03:20:16Z- AHRS_M2 msg: 1730863216.38 epoch sec; seqNo:4316 2024-11-06T03:20:16Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 6.05 psi 2024-11-06T03:20:16Z- handleDepth()- Depth_Keller msg: 1730863216.38 epoch sec; seqNo:4595; depth 50.69 2024-11-06T03:20:16Z- handleDVL() - DVL msg epochMillisec: 1730863216.38 2024-11-06T03:20:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:20:16Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:20:16Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:20:16Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:20:16Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:20:16Z- handleDvl() - RDI_Pathfinder msg: 1730863216.38 sec; seqNo:1514; #beams:0 2024-11-06T03:20:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:20:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.12 , 0.04 2024-11-06T03:20:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863216.39 epoch sec; seqNo:4595; 4061488.57 north; 594796.50 east; 36.69 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:20:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863213.96 2024-11-06T03:20:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:16Z- waiting for 1730863217.96, time is 1730863216.39 2024-11-06T03:20:16Z- AHRS_M2 msg: 1730863216.78 epoch sec; seqNo:4317 2024-11-06T03:20:16Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 6.05 psi 2024-11-06T03:20:16Z- handleDepth()- Depth_Keller msg: 1730863216.79 epoch sec; seqNo:4596; depth 50.70 2024-11-06T03:20:16Z- handleDVL() - DVL msg epochMillisec: 1730863216.83 2024-11-06T03:20:16Z- handleDvl() - RDI_Pathfinder msg: 1730863216.83 sec; seqNo:1515; #beams:4 2024-11-06T03:20:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 37.57 , 38.32 , 40.56 2024-11-06T03:20:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , 0.03 2024-11-06T03:20:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863216.85 epoch sec; seqNo:4596; 4061488.95 north; 594796.50 east; 36.69 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T03:20:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863213.96 2024-11-06T03:20:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:16Z- waiting for 1730863217.96, time is 1730863216.85 2024-11-06T03:20:17Z- AHRS_M2 msg: 1730863217.19 epoch sec; seqNo:4318 2024-11-06T03:20:17Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 6.05 psi 2024-11-06T03:20:17Z- handleDepth()- Depth_Keller msg: 1730863217.20 epoch sec; seqNo:4597; depth 50.72 2024-11-06T03:20:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863217.21 epoch sec; seqNo:4597; 4061489.33 north; 594796.50 east; 36.69 lat; -121.94 lon; 50.72 depth; utmZone: 10 2024-11-06T03:20:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863213.96 2024-11-06T03:20:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:17Z- waiting for 1730863217.96, time is 1730863217.21 2024-11-06T03:20:17Z- AHRS_M2 msg: 1730863217.59 epoch sec; seqNo:4319 2024-11-06T03:20:17Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 6.05 psi 2024-11-06T03:20:17Z- handleDepth()- Depth_Keller msg: 1730863217.59 epoch sec; seqNo:4598; depth 50.72 2024-11-06T03:20:17Z- handleDVL() - DVL msg epochMillisec: 1730863217.60 2024-11-06T03:20:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:20:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:20:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:20:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:20:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:20:17Z- handleDvl() - RDI_Pathfinder msg: 1730863217.60 sec; seqNo:1516; #beams:0 2024-11-06T03:20:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:20:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , 0.03 2024-11-06T03:20:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863217.61 epoch sec; seqNo:4598; 4061489.71 north; 594796.49 east; 36.69 lat; -121.94 lon; 50.72 depth; utmZone: 10 2024-11-06T03:20:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863213.96 2024-11-06T03:20:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:17Z- waiting for 1730863217.96, time is 1730863217.61 2024-11-06T03:20:17Z- AHRS_M2 msg: 1730863217.99 epoch sec; seqNo:4320 2024-11-06T03:20:17Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 6.04 psi 2024-11-06T03:20:18Z- handleDepth()- Depth_Keller msg: 1730863218.00 epoch sec; seqNo:4599; depth 50.75 2024-11-06T03:20:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863218.02 epoch sec; seqNo:4599; 4061490.09 north; 594796.49 east; 36.69 lat; -121.94 lon; 50.75 depth; utmZone: 10 2024-11-06T03:20:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863213.96 2024-11-06T03:20:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:18Z- waiting for 1730863217.96, time is 1730863218.02 2024-11-06T03:20:18Z- LcmTrn::updateTrn() >>>> pose time 1730863218.02, meas time 1730863217.60 <<<< 2024-11-06T03:20:18Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863218.02 2024-11-06T03:20:18Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:20:18Z- TerrainNav::motionUpdate() - currEstimate = (4061490, 594796.49, 50.75) 2024-11-06T03:20:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:20:18Z- TerrainNav:: SubcloudNIS is 0.68, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:20:18Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:20:18Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:20:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:20:18Z- variances([0], [2], and psi_berg) = 0.645911 0.834706 0.000000 2024-11-06T03:20:18Z- publishEstimatesDecoder() - at 1730863218.02 sec 2024-11-06T03:20:18Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:20:18Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061520.70 594779.61 51.31 2024-11-06T03:20:18Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061521.47 594779.55 50.95 lat/lon: 36.69 -121.94 2024-11-06T03:20:18Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.65 0.83 1.21 0.00 2024-11-06T03:20:18Z- AHRS_M2 msg: 1730863218.39 epoch sec; seqNo:4321 2024-11-06T03:20:18Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 6.04 psi 2024-11-06T03:20:18Z- handleDepth()- Depth_Keller msg: 1730863218.40 epoch sec; seqNo:4600; depth 50.76 2024-11-06T03:20:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863218.41 epoch sec; seqNo:4600; 4061490.09 north; 594796.49 east; 36.69 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T03:20:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863218.02 2024-11-06T03:20:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:18Z- waiting for 1730863222.02, time is 1730863218.41 2024-11-06T03:20:18Z- AHRS_M2 msg: 1730863218.80 epoch sec; seqNo:4322 2024-11-06T03:20:18Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 6.04 psi 2024-11-06T03:20:18Z- handleDepth()- Depth_Keller msg: 1730863218.81 epoch sec; seqNo:4601; depth 50.76 2024-11-06T03:20:18Z- handleDVL() - DVL msg epochMillisec: 1730863218.83 2024-11-06T03:20:18Z- handleDvl() - RDI_Pathfinder msg: 1730863218.83 sec; seqNo:1517; #beams:4 2024-11-06T03:20:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 37.20 , 38.50 , 40.19 2024-11-06T03:20:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , -0.00 2024-11-06T03:20:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863218.84 epoch sec; seqNo:4601; 4061490.47 north; 594796.48 east; 36.69 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T03:20:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863218.02 2024-11-06T03:20:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:18Z- waiting for 1730863222.02, time is 1730863218.84 2024-11-06T03:20:19Z- AHRS_M2 msg: 1730863219.20 epoch sec; seqNo:4323 2024-11-06T03:20:19Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 6.04 psi 2024-11-06T03:20:19Z- handleDepth()- Depth_Keller msg: 1730863219.21 epoch sec; seqNo:4602; depth 50.76 2024-11-06T03:20:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863219.23 epoch sec; seqNo:4602; 4061490.84 north; 594796.48 east; 36.69 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T03:20:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863218.02 2024-11-06T03:20:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:19Z- waiting for 1730863222.02, time is 1730863219.23 2024-11-06T03:20:19Z- AHRS_M2 msg: 1730863219.61 epoch sec; seqNo:4324 2024-11-06T03:20:19Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 6.04 psi 2024-11-06T03:20:19Z- handleDepth()- Depth_Keller msg: 1730863219.61 epoch sec; seqNo:4603; depth 50.77 2024-11-06T03:20:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863219.63 epoch sec; seqNo:4603; 4061491.21 north; 594795.25 east; 36.69 lat; -121.94 lon; 50.77 depth; utmZone: 10 2024-11-06T03:20:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863218.02 2024-11-06T03:20:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:19Z- waiting for 1730863222.02, time is 1730863219.63 2024-11-06T03:20:20Z- AHRS_M2 msg: 1730863220.02 epoch sec; seqNo:4325 2024-11-06T03:20:20Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 6.04 psi 2024-11-06T03:20:20Z- handleDepth()- Depth_Keller msg: 1730863220.02 epoch sec; seqNo:4604; depth 50.78 2024-11-06T03:20:20Z- handleDVL() - DVL msg epochMillisec: 1730863220.02 2024-11-06T03:20:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:20:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:20:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:20:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:20:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:20:20Z- handleDvl() - RDI_Pathfinder msg: 1730863220.02 sec; seqNo:1518; #beams:0 2024-11-06T03:20:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:20:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , -0.00 2024-11-06T03:20:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863220.03 epoch sec; seqNo:4604; 4061491.59 north; 594795.25 east; 36.69 lat; -121.94 lon; 50.78 depth; utmZone: 10 2024-11-06T03:20:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863218.02 2024-11-06T03:20:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:20Z- waiting for 1730863222.02, time is 1730863220.03 2024-11-06T03:20:20Z- AHRS_M2 msg: 1730863220.42 epoch sec; seqNo:4326 2024-11-06T03:20:20Z- AHRS_M2 msg: 0.11 phi; -0.01 theta; 6.04 psi 2024-11-06T03:20:20Z- handleDepth()- Depth_Keller msg: 1730863220.42 epoch sec; seqNo:4605; depth 50.78 2024-11-06T03:20:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863220.43 epoch sec; seqNo:4605; 4061491.97 north; 594795.25 east; 36.69 lat; -121.94 lon; 50.78 depth; utmZone: 10 2024-11-06T03:20:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863218.02 2024-11-06T03:20:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:20Z- waiting for 1730863222.02, time is 1730863220.43 2024-11-06T03:20:20Z- AHRS_M2 msg: 1730863220.82 epoch sec; seqNo:4327 2024-11-06T03:20:20Z- AHRS_M2 msg: 0.12 phi; -0.00 theta; 6.04 psi 2024-11-06T03:20:20Z- handleDepth()- Depth_Keller msg: 1730863220.83 epoch sec; seqNo:4606; depth 50.78 2024-11-06T03:20:20Z- handleDVL() - DVL msg epochMillisec: 1730863220.85 2024-11-06T03:20:20Z- handleDvl() - RDI_Pathfinder msg: 1730863220.85 sec; seqNo:1519; #beams:4 2024-11-06T03:20:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 37.20 , 38.50 , 40.00 2024-11-06T03:20:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , -0.04 2024-11-06T03:20:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863220.87 epoch sec; seqNo:4606; 4061492.35 north; 594795.24 east; 36.69 lat; -121.94 lon; 50.78 depth; utmZone: 10 2024-11-06T03:20:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863218.02 2024-11-06T03:20:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:20Z- waiting for 1730863222.02, time is 1730863220.87 2024-11-06T03:20:21Z- AHRS_M2 msg: 1730863221.22 epoch sec; seqNo:4328 2024-11-06T03:20:21Z- AHRS_M2 msg: 0.12 phi; -0.00 theta; 6.04 psi 2024-11-06T03:20:21Z- handleDepth()- Depth_Keller msg: 1730863221.23 epoch sec; seqNo:4607; depth 50.76 2024-11-06T03:20:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863221.24 epoch sec; seqNo:4607; 4061492.73 north; 594795.24 east; 36.69 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T03:20:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863218.02 2024-11-06T03:20:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:21Z- waiting for 1730863222.02, time is 1730863221.24 2024-11-06T03:20:21Z- AHRS_M2 msg: 1730863221.63 epoch sec; seqNo:4329 2024-11-06T03:20:21Z- AHRS_M2 msg: 0.12 phi; -0.00 theta; 6.04 psi 2024-11-06T03:20:21Z- handleDepth()- Depth_Keller msg: 1730863221.64 epoch sec; seqNo:4608; depth 50.76 2024-11-06T03:20:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863221.65 epoch sec; seqNo:4608; 4061493.10 north; 594795.23 east; 36.69 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T03:20:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863218.02 2024-11-06T03:20:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:21Z- waiting for 1730863222.02, time is 1730863221.65 2024-11-06T03:20:22Z- AHRS_M2 msg: 1730863222.03 epoch sec; seqNo:4330 2024-11-06T03:20:22Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 6.03 psi 2024-11-06T03:20:22Z- handleDepth()- Depth_Keller msg: 1730863222.04 epoch sec; seqNo:4609; depth 50.77 2024-11-06T03:20:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863222.05 epoch sec; seqNo:4609; 4061493.48 north; 594795.23 east; 36.69 lat; -121.94 lon; 50.77 depth; utmZone: 10 2024-11-06T03:20:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863218.02 2024-11-06T03:20:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:22Z- waiting for 1730863222.02, time is 1730863222.05 2024-11-06T03:20:22Z- LcmTrn::updateTrn() >>>> pose time 1730863222.05, meas time 1730863220.85 <<<< 2024-11-06T03:20:22Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863222.05 2024-11-06T03:20:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863220.85 sec, ping # 0; waiting for INS data more recent than 1730863218.02... 2024-11-06T03:20:22Z- TerrainNav::motionUpdate() - currEstimate = (4061493, 594795.23, 50.77) 2024-11-06T03:20:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:20:22Z- TerrainNav:: SubcloudNIS is 0.68, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:20:22Z- TNavPF::Projecting Measurements 2024-11-06T03:20:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:20:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:20:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:20:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:20:22Z- TNavFilter::4 good beams. 2024-11-06T03:20:22Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:20:22Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:20:22Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:20:22Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:20:22Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:20:22Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:20:22Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:20:22Z- TerrainMapOctree: (vehN, vehE ) = (4061492.47, 594795.60). 2024-11-06T03:20:22Z- TerrainMapOctree: Min Distance = 246.72. 2024-11-06T03:20:22Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:20:22Z- Forcing Subcloud Comparison 2024-11-06T03:20:22Z- Weighting particles with subcloud comparison 2024-11-06T03:20:22Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:22Z- meanExpectedMeasDiff: 2.402680 delta_rms_squared: 2.510466 alpha: 0.992096 2024-11-06T03:20:22Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:22Z- meanExpectedMeasDiff: 1.499664 delta_rms_squared: 1.706592 alpha: 0.988416 2024-11-06T03:20:22Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:22Z- meanExpectedMeasDiff: 2.133730 delta_rms_squared: 2.312567 alpha: 0.991426 2024-11-06T03:20:22Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:22Z- meanExpectedMeasDiff: -0.877980 delta_rms_squared: 2.599919 alpha: 0.992366 2024-11-06T03:20:22Z- Subcloud NIS: 0.692681 2024-11-06T03:20:22Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:20:22Z- TNavPF:: sumWeights = 0.195202 2024-11-06T03:20:22Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:20:22Z- TNavPF::First Term of Map Covariance Matrix is 0.42 2024-11-06T03:20:22Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T03:20:22Z- TerrainNav::Resampling particle filter... 2024-11-06T03:20:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863220.85 sec. 2024-11-06T03:20:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:20:22Z- variances([0], [2], and psi_berg) = 0.580197 0.758265 0.000000 2024-11-06T03:20:22Z- publishEstimatesDecoder() - at 1730863222.05 sec 2024-11-06T03:20:22Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:20:22Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061524.07 594778.32 51.13 2024-11-06T03:20:22Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061524.83 594778.18 50.23 lat/lon: 36.69 -121.94 2024-11-06T03:20:22Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.58 0.76 0.77 0.00 2024-11-06T03:20:22Z- AHRS_M2 msg: 1730863222.43 epoch sec; seqNo:4331 2024-11-06T03:20:22Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 6.03 psi 2024-11-06T03:20:22Z- handleDepth()- Depth_Keller msg: 1730863222.45 epoch sec; seqNo:4610; depth 50.75 2024-11-06T03:20:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863222.46 epoch sec; seqNo:4610; 4061493.86 north; 594795.23 east; 36.69 lat; -121.94 lon; 50.75 depth; utmZone: 10 2024-11-06T03:20:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863222.05 2024-11-06T03:20:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:22Z- waiting for 1730863226.05, time is 1730863222.46 2024-11-06T03:20:22Z- AHRS_M2 msg: 1730863222.84 epoch sec; seqNo:4332 2024-11-06T03:20:22Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 6.03 psi 2024-11-06T03:20:22Z- handleDepth()- Depth_Keller msg: 1730863222.85 epoch sec; seqNo:4611; depth 50.76 2024-11-06T03:20:22Z- handleDVL() - DVL msg epochMillisec: 1730863222.88 2024-11-06T03:20:22Z- handleDvl() - RDI_Pathfinder msg: 1730863222.88 sec; seqNo:1520; #beams:4 2024-11-06T03:20:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.49 , 37.01 , 38.13 , 39.63 2024-11-06T03:20:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.07 , -0.06 2024-11-06T03:20:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863222.89 epoch sec; seqNo:4611; 4061494.24 north; 594795.22 east; 36.69 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T03:20:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863222.05 2024-11-06T03:20:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:22Z- waiting for 1730863226.05, time is 1730863222.89 2024-11-06T03:20:23Z- AHRS_M2 msg: 1730863223.24 epoch sec; seqNo:4333 2024-11-06T03:20:23Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 6.03 psi 2024-11-06T03:20:23Z- handleDepth()- Depth_Keller msg: 1730863223.25 epoch sec; seqNo:4612; depth 50.73 2024-11-06T03:20:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863223.27 epoch sec; seqNo:4612; 4061494.62 north; 594795.22 east; 36.69 lat; -121.94 lon; 50.73 depth; utmZone: 10 2024-11-06T03:20:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863222.05 2024-11-06T03:20:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:23Z- waiting for 1730863226.05, time is 1730863223.27 2024-11-06T03:20:23Z- AHRS_M2 msg: 1730863223.65 epoch sec; seqNo:4334 2024-11-06T03:20:23Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.03 psi 2024-11-06T03:20:23Z- handleDepth()- Depth_Keller msg: 1730863223.66 epoch sec; seqNo:4613; depth 50.73 2024-11-06T03:20:23Z- handleDVL() - DVL msg epochMillisec: 1730863223.66 2024-11-06T03:20:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:20:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:20:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:20:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:20:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:20:23Z- handleDvl() - RDI_Pathfinder msg: 1730863223.66 sec; seqNo:1521; #beams:0 2024-11-06T03:20:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:20:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.07 , -0.06 2024-11-06T03:20:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863223.67 epoch sec; seqNo:4613; 4061494.62 north; 594795.22 east; 36.69 lat; -121.94 lon; 50.73 depth; utmZone: 10 2024-11-06T03:20:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863222.05 2024-11-06T03:20:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:23Z- waiting for 1730863226.05, time is 1730863223.67 2024-11-06T03:20:24Z- AHRS_M2 msg: 1730863224.05 epoch sec; seqNo:4335 2024-11-06T03:20:24Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.03 psi 2024-11-06T03:20:24Z- handleDepth()- Depth_Keller msg: 1730863224.06 epoch sec; seqNo:4614; depth 50.71 2024-11-06T03:20:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863224.07 epoch sec; seqNo:4614; 4061495.00 north; 594795.21 east; 36.69 lat; -121.94 lon; 50.71 depth; utmZone: 10 2024-11-06T03:20:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863222.05 2024-11-06T03:20:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:24Z- waiting for 1730863226.05, time is 1730863224.07 2024-11-06T03:20:24Z- AHRS_M2 msg: 1730863224.46 epoch sec; seqNo:4336 2024-11-06T03:20:24Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.02 psi 2024-11-06T03:20:24Z- handleDepth()- Depth_Keller msg: 1730863224.47 epoch sec; seqNo:4615; depth 50.72 2024-11-06T03:20:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863224.47 epoch sec; seqNo:4615; 4061495.36 north; 594793.99 east; 36.69 lat; -121.94 lon; 50.72 depth; utmZone: 10 2024-11-06T03:20:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863222.05 2024-11-06T03:20:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:24Z- waiting for 1730863226.05, time is 1730863224.47 2024-11-06T03:20:24Z- AHRS_M2 msg: 1730863224.87 epoch sec; seqNo:4337 2024-11-06T03:20:24Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.02 psi 2024-11-06T03:20:24Z- handleDepth()- Depth_Keller msg: 1730863224.88 epoch sec; seqNo:4616; depth 50.71 2024-11-06T03:20:24Z- handleDVL() - DVL msg epochMillisec: 1730863224.90 2024-11-06T03:20:24Z- handleDvl() - RDI_Pathfinder msg: 1730863224.90 sec; seqNo:1522; #beams:4 2024-11-06T03:20:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.49 , 36.63 , 38.13 , 39.25 2024-11-06T03:20:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.12 , -0.05 2024-11-06T03:20:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863224.93 epoch sec; seqNo:4616; 4061495.74 north; 594793.98 east; 36.69 lat; -121.94 lon; 50.71 depth; utmZone: 10 2024-11-06T03:20:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863222.05 2024-11-06T03:20:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:25Z- waiting for 1730863226.05, time is 1730863224.93 2024-11-06T03:20:25Z- AHRS_M2 msg: 1730863225.27 epoch sec; seqNo:4338 2024-11-06T03:20:25Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.02 psi 2024-11-06T03:20:25Z- handleDepth()- Depth_Keller msg: 1730863225.27 epoch sec; seqNo:4617; depth 50.71 2024-11-06T03:20:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863225.30 epoch sec; seqNo:4617; 4061496.12 north; 594793.98 east; 36.69 lat; -121.94 lon; 50.71 depth; utmZone: 10 2024-11-06T03:20:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863222.05 2024-11-06T03:20:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:25Z- waiting for 1730863226.05, time is 1730863225.30 2024-11-06T03:20:25Z- handleDepth()- Depth_Keller msg: 1730863225.68 epoch sec; seqNo:4618; depth 50.70 2024-11-06T03:20:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863225.70 epoch sec; seqNo:4618; 4061496.50 north; 594793.98 east; 36.69 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T03:20:25Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:20:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863222.05 2024-11-06T03:20:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:25Z- waiting for 1730863226.05, time is 1730863225.70 2024-11-06T03:20:26Z- AHRS_M2 msg: 1730863226.07 epoch sec; seqNo:4339 2024-11-06T03:20:26Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:26Z- handleDepth()- Depth_Keller msg: 1730863226.08 epoch sec; seqNo:4619; depth 50.69 2024-11-06T03:20:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863226.09 epoch sec; seqNo:4619; 4061496.88 north; 594793.97 east; 36.69 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:20:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863222.05 2024-11-06T03:20:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:26Z- waiting for 1730863226.05, time is 1730863226.09 2024-11-06T03:20:26Z- LcmTrn::updateTrn() >>>> pose time 1730863226.09, meas time 1730863224.90 <<<< 2024-11-06T03:20:26Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863226.09 2024-11-06T03:20:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863224.90 sec, ping # 0; waiting for INS data more recent than 1730863222.05... 2024-11-06T03:20:26Z- TerrainNav::motionUpdate() - currEstimate = (4061497, 594793.97, 50.69) 2024-11-06T03:20:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:20:26Z- TerrainNav:: SubcloudNIS is 0.69, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:20:26Z- TNavPF::Projecting Measurements 2024-11-06T03:20:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:20:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:20:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:20:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:20:26Z- TNavFilter::4 good beams. 2024-11-06T03:20:26Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:20:26Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:20:26Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:20:26Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:20:26Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:20:26Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:20:26Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:20:26Z- TerrainMapOctree: (vehN, vehE ) = (4061495.88, 594794.34). 2024-11-06T03:20:26Z- TerrainMapOctree: Min Distance = 249.84. 2024-11-06T03:20:26Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:20:26Z- Forcing Subcloud Comparison 2024-11-06T03:20:26Z- Weighting particles with subcloud comparison 2024-11-06T03:20:26Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:26Z- meanExpectedMeasDiff: 2.081046 delta_rms_squared: 1.573520 alpha: 0.987449 2024-11-06T03:20:26Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:26Z- meanExpectedMeasDiff: 0.104955 delta_rms_squared: 1.192655 alpha: 0.983507 2024-11-06T03:20:26Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:26Z- meanExpectedMeasDiff: 0.195945 delta_rms_squared: 1.570713 alpha: 0.987427 2024-11-06T03:20:26Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:26Z- meanExpectedMeasDiff: -1.855635 delta_rms_squared: 1.483198 alpha: 0.986695 2024-11-06T03:20:26Z- Subcloud NIS: 0.701072 2024-11-06T03:20:26Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:20:26Z- TNavPF:: sumWeights = 0.264563 2024-11-06T03:20:26Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:20:26Z- TNavPF::First Term of Map Covariance Matrix is 0.75 2024-11-06T03:20:26Z- TNavPF::Calculated NIS Value : 0.14 numBeams normalized NIS: 0.04 2024-11-06T03:20:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863224.90 sec. 2024-11-06T03:20:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:20:26Z- variances([0], [2], and psi_berg) = 0.564264 0.769121 0.000000 2024-11-06T03:20:26Z- publishEstimatesDecoder() - at 1730863226.09 sec 2024-11-06T03:20:26Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:20:26Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061527.73 594775.95 49.25 2024-11-06T03:20:26Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061528.25 594776.90 50.08 lat/lon: 36.69 -121.94 2024-11-06T03:20:26Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.56 0.77 0.68 0.00 2024-11-06T03:20:26Z- handleDepth()- Depth_Keller msg: 1730863226.50 epoch sec; seqNo:4620; depth 50.69 2024-11-06T03:20:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863226.51 epoch sec; seqNo:4620; 4061497.26 north; 594793.97 east; 36.69 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:20:26Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:20:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863226.09 2024-11-06T03:20:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:26Z- waiting for 1730863230.09, time is 1730863226.51 2024-11-06T03:20:26Z- AHRS_M2 msg: 1730863226.88 epoch sec; seqNo:4340 2024-11-06T03:20:26Z- AHRS_M2 msg: 0.11 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:26Z- handleDepth()- Depth_Keller msg: 1730863226.89 epoch sec; seqNo:4621; depth 50.68 2024-11-06T03:20:26Z- handleDVL() - DVL msg epochMillisec: 1730863226.92 2024-11-06T03:20:26Z- handleDvl() - RDI_Pathfinder msg: 1730863226.92 sec; seqNo:1523; #beams:4 2024-11-06T03:20:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.49 , 36.45 , 37.94 , 39.25 2024-11-06T03:20:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.15 , -0.02 2024-11-06T03:20:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863226.94 epoch sec; seqNo:4621; 4061497.64 north; 594793.96 east; 36.69 lat; -121.94 lon; 50.68 depth; utmZone: 10 2024-11-06T03:20:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863226.09 2024-11-06T03:20:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:27Z- waiting for 1730863230.09, time is 1730863226.94 2024-11-06T03:20:27Z- AHRS_M2 msg: 1730863227.29 epoch sec; seqNo:4341 2024-11-06T03:20:27Z- AHRS_M2 msg: 0.11 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:27Z- handleDepth()- Depth_Keller msg: 1730863227.30 epoch sec; seqNo:4622; depth 50.68 2024-11-06T03:20:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863227.33 epoch sec; seqNo:4622; 4061498.02 north; 594793.96 east; 36.69 lat; -121.94 lon; 50.68 depth; utmZone: 10 2024-11-06T03:20:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863226.09 2024-11-06T03:20:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:27Z- waiting for 1730863230.09, time is 1730863227.33 2024-11-06T03:20:27Z- AHRS_M2 msg: 1730863227.69 epoch sec; seqNo:4342 2024-11-06T03:20:27Z- AHRS_M2 msg: 0.11 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:27Z- handleDepth()- Depth_Keller msg: 1730863227.69 epoch sec; seqNo:4623; depth 50.68 2024-11-06T03:20:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863227.70 epoch sec; seqNo:4623; 4061498.38 north; 594792.73 east; 36.69 lat; -121.94 lon; 50.68 depth; utmZone: 10 2024-11-06T03:20:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863226.09 2024-11-06T03:20:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:27Z- waiting for 1730863230.09, time is 1730863227.70 2024-11-06T03:20:28Z- AHRS_M2 msg: 1730863228.09 epoch sec; seqNo:4343 2024-11-06T03:20:28Z- AHRS_M2 msg: 0.11 phi; 0.01 theta; 6.00 psi 2024-11-06T03:20:28Z- handleDepth()- Depth_Keller msg: 1730863228.10 epoch sec; seqNo:4624; depth 50.69 2024-11-06T03:20:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863228.11 epoch sec; seqNo:4624; 4061498.76 north; 594792.73 east; 36.69 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:20:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863226.09 2024-11-06T03:20:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:28Z- waiting for 1730863230.09, time is 1730863228.11 2024-11-06T03:20:28Z- AHRS_M2 msg: 1730863228.49 epoch sec; seqNo:4344 2024-11-06T03:20:28Z- AHRS_M2 msg: 0.11 phi; 0.01 theta; 6.00 psi 2024-11-06T03:20:28Z- handleDepth()- Depth_Keller msg: 1730863228.51 epoch sec; seqNo:4625; depth 50.69 2024-11-06T03:20:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863228.52 epoch sec; seqNo:4625; 4061499.14 north; 594792.73 east; 36.69 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:20:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863226.09 2024-11-06T03:20:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:28Z- waiting for 1730863230.09, time is 1730863228.52 2024-11-06T03:20:28Z- AHRS_M2 msg: 1730863228.90 epoch sec; seqNo:4345 2024-11-06T03:20:28Z- AHRS_M2 msg: 0.11 phi; 0.01 theta; 6.00 psi 2024-11-06T03:20:28Z- handleDepth()- Depth_Keller msg: 1730863228.90 epoch sec; seqNo:4626; depth 50.69 2024-11-06T03:20:28Z- handleDVL() - DVL msg epochMillisec: 1730863228.93 2024-11-06T03:20:28Z- handleDvl() - RDI_Pathfinder msg: 1730863228.93 sec; seqNo:1524; #beams:4 2024-11-06T03:20:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.49 , 36.45 , 38.13 , 39.06 2024-11-06T03:20:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.12 , 0.01 2024-11-06T03:20:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863228.95 epoch sec; seqNo:4626; 4061499.52 north; 594792.72 east; 36.69 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:20:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863226.09 2024-11-06T03:20:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:29Z- waiting for 1730863230.09, time is 1730863228.95 2024-11-06T03:20:29Z- AHRS_M2 msg: 1730863229.33 epoch sec; seqNo:4346 2024-11-06T03:20:29Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 6.00 psi 2024-11-06T03:20:29Z- handleDepth()- Depth_Keller msg: 1730863229.34 epoch sec; seqNo:4627; depth 50.70 2024-11-06T03:20:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863229.36 epoch sec; seqNo:4627; 4061499.90 north; 594792.72 east; 36.69 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T03:20:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863226.09 2024-11-06T03:20:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:29Z- waiting for 1730863230.09, time is 1730863229.36 2024-11-06T03:20:29Z- AHRS_M2 msg: 1730863229.71 epoch sec; seqNo:4347 2024-11-06T03:20:29Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 6.00 psi 2024-11-06T03:20:29Z- handleDepth()- Depth_Keller msg: 1730863229.72 epoch sec; seqNo:4628; depth 50.69 2024-11-06T03:20:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863229.73 epoch sec; seqNo:4628; 4061500.27 north; 594792.71 east; 36.69 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:20:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863226.09 2024-11-06T03:20:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:29Z- waiting for 1730863230.09, time is 1730863229.73 2024-11-06T03:20:30Z- AHRS_M2 msg: 1730863230.11 epoch sec; seqNo:4348 2024-11-06T03:20:30Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 6.00 psi 2024-11-06T03:20:30Z- handleDepth()- Depth_Keller msg: 1730863230.12 epoch sec; seqNo:4629; depth 50.70 2024-11-06T03:20:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863230.13 epoch sec; seqNo:4629; 4061500.27 north; 594792.71 east; 36.69 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T03:20:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863226.09 2024-11-06T03:20:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:30Z- waiting for 1730863230.09, time is 1730863230.13 2024-11-06T03:20:30Z- LcmTrn::updateTrn() >>>> pose time 1730863230.13, meas time 1730863228.93 <<<< 2024-11-06T03:20:30Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863230.13 2024-11-06T03:20:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863228.93 sec, ping # 0; waiting for INS data more recent than 1730863226.09... 2024-11-06T03:20:30Z- TerrainNav::motionUpdate() - currEstimate = (4061500, 594792.71, 50.70) 2024-11-06T03:20:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:20:30Z- TerrainNav:: SubcloudNIS is 0.70, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:20:30Z- TNavPF::Projecting Measurements 2024-11-06T03:20:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:20:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:20:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:20:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:20:30Z- TNavFilter::4 good beams. 2024-11-06T03:20:30Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:20:30Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:20:30Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:20:30Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:20:30Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:20:30Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:20:30Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:20:30Z- TerrainMapOctree: (vehN, vehE ) = (4061499.27, 594793.09). 2024-11-06T03:20:30Z- TerrainMapOctree: Min Distance = 252.96. 2024-11-06T03:20:30Z- TerrainMapOctree: Using tile 2. 2024-11-06T03:20:30Z- Forcing Subcloud Comparison 2024-11-06T03:20:30Z- Weighting particles with subcloud comparison 2024-11-06T03:20:30Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:30Z- meanExpectedMeasDiff: 3.132644 delta_rms_squared: 1.517184 alpha: 0.986989 2024-11-06T03:20:30Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:30Z- meanExpectedMeasDiff: -0.160211 delta_rms_squared: 1.106438 alpha: 0.982245 2024-11-06T03:20:30Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:30Z- meanExpectedMeasDiff: -0.193613 delta_rms_squared: 1.289705 alpha: 0.984729 2024-11-06T03:20:30Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:30Z- meanExpectedMeasDiff: -0.896129 delta_rms_squared: 1.253135 alpha: 0.984291 2024-11-06T03:20:30Z- Subcloud NIS: 0.699056 2024-11-06T03:20:30Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:20:30Z- TNavPF:: sumWeights = 0.220754 2024-11-06T03:20:30Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:20:30Z- TNavPF::First Term of Map Covariance Matrix is 1.22 2024-11-06T03:20:30Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T03:20:30Z- TerrainNav::Resampling particle filter... 2024-11-06T03:20:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863228.93 sec. 2024-11-06T03:20:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:20:30Z- variances([0], [2], and psi_berg) = 0.614731 0.735534 0.000000 2024-11-06T03:20:30Z- publishEstimatesDecoder() - at 1730863230.13 sec 2024-11-06T03:20:30Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:20:30Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061531.02 594775.68 49.23 2024-11-06T03:20:30Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061531.58 594775.62 49.89 lat/lon: 36.69 -121.94 2024-11-06T03:20:30Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.61 0.74 0.67 0.00 2024-11-06T03:20:30Z- AHRS_M2 msg: 1730863230.52 epoch sec; seqNo:4349 2024-11-06T03:20:30Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 6.00 psi 2024-11-06T03:20:30Z- handleDepth()- Depth_Keller msg: 1730863230.52 epoch sec; seqNo:4630; depth 50.71 2024-11-06T03:20:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863230.54 epoch sec; seqNo:4630; 4061500.65 north; 594792.71 east; 36.69 lat; -121.94 lon; 50.71 depth; utmZone: 10 2024-11-06T03:20:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863230.13 2024-11-06T03:20:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:30Z- waiting for 1730863234.13, time is 1730863230.54 2024-11-06T03:20:30Z- AHRS_M2 msg: 1730863230.92 epoch sec; seqNo:4350 2024-11-06T03:20:30Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 6.00 psi 2024-11-06T03:20:30Z- handleDepth()- Depth_Keller msg: 1730863230.93 epoch sec; seqNo:4631; depth 50.71 2024-11-06T03:20:30Z- handleDVL() - DVL msg epochMillisec: 1730863230.94 2024-11-06T03:20:30Z- handleDvl() - RDI_Pathfinder msg: 1730863230.94 sec; seqNo:1525; #beams:4 2024-11-06T03:20:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 36.26 , 38.13 , 38.88 2024-11-06T03:20:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.08 , 0.03 2024-11-06T03:20:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863230.96 epoch sec; seqNo:4631; 4061501.02 north; 594791.48 east; 36.69 lat; -121.94 lon; 50.71 depth; utmZone: 10 2024-11-06T03:20:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863230.13 2024-11-06T03:20:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:31Z- waiting for 1730863234.13, time is 1730863230.96 2024-11-06T03:20:31Z- AHRS_M2 msg: 1730863231.32 epoch sec; seqNo:4351 2024-11-06T03:20:31Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 6.00 psi 2024-11-06T03:20:31Z- handleDepth()- Depth_Keller msg: 1730863231.33 epoch sec; seqNo:4632; depth 50.70 2024-11-06T03:20:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863231.37 epoch sec; seqNo:4632; 4061501.40 north; 594791.48 east; 36.69 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T03:20:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863230.13 2024-11-06T03:20:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:31Z- waiting for 1730863234.13, time is 1730863231.37 2024-11-06T03:20:31Z- AHRS_M2 msg: 1730863231.73 epoch sec; seqNo:4352 2024-11-06T03:20:31Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 6.00 psi 2024-11-06T03:20:31Z- handleDepth()- Depth_Keller msg: 1730863231.73 epoch sec; seqNo:4633; depth 50.72 2024-11-06T03:20:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863231.74 epoch sec; seqNo:4633; 4061501.78 north; 594791.48 east; 36.69 lat; -121.94 lon; 50.72 depth; utmZone: 10 2024-11-06T03:20:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863230.13 2024-11-06T03:20:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:31Z- waiting for 1730863234.13, time is 1730863231.74 2024-11-06T03:20:32Z- AHRS_M2 msg: 1730863232.13 epoch sec; seqNo:4353 2024-11-06T03:20:32Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.00 psi 2024-11-06T03:20:32Z- handleDepth()- Depth_Keller msg: 1730863232.14 epoch sec; seqNo:4634; depth 50.72 2024-11-06T03:20:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863232.15 epoch sec; seqNo:4634; 4061502.16 north; 594791.47 east; 36.69 lat; -121.94 lon; 50.72 depth; utmZone: 10 2024-11-06T03:20:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863230.13 2024-11-06T03:20:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:32Z- waiting for 1730863234.13, time is 1730863232.15 2024-11-06T03:20:32Z- AHRS_M2 msg: 1730863232.54 epoch sec; seqNo:4354 2024-11-06T03:20:32Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.00 psi 2024-11-06T03:20:32Z- handleDepth()- Depth_Keller msg: 1730863232.55 epoch sec; seqNo:4635; depth 50.71 2024-11-06T03:20:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863232.56 epoch sec; seqNo:4635; 4061502.53 north; 594791.47 east; 36.69 lat; -121.94 lon; 50.71 depth; utmZone: 10 2024-11-06T03:20:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863230.13 2024-11-06T03:20:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:32Z- waiting for 1730863234.13, time is 1730863232.56 2024-11-06T03:20:32Z- AHRS_M2 msg: 1730863232.94 epoch sec; seqNo:4355 2024-11-06T03:20:32Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.00 psi 2024-11-06T03:20:32Z- handleDepth()- Depth_Keller msg: 1730863232.95 epoch sec; seqNo:4636; depth 50.72 2024-11-06T03:20:32Z- handleDVL() - DVL msg epochMillisec: 1730863232.97 2024-11-06T03:20:32Z- handleDvl() - RDI_Pathfinder msg: 1730863232.97 sec; seqNo:1526; #beams:4 2024-11-06T03:20:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.68 , 36.26 , 37.94 , 39.06 2024-11-06T03:20:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.03 2024-11-06T03:20:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863232.99 epoch sec; seqNo:4636; 4061502.91 north; 594791.46 east; 36.69 lat; -121.94 lon; 50.72 depth; utmZone: 10 2024-11-06T03:20:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863230.13 2024-11-06T03:20:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:33Z- waiting for 1730863234.13, time is 1730863232.99 2024-11-06T03:20:33Z- AHRS_M2 msg: 1730863233.38 epoch sec; seqNo:4356 2024-11-06T03:20:33Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.00 psi 2024-11-06T03:20:33Z- handleDepth()- Depth_Keller msg: 1730863233.39 epoch sec; seqNo:4637; depth 50.70 2024-11-06T03:20:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863233.40 epoch sec; seqNo:4637; 4061503.29 north; 594791.46 east; 36.69 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T03:20:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863230.13 2024-11-06T03:20:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:33Z- waiting for 1730863234.13, time is 1730863233.40 2024-11-06T03:20:33Z- AHRS_M2 msg: 1730863233.75 epoch sec; seqNo:4357 2024-11-06T03:20:33Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.00 psi 2024-11-06T03:20:33Z- handleDepth()- Depth_Keller msg: 1730863233.77 epoch sec; seqNo:4638; depth 50.69 2024-11-06T03:20:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863233.78 epoch sec; seqNo:4638; 4061503.67 north; 594791.46 east; 36.69 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:20:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863230.13 2024-11-06T03:20:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:33Z- waiting for 1730863234.13, time is 1730863233.78 2024-11-06T03:20:34Z- AHRS_M2 msg: 1730863234.15 epoch sec; seqNo:4358 2024-11-06T03:20:34Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.01 psi 2024-11-06T03:20:34Z- handleDepth()- Depth_Keller msg: 1730863234.15 epoch sec; seqNo:4639; depth 50.67 2024-11-06T03:20:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863234.16 epoch sec; seqNo:4639; 4061504.05 north; 594791.45 east; 36.69 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T03:20:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863230.13 2024-11-06T03:20:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:34Z- waiting for 1730863234.13, time is 1730863234.16 2024-11-06T03:20:34Z- LcmTrn::updateTrn() >>>> pose time 1730863234.16, meas time 1730863232.97 <<<< 2024-11-06T03:20:34Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863234.16 2024-11-06T03:20:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863232.97 sec, ping # 0; waiting for INS data more recent than 1730863230.13... 2024-11-06T03:20:34Z- TerrainNav::motionUpdate() - currEstimate = (4061504, 594791.45, 50.67) 2024-11-06T03:20:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:20:34Z- TerrainNav:: SubcloudNIS is 0.70, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:20:34Z- TNavPF::Projecting Measurements 2024-11-06T03:20:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:20:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:20:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:20:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:20:34Z- TNavFilter::4 good beams. 2024-11-06T03:20:34Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:20:34Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:20:34Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:20:34Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:20:34Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:20:34Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:20:34Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:20:34Z- TerrainMapOctree: (vehN, vehE ) = (4061502.94, 594791.82). 2024-11-06T03:20:34Z- TerrainMapOctree: Min Distance = 250.58. 2024-11-06T03:20:34Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:20:34Z- TerrainMapOctree: Switching to tile 5. 2024-11-06T03:20:35Z- TerrainMapOctree::Octree tile load /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile5.bo took 1.206761 seconds. 2024-11-06T03:20:35Z- Forcing Subcloud Comparison 2024-11-06T03:20:35Z- Weighting particles with subcloud comparison 2024-11-06T03:20:35Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:35Z- meanExpectedMeasDiff: 3.196816 delta_rms_squared: 1.823368 alpha: 0.989150 2024-11-06T03:20:35Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:35Z- meanExpectedMeasDiff: -0.850326 delta_rms_squared: 1.410334 alpha: 0.986017 2024-11-06T03:20:35Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:35Z- meanExpectedMeasDiff: -0.442530 delta_rms_squared: 1.113462 alpha: 0.982355 2024-11-06T03:20:35Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:35Z- meanExpectedMeasDiff: -0.203633 delta_rms_squared: 1.771905 alpha: 0.988839 2024-11-06T03:20:35Z- Subcloud NIS: 0.686310 2024-11-06T03:20:35Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:20:35Z- TNavPF:: sumWeights = 0.190219 2024-11-06T03:20:35Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:20:35Z- TNavPF::First Term of Map Covariance Matrix is 1.15 2024-11-06T03:20:35Z- TNavPF::Calculated NIS Value : 0.09 numBeams normalized NIS: 0.02 2024-11-06T03:20:35Z- TerrainNav::Resampling particle filter... 2024-11-06T03:20:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863232.97 sec. 2024-11-06T03:20:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:20:35Z- variances([0], [2], and psi_berg) = 0.599382 0.790059 0.000000 2024-11-06T03:20:35Z- publishEstimatesDecoder() - at 1730863234.16 sec 2024-11-06T03:20:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:20:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061534.77 594774.41 49.54 2024-11-06T03:20:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061535.33 594774.33 49.64 lat/lon: 36.69 -121.94 2024-11-06T03:20:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.60 0.79 0.69 0.00 2024-11-06T03:20:35Z- AHRS_M2 msg: 1730863234.56 epoch sec; seqNo:4359 2024-11-06T03:20:35Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.01 psi 2024-11-06T03:20:35Z- handleDepth()- Depth_Keller msg: 1730863234.56 epoch sec; seqNo:4640; depth 50.67 2024-11-06T03:20:35Z- handleDVL() - DVL msg epochMillisec: 1730863234.56 2024-11-06T03:20:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:20:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:20:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:20:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:20:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:20:35Z- handleDvl() - RDI_Pathfinder msg: 1730863234.56 sec; seqNo:1527; #beams:0 2024-11-06T03:20:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:20:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.03 2024-11-06T03:20:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863234.57 epoch sec; seqNo:4640; 4061504.43 north; 594791.45 east; 36.69 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T03:20:35Z- AHRS_M2 msg: 1730863234.96 epoch sec; seqNo:4360 2024-11-06T03:20:35Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.01 psi 2024-11-06T03:20:35Z- handleDepth()- Depth_Keller msg: 1730863234.96 epoch sec; seqNo:4641; depth 50.66 2024-11-06T03:20:35Z- handleDVL() - DVL msg epochMillisec: 1730863234.99 2024-11-06T03:20:35Z- handleDvl() - RDI_Pathfinder msg: 1730863234.99 sec; seqNo:1528; #beams:4 2024-11-06T03:20:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.24 , 35.89 , 37.57 , 39.06 2024-11-06T03:20:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.01 2024-11-06T03:20:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863235.01 epoch sec; seqNo:4641; 4061504.79 north; 594790.22 east; 36.69 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T03:20:35Z- AHRS_M2 msg: 1730863235.36 epoch sec; seqNo:4361 2024-11-06T03:20:35Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.01 psi 2024-11-06T03:20:35Z- handleDepth()- Depth_Keller msg: 1730863235.37 epoch sec; seqNo:4642; depth 50.64 2024-11-06T03:20:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863235.38 epoch sec; seqNo:4642; 4061505.17 north; 594790.22 east; 36.69 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:20:35Z- AHRS_M2 msg: 1730863235.77 epoch sec; seqNo:4362 2024-11-06T03:20:35Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.01 psi 2024-11-06T03:20:35Z- handleDepth()- Depth_Keller msg: 1730863235.78 epoch sec; seqNo:4643; depth 50.62 2024-11-06T03:20:35Z- handleDVL() - DVL msg epochMillisec: 1730863235.79 2024-11-06T03:20:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:20:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:20:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:20:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:20:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:20:35Z- handleDvl() - RDI_Pathfinder msg: 1730863235.79 sec; seqNo:1529; #beams:0 2024-11-06T03:20:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:20:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.01 2024-11-06T03:20:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863235.80 epoch sec; seqNo:4643; 4061505.17 north; 594790.22 east; 36.69 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:20:36Z- 15 msgs in 200 ms and 16 handle calls 2024-11-06T03:20:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863234.16 2024-11-06T03:20:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:36Z- waiting for 1730863238.16, time is 1730863235.80 2024-11-06T03:20:36Z- AHRS_M2 msg: 1730863236.17 epoch sec; seqNo:4363 2024-11-06T03:20:36Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.01 psi 2024-11-06T03:20:36Z- handleDepth()- Depth_Keller msg: 1730863236.18 epoch sec; seqNo:4644; depth 50.61 2024-11-06T03:20:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863236.19 epoch sec; seqNo:4644; 4061505.55 north; 594790.21 east; 36.69 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:20:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863234.16 2024-11-06T03:20:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:36Z- waiting for 1730863238.16, time is 1730863236.19 2024-11-06T03:20:36Z- AHRS_M2 msg: 1730863236.58 epoch sec; seqNo:4364 2024-11-06T03:20:36Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.01 psi 2024-11-06T03:20:36Z- handleDepth()- Depth_Keller msg: 1730863236.58 epoch sec; seqNo:4645; depth 50.59 2024-11-06T03:20:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863236.59 epoch sec; seqNo:4645; 4061505.93 north; 594790.21 east; 36.69 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:20:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863234.16 2024-11-06T03:20:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:36Z- waiting for 1730863238.16, time is 1730863236.59 2024-11-06T03:20:36Z- AHRS_M2 msg: 1730863236.98 epoch sec; seqNo:4365 2024-11-06T03:20:36Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.01 psi 2024-11-06T03:20:36Z- handleDepth()- Depth_Keller msg: 1730863236.99 epoch sec; seqNo:4646; depth 50.57 2024-11-06T03:20:37Z- handleDVL() - DVL msg epochMillisec: 1730863237.01 2024-11-06T03:20:37Z- handleDvl() - RDI_Pathfinder msg: 1730863237.01 sec; seqNo:1530; #beams:4 2024-11-06T03:20:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 35.89 , 36.82 , 39.06 2024-11-06T03:20:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.05 2024-11-06T03:20:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863237.02 epoch sec; seqNo:4646; 4061506.31 north; 594790.21 east; 36.69 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T03:20:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863234.16 2024-11-06T03:20:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:37Z- waiting for 1730863238.16, time is 1730863237.02 2024-11-06T03:20:37Z- AHRS_M2 msg: 1730863237.38 epoch sec; seqNo:4366 2024-11-06T03:20:37Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 6.01 psi 2024-11-06T03:20:37Z- handleDepth()- Depth_Keller msg: 1730863237.39 epoch sec; seqNo:4647; depth 50.55 2024-11-06T03:20:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863237.41 epoch sec; seqNo:4647; 4061506.69 north; 594790.20 east; 36.69 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:20:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863234.16 2024-11-06T03:20:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:37Z- waiting for 1730863238.16, time is 1730863237.41 2024-11-06T03:20:37Z- AHRS_M2 msg: 1730863237.79 epoch sec; seqNo:4367 2024-11-06T03:20:37Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 6.01 psi 2024-11-06T03:20:37Z- handleDepth()- Depth_Keller msg: 1730863237.80 epoch sec; seqNo:4648; depth 50.54 2024-11-06T03:20:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863237.82 epoch sec; seqNo:4648; 4061507.07 north; 594790.20 east; 36.69 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:20:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863234.16 2024-11-06T03:20:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:37Z- waiting for 1730863238.16, time is 1730863237.82 2024-11-06T03:20:38Z- AHRS_M2 msg: 1730863238.19 epoch sec; seqNo:4368 2024-11-06T03:20:38Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 6.01 psi 2024-11-06T03:20:38Z- handleDepth()- Depth_Keller msg: 1730863238.19 epoch sec; seqNo:4649; depth 50.52 2024-11-06T03:20:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863238.20 epoch sec; seqNo:4649; 4061507.45 north; 594790.19 east; 36.69 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:20:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863234.16 2024-11-06T03:20:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:38Z- waiting for 1730863238.16, time is 1730863238.20 2024-11-06T03:20:38Z- LcmTrn::updateTrn() >>>> pose time 1730863238.20, meas time 1730863237.01 <<<< 2024-11-06T03:20:38Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863238.20 2024-11-06T03:20:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863237.01 sec, ping # 0; waiting for INS data more recent than 1730863234.16... 2024-11-06T03:20:38Z- TerrainNav::motionUpdate() - currEstimate = (4061507, 594790.19, 50.52) 2024-11-06T03:20:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:20:38Z- TerrainNav:: SubcloudNIS is 0.69, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:20:38Z- TNavPF::Projecting Measurements 2024-11-06T03:20:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:20:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:20:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:20:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:20:38Z- TNavFilter::4 good beams. 2024-11-06T03:20:38Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:20:38Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:20:38Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:20:38Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:20:38Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:20:38Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:20:38Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:20:38Z- TerrainMapOctree: (vehN, vehE ) = (4061506.44, 594790.56). 2024-11-06T03:20:38Z- TerrainMapOctree: Min Distance = 246.91. 2024-11-06T03:20:38Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:20:38Z- Forcing Subcloud Comparison 2024-11-06T03:20:38Z- Weighting particles with subcloud comparison 2024-11-06T03:20:38Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:38Z- meanExpectedMeasDiff: 2.766869 delta_rms_squared: 1.638173 alpha: 0.987939 2024-11-06T03:20:38Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:38Z- meanExpectedMeasDiff: -1.146991 delta_rms_squared: 1.095064 alpha: 0.982064 2024-11-06T03:20:38Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:38Z- meanExpectedMeasDiff: -0.351475 delta_rms_squared: 1.659514 alpha: 0.988092 2024-11-06T03:20:38Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:38Z- meanExpectedMeasDiff: -0.323755 delta_rms_squared: 1.240800 alpha: 0.984137 2024-11-06T03:20:38Z- Subcloud NIS: 0.654962 2024-11-06T03:20:38Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:20:38Z- TNavPF:: sumWeights = 0.236304 2024-11-06T03:20:38Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:20:38Z- TNavPF::First Term of Map Covariance Matrix is 1.09 2024-11-06T03:20:38Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T03:20:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863237.01 sec. 2024-11-06T03:20:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:20:38Z- variances([0], [2], and psi_berg) = 0.577696 0.779701 0.000000 2024-11-06T03:20:38Z- publishEstimatesDecoder() - at 1730863238.20 sec 2024-11-06T03:20:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:20:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061539.11 594772.86 48.92 2024-11-06T03:20:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061538.74 594773.09 49.39 lat/lon: 36.69 -121.94 2024-11-06T03:20:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.58 0.78 0.65 0.00 2024-11-06T03:20:38Z- AHRS_M2 msg: 1730863238.60 epoch sec; seqNo:4369 2024-11-06T03:20:38Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.01 psi 2024-11-06T03:20:38Z- handleDepth()- Depth_Keller msg: 1730863238.60 epoch sec; seqNo:4650; depth 50.51 2024-11-06T03:20:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863238.61 epoch sec; seqNo:4650; 4061507.82 north; 594790.19 east; 36.69 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:20:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863238.20 2024-11-06T03:20:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:38Z- waiting for 1730863242.20, time is 1730863238.61 2024-11-06T03:20:39Z- AHRS_M2 msg: 1730863239.00 epoch sec; seqNo:4370 2024-11-06T03:20:39Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:39Z- handleDepth()- Depth_Keller msg: 1730863239.02 epoch sec; seqNo:4651; depth 50.49 2024-11-06T03:20:39Z- handleDVL() - DVL msg epochMillisec: 1730863239.04 2024-11-06T03:20:39Z- handleDvl() - RDI_Pathfinder msg: 1730863239.04 sec; seqNo:1531; #beams:4 2024-11-06T03:20:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.68 , 35.88 , 36.26 , 39.06 2024-11-06T03:20:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.12 , -0.07 2024-11-06T03:20:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863239.05 epoch sec; seqNo:4651; 4061508.20 north; 594790.18 east; 36.69 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:20:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863238.20 2024-11-06T03:20:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:39Z- waiting for 1730863242.20, time is 1730863239.05 2024-11-06T03:20:39Z- AHRS_M2 msg: 1730863239.40 epoch sec; seqNo:4371 2024-11-06T03:20:39Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.01 psi 2024-11-06T03:20:39Z- handleDepth()- Depth_Keller msg: 1730863239.41 epoch sec; seqNo:4652; depth 50.47 2024-11-06T03:20:39Z- handleDVL() - DVL msg epochMillisec: 1730863239.41 2024-11-06T03:20:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:20:39Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:20:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:20:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:20:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:20:39Z- handleDvl() - RDI_Pathfinder msg: 1730863239.41 sec; seqNo:1532; #beams:0 2024-11-06T03:20:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:20:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.12 , -0.07 2024-11-06T03:20:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863239.42 epoch sec; seqNo:4652; 4061508.57 north; 594788.96 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:20:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863238.20 2024-11-06T03:20:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:39Z- waiting for 1730863242.20, time is 1730863239.42 2024-11-06T03:20:39Z- AHRS_M2 msg: 1730863239.81 epoch sec; seqNo:4372 2024-11-06T03:20:39Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:39Z- handleDepth()- Depth_Keller msg: 1730863239.82 epoch sec; seqNo:4653; depth 50.47 2024-11-06T03:20:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863239.83 epoch sec; seqNo:4653; 4061508.95 north; 594788.96 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:20:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863238.20 2024-11-06T03:20:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:39Z- waiting for 1730863242.20, time is 1730863239.83 2024-11-06T03:20:40Z- AHRS_M2 msg: 1730863240.21 epoch sec; seqNo:4373 2024-11-06T03:20:40Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:40Z- handleDepth()- Depth_Keller msg: 1730863240.23 epoch sec; seqNo:4654; depth 50.47 2024-11-06T03:20:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863240.23 epoch sec; seqNo:4654; 4061509.33 north; 594788.95 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:20:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863238.20 2024-11-06T03:20:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:40Z- waiting for 1730863242.20, time is 1730863240.23 2024-11-06T03:20:40Z- AHRS_M2 msg: 1730863240.62 epoch sec; seqNo:4374 2024-11-06T03:20:40Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:40Z- handleDepth()- Depth_Keller msg: 1730863240.63 epoch sec; seqNo:4655; depth 50.45 2024-11-06T03:20:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863240.65 epoch sec; seqNo:4655; 4061509.33 north; 594788.95 east; 36.69 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:20:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863238.20 2024-11-06T03:20:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:40Z- waiting for 1730863242.20, time is 1730863240.65 2024-11-06T03:20:41Z- handleDepth()- Depth_Keller msg: 1730863241.04 epoch sec; seqNo:4656; depth 50.44 2024-11-06T03:20:41Z- handleDVL() - DVL msg epochMillisec: 1730863241.06 2024-11-06T03:20:41Z- handleDvl() - RDI_Pathfinder msg: 1730863241.06 sec; seqNo:1533; #beams:4 2024-11-06T03:20:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.68 , 35.51 , 35.89 , 38.88 2024-11-06T03:20:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.16 , -0.04 2024-11-06T03:20:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863241.07 epoch sec; seqNo:4656; 4061509.71 north; 594788.95 east; 36.69 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:20:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863238.20 2024-11-06T03:20:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:41Z- waiting for 1730863242.20, time is 1730863241.07 2024-11-06T03:20:41Z- AHRS_M2 msg: 1730863241.43 epoch sec; seqNo:4375 2024-11-06T03:20:41Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:41Z- handleDepth()- Depth_Keller msg: 1730863241.44 epoch sec; seqNo:4657; depth 50.44 2024-11-06T03:20:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863241.45 epoch sec; seqNo:4657; 4061510.08 north; 594788.94 east; 36.69 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:20:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863238.20 2024-11-06T03:20:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:41Z- waiting for 1730863242.20, time is 1730863241.45 2024-11-06T03:20:41Z- AHRS_M2 msg: 1730863241.83 epoch sec; seqNo:4376 2024-11-06T03:20:41Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:41Z- handleDepth()- Depth_Keller msg: 1730863241.84 epoch sec; seqNo:4658; depth 50.44 2024-11-06T03:20:41Z- handleDVL() - DVL msg epochMillisec: 1730863241.84 2024-11-06T03:20:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:20:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:20:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:20:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:20:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:20:41Z- handleDvl() - RDI_Pathfinder msg: 1730863241.84 sec; seqNo:1534; #beams:0 2024-11-06T03:20:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:20:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.16 , -0.04 2024-11-06T03:20:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863241.86 epoch sec; seqNo:4658; 4061510.46 north; 594788.94 east; 36.69 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:20:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863238.20 2024-11-06T03:20:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:41Z- waiting for 1730863242.20, time is 1730863241.86 2024-11-06T03:20:42Z- AHRS_M2 msg: 1730863242.23 epoch sec; seqNo:4377 2024-11-06T03:20:42Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.00 psi 2024-11-06T03:20:42Z- handleDepth()- Depth_Keller msg: 1730863242.23 epoch sec; seqNo:4659; depth 50.45 2024-11-06T03:20:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863242.24 epoch sec; seqNo:4659; 4061510.84 north; 594788.93 east; 36.69 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:20:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863238.20 2024-11-06T03:20:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:42Z- waiting for 1730863242.20, time is 1730863242.24 2024-11-06T03:20:42Z- LcmTrn::updateTrn() >>>> pose time 1730863242.24, meas time 1730863241.84 <<<< 2024-11-06T03:20:42Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863242.24 2024-11-06T03:20:42Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:20:42Z- TerrainNav::motionUpdate() - currEstimate = (4061511, 594788.93, 50.45) 2024-11-06T03:20:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:20:42Z- TerrainNav:: SubcloudNIS is 0.65, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:20:42Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:20:42Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:20:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:20:42Z- variances([0], [2], and psi_berg) = 0.580038 0.782064 0.000000 2024-11-06T03:20:42Z- publishEstimatesDecoder() - at 1730863242.24 sec 2024-11-06T03:20:42Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:20:42Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061542.54 594771.60 48.49 2024-11-06T03:20:42Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061542.13 594771.83 49.31 lat/lon: 36.69 -121.94 2024-11-06T03:20:42Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.58 0.78 0.91 0.00 2024-11-06T03:20:42Z- AHRS_M2 msg: 1730863242.64 epoch sec; seqNo:4378 2024-11-06T03:20:42Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.00 psi 2024-11-06T03:20:42Z- handleDepth()- Depth_Keller msg: 1730863242.65 epoch sec; seqNo:4660; depth 50.44 2024-11-06T03:20:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863242.66 epoch sec; seqNo:4660; 4061511.21 north; 594787.71 east; 36.69 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:20:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863242.24 2024-11-06T03:20:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:42Z- waiting for 1730863246.24, time is 1730863242.66 2024-11-06T03:20:43Z- handleDepth()- Depth_Keller msg: 1730863243.06 epoch sec; seqNo:4661; depth 50.44 2024-11-06T03:20:43Z- handleDVL() - DVL msg epochMillisec: 1730863243.08 2024-11-06T03:20:43Z- handleDvl() - RDI_Pathfinder msg: 1730863243.08 sec; seqNo:1535; #beams:4 2024-11-06T03:20:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.80 , 35.34 , 35.68 , 38.91 2024-11-06T03:20:43Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.14 , 0.02 2024-11-06T03:20:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863243.09 epoch sec; seqNo:4661; 4061511.59 north; 594787.71 east; 36.69 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:20:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863242.24 2024-11-06T03:20:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:43Z- waiting for 1730863246.24, time is 1730863243.09 2024-11-06T03:20:43Z- AHRS_M2 msg: 1730863243.44 epoch sec; seqNo:4379 2024-11-06T03:20:43Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.00 psi 2024-11-06T03:20:43Z- handleDepth()- Depth_Keller msg: 1730863243.46 epoch sec; seqNo:4662; depth 50.46 2024-11-06T03:20:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863243.47 epoch sec; seqNo:4662; 4061511.97 north; 594787.70 east; 36.69 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:20:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863242.24 2024-11-06T03:20:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:43Z- waiting for 1730863246.24, time is 1730863243.47 2024-11-06T03:20:43Z- AHRS_M2 msg: 1730863243.85 epoch sec; seqNo:4380 2024-11-06T03:20:43Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.00 psi 2024-11-06T03:20:43Z- handleDepth()- Depth_Keller msg: 1730863243.86 epoch sec; seqNo:4663; depth 50.46 2024-11-06T03:20:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863243.87 epoch sec; seqNo:4663; 4061512.34 north; 594787.70 east; 36.69 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:20:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863242.24 2024-11-06T03:20:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:44Z- waiting for 1730863246.24, time is 1730863243.87 2024-11-06T03:20:44Z- AHRS_M2 msg: 1730863244.25 epoch sec; seqNo:4381 2024-11-06T03:20:44Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.00 psi 2024-11-06T03:20:44Z- handleDepth()- Depth_Keller msg: 1730863244.26 epoch sec; seqNo:4664; depth 50.47 2024-11-06T03:20:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863244.28 epoch sec; seqNo:4664; 4061512.72 north; 594787.69 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:20:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863242.24 2024-11-06T03:20:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:44Z- waiting for 1730863246.24, time is 1730863244.28 2024-11-06T03:20:44Z- AHRS_M2 msg: 1730863244.65 epoch sec; seqNo:4382 2024-11-06T03:20:44Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.00 psi 2024-11-06T03:20:44Z- handleDepth()- Depth_Keller msg: 1730863244.67 epoch sec; seqNo:4665; depth 50.47 2024-11-06T03:20:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863244.68 epoch sec; seqNo:4665; 4061513.10 north; 594787.69 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:20:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863242.24 2024-11-06T03:20:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:44Z- waiting for 1730863246.24, time is 1730863244.68 2024-11-06T03:20:45Z- handleDepth()- Depth_Keller msg: 1730863245.08 epoch sec; seqNo:4666; depth 50.47 2024-11-06T03:20:45Z- handleDVL() - DVL msg epochMillisec: 1730863245.10 2024-11-06T03:20:45Z- handleDvl() - RDI_Pathfinder msg: 1730863245.10 sec; seqNo:1536; #beams:4 2024-11-06T03:20:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.80 , 34.66 , 35.17 , 39.08 2024-11-06T03:20:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.07 , 0.05 2024-11-06T03:20:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863245.12 epoch sec; seqNo:4666; 4061513.48 north; 594787.69 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:20:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863242.24 2024-11-06T03:20:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:45Z- waiting for 1730863246.24, time is 1730863245.12 2024-11-06T03:20:45Z- AHRS_M2 msg: 1730863245.46 epoch sec; seqNo:4383 2024-11-06T03:20:45Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.00 psi 2024-11-06T03:20:45Z- handleDepth()- Depth_Keller msg: 1730863245.47 epoch sec; seqNo:4667; depth 50.45 2024-11-06T03:20:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863245.49 epoch sec; seqNo:4667; 4061513.85 north; 594786.46 east; 36.69 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:20:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863242.24 2024-11-06T03:20:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:45Z- waiting for 1730863246.24, time is 1730863245.49 2024-11-06T03:20:45Z- AHRS_M2 msg: 1730863245.87 epoch sec; seqNo:4384 2024-11-06T03:20:45Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.00 psi 2024-11-06T03:20:45Z- handleDepth()- Depth_Keller msg: 1730863245.87 epoch sec; seqNo:4668; depth 50.48 2024-11-06T03:20:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863245.88 epoch sec; seqNo:4668; 4061514.22 north; 594786.46 east; 36.69 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:20:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863242.24 2024-11-06T03:20:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:46Z- waiting for 1730863246.24, time is 1730863245.88 2024-11-06T03:20:46Z- AHRS_M2 msg: 1730863246.27 epoch sec; seqNo:4385 2024-11-06T03:20:46Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.00 psi 2024-11-06T03:20:46Z- handleDepth()- Depth_Keller msg: 1730863246.28 epoch sec; seqNo:4669; depth 50.48 2024-11-06T03:20:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863246.29 epoch sec; seqNo:4669; 4061514.60 north; 594786.45 east; 36.69 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:20:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863242.24 2024-11-06T03:20:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:46Z- waiting for 1730863246.24, time is 1730863246.29 2024-11-06T03:20:46Z- LcmTrn::updateTrn() >>>> pose time 1730863246.29, meas time 1730863245.10 <<<< 2024-11-06T03:20:46Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863246.29 2024-11-06T03:20:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863245.10 sec, ping # 0; waiting for INS data more recent than 1730863242.24... 2024-11-06T03:20:46Z- TerrainNav::motionUpdate() - currEstimate = (4061515, 594786.45, 50.48) 2024-11-06T03:20:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:20:46Z- TerrainNav:: SubcloudNIS is 0.65, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:20:46Z- TNavPF::Projecting Measurements 2024-11-06T03:20:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:20:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:20:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:20:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:20:46Z- TNavFilter::4 good beams. 2024-11-06T03:20:46Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:20:46Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:20:46Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:20:46Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:20:46Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:20:46Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:20:46Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:20:46Z- TerrainMapOctree: (vehN, vehE ) = (4061513.50, 594787.18). 2024-11-06T03:20:46Z- TerrainMapOctree: Min Distance = 239.40. 2024-11-06T03:20:46Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:20:46Z- Forcing Subcloud Comparison 2024-11-06T03:20:46Z- Weighting particles with subcloud comparison 2024-11-06T03:20:46Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:46Z- meanExpectedMeasDiff: 2.383060 delta_rms_squared: 1.859707 alpha: 0.989360 2024-11-06T03:20:46Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:46Z- meanExpectedMeasDiff: -2.577885 delta_rms_squared: 1.380217 alpha: 0.985716 2024-11-06T03:20:46Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:46Z- meanExpectedMeasDiff: -1.881940 delta_rms_squared: 2.362671 alpha: 0.991606 2024-11-06T03:20:46Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:46Z- meanExpectedMeasDiff: 0.651104 delta_rms_squared: 2.138548 alpha: 0.990735 2024-11-06T03:20:46Z- Subcloud NIS: 0.656267 2024-11-06T03:20:46Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:20:46Z- TNavPF:: sumWeights = 0.083929 2024-11-06T03:20:46Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:20:46Z- TNavPF::First Term of Map Covariance Matrix is 0.56 2024-11-06T03:20:46Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T03:20:46Z- TerrainNav::Resampling particle filter... 2024-11-06T03:20:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863245.10 sec. 2024-11-06T03:20:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:20:46Z- variances([0], [2], and psi_berg) = 0.550069 0.630914 0.000000 2024-11-06T03:20:46Z- publishEstimatesDecoder() - at 1730863246.29 sec 2024-11-06T03:20:46Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:20:46Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061545.25 594769.41 49.87 2024-11-06T03:20:46Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061545.75 594769.28 49.57 lat/lon: 36.69 -121.94 2024-11-06T03:20:46Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.55 0.63 0.68 0.00 2024-11-06T03:20:46Z- AHRS_M2 msg: 1730863246.69 epoch sec; seqNo:4386 2024-11-06T03:20:46Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:46Z- handleDepth()- Depth_Keller msg: 1730863246.71 epoch sec; seqNo:4670; depth 50.48 2024-11-06T03:20:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863246.76 epoch sec; seqNo:4670; 4061514.98 north; 594786.45 east; 36.69 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:20:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863246.29 2024-11-06T03:20:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:46Z- waiting for 1730863250.29, time is 1730863246.76 2024-11-06T03:20:47Z- AHRS_M2 msg: 1730863247.09 epoch sec; seqNo:4387 2024-11-06T03:20:47Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:47Z- handleDepth()- Depth_Keller msg: 1730863247.09 epoch sec; seqNo:4671; depth 50.47 2024-11-06T03:20:47Z- handleDVL() - DVL msg epochMillisec: 1730863247.12 2024-11-06T03:20:47Z- handleDvl() - RDI_Pathfinder msg: 1730863247.12 sec; seqNo:1537; #beams:4 2024-11-06T03:20:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.14 , 34.66 , 34.49 , 39.25 2024-11-06T03:20:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.02 2024-11-06T03:20:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863247.13 epoch sec; seqNo:4671; 4061515.36 north; 594786.44 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:20:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863246.29 2024-11-06T03:20:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:47Z- waiting for 1730863250.29, time is 1730863247.13 2024-11-06T03:20:47Z- AHRS_M2 msg: 1730863247.48 epoch sec; seqNo:4388 2024-11-06T03:20:47Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:47Z- handleDepth()- Depth_Keller msg: 1730863247.49 epoch sec; seqNo:4672; depth 50.47 2024-11-06T03:20:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863247.50 epoch sec; seqNo:4672; 4061515.74 north; 594786.44 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:20:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863246.29 2024-11-06T03:20:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:47Z- waiting for 1730863250.29, time is 1730863247.50 2024-11-06T03:20:47Z- AHRS_M2 msg: 1730863247.89 epoch sec; seqNo:4389 2024-11-06T03:20:47Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:47Z- handleDepth()- Depth_Keller msg: 1730863247.89 epoch sec; seqNo:4673; depth 50.47 2024-11-06T03:20:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863247.90 epoch sec; seqNo:4673; 4061516.12 north; 594786.44 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:20:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863246.29 2024-11-06T03:20:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:48Z- waiting for 1730863250.29, time is 1730863247.90 2024-11-06T03:20:48Z- AHRS_M2 msg: 1730863248.29 epoch sec; seqNo:4390 2024-11-06T03:20:48Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:48Z- handleDepth()- Depth_Keller msg: 1730863248.30 epoch sec; seqNo:4674; depth 50.45 2024-11-06T03:20:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863248.31 epoch sec; seqNo:4674; 4061516.12 north; 594786.44 east; 36.69 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:20:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863246.29 2024-11-06T03:20:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:48Z- waiting for 1730863250.29, time is 1730863248.31 2024-11-06T03:20:48Z- AHRS_M2 msg: 1730863248.71 epoch sec; seqNo:4391 2024-11-06T03:20:48Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:48Z- handleDepth()- Depth_Keller msg: 1730863248.75 epoch sec; seqNo:4675; depth 50.44 2024-11-06T03:20:48Z- handleDVL() - DVL msg epochMillisec: 1730863248.78 2024-11-06T03:20:48Z- handleDvl() - RDI_Pathfinder msg: 1730863248.78 sec; seqNo:1538; #beams:4 2024-11-06T03:20:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.14 , 34.83 , 34.32 , 39.25 2024-11-06T03:20:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.05 2024-11-06T03:20:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863248.79 epoch sec; seqNo:4675; 4061516.88 north; 594786.43 east; 36.69 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:20:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863246.29 2024-11-06T03:20:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:48Z- waiting for 1730863250.29, time is 1730863248.79 2024-11-06T03:20:49Z- AHRS_M2 msg: 1730863249.10 epoch sec; seqNo:4392 2024-11-06T03:20:49Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:49Z- handleDepth()- Depth_Keller msg: 1730863249.11 epoch sec; seqNo:4676; depth 50.43 2024-11-06T03:20:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863249.12 epoch sec; seqNo:4676; 4061516.88 north; 594786.43 east; 36.69 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:20:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863246.29 2024-11-06T03:20:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:49Z- waiting for 1730863250.29, time is 1730863249.12 2024-11-06T03:20:49Z- AHRS_M2 msg: 1730863249.50 epoch sec; seqNo:4393 2024-11-06T03:20:49Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:49Z- handleDepth()- Depth_Keller msg: 1730863249.51 epoch sec; seqNo:4677; depth 50.41 2024-11-06T03:20:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863249.53 epoch sec; seqNo:4677; 4061517.26 north; 594786.42 east; 36.69 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:20:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863246.29 2024-11-06T03:20:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:49Z- waiting for 1730863250.29, time is 1730863249.53 2024-11-06T03:20:49Z- AHRS_M2 msg: 1730863249.91 epoch sec; seqNo:4394 2024-11-06T03:20:49Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:49Z- handleDepth()- Depth_Keller msg: 1730863249.91 epoch sec; seqNo:4678; depth 50.41 2024-11-06T03:20:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863249.92 epoch sec; seqNo:4678; 4061517.62 north; 594785.20 east; 36.69 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:20:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863246.29 2024-11-06T03:20:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:50Z- waiting for 1730863250.29, time is 1730863249.92 2024-11-06T03:20:50Z- AHRS_M2 msg: 1730863250.31 epoch sec; seqNo:4395 2024-11-06T03:20:50Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:50Z- handleDepth()- Depth_Keller msg: 1730863250.32 epoch sec; seqNo:4679; depth 50.39 2024-11-06T03:20:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863250.33 epoch sec; seqNo:4679; 4061518.00 north; 594785.19 east; 36.69 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:20:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863246.29 2024-11-06T03:20:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:50Z- waiting for 1730863250.29, time is 1730863250.33 2024-11-06T03:20:50Z- LcmTrn::updateTrn() >>>> pose time 1730863250.33, meas time 1730863248.78 <<<< 2024-11-06T03:20:50Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863250.33 2024-11-06T03:20:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863248.78 sec, ping # 0; waiting for INS data more recent than 1730863246.29... 2024-11-06T03:20:50Z- TerrainNav::motionUpdate() - currEstimate = (4061518, 594785.19, 50.39) 2024-11-06T03:20:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:20:50Z- TerrainNav:: SubcloudNIS is 0.66, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:20:50Z- TNavPF::Projecting Measurements 2024-11-06T03:20:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:20:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:20:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:20:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:20:50Z- TNavFilter::4 good beams. 2024-11-06T03:20:50Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:20:50Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:20:50Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:20:50Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:20:50Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:20:50Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:20:50Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:20:50Z- TerrainMapOctree: (vehN, vehE ) = (4061516.70, 594785.68). 2024-11-06T03:20:50Z- TerrainMapOctree: Min Distance = 236.02. 2024-11-06T03:20:50Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:20:50Z- Forcing Subcloud Comparison 2024-11-06T03:20:50Z- Weighting particles with subcloud comparison 2024-11-06T03:20:50Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:50Z- meanExpectedMeasDiff: 3.210677 delta_rms_squared: 2.028705 alpha: 0.990238 2024-11-06T03:20:50Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:50Z- meanExpectedMeasDiff: -2.181032 delta_rms_squared: 1.054965 alpha: 0.981395 2024-11-06T03:20:50Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:50Z- meanExpectedMeasDiff: -1.595846 delta_rms_squared: 1.072626 alpha: 0.981695 2024-11-06T03:20:50Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:50Z- meanExpectedMeasDiff: 2.044854 delta_rms_squared: 1.205569 alpha: 0.983681 2024-11-06T03:20:50Z- Subcloud NIS: 0.683224 2024-11-06T03:20:50Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:20:50Z- TNavPF:: sumWeights = 0.061513 2024-11-06T03:20:50Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:20:50Z- TNavPF::First Term of Map Covariance Matrix is 0.44 2024-11-06T03:20:50Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.01 2024-11-06T03:20:50Z- TerrainNav::Resampling particle filter... 2024-11-06T03:20:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863248.78 sec. 2024-11-06T03:20:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:20:50Z- variances([0], [2], and psi_berg) = 0.340801 0.255349 0.000000 2024-11-06T03:20:50Z- publishEstimatesDecoder() - at 1730863250.33 sec 2024-11-06T03:20:50Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:20:50Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061548.65 594768.13 49.43 2024-11-06T03:20:50Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061548.99 594767.74 49.26 lat/lon: 36.69 -121.94 2024-11-06T03:20:50Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.34 0.26 0.65 0.00 2024-11-06T03:20:50Z- AHRS_M2 msg: 1730863250.77 epoch sec; seqNo:4396 2024-11-06T03:20:50Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:50Z- handleDepth()- Depth_Keller msg: 1730863250.78 epoch sec; seqNo:4680; depth 50.38 2024-11-06T03:20:50Z- handleDVL() - DVL msg epochMillisec: 1730863250.81 2024-11-06T03:20:50Z- handleDvl() - RDI_Pathfinder msg: 1730863250.81 sec; seqNo:1539; #beams:4 2024-11-06T03:20:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.97 , 33.99 , 33.81 , 39.08 2024-11-06T03:20:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.10 , -0.08 2024-11-06T03:20:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863250.82 epoch sec; seqNo:4680; 4061518.38 north; 594785.19 east; 36.69 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:20:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863250.33 2024-11-06T03:20:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:50Z- waiting for 1730863254.33, time is 1730863250.82 2024-11-06T03:20:51Z- AHRS_M2 msg: 1730863251.12 epoch sec; seqNo:4397 2024-11-06T03:20:51Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:51Z- handleDepth()- Depth_Keller msg: 1730863251.13 epoch sec; seqNo:4681; depth 50.37 2024-11-06T03:20:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863251.14 epoch sec; seqNo:4681; 4061518.76 north; 594785.19 east; 36.69 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:20:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863250.33 2024-11-06T03:20:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:51Z- waiting for 1730863254.33, time is 1730863251.14 2024-11-06T03:20:51Z- AHRS_M2 msg: 1730863251.53 epoch sec; seqNo:4398 2024-11-06T03:20:51Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.01 psi 2024-11-06T03:20:51Z- handleDepth()- Depth_Keller msg: 1730863251.54 epoch sec; seqNo:4682; depth 50.36 2024-11-06T03:20:51Z- handleDVL() - DVL msg epochMillisec: 1730863251.54 2024-11-06T03:20:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:20:51Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:20:51Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:20:51Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:20:51Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:20:51Z- handleDvl() - RDI_Pathfinder msg: 1730863251.54 sec; seqNo:1540; #beams:0 2024-11-06T03:20:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:20:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.10 , -0.08 2024-11-06T03:20:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863251.55 epoch sec; seqNo:4682; 4061519.14 north; 594785.18 east; 36.69 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:20:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863250.33 2024-11-06T03:20:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:51Z- waiting for 1730863254.33, time is 1730863251.55 2024-11-06T03:20:51Z- AHRS_M2 msg: 1730863251.93 epoch sec; seqNo:4399 2024-11-06T03:20:51Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.00 psi 2024-11-06T03:20:51Z- handleDepth()- Depth_Keller msg: 1730863251.94 epoch sec; seqNo:4683; depth 50.35 2024-11-06T03:20:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863251.95 epoch sec; seqNo:4683; 4061519.52 north; 594785.18 east; 36.69 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:20:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863250.33 2024-11-06T03:20:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:52Z- waiting for 1730863254.33, time is 1730863251.95 2024-11-06T03:20:52Z- handleDepth()- Depth_Keller msg: 1730863252.35 epoch sec; seqNo:4684; depth 50.33 2024-11-06T03:20:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863252.36 epoch sec; seqNo:4684; 4061519.52 north; 594785.18 east; 36.69 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:20:52Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:20:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863250.33 2024-11-06T03:20:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:52Z- waiting for 1730863254.33, time is 1730863252.36 2024-11-06T03:20:52Z- AHRS_M2 msg: 1730863252.76 epoch sec; seqNo:4400 2024-11-06T03:20:52Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.00 psi 2024-11-06T03:20:52Z- handleDepth()- Depth_Keller msg: 1730863252.77 epoch sec; seqNo:4685; depth 50.33 2024-11-06T03:20:52Z- handleDVL() - DVL msg epochMillisec: 1730863252.79 2024-11-06T03:20:52Z- handleDvl() - RDI_Pathfinder msg: 1730863252.79 sec; seqNo:1541; #beams:4 2024-11-06T03:20:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.80 , 32.97 , 33.47 , 38.74 2024-11-06T03:20:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.14 , -0.04 2024-11-06T03:20:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863252.81 epoch sec; seqNo:4685; 4061520.27 north; 594785.17 east; 36.69 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:20:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863250.33 2024-11-06T03:20:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:52Z- waiting for 1730863254.33, time is 1730863252.81 2024-11-06T03:20:53Z- AHRS_M2 msg: 1730863253.14 epoch sec; seqNo:4401 2024-11-06T03:20:53Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.00 psi 2024-11-06T03:20:53Z- handleDepth()- Depth_Keller msg: 1730863253.15 epoch sec; seqNo:4686; depth 50.33 2024-11-06T03:20:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863253.17 epoch sec; seqNo:4686; 4061520.27 north; 594785.17 east; 36.69 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:20:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863250.33 2024-11-06T03:20:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:53Z- waiting for 1730863254.33, time is 1730863253.17 2024-11-06T03:20:53Z- handleDepth()- Depth_Keller msg: 1730863253.56 epoch sec; seqNo:4687; depth 50.32 2024-11-06T03:20:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863253.57 epoch sec; seqNo:4687; 4061520.64 north; 594783.94 east; 36.69 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:20:53Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:20:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863250.33 2024-11-06T03:20:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:53Z- waiting for 1730863254.33, time is 1730863253.57 2024-11-06T03:20:53Z- AHRS_M2 msg: 1730863253.95 epoch sec; seqNo:4402 2024-11-06T03:20:53Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.00 psi 2024-11-06T03:20:53Z- handleDepth()- Depth_Keller msg: 1730863253.95 epoch sec; seqNo:4688; depth 50.31 2024-11-06T03:20:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863253.96 epoch sec; seqNo:4688; 4061521.02 north; 594783.94 east; 36.69 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:20:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863250.33 2024-11-06T03:20:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:54Z- waiting for 1730863254.33, time is 1730863253.96 2024-11-06T03:20:54Z- AHRS_M2 msg: 1730863254.35 epoch sec; seqNo:4403 2024-11-06T03:20:54Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 5.99 psi 2024-11-06T03:20:54Z- handleDepth()- Depth_Keller msg: 1730863254.36 epoch sec; seqNo:4689; depth 50.30 2024-11-06T03:20:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863254.37 epoch sec; seqNo:4689; 4061521.40 north; 594783.94 east; 36.69 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:20:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863250.33 2024-11-06T03:20:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:54Z- waiting for 1730863254.33, time is 1730863254.37 2024-11-06T03:20:54Z- LcmTrn::updateTrn() >>>> pose time 1730863254.37, meas time 1730863252.79 <<<< 2024-11-06T03:20:54Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863254.37 2024-11-06T03:20:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863252.79 sec, ping # 0; waiting for INS data more recent than 1730863250.33... 2024-11-06T03:20:54Z- TerrainNav::motionUpdate() - currEstimate = (4061521, 594783.94, 50.30) 2024-11-06T03:20:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:20:54Z- TerrainNav:: SubcloudNIS is 0.68, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:20:54Z- TNavPF::Projecting Measurements 2024-11-06T03:20:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:20:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:20:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:20:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:20:54Z- TNavFilter::4 good beams. 2024-11-06T03:20:54Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:20:54Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:20:54Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:20:54Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:20:54Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:20:54Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:20:54Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:20:54Z- TerrainMapOctree: (vehN, vehE ) = (4061520.07, 594784.43). 2024-11-06T03:20:54Z- TerrainMapOctree: Min Distance = 232.50. 2024-11-06T03:20:54Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:20:54Z- Forcing Subcloud Comparison 2024-11-06T03:20:54Z- Weighting particles with subcloud comparison 2024-11-06T03:20:54Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:54Z- meanExpectedMeasDiff: 3.458958 delta_rms_squared: 1.445903 alpha: 0.986357 2024-11-06T03:20:54Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:54Z- meanExpectedMeasDiff: -4.407617 delta_rms_squared: 1.059922 alpha: 0.981480 2024-11-06T03:20:54Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:54Z- meanExpectedMeasDiff: -2.309160 delta_rms_squared: 1.236585 alpha: 0.984084 2024-11-06T03:20:54Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:20:54Z- meanExpectedMeasDiff: 1.225419 delta_rms_squared: 1.205040 alpha: 0.983674 2024-11-06T03:20:54Z- Subcloud NIS: 0.720700 2024-11-06T03:20:54Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:20:54Z- TNavPF:: sumWeights = 0.005300 2024-11-06T03:20:54Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:20:54Z- TNavPF::First Term of Map Covariance Matrix is 0.08 2024-11-06T03:20:54Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:20:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863252.79 sec. 2024-11-06T03:20:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:20:54Z- variances([0], [2], and psi_berg) = 0.342019 0.256521 0.000000 2024-11-06T03:20:54Z- publishEstimatesDecoder() - at 1730863254.37 sec 2024-11-06T03:20:54Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:20:54Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061550.38 594766.98 50.50 2024-11-06T03:20:54Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061552.38 594766.48 49.49 lat/lon: 36.69 -121.94 2024-11-06T03:20:54Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.34 0.26 0.65 0.00 2024-11-06T03:20:54Z- AHRS_M2 msg: 1730863254.78 epoch sec; seqNo:4404 2024-11-06T03:20:54Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 5.99 psi 2024-11-06T03:20:54Z- handleDepth()- Depth_Keller msg: 1730863254.80 epoch sec; seqNo:4690; depth 50.29 2024-11-06T03:20:54Z- handleDVL() - DVL msg epochMillisec: 1730863254.84 2024-11-06T03:20:54Z- handleDvl() - RDI_Pathfinder msg: 1730863254.84 sec; seqNo:1542; #beams:4 2024-11-06T03:20:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 32.62 , 33.47 , 38.57 2024-11-06T03:20:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.12 , 0.00 2024-11-06T03:20:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863254.86 epoch sec; seqNo:4690; 4061521.77 north; 594783.93 east; 36.69 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:20:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863254.37 2024-11-06T03:20:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:54Z- waiting for 1730863258.37, time is 1730863254.86 2024-11-06T03:20:55Z- AHRS_M2 msg: 1730863255.16 epoch sec; seqNo:4405 2024-11-06T03:20:55Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 5.99 psi 2024-11-06T03:20:55Z- handleDepth()- Depth_Keller msg: 1730863255.17 epoch sec; seqNo:4691; depth 50.29 2024-11-06T03:20:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863255.18 epoch sec; seqNo:4691; 4061522.15 north; 594783.93 east; 36.69 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:20:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863254.37 2024-11-06T03:20:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:55Z- waiting for 1730863258.37, time is 1730863255.18 2024-11-06T03:20:55Z- AHRS_M2 msg: 1730863255.56 epoch sec; seqNo:4406 2024-11-06T03:20:55Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 5.99 psi 2024-11-06T03:20:55Z- handleDepth()- Depth_Keller msg: 1730863255.57 epoch sec; seqNo:4692; depth 50.29 2024-11-06T03:20:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863255.58 epoch sec; seqNo:4692; 4061522.53 north; 594783.92 east; 36.69 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:20:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863254.37 2024-11-06T03:20:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:55Z- waiting for 1730863258.37, time is 1730863255.58 2024-11-06T03:20:55Z- AHRS_M2 msg: 1730863255.97 epoch sec; seqNo:4407 2024-11-06T03:20:55Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 5.99 psi 2024-11-06T03:20:55Z- handleDepth()- Depth_Keller msg: 1730863255.98 epoch sec; seqNo:4693; depth 50.29 2024-11-06T03:20:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863255.99 epoch sec; seqNo:4693; 4061522.91 north; 594783.92 east; 36.69 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:20:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863254.37 2024-11-06T03:20:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:56Z- waiting for 1730863258.37, time is 1730863255.99 2024-11-06T03:20:56Z- AHRS_M2 msg: 1730863256.37 epoch sec; seqNo:4408 2024-11-06T03:20:56Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 5.99 psi 2024-11-06T03:20:56Z- handleDepth()- Depth_Keller msg: 1730863256.38 epoch sec; seqNo:4694; depth 50.28 2024-11-06T03:20:56Z- handleDVL() - DVL msg epochMillisec: 1730863256.38 2024-11-06T03:20:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:20:56Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:20:56Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:20:56Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:20:56Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:20:56Z- handleDvl() - RDI_Pathfinder msg: 1730863256.38 sec; seqNo:1543; #beams:0 2024-11-06T03:20:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:20:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.12 , 0.00 2024-11-06T03:20:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863256.39 epoch sec; seqNo:4694; 4061523.29 north; 594783.92 east; 36.69 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:20:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863254.37 2024-11-06T03:20:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:56Z- waiting for 1730863258.37, time is 1730863256.39 2024-11-06T03:20:56Z- AHRS_M2 msg: 1730863256.79 epoch sec; seqNo:4409 2024-11-06T03:20:56Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 5.99 psi 2024-11-06T03:20:56Z- handleDepth()- Depth_Keller msg: 1730863256.79 epoch sec; seqNo:4695; depth 50.28 2024-11-06T03:20:56Z- handleDVL() - DVL msg epochMillisec: 1730863256.81 2024-11-06T03:20:56Z- handleDvl() - RDI_Pathfinder msg: 1730863256.81 sec; seqNo:1544; #beams:4 2024-11-06T03:20:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.97 , 31.94 , 33.65 , 38.74 2024-11-06T03:20:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.08 , 0.02 2024-11-06T03:20:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863256.84 epoch sec; seqNo:4695; 4061523.66 north; 594782.69 east; 36.69 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:20:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863254.37 2024-11-06T03:20:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:56Z- waiting for 1730863258.37, time is 1730863256.84 2024-11-06T03:20:57Z- AHRS_M2 msg: 1730863257.18 epoch sec; seqNo:4410 2024-11-06T03:20:57Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 5.99 psi 2024-11-06T03:20:57Z- handleDepth()- Depth_Keller msg: 1730863257.20 epoch sec; seqNo:4696; depth 50.27 2024-11-06T03:20:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863257.22 epoch sec; seqNo:4696; 4061524.03 north; 594782.69 east; 36.69 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:20:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863254.37 2024-11-06T03:20:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:57Z- waiting for 1730863258.37, time is 1730863257.22 2024-11-06T03:20:57Z- AHRS_M2 msg: 1730863257.58 epoch sec; seqNo:4411 2024-11-06T03:20:57Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 5.99 psi 2024-11-06T03:20:57Z- handleDepth()- Depth_Keller msg: 1730863257.59 epoch sec; seqNo:4697; depth 50.27 2024-11-06T03:20:57Z- handleDVL() - DVL msg epochMillisec: 1730863257.59 2024-11-06T03:20:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:20:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:20:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:20:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:20:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:20:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:20:57Z- handleDvl() - RDI_Pathfinder msg: 1730863257.59 sec; seqNo:1545; #beams:0 2024-11-06T03:20:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:20:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.08 , 0.02 2024-11-06T03:20:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863257.60 epoch sec; seqNo:4697; 4061524.41 north; 594782.68 east; 36.69 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:20:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863254.37 2024-11-06T03:20:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:57Z- waiting for 1730863258.37, time is 1730863257.60 2024-11-06T03:20:57Z- AHRS_M2 msg: 1730863257.99 epoch sec; seqNo:4412 2024-11-06T03:20:58Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 5.99 psi 2024-11-06T03:20:58Z- handleDepth()- Depth_Keller msg: 1730863257.99 epoch sec; seqNo:4698; depth 50.28 2024-11-06T03:20:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863258.00 epoch sec; seqNo:4698; 4061524.41 north; 594782.68 east; 36.69 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:20:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863254.37 2024-11-06T03:20:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:58Z- waiting for 1730863258.37, time is 1730863258.00 2024-11-06T03:20:58Z- AHRS_M2 msg: 1730863258.41 epoch sec; seqNo:4413 2024-11-06T03:20:58Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 5.99 psi 2024-11-06T03:20:58Z- handleDepth()- Depth_Keller msg: 1730863258.41 epoch sec; seqNo:4699; depth 50.27 2024-11-06T03:20:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863258.43 epoch sec; seqNo:4699; 4061524.79 north; 594782.68 east; 36.69 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:20:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863254.37 2024-11-06T03:20:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:58Z- waiting for 1730863258.37, time is 1730863258.43 2024-11-06T03:20:58Z- LcmTrn::updateTrn() >>>> pose time 1730863258.43, meas time 1730863257.59 <<<< 2024-11-06T03:20:58Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863258.43 2024-11-06T03:20:58Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:20:58Z- TerrainNav::motionUpdate() - currEstimate = (4061525, 594782.68, 50.27) 2024-11-06T03:20:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:20:58Z- TerrainNav:: SubcloudNIS is 0.72, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:20:58Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:20:58Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:20:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:20:58Z- variances([0], [2], and psi_berg) = 0.343104 0.258608 0.000000 2024-11-06T03:20:58Z- publishEstimatesDecoder() - at 1730863258.43 sec 2024-11-06T03:20:58Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:20:58Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061553.82 594765.75 50.98 2024-11-06T03:20:58Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061555.78 594765.22 49.46 lat/lon: 36.69 -121.94 2024-11-06T03:20:58Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.34 0.26 0.91 0.00 2024-11-06T03:20:58Z- AHRS_M2 msg: 1730863258.79 epoch sec; seqNo:4414 2024-11-06T03:20:58Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 5.99 psi 2024-11-06T03:20:58Z- handleDepth()- Depth_Keller msg: 1730863258.80 epoch sec; seqNo:4700; depth 50.27 2024-11-06T03:20:58Z- handleDVL() - DVL msg epochMillisec: 1730863258.83 2024-11-06T03:20:58Z- handleDvl() - RDI_Pathfinder msg: 1730863258.83 sec; seqNo:1546; #beams:4 2024-11-06T03:20:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.31 , 31.10 , 33.31 , 38.74 2024-11-06T03:20:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , 0.01 2024-11-06T03:20:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863258.85 epoch sec; seqNo:4700; 4061525.17 north; 594782.67 east; 36.69 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:20:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:20:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863258.43 2024-11-06T03:20:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:58Z- waiting for 1730863262.43, time is 1730863258.85 2024-11-06T03:20:59Z- AHRS_M2 msg: 1730863259.20 epoch sec; seqNo:4415 2024-11-06T03:20:59Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 5.99 psi 2024-11-06T03:20:59Z- handleDepth()- Depth_Keller msg: 1730863259.20 epoch sec; seqNo:4701; depth 50.24 2024-11-06T03:20:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863259.22 epoch sec; seqNo:4701; 4061525.55 north; 594782.67 east; 36.69 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:20:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863258.43 2024-11-06T03:20:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:59Z- waiting for 1730863262.43, time is 1730863259.22 2024-11-06T03:20:59Z- AHRS_M2 msg: 1730863259.60 epoch sec; seqNo:4416 2024-11-06T03:20:59Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 5.99 psi 2024-11-06T03:20:59Z- handleDepth()- Depth_Keller msg: 1730863259.61 epoch sec; seqNo:4702; depth 50.24 2024-11-06T03:20:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863259.63 epoch sec; seqNo:4702; 4061525.93 north; 594782.67 east; 36.69 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:20:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:20:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863258.43 2024-11-06T03:20:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:20:59Z- waiting for 1730863262.43, time is 1730863259.63 2024-11-06T03:21:00Z- AHRS_M2 msg: 1730863260.01 epoch sec; seqNo:4417 2024-11-06T03:21:00Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 5.99 psi 2024-11-06T03:21:00Z- handleDepth()- Depth_Keller msg: 1730863260.01 epoch sec; seqNo:4703; depth 50.21 2024-11-06T03:21:00Z- handleDVL() - DVL msg epochMillisec: 1730863260.01 2024-11-06T03:21:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:00Z- handleDvl() - RDI_Pathfinder msg: 1730863260.01 sec; seqNo:1547; #beams:0 2024-11-06T03:21:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , 0.01 2024-11-06T03:21:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863260.04 epoch sec; seqNo:4703; 4061526.31 north; 594782.66 east; 36.69 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:21:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863258.43 2024-11-06T03:21:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:00Z- waiting for 1730863262.43, time is 1730863260.04 2024-11-06T03:21:00Z- AHRS_M2 msg: 1730863260.41 epoch sec; seqNo:4418 2024-11-06T03:21:00Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 5.99 psi 2024-11-06T03:21:00Z- handleDepth()- Depth_Keller msg: 1730863260.42 epoch sec; seqNo:4704; depth 50.22 2024-11-06T03:21:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863260.44 epoch sec; seqNo:4704; 4061526.67 north; 594781.44 east; 36.69 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:21:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863258.43 2024-11-06T03:21:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:00Z- waiting for 1730863262.43, time is 1730863260.44 2024-11-06T03:21:00Z- AHRS_M2 msg: 1730863260.82 epoch sec; seqNo:4419 2024-11-06T03:21:00Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 5.99 psi 2024-11-06T03:21:00Z- handleDepth()- Depth_Keller msg: 1730863260.83 epoch sec; seqNo:4705; depth 50.20 2024-11-06T03:21:00Z- handleDVL() - DVL msg epochMillisec: 1730863260.88 2024-11-06T03:21:00Z- handleDvl() - RDI_Pathfinder msg: 1730863260.88 sec; seqNo:1548; #beams:4 2024-11-06T03:21:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.14 , 30.59 , 33.13 , 38.74 2024-11-06T03:21:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , -0.02 2024-11-06T03:21:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863260.89 epoch sec; seqNo:4705; 4061527.05 north; 594781.43 east; 36.69 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:21:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863258.43 2024-11-06T03:21:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:00Z- waiting for 1730863262.43, time is 1730863260.89 2024-11-06T03:21:01Z- AHRS_M2 msg: 1730863261.22 epoch sec; seqNo:4420 2024-11-06T03:21:01Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:01Z- handleDepth()- Depth_Keller msg: 1730863261.23 epoch sec; seqNo:4706; depth 50.20 2024-11-06T03:21:01Z- handleDVL() - DVL msg epochMillisec: 1730863261.23 2024-11-06T03:21:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:01Z- handleDvl() - RDI_Pathfinder msg: 1730863261.23 sec; seqNo:1549; #beams:0 2024-11-06T03:21:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , -0.02 2024-11-06T03:21:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863261.24 epoch sec; seqNo:4706; 4061527.43 north; 594781.43 east; 36.69 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:21:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863258.43 2024-11-06T03:21:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:01Z- waiting for 1730863262.43, time is 1730863261.24 2024-11-06T03:21:01Z- AHRS_M2 msg: 1730863261.63 epoch sec; seqNo:4421 2024-11-06T03:21:01Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:01Z- handleDepth()- Depth_Keller msg: 1730863261.63 epoch sec; seqNo:4707; depth 50.18 2024-11-06T03:21:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863261.65 epoch sec; seqNo:4707; 4061527.81 north; 594781.42 east; 36.69 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:21:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863258.43 2024-11-06T03:21:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:01Z- waiting for 1730863262.43, time is 1730863261.65 2024-11-06T03:21:02Z- AHRS_M2 msg: 1730863262.03 epoch sec; seqNo:4422 2024-11-06T03:21:02Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:02Z- handleDepth()- Depth_Keller msg: 1730863262.04 epoch sec; seqNo:4708; depth 50.17 2024-11-06T03:21:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863262.05 epoch sec; seqNo:4708; 4061528.19 north; 594781.42 east; 36.69 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:21:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863258.43 2024-11-06T03:21:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:02Z- waiting for 1730863262.43, time is 1730863262.05 2024-11-06T03:21:02Z- AHRS_M2 msg: 1730863262.43 epoch sec; seqNo:4423 2024-11-06T03:21:02Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:02Z- handleDepth()- Depth_Keller msg: 1730863262.44 epoch sec; seqNo:4709; depth 50.16 2024-11-06T03:21:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863262.45 epoch sec; seqNo:4709; 4061528.57 north; 594781.42 east; 36.69 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:21:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863258.43 2024-11-06T03:21:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:02Z- waiting for 1730863262.43, time is 1730863262.45 2024-11-06T03:21:02Z- LcmTrn::updateTrn() >>>> pose time 1730863262.45, meas time 1730863261.23 <<<< 2024-11-06T03:21:02Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863262.45 2024-11-06T03:21:02Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:21:02Z- TerrainNav::motionUpdate() - currEstimate = (4061529, 594781.42, 50.16) 2024-11-06T03:21:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:21:02Z- TerrainNav:: SubcloudNIS is 0.72, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:21:02Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:21:02Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:21:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:21:02Z- variances([0], [2], and psi_berg) = 0.345722 0.261094 0.000000 2024-11-06T03:21:02Z- publishEstimatesDecoder() - at 1730863262.45 sec 2024-11-06T03:21:02Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:21:02Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061557.54 594764.49 50.63 2024-11-06T03:21:02Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061559.55 594763.96 49.35 lat/lon: 36.69 -121.94 2024-11-06T03:21:02Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.35 0.26 1.17 0.00 2024-11-06T03:21:02Z- AHRS_M2 msg: 1730863262.84 epoch sec; seqNo:4424 2024-11-06T03:21:02Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:02Z- handleDepth()- Depth_Keller msg: 1730863262.86 epoch sec; seqNo:4710; depth 50.13 2024-11-06T03:21:02Z- handleDVL() - DVL msg epochMillisec: 1730863262.88 2024-11-06T03:21:02Z- handleDvl() - RDI_Pathfinder msg: 1730863262.88 sec; seqNo:1550; #beams:4 2024-11-06T03:21:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.65 , 30.25 , 32.62 , 38.91 2024-11-06T03:21:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , -0.05 2024-11-06T03:21:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863262.89 epoch sec; seqNo:4710; 4061528.95 north; 594781.41 east; 36.69 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:21:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863262.45 2024-11-06T03:21:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:03Z- waiting for 1730863266.45, time is 1730863262.89 2024-11-06T03:21:03Z- AHRS_M2 msg: 1730863263.24 epoch sec; seqNo:4425 2024-11-06T03:21:03Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:03Z- handleDepth()- Depth_Keller msg: 1730863263.25 epoch sec; seqNo:4711; depth 50.12 2024-11-06T03:21:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863263.28 epoch sec; seqNo:4711; 4061528.95 north; 594781.41 east; 36.69 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:21:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863262.45 2024-11-06T03:21:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:03Z- waiting for 1730863266.45, time is 1730863263.28 2024-11-06T03:21:03Z- AHRS_M2 msg: 1730863263.65 epoch sec; seqNo:4426 2024-11-06T03:21:03Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.01 psi 2024-11-06T03:21:03Z- handleDepth()- Depth_Keller msg: 1730863263.66 epoch sec; seqNo:4712; depth 50.12 2024-11-06T03:21:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863263.67 epoch sec; seqNo:4712; 4061529.32 north; 594781.41 east; 36.69 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:21:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863262.45 2024-11-06T03:21:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:03Z- waiting for 1730863266.45, time is 1730863263.67 2024-11-06T03:21:04Z- AHRS_M2 msg: 1730863264.06 epoch sec; seqNo:4427 2024-11-06T03:21:04Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:04Z- handleDepth()- Depth_Keller msg: 1730863264.07 epoch sec; seqNo:4713; depth 50.10 2024-11-06T03:21:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863264.08 epoch sec; seqNo:4713; 4061529.69 north; 594780.18 east; 36.69 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:21:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863262.45 2024-11-06T03:21:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:04Z- waiting for 1730863266.45, time is 1730863264.08 2024-11-06T03:21:04Z- AHRS_M2 msg: 1730863264.46 epoch sec; seqNo:4428 2024-11-06T03:21:04Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:04Z- handleDepth()- Depth_Keller msg: 1730863264.47 epoch sec; seqNo:4714; depth 50.09 2024-11-06T03:21:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863264.48 epoch sec; seqNo:4714; 4061530.07 north; 594780.18 east; 36.69 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:21:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863262.45 2024-11-06T03:21:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:04Z- waiting for 1730863266.45, time is 1730863264.48 2024-11-06T03:21:04Z- AHRS_M2 msg: 1730863264.85 epoch sec; seqNo:4429 2024-11-06T03:21:04Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:04Z- handleDepth()- Depth_Keller msg: 1730863264.86 epoch sec; seqNo:4715; depth 50.08 2024-11-06T03:21:04Z- handleDVL() - DVL msg epochMillisec: 1730863264.90 2024-11-06T03:21:04Z- handleDvl() - RDI_Pathfinder msg: 1730863264.90 sec; seqNo:1551; #beams:4 2024-11-06T03:21:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.48 , 29.91 , 32.79 , 38.74 2024-11-06T03:21:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.15 , -0.04 2024-11-06T03:21:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863264.91 epoch sec; seqNo:4715; 4061530.45 north; 594780.17 east; 36.69 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:21:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863262.45 2024-11-06T03:21:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:05Z- waiting for 1730863266.45, time is 1730863264.91 2024-11-06T03:21:05Z- AHRS_M2 msg: 1730863265.26 epoch sec; seqNo:4430 2024-11-06T03:21:05Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:05Z- handleDepth()- Depth_Keller msg: 1730863265.27 epoch sec; seqNo:4716; depth 50.08 2024-11-06T03:21:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863265.28 epoch sec; seqNo:4716; 4061530.83 north; 594780.17 east; 36.69 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:21:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863262.45 2024-11-06T03:21:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:05Z- waiting for 1730863266.45, time is 1730863265.28 2024-11-06T03:21:05Z- AHRS_M2 msg: 1730863265.66 epoch sec; seqNo:4431 2024-11-06T03:21:05Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:05Z- handleDepth()- Depth_Keller msg: 1730863265.68 epoch sec; seqNo:4717; depth 50.06 2024-11-06T03:21:05Z- handleDVL() - DVL msg epochMillisec: 1730863265.70 2024-11-06T03:21:05Z- handleDvl() - RDI_Pathfinder msg: 1730863265.70 sec; seqNo:1552; #beams:4 2024-11-06T03:21:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.65 , 29.74 , 32.62 , 38.74 2024-11-06T03:21:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.14 , -0.01 2024-11-06T03:21:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863265.71 epoch sec; seqNo:4717; 4061531.21 north; 594780.17 east; 36.69 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:21:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863262.45 2024-11-06T03:21:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:05Z- waiting for 1730863266.45, time is 1730863265.71 2024-11-06T03:21:06Z- AHRS_M2 msg: 1730863266.07 epoch sec; seqNo:4432 2024-11-06T03:21:06Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:06Z- handleDepth()- Depth_Keller msg: 1730863266.08 epoch sec; seqNo:4718; depth 50.04 2024-11-06T03:21:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863266.12 epoch sec; seqNo:4718; 4061531.58 north; 594780.16 east; 36.69 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:21:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863262.45 2024-11-06T03:21:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:06Z- waiting for 1730863266.45, time is 1730863266.12 2024-11-06T03:21:06Z- AHRS_M2 msg: 1730863266.47 epoch sec; seqNo:4433 2024-11-06T03:21:06Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:06Z- handleDepth()- Depth_Keller msg: 1730863266.48 epoch sec; seqNo:4719; depth 50.05 2024-11-06T03:21:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863266.49 epoch sec; seqNo:4719; 4061531.96 north; 594780.16 east; 36.69 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:21:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863262.45 2024-11-06T03:21:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:06Z- waiting for 1730863266.45, time is 1730863266.49 2024-11-06T03:21:06Z- LcmTrn::updateTrn() >>>> pose time 1730863266.49, meas time 1730863265.70 <<<< 2024-11-06T03:21:06Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863266.49 2024-11-06T03:21:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863265.70 sec, ping # 0; waiting for INS data more recent than 1730863262.45... 2024-11-06T03:21:06Z- TerrainNav::motionUpdate() - currEstimate = (4061532, 594780.16, 50.05) 2024-11-06T03:21:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:21:06Z- TerrainNav:: SubcloudNIS is 0.72, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:21:06Z- TNavPF::Projecting Measurements 2024-11-06T03:21:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:21:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:21:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:21:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:21:06Z- TNavFilter::4 good beams. 2024-11-06T03:21:06Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:21:06Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:21:06Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:21:06Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:21:06Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:21:06Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:21:06Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:21:06Z- TerrainMapOctree: (vehN, vehE ) = (4061531.29, 594780.41). 2024-11-06T03:21:06Z- TerrainMapOctree: Min Distance = 220.81. 2024-11-06T03:21:06Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:21:06Z- Forcing Subcloud Comparison 2024-11-06T03:21:06Z- Weighting particles with subcloud comparison 2024-11-06T03:21:06Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:06Z- meanExpectedMeasDiff: 5.881561 delta_rms_squared: 1.036544 alpha: 0.981070 2024-11-06T03:21:06Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:06Z- meanExpectedMeasDiff: -7.285633 delta_rms_squared: 1.626022 alpha: 0.987849 2024-11-06T03:21:06Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:06Z- meanExpectedMeasDiff: -4.273642 delta_rms_squared: 1.602868 alpha: 0.987676 2024-11-06T03:21:06Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:06Z- meanExpectedMeasDiff: 1.289935 delta_rms_squared: 1.897478 alpha: 0.989570 2024-11-06T03:21:06Z- Subcloud NIS: 0.766299 2024-11-06T03:21:06Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:21:06Z- TNavPF:: sumWeights = 0.000001 2024-11-06T03:21:06Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:21:06Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:21:06Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:21:06Z- TerrainNav::Resampling particle filter... 2024-11-06T03:21:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863265.70 sec. 2024-11-06T03:21:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:21:06Z- variances([0], [2], and psi_berg) = 0.336217 0.229318 0.000000 2024-11-06T03:21:06Z- publishEstimatesDecoder() - at 1730863266.49 sec 2024-11-06T03:21:06Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:21:06Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061562.59 594762.94 50.57 2024-11-06T03:21:06Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061562.92 594762.76 50.42 lat/lon: 36.69 -121.94 2024-11-06T03:21:06Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.34 0.23 0.68 0.00 2024-11-06T03:21:06Z- AHRS_M2 msg: 1730863266.88 epoch sec; seqNo:4434 2024-11-06T03:21:06Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:06Z- handleDepth()- Depth_Keller msg: 1730863266.88 epoch sec; seqNo:4720; depth 50.06 2024-11-06T03:21:06Z- handleDVL() - DVL msg epochMillisec: 1730863266.92 2024-11-06T03:21:06Z- handleDvl() - RDI_Pathfinder msg: 1730863266.92 sec; seqNo:1553; #beams:4 2024-11-06T03:21:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.31 , 29.74 , 32.62 , 38.91 2024-11-06T03:21:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.14 , 0.01 2024-11-06T03:21:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863266.94 epoch sec; seqNo:4720; 4061532.34 north; 594780.15 east; 36.69 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:21:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863266.49 2024-11-06T03:21:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:07Z- waiting for 1730863270.49, time is 1730863266.94 2024-11-06T03:21:07Z- AHRS_M2 msg: 1730863267.28 epoch sec; seqNo:4435 2024-11-06T03:21:07Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:07Z- handleDepth()- Depth_Keller msg: 1730863267.29 epoch sec; seqNo:4721; depth 50.03 2024-11-06T03:21:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863267.30 epoch sec; seqNo:4721; 4061532.33 north; 594778.93 east; 36.69 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:21:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863266.49 2024-11-06T03:21:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:07Z- waiting for 1730863270.49, time is 1730863267.30 2024-11-06T03:21:07Z- AHRS_M2 msg: 1730863267.69 epoch sec; seqNo:4436 2024-11-06T03:21:07Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:07Z- handleDepth()- Depth_Keller msg: 1730863267.69 epoch sec; seqNo:4722; depth 50.02 2024-11-06T03:21:07Z- handleDVL() - DVL msg epochMillisec: 1730863267.72 2024-11-06T03:21:07Z- handleDvl() - RDI_Pathfinder msg: 1730863267.72 sec; seqNo:1554; #beams:4 2024-11-06T03:21:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.14 , 29.57 , 32.46 , 38.91 2024-11-06T03:21:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , 0.03 2024-11-06T03:21:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863267.75 epoch sec; seqNo:4722; 4061532.71 north; 594778.93 east; 36.69 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:21:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863266.49 2024-11-06T03:21:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:07Z- waiting for 1730863270.49, time is 1730863267.75 2024-11-06T03:21:08Z- AHRS_M2 msg: 1730863268.09 epoch sec; seqNo:4437 2024-11-06T03:21:08Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:08Z- handleDepth()- Depth_Keller msg: 1730863268.10 epoch sec; seqNo:4723; depth 50.03 2024-11-06T03:21:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863268.12 epoch sec; seqNo:4723; 4061533.09 north; 594778.92 east; 36.69 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:21:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863266.49 2024-11-06T03:21:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:08Z- waiting for 1730863270.49, time is 1730863268.12 2024-11-06T03:21:08Z- AHRS_M2 msg: 1730863268.50 epoch sec; seqNo:4438 2024-11-06T03:21:08Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:08Z- handleDepth()- Depth_Keller msg: 1730863268.50 epoch sec; seqNo:4724; depth 50.00 2024-11-06T03:21:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863268.53 epoch sec; seqNo:4724; 4061533.46 north; 594778.92 east; 36.69 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:21:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863266.49 2024-11-06T03:21:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:08Z- waiting for 1730863270.49, time is 1730863268.53 2024-11-06T03:21:08Z- AHRS_M2 msg: 1730863268.90 epoch sec; seqNo:4439 2024-11-06T03:21:08Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:08Z- handleDepth()- Depth_Keller msg: 1730863268.91 epoch sec; seqNo:4725; depth 50.00 2024-11-06T03:21:08Z- handleDVL() - DVL msg epochMillisec: 1730863268.93 2024-11-06T03:21:08Z- handleDvl() - RDI_Pathfinder msg: 1730863268.93 sec; seqNo:1555; #beams:4 2024-11-06T03:21:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.82 , 29.57 , 32.46 , 38.91 2024-11-06T03:21:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.07 , 0.04 2024-11-06T03:21:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863268.94 epoch sec; seqNo:4725; 4061533.84 north; 594778.92 east; 36.69 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:21:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863266.49 2024-11-06T03:21:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:09Z- waiting for 1730863270.49, time is 1730863268.94 2024-11-06T03:21:09Z- AHRS_M2 msg: 1730863269.30 epoch sec; seqNo:4440 2024-11-06T03:21:09Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:09Z- handleDepth()- Depth_Keller msg: 1730863269.31 epoch sec; seqNo:4726; depth 50.00 2024-11-06T03:21:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863269.33 epoch sec; seqNo:4726; 4061534.22 north; 594778.91 east; 36.69 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:21:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863266.49 2024-11-06T03:21:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:09Z- waiting for 1730863270.49, time is 1730863269.33 2024-11-06T03:21:09Z- AHRS_M2 msg: 1730863269.71 epoch sec; seqNo:4441 2024-11-06T03:21:09Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:09Z- handleDepth()- Depth_Keller msg: 1730863269.72 epoch sec; seqNo:4727; depth 49.98 2024-11-06T03:21:09Z- handleDVL() - DVL msg epochMillisec: 1730863269.75 2024-11-06T03:21:09Z- handleDvl() - RDI_Pathfinder msg: 1730863269.75 sec; seqNo:1556; #beams:4 2024-11-06T03:21:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.31 , 29.74 , 32.28 , 38.91 2024-11-06T03:21:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , 0.03 2024-11-06T03:21:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863269.76 epoch sec; seqNo:4727; 4061534.60 north; 594778.91 east; 36.69 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:21:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863266.49 2024-11-06T03:21:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:09Z- waiting for 1730863270.49, time is 1730863269.76 2024-11-06T03:21:10Z- AHRS_M2 msg: 1730863270.13 epoch sec; seqNo:4442 2024-11-06T03:21:10Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:10Z- handleDepth()- Depth_Keller msg: 1730863270.13 epoch sec; seqNo:4728; depth 49.97 2024-11-06T03:21:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863270.16 epoch sec; seqNo:4728; 4061534.98 north; 594778.90 east; 36.69 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:21:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863266.49 2024-11-06T03:21:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:10Z- waiting for 1730863270.49, time is 1730863270.16 2024-11-06T03:21:10Z- AHRS_M2 msg: 1730863270.51 epoch sec; seqNo:4443 2024-11-06T03:21:10Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:10Z- handleDepth()- Depth_Keller msg: 1730863270.52 epoch sec; seqNo:4729; depth 49.96 2024-11-06T03:21:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863270.54 epoch sec; seqNo:4729; 4061535.36 north; 594778.90 east; 36.69 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:21:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863266.49 2024-11-06T03:21:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:10Z- waiting for 1730863270.49, time is 1730863270.54 2024-11-06T03:21:10Z- LcmTrn::updateTrn() >>>> pose time 1730863270.54, meas time 1730863269.75 <<<< 2024-11-06T03:21:10Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863270.54 2024-11-06T03:21:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863269.75 sec, ping # 0; waiting for INS data more recent than 1730863266.49... 2024-11-06T03:21:10Z- TerrainNav::motionUpdate() - currEstimate = (4061535, 594778.90, 49.96) 2024-11-06T03:21:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:21:10Z- TerrainNav:: SubcloudNIS is 0.77, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:21:10Z- TNavPF::Projecting Measurements 2024-11-06T03:21:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:21:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:21:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:21:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:21:10Z- TNavFilter::4 good beams. 2024-11-06T03:21:10Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:21:10Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:21:10Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:21:10Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:21:10Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:21:10Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:21:10Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:21:10Z- TerrainMapOctree: (vehN, vehE ) = (4061534.70, 594779.14). 2024-11-06T03:21:10Z- TerrainMapOctree: Min Distance = 217.27. 2024-11-06T03:21:10Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:21:10Z- Forcing Subcloud Comparison 2024-11-06T03:21:10Z- Weighting particles with subcloud comparison 2024-11-06T03:21:10Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:10Z- meanExpectedMeasDiff: 7.241927 delta_rms_squared: 1.374908 alpha: 0.985662 2024-11-06T03:21:10Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:10Z- meanExpectedMeasDiff: -6.072901 delta_rms_squared: 1.060413 alpha: 0.981489 2024-11-06T03:21:10Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:10Z- meanExpectedMeasDiff: -3.282921 delta_rms_squared: 1.400939 alpha: 0.985925 2024-11-06T03:21:10Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:10Z- meanExpectedMeasDiff: 2.775884 delta_rms_squared: 1.621675 alpha: 0.987817 2024-11-06T03:21:10Z- Subcloud NIS: 0.828457 2024-11-06T03:21:10Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:21:10Z- TNavPF:: sumWeights = 0.000001 2024-11-06T03:21:10Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:21:10Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:21:10Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:21:10Z- TerrainNav::Resampling particle filter... 2024-11-06T03:21:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863269.75 sec. 2024-11-06T03:21:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:21:10Z- variances([0], [2], and psi_berg) = 0.421340 0.320260 0.000000 2024-11-06T03:21:10Z- publishEstimatesDecoder() - at 1730863270.54 sec 2024-11-06T03:21:10Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:21:10Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061566.06 594761.78 50.29 2024-11-06T03:21:10Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061566.15 594761.56 50.52 lat/lon: 36.70 -121.94 2024-11-06T03:21:10Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.42 0.32 0.74 0.00 2024-11-06T03:21:10Z- AHRS_M2 msg: 1730863270.91 epoch sec; seqNo:4444 2024-11-06T03:21:10Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:10Z- handleDepth()- Depth_Keller msg: 1730863270.92 epoch sec; seqNo:4730; depth 49.94 2024-11-06T03:21:10Z- handleDVL() - DVL msg epochMillisec: 1730863270.96 2024-11-06T03:21:10Z- handleDvl() - RDI_Pathfinder msg: 1730863270.96 sec; seqNo:1557; #beams:4 2024-11-06T03:21:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.31 , 29.57 , 32.12 , 38.91 2024-11-06T03:21:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , 0.01 2024-11-06T03:21:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863271.00 epoch sec; seqNo:4730; 4061535.72 north; 594777.67 east; 36.69 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:21:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863270.54 2024-11-06T03:21:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:11Z- waiting for 1730863274.54, time is 1730863271.00 2024-11-06T03:21:11Z- AHRS_M2 msg: 1730863271.32 epoch sec; seqNo:4445 2024-11-06T03:21:11Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:11Z- handleDepth()- Depth_Keller msg: 1730863271.33 epoch sec; seqNo:4731; depth 49.93 2024-11-06T03:21:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863271.34 epoch sec; seqNo:4731; 4061535.72 north; 594777.67 east; 36.69 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:21:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863270.54 2024-11-06T03:21:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:11Z- waiting for 1730863274.54, time is 1730863271.34 2024-11-06T03:21:11Z- AHRS_M2 msg: 1730863271.74 epoch sec; seqNo:4446 2024-11-06T03:21:11Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 6.00 psi 2024-11-06T03:21:11Z- handleDepth()- Depth_Keller msg: 1730863271.75 epoch sec; seqNo:4732; depth 49.91 2024-11-06T03:21:11Z- handleDVL() - DVL msg epochMillisec: 1730863271.77 2024-11-06T03:21:11Z- handleDvl() - RDI_Pathfinder msg: 1730863271.77 sec; seqNo:1558; #beams:4 2024-11-06T03:21:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.80 , 28.72 , 31.78 , 39.08 2024-11-06T03:21:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , -0.01 2024-11-06T03:21:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863271.79 epoch sec; seqNo:4732; 4061536.48 north; 594777.67 east; 36.69 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:21:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863270.54 2024-11-06T03:21:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:11Z- waiting for 1730863274.54, time is 1730863271.79 2024-11-06T03:21:12Z- AHRS_M2 msg: 1730863272.13 epoch sec; seqNo:4447 2024-11-06T03:21:12Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 6.01 psi 2024-11-06T03:21:12Z- handleDepth()- Depth_Keller msg: 1730863272.14 epoch sec; seqNo:4733; depth 49.90 2024-11-06T03:21:12Z- handleDVL() - DVL msg epochMillisec: 1730863272.15 2024-11-06T03:21:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:12Z- handleDvl() - RDI_Pathfinder msg: 1730863272.15 sec; seqNo:1559; #beams:0 2024-11-06T03:21:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , -0.01 2024-11-06T03:21:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863272.16 epoch sec; seqNo:4733; 4061536.48 north; 594777.67 east; 36.69 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:21:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863270.54 2024-11-06T03:21:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:12Z- waiting for 1730863274.54, time is 1730863272.16 2024-11-06T03:21:12Z- AHRS_M2 msg: 1730863272.53 epoch sec; seqNo:4448 2024-11-06T03:21:12Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 6.01 psi 2024-11-06T03:21:12Z- handleDepth()- Depth_Keller msg: 1730863272.54 epoch sec; seqNo:4734; depth 49.89 2024-11-06T03:21:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863272.55 epoch sec; seqNo:4734; 4061536.86 north; 594777.66 east; 36.69 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:21:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863270.54 2024-11-06T03:21:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:12Z- waiting for 1730863274.54, time is 1730863272.55 2024-11-06T03:21:12Z- AHRS_M2 msg: 1730863272.93 epoch sec; seqNo:4449 2024-11-06T03:21:12Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 6.01 psi 2024-11-06T03:21:12Z- handleDepth()- Depth_Keller msg: 1730863272.94 epoch sec; seqNo:4735; depth 49.87 2024-11-06T03:21:12Z- handleDVL() - DVL msg epochMillisec: 1730863272.96 2024-11-06T03:21:12Z- handleDvl() - RDI_Pathfinder msg: 1730863272.96 sec; seqNo:1560; #beams:4 2024-11-06T03:21:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 28.72 , 31.44 , 39.08 2024-11-06T03:21:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.03 2024-11-06T03:21:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863273.00 epoch sec; seqNo:4735; 4061537.24 north; 594777.66 east; 36.69 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:21:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863270.54 2024-11-06T03:21:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:13Z- waiting for 1730863274.54, time is 1730863273.00 2024-11-06T03:21:13Z- AHRS_M2 msg: 1730863273.34 epoch sec; seqNo:4450 2024-11-06T03:21:13Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 6.01 psi 2024-11-06T03:21:13Z- handleDepth()- Depth_Keller msg: 1730863273.35 epoch sec; seqNo:4736; depth 49.85 2024-11-06T03:21:13Z- handleDVL() - DVL msg epochMillisec: 1730863273.35 2024-11-06T03:21:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:13Z- handleDvl() - RDI_Pathfinder msg: 1730863273.35 sec; seqNo:1561; #beams:0 2024-11-06T03:21:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.03 2024-11-06T03:21:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863273.36 epoch sec; seqNo:4736; 4061537.62 north; 594777.65 east; 36.69 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:21:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863270.54 2024-11-06T03:21:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:13Z- waiting for 1730863274.54, time is 1730863273.36 2024-11-06T03:21:13Z- AHRS_M2 msg: 1730863273.77 epoch sec; seqNo:4451 2024-11-06T03:21:13Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 6.01 psi 2024-11-06T03:21:13Z- handleDepth()- Depth_Keller msg: 1730863273.78 epoch sec; seqNo:4737; depth 49.83 2024-11-06T03:21:13Z- handleDVL() - DVL msg epochMillisec: 1730863273.80 2024-11-06T03:21:13Z- handleDvl() - RDI_Pathfinder msg: 1730863273.80 sec; seqNo:1562; #beams:4 2024-11-06T03:21:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 28.55 , 31.44 , 39.08 2024-11-06T03:21:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.05 2024-11-06T03:21:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863273.82 epoch sec; seqNo:4737; 4061538.00 north; 594777.65 east; 36.69 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:21:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863270.54 2024-11-06T03:21:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:13Z- waiting for 1730863274.54, time is 1730863273.82 2024-11-06T03:21:14Z- AHRS_M2 msg: 1730863274.15 epoch sec; seqNo:4452 2024-11-06T03:21:14Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 6.01 psi 2024-11-06T03:21:14Z- handleDepth()- Depth_Keller msg: 1730863274.16 epoch sec; seqNo:4738; depth 49.81 2024-11-06T03:21:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863274.18 epoch sec; seqNo:4738; 4061538.38 north; 594777.65 east; 36.69 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:21:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863270.54 2024-11-06T03:21:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:14Z- waiting for 1730863274.54, time is 1730863274.18 2024-11-06T03:21:14Z- AHRS_M2 msg: 1730863274.55 epoch sec; seqNo:4453 2024-11-06T03:21:14Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 6.01 psi 2024-11-06T03:21:14Z- handleDepth()- Depth_Keller msg: 1730863274.56 epoch sec; seqNo:4739; depth 49.79 2024-11-06T03:21:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863274.56 epoch sec; seqNo:4739; 4061538.76 north; 594777.64 east; 36.69 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:21:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863270.54 2024-11-06T03:21:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:14Z- waiting for 1730863274.54, time is 1730863274.56 2024-11-06T03:21:14Z- LcmTrn::updateTrn() >>>> pose time 1730863274.56, meas time 1730863273.80 <<<< 2024-11-06T03:21:14Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863274.56 2024-11-06T03:21:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863273.80 sec, ping # 0; waiting for INS data more recent than 1730863270.54... 2024-11-06T03:21:14Z- TerrainNav::motionUpdate() - currEstimate = (4061539, 594777.64, 49.79) 2024-11-06T03:21:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:21:14Z- TerrainNav:: SubcloudNIS is 0.83, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:21:14Z- TNavPF::Projecting Measurements 2024-11-06T03:21:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:21:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:21:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:21:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:21:14Z- TNavFilter::4 good beams. 2024-11-06T03:21:14Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:21:14Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:21:14Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:21:14Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:21:14Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:21:14Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:21:14Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:21:14Z- TerrainMapOctree: (vehN, vehE ) = (4061538.11, 594777.88). 2024-11-06T03:21:14Z- TerrainMapOctree: Min Distance = 213.72. 2024-11-06T03:21:14Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:21:14Z- Forcing Subcloud Comparison 2024-11-06T03:21:14Z- Weighting particles with subcloud comparison 2024-11-06T03:21:14Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:14Z- meanExpectedMeasDiff: 7.242207 delta_rms_squared: 5.281690 alpha: 0.996228 2024-11-06T03:21:14Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:14Z- meanExpectedMeasDiff: -7.196948 delta_rms_squared: 1.181520 alpha: 0.983354 2024-11-06T03:21:14Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:14Z- meanExpectedMeasDiff: -4.304072 delta_rms_squared: 1.385496 alpha: 0.985770 2024-11-06T03:21:14Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:14Z- meanExpectedMeasDiff: 3.918452 delta_rms_squared: 1.317639 alpha: 0.985048 2024-11-06T03:21:14Z- Subcloud NIS: 0.897627 2024-11-06T03:21:14Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:21:14Z- TNavPF:: sumWeights = 0.000000 2024-11-06T03:21:14Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:21:14Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:21:14Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:21:14Z- TerrainNav::Resampling particle filter... 2024-11-06T03:21:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863273.80 sec. 2024-11-06T03:21:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:21:14Z- variances([0], [2], and psi_berg) = 0.353961 0.205695 0.000000 2024-11-06T03:21:14Z- publishEstimatesDecoder() - at 1730863274.56 sec 2024-11-06T03:21:14Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:21:14Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061569.46 594760.53 50.43 2024-11-06T03:21:14Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061569.38 594760.23 50.77 lat/lon: 36.70 -121.94 2024-11-06T03:21:14Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.35 0.21 0.60 0.00 2024-11-06T03:21:14Z- AHRS_M2 msg: 1730863274.95 epoch sec; seqNo:4454 2024-11-06T03:21:14Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 6.01 psi 2024-11-06T03:21:14Z- handleDepth()- Depth_Keller msg: 1730863274.97 epoch sec; seqNo:4740; depth 49.77 2024-11-06T03:21:15Z- handleDVL() - DVL msg epochMillisec: 1730863275.00 2024-11-06T03:21:15Z- handleDvl() - RDI_Pathfinder msg: 1730863275.00 sec; seqNo:1563; #beams:4 2024-11-06T03:21:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.29 , 28.38 , 31.61 , 38.74 2024-11-06T03:21:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.09 , -0.06 2024-11-06T03:21:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863275.01 epoch sec; seqNo:4740; 4061539.12 north; 594776.42 east; 36.69 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:21:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863274.56 2024-11-06T03:21:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:15Z- waiting for 1730863278.56, time is 1730863275.01 2024-11-06T03:21:15Z- AHRS_M2 msg: 1730863275.36 epoch sec; seqNo:4455 2024-11-06T03:21:15Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.01 psi 2024-11-06T03:21:15Z- handleDepth()- Depth_Keller msg: 1730863275.36 epoch sec; seqNo:4741; depth 49.75 2024-11-06T03:21:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863275.38 epoch sec; seqNo:4741; 4061539.50 north; 594776.41 east; 36.69 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:21:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863274.56 2024-11-06T03:21:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:15Z- waiting for 1730863278.56, time is 1730863275.38 2024-11-06T03:21:15Z- AHRS_M2 msg: 1730863275.80 epoch sec; seqNo:4456 2024-11-06T03:21:15Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.01 psi 2024-11-06T03:21:15Z- handleDepth()- Depth_Keller msg: 1730863275.80 epoch sec; seqNo:4742; depth 49.75 2024-11-06T03:21:15Z- handleDVL() - DVL msg epochMillisec: 1730863275.83 2024-11-06T03:21:15Z- handleDvl() - RDI_Pathfinder msg: 1730863275.83 sec; seqNo:1564; #beams:4 2024-11-06T03:21:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.12 , 28.72 , 31.44 , 38.91 2024-11-06T03:21:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.13 , -0.05 2024-11-06T03:21:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863275.84 epoch sec; seqNo:4742; 4061539.88 north; 594776.41 east; 36.69 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:21:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863274.56 2024-11-06T03:21:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:15Z- waiting for 1730863278.56, time is 1730863275.84 2024-11-06T03:21:16Z- AHRS_M2 msg: 1730863276.17 epoch sec; seqNo:4457 2024-11-06T03:21:16Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.00 psi 2024-11-06T03:21:16Z- handleDepth()- Depth_Keller msg: 1730863276.18 epoch sec; seqNo:4743; depth 49.72 2024-11-06T03:21:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863276.19 epoch sec; seqNo:4743; 4061540.26 north; 594776.40 east; 36.69 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:21:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863274.56 2024-11-06T03:21:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:16Z- waiting for 1730863278.56, time is 1730863276.19 2024-11-06T03:21:16Z- AHRS_M2 msg: 1730863276.57 epoch sec; seqNo:4458 2024-11-06T03:21:16Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.00 psi 2024-11-06T03:21:16Z- handleDepth()- Depth_Keller msg: 1730863276.58 epoch sec; seqNo:4744; depth 49.72 2024-11-06T03:21:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863276.60 epoch sec; seqNo:4744; 4061540.64 north; 594776.40 east; 36.69 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:21:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863274.56 2024-11-06T03:21:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:16Z- waiting for 1730863278.56, time is 1730863276.60 2024-11-06T03:21:16Z- AHRS_M2 msg: 1730863276.97 epoch sec; seqNo:4459 2024-11-06T03:21:16Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.00 psi 2024-11-06T03:21:16Z- handleDepth()- Depth_Keller msg: 1730863276.98 epoch sec; seqNo:4745; depth 49.71 2024-11-06T03:21:17Z- handleDVL() - DVL msg epochMillisec: 1730863277.00 2024-11-06T03:21:17Z- handleDvl() - RDI_Pathfinder msg: 1730863277.00 sec; seqNo:1565; #beams:4 2024-11-06T03:21:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.12 , 28.89 , 31.27 , 38.91 2024-11-06T03:21:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.16 , -0.03 2024-11-06T03:21:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863277.03 epoch sec; seqNo:4745; 4061541.01 north; 594776.40 east; 36.69 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:21:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863274.56 2024-11-06T03:21:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:17Z- waiting for 1730863278.56, time is 1730863277.03 2024-11-06T03:21:17Z- AHRS_M2 msg: 1730863277.38 epoch sec; seqNo:4460 2024-11-06T03:21:17Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.00 psi 2024-11-06T03:21:17Z- handleDepth()- Depth_Keller msg: 1730863277.39 epoch sec; seqNo:4746; depth 49.70 2024-11-06T03:21:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863277.40 epoch sec; seqNo:4746; 4061541.01 north; 594776.40 east; 36.69 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:21:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863274.56 2024-11-06T03:21:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:17Z- waiting for 1730863278.56, time is 1730863277.40 2024-11-06T03:21:17Z- AHRS_M2 msg: 1730863277.84 epoch sec; seqNo:4461 2024-11-06T03:21:17Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.00 psi 2024-11-06T03:21:17Z- handleDepth()- Depth_Keller msg: 1730863277.85 epoch sec; seqNo:4747; depth 49.70 2024-11-06T03:21:17Z- handleDVL() - DVL msg epochMillisec: 1730863277.88 2024-11-06T03:21:17Z- handleDvl() - RDI_Pathfinder msg: 1730863277.88 sec; seqNo:1566; #beams:4 2024-11-06T03:21:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.29 , 28.72 , 31.44 , 39.08 2024-11-06T03:21:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.17 , -0.00 2024-11-06T03:21:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863277.89 epoch sec; seqNo:4747; 4061541.39 north; 594776.39 east; 36.69 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:21:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863274.56 2024-11-06T03:21:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:18Z- waiting for 1730863278.56, time is 1730863277.89 2024-11-06T03:21:18Z- AHRS_M2 msg: 1730863278.19 epoch sec; seqNo:4462 2024-11-06T03:21:18Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.00 psi 2024-11-06T03:21:18Z- handleDepth()- Depth_Keller msg: 1730863278.20 epoch sec; seqNo:4748; depth 49.68 2024-11-06T03:21:18Z- handleDVL() - DVL msg epochMillisec: 1730863278.20 2024-11-06T03:21:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:18Z- handleDvl() - RDI_Pathfinder msg: 1730863278.20 sec; seqNo:1567; #beams:0 2024-11-06T03:21:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.17 , -0.00 2024-11-06T03:21:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863278.21 epoch sec; seqNo:4748; 4061541.76 north; 594775.17 east; 36.69 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:21:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863274.56 2024-11-06T03:21:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:18Z- waiting for 1730863278.56, time is 1730863278.21 2024-11-06T03:21:18Z- AHRS_M2 msg: 1730863278.59 epoch sec; seqNo:4463 2024-11-06T03:21:18Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.00 psi 2024-11-06T03:21:18Z- handleDepth()- Depth_Keller msg: 1730863278.60 epoch sec; seqNo:4749; depth 49.68 2024-11-06T03:21:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863278.62 epoch sec; seqNo:4749; 4061542.14 north; 594775.16 east; 36.69 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:21:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863274.56 2024-11-06T03:21:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:18Z- waiting for 1730863278.56, time is 1730863278.62 2024-11-06T03:21:18Z- LcmTrn::updateTrn() >>>> pose time 1730863278.62, meas time 1730863278.20 <<<< 2024-11-06T03:21:18Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863278.62 2024-11-06T03:21:18Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:21:18Z- TerrainNav::motionUpdate() - currEstimate = (4061542, 594775.16, 49.68) 2024-11-06T03:21:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:21:18Z- TerrainNav:: SubcloudNIS is 0.90, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:21:18Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:21:18Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:21:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:21:18Z- variances([0], [2], and psi_berg) = 0.357139 0.208372 0.000000 2024-11-06T03:21:18Z- publishEstimatesDecoder() - at 1730863278.62 sec 2024-11-06T03:21:18Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:21:18Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061572.83 594758.07 50.72 2024-11-06T03:21:18Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061572.77 594757.75 50.66 lat/lon: 36.70 -121.94 2024-11-06T03:21:18Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.36 0.21 0.84 0.00 2024-11-06T03:21:19Z- AHRS_M2 msg: 1730863278.99 epoch sec; seqNo:4464 2024-11-06T03:21:19Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.00 psi 2024-11-06T03:21:19Z- handleDepth()- Depth_Keller msg: 1730863279.01 epoch sec; seqNo:4750; depth 49.67 2024-11-06T03:21:19Z- handleDVL() - DVL msg epochMillisec: 1730863279.03 2024-11-06T03:21:19Z- handleDvl() - RDI_Pathfinder msg: 1730863279.03 sec; seqNo:1568; #beams:4 2024-11-06T03:21:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.95 , 28.55 , 30.93 , 38.91 2024-11-06T03:21:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.16 , 0.02 2024-11-06T03:21:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863279.05 epoch sec; seqNo:4750; 4061542.52 north; 594775.16 east; 36.69 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:21:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863278.62 2024-11-06T03:21:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:19Z- waiting for 1730863282.62, time is 1730863279.05 2024-11-06T03:21:19Z- AHRS_M2 msg: 1730863279.40 epoch sec; seqNo:4465 2024-11-06T03:21:19Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.00 psi 2024-11-06T03:21:19Z- handleDepth()- Depth_Keller msg: 1730863279.41 epoch sec; seqNo:4751; depth 49.68 2024-11-06T03:21:19Z- handleDVL() - DVL msg epochMillisec: 1730863279.41 2024-11-06T03:21:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:19Z- handleDvl() - RDI_Pathfinder msg: 1730863279.41 sec; seqNo:1569; #beams:0 2024-11-06T03:21:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.16 , 0.02 2024-11-06T03:21:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863279.43 epoch sec; seqNo:4751; 4061542.90 north; 594775.15 east; 36.69 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:21:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863278.62 2024-11-06T03:21:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:19Z- waiting for 1730863282.62, time is 1730863279.43 2024-11-06T03:21:19Z- AHRS_M2 msg: 1730863279.86 epoch sec; seqNo:4466 2024-11-06T03:21:19Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.00 psi 2024-11-06T03:21:19Z- handleDepth()- Depth_Keller msg: 1730863279.88 epoch sec; seqNo:4752; depth 49.66 2024-11-06T03:21:19Z- handleDVL() - DVL msg epochMillisec: 1730863279.90 2024-11-06T03:21:19Z- handleDvl() - RDI_Pathfinder msg: 1730863279.90 sec; seqNo:1570; #beams:4 2024-11-06T03:21:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.10 , 28.72 , 30.59 , 38.91 2024-11-06T03:21:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.14 , 0.04 2024-11-06T03:21:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863279.91 epoch sec; seqNo:4752; 4061543.27 north; 594775.15 east; 36.69 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:21:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863278.62 2024-11-06T03:21:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:20Z- waiting for 1730863282.62, time is 1730863279.91 2024-11-06T03:21:20Z- handleDepth()- Depth_Keller msg: 1730863280.24 epoch sec; seqNo:4753; depth 49.67 2024-11-06T03:21:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863280.25 epoch sec; seqNo:4753; 4061543.65 north; 594775.15 east; 36.69 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:21:20Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:21:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863278.62 2024-11-06T03:21:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:20Z- waiting for 1730863282.62, time is 1730863280.25 2024-11-06T03:21:20Z- AHRS_M2 msg: 1730863280.61 epoch sec; seqNo:4467 2024-11-06T03:21:20Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.00 psi 2024-11-06T03:21:20Z- handleDepth()- Depth_Keller msg: 1730863280.63 epoch sec; seqNo:4754; depth 49.66 2024-11-06T03:21:20Z- handleDVL() - DVL msg epochMillisec: 1730863280.63 2024-11-06T03:21:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:20Z- handleDvl() - RDI_Pathfinder msg: 1730863280.63 sec; seqNo:1571; #beams:0 2024-11-06T03:21:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.14 , 0.04 2024-11-06T03:21:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863280.65 epoch sec; seqNo:4754; 4061544.03 north; 594775.14 east; 36.69 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:21:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863278.62 2024-11-06T03:21:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:20Z- waiting for 1730863282.62, time is 1730863280.65 2024-11-06T03:21:21Z- AHRS_M2 msg: 1730863281.02 epoch sec; seqNo:4468 2024-11-06T03:21:21Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 6.00 psi 2024-11-06T03:21:21Z- handleDepth()- Depth_Keller msg: 1730863281.03 epoch sec; seqNo:4755; depth 49.67 2024-11-06T03:21:21Z- handleDVL() - DVL msg epochMillisec: 1730863281.06 2024-11-06T03:21:21Z- handleDvl() - RDI_Pathfinder msg: 1730863281.06 sec; seqNo:1572; #beams:4 2024-11-06T03:21:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.10 , 28.89 , 30.42 , 39.08 2024-11-06T03:21:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , 0.04 2024-11-06T03:21:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863281.08 epoch sec; seqNo:4755; 4061544.41 north; 594775.14 east; 36.69 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:21:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863278.62 2024-11-06T03:21:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:21Z- waiting for 1730863282.62, time is 1730863281.08 2024-11-06T03:21:21Z- AHRS_M2 msg: 1730863281.42 epoch sec; seqNo:4469 2024-11-06T03:21:21Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 6.01 psi 2024-11-06T03:21:21Z- handleDepth()- Depth_Keller msg: 1730863281.43 epoch sec; seqNo:4756; depth 49.65 2024-11-06T03:21:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863281.44 epoch sec; seqNo:4756; 4061544.40 north; 594773.92 east; 36.69 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:21:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863278.62 2024-11-06T03:21:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:21Z- waiting for 1730863282.62, time is 1730863281.44 2024-11-06T03:21:21Z- AHRS_M2 msg: 1730863281.83 epoch sec; seqNo:4470 2024-11-06T03:21:21Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 6.01 psi 2024-11-06T03:21:21Z- handleDepth()- Depth_Keller msg: 1730863281.86 epoch sec; seqNo:4757; depth 49.65 2024-11-06T03:21:21Z- handleDVL() - DVL msg epochMillisec: 1730863281.86 2024-11-06T03:21:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:21Z- handleDvl() - RDI_Pathfinder msg: 1730863281.86 sec; seqNo:1573; #beams:0 2024-11-06T03:21:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , 0.04 2024-11-06T03:21:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863281.90 epoch sec; seqNo:4757; 4061544.78 north; 594773.91 east; 36.69 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:21:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863278.62 2024-11-06T03:21:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:22Z- waiting for 1730863282.62, time is 1730863281.90 2024-11-06T03:21:22Z- AHRS_M2 msg: 1730863282.23 epoch sec; seqNo:4471 2024-11-06T03:21:22Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 6.01 psi 2024-11-06T03:21:22Z- handleDepth()- Depth_Keller msg: 1730863282.23 epoch sec; seqNo:4758; depth 49.65 2024-11-06T03:21:22Z- handleDVL() - DVL msg epochMillisec: 1730863282.26 2024-11-06T03:21:22Z- handleDvl() - RDI_Pathfinder msg: 1730863282.26 sec; seqNo:1574; #beams:4 2024-11-06T03:21:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.93 , 28.89 , 30.25 , 38.91 2024-11-06T03:21:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , 0.04 2024-11-06T03:21:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863282.28 epoch sec; seqNo:4758; 4061545.16 north; 594773.91 east; 36.69 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:21:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863278.62 2024-11-06T03:21:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:22Z- waiting for 1730863282.62, time is 1730863282.28 2024-11-06T03:21:22Z- AHRS_M2 msg: 1730863282.64 epoch sec; seqNo:4472 2024-11-06T03:21:22Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 6.01 psi 2024-11-06T03:21:22Z- handleDepth()- Depth_Keller msg: 1730863282.65 epoch sec; seqNo:4759; depth 49.64 2024-11-06T03:21:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863282.66 epoch sec; seqNo:4759; 4061545.53 north; 594773.90 east; 36.69 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:21:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863278.62 2024-11-06T03:21:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:22Z- waiting for 1730863282.62, time is 1730863282.66 2024-11-06T03:21:22Z- LcmTrn::updateTrn() >>>> pose time 1730863282.66, meas time 1730863282.26 <<<< 2024-11-06T03:21:22Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863282.66 2024-11-06T03:21:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863282.26 sec, ping # 0; waiting for INS data more recent than 1730863278.62... 2024-11-06T03:21:22Z- TerrainNav::motionUpdate() - currEstimate = (4061546, 594773.90, 49.64) 2024-11-06T03:21:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:21:22Z- TerrainNav:: SubcloudNIS is 0.90, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:21:22Z- TNavPF::Projecting Measurements 2024-11-06T03:21:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:21:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:21:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:21:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:21:22Z- TNavFilter::4 good beams. 2024-11-06T03:21:22Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:21:22Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:21:22Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:21:22Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:21:22Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:21:22Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:21:22Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:21:22Z- TerrainMapOctree: (vehN, vehE ) = (4061545.20, 594774.03). 2024-11-06T03:21:22Z- TerrainMapOctree: Min Distance = 206.20. 2024-11-06T03:21:22Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:21:22Z- Forcing Subcloud Comparison 2024-11-06T03:21:22Z- Weighting particles with subcloud comparison 2024-11-06T03:21:22Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:22Z- meanExpectedMeasDiff: 9.214911 delta_rms_squared: 2.271255 alpha: 0.991271 2024-11-06T03:21:22Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:22Z- meanExpectedMeasDiff: -6.564795 delta_rms_squared: 1.302205 alpha: 0.984874 2024-11-06T03:21:22Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:22Z- meanExpectedMeasDiff: -4.980702 delta_rms_squared: 1.792619 alpha: 0.988966 2024-11-06T03:21:22Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:22Z- meanExpectedMeasDiff: 2.735851 delta_rms_squared: 1.559587 alpha: 0.987338 2024-11-06T03:21:22Z- Subcloud NIS: 0.972222 2024-11-06T03:21:22Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:21:22Z- TNavPF:: sumWeights = 0.000000 2024-11-06T03:21:22Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:21:22Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:21:22Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:21:22Z- TerrainNav::Resampling particle filter... 2024-11-06T03:21:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863282.26 sec. 2024-11-06T03:21:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:21:22Z- variances([0], [2], and psi_berg) = 0.289222 0.251834 0.000000 2024-11-06T03:21:22Z- publishEstimatesDecoder() - at 1730863282.66 sec 2024-11-06T03:21:22Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:21:22Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061576.27 594756.80 51.94 2024-11-06T03:21:22Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061575.98 594756.47 50.88 lat/lon: 36.70 -121.94 2024-11-06T03:21:22Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.29 0.25 0.66 0.00 2024-11-06T03:21:23Z- AHRS_M2 msg: 1730863283.04 epoch sec; seqNo:4473 2024-11-06T03:21:23Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 6.01 psi 2024-11-06T03:21:23Z- handleDepth()- Depth_Keller msg: 1730863283.05 epoch sec; seqNo:4760; depth 49.63 2024-11-06T03:21:23Z- handleDVL() - DVL msg epochMillisec: 1730863283.09 2024-11-06T03:21:23Z- handleDvl() - RDI_Pathfinder msg: 1730863283.09 sec; seqNo:1575; #beams:4 2024-11-06T03:21:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.42 , 28.72 , 29.91 , 38.74 2024-11-06T03:21:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , 0.02 2024-11-06T03:21:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863283.10 epoch sec; seqNo:4760; 4061545.91 north; 594773.90 east; 36.69 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:21:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863282.66 2024-11-06T03:21:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:23Z- waiting for 1730863286.66, time is 1730863283.10 2024-11-06T03:21:23Z- AHRS_M2 msg: 1730863283.44 epoch sec; seqNo:4474 2024-11-06T03:21:23Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 6.01 psi 2024-11-06T03:21:23Z- handleDepth()- Depth_Keller msg: 1730863283.45 epoch sec; seqNo:4761; depth 49.64 2024-11-06T03:21:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863283.46 epoch sec; seqNo:4761; 4061546.29 north; 594773.90 east; 36.69 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:21:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863282.66 2024-11-06T03:21:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:23Z- waiting for 1730863286.66, time is 1730863283.46 2024-11-06T03:21:23Z- AHRS_M2 msg: 1730863283.85 epoch sec; seqNo:4475 2024-11-06T03:21:23Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 6.01 psi 2024-11-06T03:21:23Z- handleDepth()- Depth_Keller msg: 1730863283.85 epoch sec; seqNo:4762; depth 49.63 2024-11-06T03:21:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863283.88 epoch sec; seqNo:4762; 4061546.67 north; 594773.89 east; 36.69 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:21:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863282.66 2024-11-06T03:21:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:24Z- waiting for 1730863286.66, time is 1730863283.88 2024-11-06T03:21:24Z- AHRS_M2 msg: 1730863284.25 epoch sec; seqNo:4476 2024-11-06T03:21:24Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 6.01 psi 2024-11-06T03:21:24Z- handleDepth()- Depth_Keller msg: 1730863284.26 epoch sec; seqNo:4763; depth 49.62 2024-11-06T03:21:24Z- handleDVL() - DVL msg epochMillisec: 1730863284.29 2024-11-06T03:21:24Z- handleDvl() - RDI_Pathfinder msg: 1730863284.29 sec; seqNo:1576; #beams:4 2024-11-06T03:21:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.84 , 28.90 , 29.82 , 38.69 2024-11-06T03:21:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , 0.00 2024-11-06T03:21:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863284.32 epoch sec; seqNo:4763; 4061547.05 north; 594773.89 east; 36.69 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:21:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863282.66 2024-11-06T03:21:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:24Z- waiting for 1730863286.66, time is 1730863284.32 2024-11-06T03:21:24Z- AHRS_M2 msg: 1730863284.65 epoch sec; seqNo:4477 2024-11-06T03:21:24Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 6.01 psi 2024-11-06T03:21:24Z- handleDepth()- Depth_Keller msg: 1730863284.66 epoch sec; seqNo:4764; depth 49.60 2024-11-06T03:21:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863284.68 epoch sec; seqNo:4764; 4061547.43 north; 594773.88 east; 36.69 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:21:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863282.66 2024-11-06T03:21:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:24Z- waiting for 1730863286.66, time is 1730863284.68 2024-11-06T03:21:25Z- AHRS_M2 msg: 1730863285.06 epoch sec; seqNo:4478 2024-11-06T03:21:25Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.01 psi 2024-11-06T03:21:25Z- handleDepth()- Depth_Keller msg: 1730863285.06 epoch sec; seqNo:4765; depth 49.58 2024-11-06T03:21:25Z- handleDVL() - DVL msg epochMillisec: 1730863285.09 2024-11-06T03:21:25Z- handleDvl() - RDI_Pathfinder msg: 1730863285.09 sec; seqNo:1577; #beams:4 2024-11-06T03:21:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.23 , 28.60 , 29.52 , 38.69 2024-11-06T03:21:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , -0.01 2024-11-06T03:21:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863285.10 epoch sec; seqNo:4765; 4061547.81 north; 594773.88 east; 36.69 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:21:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863282.66 2024-11-06T03:21:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:25Z- waiting for 1730863286.66, time is 1730863285.10 2024-11-06T03:21:25Z- AHRS_M2 msg: 1730863285.46 epoch sec; seqNo:4479 2024-11-06T03:21:25Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.01 psi 2024-11-06T03:21:25Z- handleDepth()- Depth_Keller msg: 1730863285.47 epoch sec; seqNo:4766; depth 49.58 2024-11-06T03:21:25Z- handleDVL() - DVL msg epochMillisec: 1730863285.47 2024-11-06T03:21:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:25Z- handleDvl() - RDI_Pathfinder msg: 1730863285.47 sec; seqNo:1578; #beams:0 2024-11-06T03:21:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , -0.01 2024-11-06T03:21:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863285.49 epoch sec; seqNo:4766; 4061548.17 north; 594772.66 east; 36.69 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:21:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863282.66 2024-11-06T03:21:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:25Z- waiting for 1730863286.66, time is 1730863285.49 2024-11-06T03:21:25Z- AHRS_M2 msg: 1730863285.88 epoch sec; seqNo:4480 2024-11-06T03:21:25Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.02 psi 2024-11-06T03:21:25Z- handleDepth()- Depth_Keller msg: 1730863285.91 epoch sec; seqNo:4767; depth 49.58 2024-11-06T03:21:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863285.96 epoch sec; seqNo:4767; 4061548.55 north; 594772.65 east; 36.69 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:21:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863282.66 2024-11-06T03:21:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:26Z- waiting for 1730863286.66, time is 1730863285.96 2024-11-06T03:21:26Z- AHRS_M2 msg: 1730863286.27 epoch sec; seqNo:4481 2024-11-06T03:21:26Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.02 psi 2024-11-06T03:21:26Z- handleDepth()- Depth_Keller msg: 1730863286.27 epoch sec; seqNo:4768; depth 49.56 2024-11-06T03:21:26Z- handleDVL() - DVL msg epochMillisec: 1730863286.30 2024-11-06T03:21:26Z- handleDvl() - RDI_Pathfinder msg: 1730863286.30 sec; seqNo:1579; #beams:4 2024-11-06T03:21:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.08 , 27.99 , 29.36 , 38.69 2024-11-06T03:21:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.03 2024-11-06T03:21:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863286.32 epoch sec; seqNo:4768; 4061548.55 north; 594772.65 east; 36.69 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:21:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863282.66 2024-11-06T03:21:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:26Z- waiting for 1730863286.66, time is 1730863286.32 2024-11-06T03:21:26Z- AHRS_M2 msg: 1730863286.68 epoch sec; seqNo:4482 2024-11-06T03:21:26Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.02 psi 2024-11-06T03:21:26Z- handleDepth()- Depth_Keller msg: 1730863286.69 epoch sec; seqNo:4769; depth 49.55 2024-11-06T03:21:26Z- handleDVL() - DVL msg epochMillisec: 1730863286.69 2024-11-06T03:21:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:26Z- handleDvl() - RDI_Pathfinder msg: 1730863286.69 sec; seqNo:1580; #beams:0 2024-11-06T03:21:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.03 2024-11-06T03:21:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863286.71 epoch sec; seqNo:4769; 4061548.93 north; 594772.65 east; 36.69 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:21:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863282.66 2024-11-06T03:21:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:26Z- waiting for 1730863286.66, time is 1730863286.71 2024-11-06T03:21:26Z- LcmTrn::updateTrn() >>>> pose time 1730863286.71, meas time 1730863286.69 <<<< 2024-11-06T03:21:26Z- LcmTrn::updateTrn() equating pose 1730863286.69 and meas times 1730863286.69 2024-11-06T03:21:26Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863286.69 2024-11-06T03:21:26Z- TerrainNav::motionUpdate() - currEstimate = (4061549, 594772.65, 49.55) 2024-11-06T03:21:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:21:26Z- TerrainNav:: SubcloudNIS is 0.97, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:21:26Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:21:26Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:21:26Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:21:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:21:26Z- variances([0], [2], and psi_berg) = 0.292205 0.254136 0.000000 2024-11-06T03:21:26Z- publishEstimatesDecoder() - at 1730863286.69 sec 2024-11-06T03:21:26Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:21:26Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061579.58 594755.56 51.80 2024-11-06T03:21:26Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061579.38 594755.21 50.79 lat/lon: 36.70 -121.94 2024-11-06T03:21:26Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.29 0.25 0.92 0.00 2024-11-06T03:21:27Z- handleDepth()- Depth_Keller msg: 1730863287.09 epoch sec; seqNo:4770; depth 49.53 2024-11-06T03:21:27Z- handleDVL() - DVL msg epochMillisec: 1730863287.11 2024-11-06T03:21:27Z- handleDvl() - RDI_Pathfinder msg: 1730863287.11 sec; seqNo:1581; #beams:4 2024-11-06T03:21:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.08 , 28.60 , 29.06 , 38.54 2024-11-06T03:21:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.08 , -0.03 2024-11-06T03:21:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863287.12 epoch sec; seqNo:4770; 4061549.31 north; 594772.64 east; 36.69 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T03:21:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863286.69 2024-11-06T03:21:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:27Z- waiting for 1730863290.69, time is 1730863287.12 2024-11-06T03:21:27Z- AHRS_M2 msg: 1730863287.48 epoch sec; seqNo:4483 2024-11-06T03:21:27Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.02 psi 2024-11-06T03:21:27Z- handleDepth()- Depth_Keller msg: 1730863287.49 epoch sec; seqNo:4771; depth 49.52 2024-11-06T03:21:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863287.51 epoch sec; seqNo:4771; 4061549.69 north; 594772.64 east; 36.69 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T03:21:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863286.69 2024-11-06T03:21:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:27Z- waiting for 1730863290.69, time is 1730863287.51 2024-11-06T03:21:27Z- AHRS_M2 msg: 1730863287.89 epoch sec; seqNo:4484 2024-11-06T03:21:27Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.02 psi 2024-11-06T03:21:27Z- handleDepth()- Depth_Keller msg: 1730863287.89 epoch sec; seqNo:4772; depth 49.51 2024-11-06T03:21:27Z- handleDVL() - DVL msg epochMillisec: 1730863287.90 2024-11-06T03:21:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:27Z- handleDvl() - RDI_Pathfinder msg: 1730863287.90 sec; seqNo:1582; #beams:0 2024-11-06T03:21:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.08 , -0.03 2024-11-06T03:21:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863287.93 epoch sec; seqNo:4772; 4061550.07 north; 594772.63 east; 36.69 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T03:21:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863286.69 2024-11-06T03:21:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:28Z- waiting for 1730863290.69, time is 1730863287.93 2024-11-06T03:21:28Z- AHRS_M2 msg: 1730863288.29 epoch sec; seqNo:4485 2024-11-06T03:21:28Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.02 psi 2024-11-06T03:21:28Z- handleDepth()- Depth_Keller msg: 1730863288.29 epoch sec; seqNo:4773; depth 49.49 2024-11-06T03:21:28Z- handleDVL() - DVL msg epochMillisec: 1730863288.31 2024-11-06T03:21:28Z- handleDvl() - RDI_Pathfinder msg: 1730863288.31 sec; seqNo:1583; #beams:4 2024-11-06T03:21:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.93 , 28.44 , 28.90 , 38.39 2024-11-06T03:21:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.10 , -0.03 2024-11-06T03:21:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863288.35 epoch sec; seqNo:4773; 4061550.45 north; 594772.63 east; 36.69 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:21:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863286.69 2024-11-06T03:21:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:28Z- waiting for 1730863290.69, time is 1730863288.35 2024-11-06T03:21:28Z- AHRS_M2 msg: 1730863288.69 epoch sec; seqNo:4486 2024-11-06T03:21:28Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.02 psi 2024-11-06T03:21:28Z- handleDepth()- Depth_Keller msg: 1730863288.70 epoch sec; seqNo:4774; depth 49.48 2024-11-06T03:21:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863288.72 epoch sec; seqNo:4774; 4061550.82 north; 594772.63 east; 36.69 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:21:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863286.69 2024-11-06T03:21:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:28Z- waiting for 1730863290.69, time is 1730863288.72 2024-11-06T03:21:29Z- AHRS_M2 msg: 1730863289.10 epoch sec; seqNo:4487 2024-11-06T03:21:29Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.02 psi 2024-11-06T03:21:29Z- handleDepth()- Depth_Keller msg: 1730863289.11 epoch sec; seqNo:4775; depth 49.48 2024-11-06T03:21:29Z- handleDVL() - DVL msg epochMillisec: 1730863289.13 2024-11-06T03:21:29Z- handleDvl() - RDI_Pathfinder msg: 1730863289.13 sec; seqNo:1584; #beams:4 2024-11-06T03:21:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.93 , 28.44 , 28.44 , 37.62 2024-11-06T03:21:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.12 , -0.02 2024-11-06T03:21:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863289.15 epoch sec; seqNo:4775; 4061551.20 north; 594772.62 east; 36.69 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:21:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863286.69 2024-11-06T03:21:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:29Z- waiting for 1730863290.69, time is 1730863289.15 2024-11-06T03:21:29Z- AHRS_M2 msg: 1730863289.50 epoch sec; seqNo:4488 2024-11-06T03:21:29Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.02 psi 2024-11-06T03:21:29Z- handleDepth()- Depth_Keller msg: 1730863289.51 epoch sec; seqNo:4776; depth 49.47 2024-11-06T03:21:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863289.52 epoch sec; seqNo:4776; 4061551.57 north; 594771.40 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:21:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863286.69 2024-11-06T03:21:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:29Z- waiting for 1730863290.69, time is 1730863289.52 2024-11-06T03:21:29Z- AHRS_M2 msg: 1730863289.91 epoch sec; seqNo:4489 2024-11-06T03:21:29Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.02 psi 2024-11-06T03:21:29Z- handleDepth()- Depth_Keller msg: 1730863289.92 epoch sec; seqNo:4777; depth 49.47 2024-11-06T03:21:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863289.94 epoch sec; seqNo:4777; 4061551.95 north; 594771.39 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:21:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863286.69 2024-11-06T03:21:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:30Z- waiting for 1730863290.69, time is 1730863289.94 2024-11-06T03:21:30Z- AHRS_M2 msg: 1730863290.31 epoch sec; seqNo:4490 2024-11-06T03:21:30Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.02 psi 2024-11-06T03:21:30Z- handleDepth()- Depth_Keller msg: 1730863290.31 epoch sec; seqNo:4778; depth 49.46 2024-11-06T03:21:30Z- handleDVL() - DVL msg epochMillisec: 1730863290.33 2024-11-06T03:21:30Z- handleDvl() - RDI_Pathfinder msg: 1730863290.33 sec; seqNo:1585; #beams:4 2024-11-06T03:21:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.16 , 28.60 , 28.14 , 37.62 2024-11-06T03:21:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.13 , -0.01 2024-11-06T03:21:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863290.35 epoch sec; seqNo:4778; 4061552.33 north; 594771.39 east; 36.69 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T03:21:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863286.69 2024-11-06T03:21:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:30Z- waiting for 1730863290.69, time is 1730863290.35 2024-11-06T03:21:30Z- AHRS_M2 msg: 1730863290.71 epoch sec; seqNo:4491 2024-11-06T03:21:30Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.02 psi 2024-11-06T03:21:30Z- handleDepth()- Depth_Keller msg: 1730863290.72 epoch sec; seqNo:4779; depth 49.45 2024-11-06T03:21:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863290.74 epoch sec; seqNo:4779; 4061552.71 north; 594771.38 east; 36.69 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:21:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863286.69 2024-11-06T03:21:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:30Z- waiting for 1730863290.69, time is 1730863290.74 2024-11-06T03:21:30Z- LcmTrn::updateTrn() >>>> pose time 1730863290.74, meas time 1730863290.33 <<<< 2024-11-06T03:21:30Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863290.74 2024-11-06T03:21:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863290.33 sec, ping # 0; waiting for INS data more recent than 1730863286.69... 2024-11-06T03:21:30Z- TerrainNav::motionUpdate() - currEstimate = (4061553, 594771.38, 49.45) 2024-11-06T03:21:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:21:30Z- TerrainNav:: SubcloudNIS is 0.97, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:21:30Z- TNavPF::Projecting Measurements 2024-11-06T03:21:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:21:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:21:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:21:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:21:30Z- TNavFilter::4 good beams. 2024-11-06T03:21:30Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:21:30Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:21:30Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:21:30Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:21:30Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:21:30Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:21:30Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:21:30Z- TerrainMapOctree: (vehN, vehE ) = (4061552.33, 594771.51). 2024-11-06T03:21:30Z- TerrainMapOctree: Min Distance = 198.84. 2024-11-06T03:21:30Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:21:30Z- Forcing Subcloud Comparison 2024-11-06T03:21:30Z- Weighting particles with subcloud comparison 2024-11-06T03:21:30Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:30Z- meanExpectedMeasDiff: 8.149588 delta_rms_squared: 1.905678 alpha: 0.989614 2024-11-06T03:21:30Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:30Z- meanExpectedMeasDiff: -6.733974 delta_rms_squared: 1.574501 alpha: 0.987457 2024-11-06T03:21:30Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:30Z- meanExpectedMeasDiff: -6.780878 delta_rms_squared: 1.482272 alpha: 0.986687 2024-11-06T03:21:31Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:31Z- meanExpectedMeasDiff: 5.289930 delta_rms_squared: 1.875552 alpha: 0.989449 2024-11-06T03:21:31Z- Subcloud NIS: 1.065148 2024-11-06T03:21:31Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:21:31Z- TNavPF:: sumWeights = 0.000000 2024-11-06T03:21:31Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:21:31Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:21:31Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:21:31Z- TerrainNav::Resampling particle filter... 2024-11-06T03:21:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863290.33 sec. 2024-11-06T03:21:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:21:31Z- variances([0], [2], and psi_berg) = 0.149239 0.098338 0.000000 2024-11-06T03:21:31Z- publishEstimatesDecoder() - at 1730863290.74 sec 2024-11-06T03:21:31Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:21:31Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061583.35 594754.28 51.86 2024-11-06T03:21:31Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061582.94 594754.20 51.02 lat/lon: 36.70 -121.94 2024-11-06T03:21:31Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.10 0.51 0.00 2024-11-06T03:21:31Z- AHRS_M2 msg: 1730863291.11 epoch sec; seqNo:4492 2024-11-06T03:21:31Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.02 psi 2024-11-06T03:21:31Z- handleDepth()- Depth_Keller msg: 1730863291.13 epoch sec; seqNo:4780; depth 49.44 2024-11-06T03:21:31Z- handleDVL() - DVL msg epochMillisec: 1730863291.15 2024-11-06T03:21:31Z- handleDvl() - RDI_Pathfinder msg: 1730863291.15 sec; seqNo:1586; #beams:4 2024-11-06T03:21:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.70 , 28.44 , 27.99 , 37.31 2024-11-06T03:21:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.13 , -0.01 2024-11-06T03:21:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863291.16 epoch sec; seqNo:4780; 4061553.08 north; 594771.38 east; 36.69 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:21:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863290.74 2024-11-06T03:21:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:31Z- waiting for 1730863294.74, time is 1730863291.16 2024-11-06T03:21:31Z- AHRS_M2 msg: 1730863291.52 epoch sec; seqNo:4493 2024-11-06T03:21:31Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.02 psi 2024-11-06T03:21:31Z- handleDepth()- Depth_Keller msg: 1730863291.53 epoch sec; seqNo:4781; depth 49.45 2024-11-06T03:21:31Z- handleDVL() - DVL msg epochMillisec: 1730863291.53 2024-11-06T03:21:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:31Z- handleDvl() - RDI_Pathfinder msg: 1730863291.53 sec; seqNo:1587; #beams:0 2024-11-06T03:21:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.13 , -0.01 2024-11-06T03:21:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863291.55 epoch sec; seqNo:4781; 4061553.46 north; 594771.38 east; 36.69 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:21:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863290.74 2024-11-06T03:21:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:31Z- waiting for 1730863294.74, time is 1730863291.55 2024-11-06T03:21:31Z- AHRS_M2 msg: 1730863291.92 epoch sec; seqNo:4494 2024-11-06T03:21:31Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.02 psi 2024-11-06T03:21:31Z- handleDepth()- Depth_Keller msg: 1730863291.93 epoch sec; seqNo:4782; depth 49.45 2024-11-06T03:21:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863291.94 epoch sec; seqNo:4782; 4061553.46 north; 594771.38 east; 36.69 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:21:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863290.74 2024-11-06T03:21:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:32Z- waiting for 1730863294.74, time is 1730863291.94 2024-11-06T03:21:32Z- AHRS_M2 msg: 1730863292.33 epoch sec; seqNo:4495 2024-11-06T03:21:32Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.02 psi 2024-11-06T03:21:32Z- handleDepth()- Depth_Keller msg: 1730863292.34 epoch sec; seqNo:4783; depth 49.44 2024-11-06T03:21:32Z- handleDVL() - DVL msg epochMillisec: 1730863292.36 2024-11-06T03:21:32Z- handleDvl() - RDI_Pathfinder msg: 1730863292.36 sec; seqNo:1588; #beams:4 2024-11-06T03:21:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.70 , 28.60 , 27.83 , 37.31 2024-11-06T03:21:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.12 , -0.00 2024-11-06T03:21:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863292.39 epoch sec; seqNo:4783; 4061553.84 north; 594771.37 east; 36.69 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:21:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863290.74 2024-11-06T03:21:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:32Z- waiting for 1730863294.74, time is 1730863292.39 2024-11-06T03:21:32Z- AHRS_M2 msg: 1730863292.73 epoch sec; seqNo:4496 2024-11-06T03:21:32Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.02 psi 2024-11-06T03:21:32Z- handleDepth()- Depth_Keller msg: 1730863292.74 epoch sec; seqNo:4784; depth 49.45 2024-11-06T03:21:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863292.76 epoch sec; seqNo:4784; 4061554.21 north; 594770.15 east; 36.69 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:21:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863290.74 2024-11-06T03:21:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:32Z- waiting for 1730863294.74, time is 1730863292.76 2024-11-06T03:21:33Z- AHRS_M2 msg: 1730863293.13 epoch sec; seqNo:4497 2024-11-06T03:21:33Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 6.02 psi 2024-11-06T03:21:33Z- handleDepth()- Depth_Keller msg: 1730863293.14 epoch sec; seqNo:4785; depth 49.44 2024-11-06T03:21:33Z- handleDVL() - DVL msg epochMillisec: 1730863293.17 2024-11-06T03:21:33Z- handleDvl() - RDI_Pathfinder msg: 1730863293.17 sec; seqNo:1589; #beams:4 2024-11-06T03:21:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.01 , 28.90 , 27.83 , 37.62 2024-11-06T03:21:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.12 , 0.00 2024-11-06T03:21:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863293.18 epoch sec; seqNo:4785; 4061554.59 north; 594770.14 east; 36.69 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:21:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863290.74 2024-11-06T03:21:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:33Z- waiting for 1730863294.74, time is 1730863293.18 2024-11-06T03:21:33Z- AHRS_M2 msg: 1730863293.55 epoch sec; seqNo:4498 2024-11-06T03:21:33Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 6.02 psi 2024-11-06T03:21:33Z- handleDepth()- Depth_Keller msg: 1730863293.55 epoch sec; seqNo:4786; depth 49.45 2024-11-06T03:21:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863293.58 epoch sec; seqNo:4786; 4061554.96 north; 594770.14 east; 36.69 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:21:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863290.74 2024-11-06T03:21:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:33Z- waiting for 1730863294.74, time is 1730863293.58 2024-11-06T03:21:33Z- AHRS_M2 msg: 1730863293.94 epoch sec; seqNo:4499 2024-11-06T03:21:33Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 6.02 psi 2024-11-06T03:21:33Z- handleDepth()- Depth_Keller msg: 1730863293.95 epoch sec; seqNo:4787; depth 49.45 2024-11-06T03:21:33Z- handleDVL() - DVL msg epochMillisec: 1730863293.96 2024-11-06T03:21:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:33Z- handleDvl() - RDI_Pathfinder msg: 1730863293.96 sec; seqNo:1590; #beams:0 2024-11-06T03:21:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.12 , 0.00 2024-11-06T03:21:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863293.96 epoch sec; seqNo:4787; 4061555.34 north; 594770.13 east; 36.69 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:21:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863290.74 2024-11-06T03:21:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:34Z- waiting for 1730863294.74, time is 1730863293.96 2024-11-06T03:21:34Z- AHRS_M2 msg: 1730863294.35 epoch sec; seqNo:4500 2024-11-06T03:21:34Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 6.02 psi 2024-11-06T03:21:34Z- handleDepth()- Depth_Keller msg: 1730863294.36 epoch sec; seqNo:4788; depth 49.46 2024-11-06T03:21:34Z- handleDVL() - DVL msg epochMillisec: 1730863294.38 2024-11-06T03:21:34Z- handleDvl() - RDI_Pathfinder msg: 1730863294.38 sec; seqNo:1591; #beams:4 2024-11-06T03:21:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.70 , 28.44 , 28.14 , 37.16 2024-11-06T03:21:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.10 , 0.01 2024-11-06T03:21:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863294.39 epoch sec; seqNo:4788; 4061555.72 north; 594770.13 east; 36.69 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T03:21:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863290.74 2024-11-06T03:21:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:34Z- waiting for 1730863294.74, time is 1730863294.39 2024-11-06T03:21:34Z- AHRS_M2 msg: 1730863294.75 epoch sec; seqNo:4501 2024-11-06T03:21:34Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 6.02 psi 2024-11-06T03:21:34Z- handleDepth()- Depth_Keller msg: 1730863294.76 epoch sec; seqNo:4789; depth 49.47 2024-11-06T03:21:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863294.79 epoch sec; seqNo:4789; 4061556.10 north; 594770.13 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:21:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863290.74 2024-11-06T03:21:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:34Z- waiting for 1730863294.74, time is 1730863294.79 2024-11-06T03:21:34Z- LcmTrn::updateTrn() >>>> pose time 1730863294.79, meas time 1730863294.38 <<<< 2024-11-06T03:21:34Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863294.79 2024-11-06T03:21:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863294.38 sec, ping # 0; waiting for INS data more recent than 1730863290.74... 2024-11-06T03:21:34Z- TerrainNav::motionUpdate() - currEstimate = (4061556, 594770.13, 49.47) 2024-11-06T03:21:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:21:34Z- TerrainNav:: SubcloudNIS is 1.07, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:21:34Z- TNavPF::Projecting Measurements 2024-11-06T03:21:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:21:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:21:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:21:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:21:34Z- TNavFilter::4 good beams. 2024-11-06T03:21:34Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:21:34Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:21:34Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:21:34Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:21:34Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:21:34Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:21:34Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:21:34Z- TerrainMapOctree: (vehN, vehE ) = (4061555.76, 594770.25). 2024-11-06T03:21:34Z- TerrainMapOctree: Min Distance = 195.29. 2024-11-06T03:21:34Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:21:35Z- Forcing Subcloud Comparison 2024-11-06T03:21:35Z- Weighting particles with subcloud comparison 2024-11-06T03:21:35Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:35Z- meanExpectedMeasDiff: 7.594983 delta_rms_squared: 1.001369 alpha: 0.980418 2024-11-06T03:21:35Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:35Z- meanExpectedMeasDiff: -6.606404 delta_rms_squared: 1.210335 alpha: 0.983744 2024-11-06T03:21:35Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:35Z- meanExpectedMeasDiff: -6.234042 delta_rms_squared: 1.359883 alpha: 0.985506 2024-11-06T03:21:35Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:35Z- meanExpectedMeasDiff: 5.030582 delta_rms_squared: 1.015386 alpha: 0.980684 2024-11-06T03:21:35Z- Subcloud NIS: 1.161151 2024-11-06T03:21:35Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:21:35Z- TNavPF:: sumWeights = 0.000000 2024-11-06T03:21:35Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:21:35Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:21:35Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:21:35Z- TerrainNav::Resampling particle filter... 2024-11-06T03:21:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863294.38 sec. 2024-11-06T03:21:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:21:35Z- variances([0], [2], and psi_berg) = 0.166767 0.067918 0.000000 2024-11-06T03:21:35Z- publishEstimatesDecoder() - at 1730863294.79 sec 2024-11-06T03:21:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:21:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061586.75 594752.95 52.45 2024-11-06T03:21:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061586.47 594753.00 51.61 lat/lon: 36.70 -121.94 2024-11-06T03:21:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.07 0.71 0.00 2024-11-06T03:21:35Z- AHRS_M2 msg: 1730863295.15 epoch sec; seqNo:4502 2024-11-06T03:21:35Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 6.02 psi 2024-11-06T03:21:35Z- handleDepth()- Depth_Keller msg: 1730863295.16 epoch sec; seqNo:4790; depth 49.46 2024-11-06T03:21:35Z- handleDVL() - DVL msg epochMillisec: 1730863295.20 2024-11-06T03:21:35Z- handleDvl() - RDI_Pathfinder msg: 1730863295.20 sec; seqNo:1592; #beams:4 2024-11-06T03:21:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.40 , 28.44 , 27.83 , 37.47 2024-11-06T03:21:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.10 , 0.02 2024-11-06T03:21:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863295.23 epoch sec; seqNo:4790; 4061556.48 north; 594770.12 east; 36.69 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T03:21:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863294.79 2024-11-06T03:21:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:35Z- waiting for 1730863298.79, time is 1730863295.23 2024-11-06T03:21:35Z- AHRS_M2 msg: 1730863295.56 epoch sec; seqNo:4503 2024-11-06T03:21:35Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 6.02 psi 2024-11-06T03:21:35Z- handleDepth()- Depth_Keller msg: 1730863295.58 epoch sec; seqNo:4791; depth 49.49 2024-11-06T03:21:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863295.62 epoch sec; seqNo:4791; 4061556.86 north; 594770.12 east; 36.69 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:21:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863294.79 2024-11-06T03:21:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:35Z- waiting for 1730863298.79, time is 1730863295.62 2024-11-06T03:21:35Z- AHRS_M2 msg: 1730863295.97 epoch sec; seqNo:4504 2024-11-06T03:21:35Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 6.02 psi 2024-11-06T03:21:35Z- handleDepth()- Depth_Keller msg: 1730863295.97 epoch sec; seqNo:4792; depth 49.50 2024-11-06T03:21:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863296.00 epoch sec; seqNo:4792; 4061557.24 north; 594770.11 east; 36.69 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:21:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863294.79 2024-11-06T03:21:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:36Z- waiting for 1730863298.79, time is 1730863296.00 2024-11-06T03:21:36Z- AHRS_M2 msg: 1730863296.37 epoch sec; seqNo:4505 2024-11-06T03:21:36Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 6.02 psi 2024-11-06T03:21:36Z- handleDepth()- Depth_Keller msg: 1730863296.37 epoch sec; seqNo:4793; depth 49.49 2024-11-06T03:21:36Z- handleDVL() - DVL msg epochMillisec: 1730863296.39 2024-11-06T03:21:36Z- handleDvl() - RDI_Pathfinder msg: 1730863296.39 sec; seqNo:1593; #beams:4 2024-11-06T03:21:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.40 , 28.60 , 27.83 , 37.16 2024-11-06T03:21:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.08 , 0.03 2024-11-06T03:21:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863296.42 epoch sec; seqNo:4793; 4061557.60 north; 594768.89 east; 36.69 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:21:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863294.79 2024-11-06T03:21:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:36Z- waiting for 1730863298.79, time is 1730863296.42 2024-11-06T03:21:36Z- AHRS_M2 msg: 1730863296.77 epoch sec; seqNo:4506 2024-11-06T03:21:36Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 6.02 psi 2024-11-06T03:21:36Z- handleDepth()- Depth_Keller msg: 1730863296.78 epoch sec; seqNo:4794; depth 49.50 2024-11-06T03:21:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863296.80 epoch sec; seqNo:4794; 4061557.60 north; 594768.89 east; 36.69 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:21:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863294.79 2024-11-06T03:21:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:36Z- waiting for 1730863298.79, time is 1730863296.80 2024-11-06T03:21:37Z- AHRS_M2 msg: 1730863297.18 epoch sec; seqNo:4507 2024-11-06T03:21:37Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 6.02 psi 2024-11-06T03:21:37Z- handleDepth()- Depth_Keller msg: 1730863297.18 epoch sec; seqNo:4795; depth 49.50 2024-11-06T03:21:37Z- handleDVL() - DVL msg epochMillisec: 1730863297.21 2024-11-06T03:21:37Z- handleDvl() - RDI_Pathfinder msg: 1730863297.21 sec; seqNo:1594; #beams:4 2024-11-06T03:21:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.09 , 28.60 , 28.44 , 37.47 2024-11-06T03:21:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , 0.04 2024-11-06T03:21:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863297.22 epoch sec; seqNo:4795; 4061557.98 north; 594768.89 east; 36.69 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:21:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863294.79 2024-11-06T03:21:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:37Z- waiting for 1730863298.79, time is 1730863297.22 2024-11-06T03:21:37Z- AHRS_M2 msg: 1730863297.58 epoch sec; seqNo:4508 2024-11-06T03:21:37Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 6.02 psi 2024-11-06T03:21:37Z- handleDepth()- Depth_Keller msg: 1730863297.59 epoch sec; seqNo:4796; depth 49.50 2024-11-06T03:21:37Z- handleDVL() - DVL msg epochMillisec: 1730863297.59 2024-11-06T03:21:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:37Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:37Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:37Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:37Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:37Z- handleDvl() - RDI_Pathfinder msg: 1730863297.59 sec; seqNo:1595; #beams:0 2024-11-06T03:21:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , 0.04 2024-11-06T03:21:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863297.64 epoch sec; seqNo:4796; 4061558.36 north; 594768.88 east; 36.69 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:21:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863294.79 2024-11-06T03:21:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:37Z- waiting for 1730863298.79, time is 1730863297.64 2024-11-06T03:21:37Z- AHRS_M2 msg: 1730863297.99 epoch sec; seqNo:4509 2024-11-06T03:21:37Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 6.02 psi 2024-11-06T03:21:38Z- handleDepth()- Depth_Keller msg: 1730863298.00 epoch sec; seqNo:4797; depth 49.50 2024-11-06T03:21:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863298.02 epoch sec; seqNo:4797; 4061558.74 north; 594768.88 east; 36.69 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:21:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863294.79 2024-11-06T03:21:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:38Z- waiting for 1730863298.79, time is 1730863298.02 2024-11-06T03:21:38Z- AHRS_M2 msg: 1730863298.39 epoch sec; seqNo:4510 2024-11-06T03:21:38Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 6.02 psi 2024-11-06T03:21:38Z- handleDepth()- Depth_Keller msg: 1730863298.40 epoch sec; seqNo:4798; depth 49.49 2024-11-06T03:21:38Z- handleDVL() - DVL msg epochMillisec: 1730863298.45 2024-11-06T03:21:38Z- handleDvl() - RDI_Pathfinder msg: 1730863298.45 sec; seqNo:1596; #beams:4 2024-11-06T03:21:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.79 , 28.44 , 29.52 , 37.31 2024-11-06T03:21:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , 0.03 2024-11-06T03:21:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863298.46 epoch sec; seqNo:4798; 4061559.12 north; 594768.87 east; 36.69 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:21:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863294.79 2024-11-06T03:21:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:38Z- waiting for 1730863298.79, time is 1730863298.46 2024-11-06T03:21:38Z- AHRS_M2 msg: 1730863298.79 epoch sec; seqNo:4511 2024-11-06T03:21:38Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 6.02 psi 2024-11-06T03:21:38Z- handleDepth()- Depth_Keller msg: 1730863298.80 epoch sec; seqNo:4799; depth 49.49 2024-11-06T03:21:38Z- handleDVL() - DVL msg epochMillisec: 1730863298.81 2024-11-06T03:21:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:38Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:38Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:38Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:38Z- handleDvl() - RDI_Pathfinder msg: 1730863298.81 sec; seqNo:1597; #beams:0 2024-11-06T03:21:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , 0.03 2024-11-06T03:21:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863298.82 epoch sec; seqNo:4799; 4061559.50 north; 594768.87 east; 36.69 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:21:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863294.79 2024-11-06T03:21:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:38Z- waiting for 1730863298.79, time is 1730863298.82 2024-11-06T03:21:38Z- LcmTrn::updateTrn() >>>> pose time 1730863298.82, meas time 1730863298.81 <<<< 2024-11-06T03:21:38Z- LcmTrn::updateTrn() equating pose 1730863298.81 and meas times 1730863298.81 2024-11-06T03:21:38Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863298.81 2024-11-06T03:21:38Z- TerrainNav::motionUpdate() - currEstimate = (4061559, 594768.87, 49.49) 2024-11-06T03:21:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:21:38Z- TerrainNav:: SubcloudNIS is 1.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:21:38Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:21:38Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:21:38Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:21:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:21:38Z- variances([0], [2], and psi_berg) = 0.169189 0.069874 0.000000 2024-11-06T03:21:38Z- publishEstimatesDecoder() - at 1730863298.81 sec 2024-11-06T03:21:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:21:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061590.11 594751.69 52.17 2024-11-06T03:21:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061589.87 594751.74 51.62 lat/lon: 36.70 -121.94 2024-11-06T03:21:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.07 0.97 0.00 2024-11-06T03:21:39Z- AHRS_M2 msg: 1730863299.19 epoch sec; seqNo:4512 2024-11-06T03:21:39Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.02 psi 2024-11-06T03:21:39Z- handleDepth()- Depth_Keller msg: 1730863299.21 epoch sec; seqNo:4800; depth 49.49 2024-11-06T03:21:39Z- handleDVL() - DVL msg epochMillisec: 1730863299.23 2024-11-06T03:21:39Z- handleDvl() - RDI_Pathfinder msg: 1730863299.23 sec; seqNo:1598; #beams:4 2024-11-06T03:21:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.24 , 28.44 , 29.06 , 37.16 2024-11-06T03:21:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , 0.01 2024-11-06T03:21:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863299.24 epoch sec; seqNo:4800; 4061559.88 north; 594768.86 east; 36.69 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:21:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863298.81 2024-11-06T03:21:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:39Z- waiting for 1730863302.81, time is 1730863299.24 2024-11-06T03:21:39Z- AHRS_M2 msg: 1730863299.61 epoch sec; seqNo:4513 2024-11-06T03:21:39Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.02 psi 2024-11-06T03:21:39Z- handleDepth()- Depth_Keller msg: 1730863299.63 epoch sec; seqNo:4801; depth 49.48 2024-11-06T03:21:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863299.65 epoch sec; seqNo:4801; 4061560.26 north; 594768.86 east; 36.69 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:21:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863298.81 2024-11-06T03:21:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:39Z- waiting for 1730863302.81, time is 1730863299.65 2024-11-06T03:21:40Z- AHRS_M2 msg: 1730863300.06 epoch sec; seqNo:4514 2024-11-06T03:21:40Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.02 psi 2024-11-06T03:21:40Z- handleDepth()- Depth_Keller msg: 1730863300.07 epoch sec; seqNo:4802; depth 49.50 2024-11-06T03:21:40Z- handleDVL() - DVL msg epochMillisec: 1730863300.07 2024-11-06T03:21:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:40Z- handleDvl() - RDI_Pathfinder msg: 1730863300.07 sec; seqNo:1599; #beams:0 2024-11-06T03:21:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , 0.01 2024-11-06T03:21:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863300.08 epoch sec; seqNo:4802; 4061560.63 north; 594768.86 east; 36.69 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:21:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863298.81 2024-11-06T03:21:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:40Z- waiting for 1730863302.81, time is 1730863300.08 2024-11-06T03:21:40Z- AHRS_M2 msg: 1730863300.41 epoch sec; seqNo:4515 2024-11-06T03:21:40Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.02 psi 2024-11-06T03:21:40Z- handleDepth()- Depth_Keller msg: 1730863300.41 epoch sec; seqNo:4803; depth 49.49 2024-11-06T03:21:40Z- handleDVL() - DVL msg epochMillisec: 1730863300.44 2024-11-06T03:21:40Z- handleDvl() - RDI_Pathfinder msg: 1730863300.44 sec; seqNo:1600; #beams:4 2024-11-06T03:21:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.48 , 27.68 , 29.06 , 37.01 2024-11-06T03:21:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , -0.01 2024-11-06T03:21:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863300.46 epoch sec; seqNo:4803; 4061561.00 north; 594767.63 east; 36.69 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:21:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863298.81 2024-11-06T03:21:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:40Z- waiting for 1730863302.81, time is 1730863300.46 2024-11-06T03:21:40Z- AHRS_M2 msg: 1730863300.81 epoch sec; seqNo:4516 2024-11-06T03:21:40Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.03 psi 2024-11-06T03:21:40Z- handleDepth()- Depth_Keller msg: 1730863300.82 epoch sec; seqNo:4804; depth 49.49 2024-11-06T03:21:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863300.83 epoch sec; seqNo:4804; 4061561.00 north; 594767.63 east; 36.69 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:21:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863298.81 2024-11-06T03:21:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:40Z- waiting for 1730863302.81, time is 1730863300.83 2024-11-06T03:21:41Z- AHRS_M2 msg: 1730863301.22 epoch sec; seqNo:4517 2024-11-06T03:21:41Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.03 psi 2024-11-06T03:21:41Z- handleDepth()- Depth_Keller msg: 1730863301.23 epoch sec; seqNo:4805; depth 49.48 2024-11-06T03:21:41Z- handleDVL() - DVL msg epochMillisec: 1730863301.25 2024-11-06T03:21:41Z- handleDvl() - RDI_Pathfinder msg: 1730863301.25 sec; seqNo:1601; #beams:4 2024-11-06T03:21:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.33 , 27.99 , 29.52 , 36.55 2024-11-06T03:21:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.08 , -0.03 2024-11-06T03:21:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863301.28 epoch sec; seqNo:4805; 4061561.38 north; 594767.63 east; 36.69 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:21:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863298.81 2024-11-06T03:21:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:41Z- waiting for 1730863302.81, time is 1730863301.28 2024-11-06T03:21:41Z- AHRS_M2 msg: 1730863301.64 epoch sec; seqNo:4518 2024-11-06T03:21:41Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.03 psi 2024-11-06T03:21:41Z- handleDepth()- Depth_Keller msg: 1730863301.64 epoch sec; seqNo:4806; depth 49.47 2024-11-06T03:21:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863301.66 epoch sec; seqNo:4806; 4061561.76 north; 594767.62 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:21:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863298.81 2024-11-06T03:21:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:41Z- waiting for 1730863302.81, time is 1730863301.66 2024-11-06T03:21:42Z- handleDepth()- Depth_Keller msg: 1730863302.05 epoch sec; seqNo:4807; depth 49.47 2024-11-06T03:21:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863302.06 epoch sec; seqNo:4807; 4061562.14 north; 594767.62 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:21:42Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:21:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863298.81 2024-11-06T03:21:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:42Z- waiting for 1730863302.81, time is 1730863302.06 2024-11-06T03:21:42Z- AHRS_M2 msg: 1730863302.43 epoch sec; seqNo:4519 2024-11-06T03:21:42Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.03 psi 2024-11-06T03:21:42Z- handleDepth()- Depth_Keller msg: 1730863302.44 epoch sec; seqNo:4808; depth 49.47 2024-11-06T03:21:42Z- handleDVL() - DVL msg epochMillisec: 1730863302.46 2024-11-06T03:21:42Z- handleDvl() - RDI_Pathfinder msg: 1730863302.46 sec; seqNo:1602; #beams:4 2024-11-06T03:21:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.72 , 27.83 , 29.36 , 36.40 2024-11-06T03:21:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.10 , -0.05 2024-11-06T03:21:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863302.47 epoch sec; seqNo:4808; 4061562.51 north; 594767.61 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:21:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863298.81 2024-11-06T03:21:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:42Z- waiting for 1730863302.81, time is 1730863302.47 2024-11-06T03:21:42Z- AHRS_M2 msg: 1730863302.83 epoch sec; seqNo:4520 2024-11-06T03:21:42Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.03 psi 2024-11-06T03:21:42Z- handleDepth()- Depth_Keller msg: 1730863302.85 epoch sec; seqNo:4809; depth 49.48 2024-11-06T03:21:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863302.86 epoch sec; seqNo:4809; 4061562.89 north; 594767.61 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:21:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863298.81 2024-11-06T03:21:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:42Z- waiting for 1730863302.81, time is 1730863302.86 2024-11-06T03:21:42Z- LcmTrn::updateTrn() >>>> pose time 1730863302.86, meas time 1730863302.46 <<<< 2024-11-06T03:21:42Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863302.86 2024-11-06T03:21:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863302.46 sec, ping # 0; waiting for INS data more recent than 1730863298.81... 2024-11-06T03:21:42Z- TerrainNav::motionUpdate() - currEstimate = (4061563, 594767.61, 49.48) 2024-11-06T03:21:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:21:42Z- TerrainNav:: SubcloudNIS is 1.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:21:43Z- TNavPF::Projecting Measurements 2024-11-06T03:21:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:21:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:21:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:21:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:21:43Z- TNavFilter::4 good beams. 2024-11-06T03:21:43Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:21:43Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:21:43Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:21:43Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:21:43Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:21:43Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:21:43Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:21:43Z- TerrainMapOctree: (vehN, vehE ) = (4061562.56, 594767.73). 2024-11-06T03:21:43Z- TerrainMapOctree: Min Distance = 188.28. 2024-11-06T03:21:43Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:21:43Z- Forcing Subcloud Comparison 2024-11-06T03:21:43Z- Weighting particles with subcloud comparison 2024-11-06T03:21:43Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:43Z- meanExpectedMeasDiff: 4.055417 delta_rms_squared: 0.809613 alpha: 0.975892 2024-11-06T03:21:43Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:43Z- meanExpectedMeasDiff: -6.509463 delta_rms_squared: 1.849700 alpha: 0.989303 2024-11-06T03:21:43Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:43Z- meanExpectedMeasDiff: -6.102273 delta_rms_squared: 1.939187 alpha: 0.989792 2024-11-06T03:21:43Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:43Z- meanExpectedMeasDiff: 3.283608 delta_rms_squared: 1.777089 alpha: 0.988871 2024-11-06T03:21:43Z- Subcloud NIS: 1.231796 2024-11-06T03:21:43Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:21:43Z- TNavPF:: sumWeights = 0.000001 2024-11-06T03:21:43Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:21:43Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:21:43Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:21:43Z- TerrainNav::Resampling particle filter... 2024-11-06T03:21:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863302.46 sec. 2024-11-06T03:21:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:21:43Z- variances([0], [2], and psi_berg) = 0.329162 0.091875 0.000000 2024-11-06T03:21:43Z- publishEstimatesDecoder() - at 1730863302.86 sec 2024-11-06T03:21:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:21:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061593.58 594750.46 52.48 2024-11-06T03:21:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061593.50 594750.37 53.43 lat/lon: 36.70 -121.94 2024-11-06T03:21:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.09 0.61 0.00 2024-11-06T03:21:43Z- AHRS_M2 msg: 1730863303.25 epoch sec; seqNo:4521 2024-11-06T03:21:43Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.03 psi 2024-11-06T03:21:43Z- handleDepth()- Depth_Keller msg: 1730863303.25 epoch sec; seqNo:4810; depth 49.46 2024-11-06T03:21:43Z- handleDVL() - DVL msg epochMillisec: 1730863303.27 2024-11-06T03:21:43Z- handleDvl() - RDI_Pathfinder msg: 1730863303.27 sec; seqNo:1603; #beams:4 2024-11-06T03:21:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.48 , 27.68 , 29.06 , 35.63 2024-11-06T03:21:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.13 , -0.05 2024-11-06T03:21:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863303.29 epoch sec; seqNo:4810; 4061563.27 north; 594767.61 east; 36.70 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T03:21:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863302.86 2024-11-06T03:21:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:43Z- waiting for 1730863306.86, time is 1730863303.29 2024-11-06T03:21:43Z- AHRS_M2 msg: 1730863303.65 epoch sec; seqNo:4522 2024-11-06T03:21:43Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.03 psi 2024-11-06T03:21:43Z- handleDepth()- Depth_Keller msg: 1730863303.65 epoch sec; seqNo:4811; depth 49.47 2024-11-06T03:21:43Z- handleDVL() - DVL msg epochMillisec: 1730863303.65 2024-11-06T03:21:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:43Z- handleDvl() - RDI_Pathfinder msg: 1730863303.65 sec; seqNo:1604; #beams:0 2024-11-06T03:21:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.13 , -0.05 2024-11-06T03:21:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863303.68 epoch sec; seqNo:4811; 4061563.65 north; 594767.60 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:21:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863302.86 2024-11-06T03:21:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:43Z- waiting for 1730863306.86, time is 1730863303.68 2024-11-06T03:21:44Z- AHRS_M2 msg: 1730863304.05 epoch sec; seqNo:4523 2024-11-06T03:21:44Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.03 psi 2024-11-06T03:21:44Z- handleDepth()- Depth_Keller msg: 1730863304.06 epoch sec; seqNo:4812; depth 49.47 2024-11-06T03:21:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863304.07 epoch sec; seqNo:4812; 4061564.03 north; 594767.60 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:21:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863302.86 2024-11-06T03:21:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:44Z- waiting for 1730863306.86, time is 1730863304.07 2024-11-06T03:21:44Z- AHRS_M2 msg: 1730863304.45 epoch sec; seqNo:4524 2024-11-06T03:21:44Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.03 psi 2024-11-06T03:21:44Z- handleDepth()- Depth_Keller msg: 1730863304.46 epoch sec; seqNo:4813; depth 49.48 2024-11-06T03:21:44Z- handleDVL() - DVL msg epochMillisec: 1730863304.47 2024-11-06T03:21:44Z- handleDvl() - RDI_Pathfinder msg: 1730863304.47 sec; seqNo:1605; #beams:4 2024-11-06T03:21:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.34 , 27.53 , 29.36 , 35.33 2024-11-06T03:21:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.12 , -0.02 2024-11-06T03:21:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863304.49 epoch sec; seqNo:4813; 4061564.40 north; 594766.37 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:21:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863302.86 2024-11-06T03:21:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:44Z- waiting for 1730863306.86, time is 1730863304.49 2024-11-06T03:21:44Z- AHRS_M2 msg: 1730863304.85 epoch sec; seqNo:4525 2024-11-06T03:21:44Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.03 psi 2024-11-06T03:21:44Z- handleDepth()- Depth_Keller msg: 1730863304.87 epoch sec; seqNo:4814; depth 49.50 2024-11-06T03:21:44Z- handleDVL() - DVL msg epochMillisec: 1730863304.87 2024-11-06T03:21:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:44Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:44Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:44Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:44Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:44Z- handleDvl() - RDI_Pathfinder msg: 1730863304.87 sec; seqNo:1606; #beams:0 2024-11-06T03:21:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.12 , -0.02 2024-11-06T03:21:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863304.89 epoch sec; seqNo:4814; 4061564.77 north; 594766.37 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:21:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863302.86 2024-11-06T03:21:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:45Z- waiting for 1730863306.86, time is 1730863304.89 2024-11-06T03:21:45Z- AHRS_M2 msg: 1730863305.26 epoch sec; seqNo:4526 2024-11-06T03:21:45Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 6.03 psi 2024-11-06T03:21:45Z- handleDepth()- Depth_Keller msg: 1730863305.26 epoch sec; seqNo:4815; depth 49.51 2024-11-06T03:21:45Z- handleDVL() - DVL msg epochMillisec: 1730863305.29 2024-11-06T03:21:45Z- handleDvl() - RDI_Pathfinder msg: 1730863305.29 sec; seqNo:1607; #beams:4 2024-11-06T03:21:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.03 , 27.22 , 29.36 , 34.87 2024-11-06T03:21:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.14 , 0.01 2024-11-06T03:21:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863305.31 epoch sec; seqNo:4815; 4061565.15 north; 594766.36 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T03:21:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863302.86 2024-11-06T03:21:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:45Z- waiting for 1730863306.86, time is 1730863305.31 2024-11-06T03:21:45Z- AHRS_M2 msg: 1730863305.66 epoch sec; seqNo:4527 2024-11-06T03:21:45Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 6.03 psi 2024-11-06T03:21:45Z- handleDepth()- Depth_Keller msg: 1730863305.67 epoch sec; seqNo:4816; depth 49.50 2024-11-06T03:21:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863305.70 epoch sec; seqNo:4816; 4061565.53 north; 594766.36 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:21:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863302.86 2024-11-06T03:21:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:45Z- waiting for 1730863306.86, time is 1730863305.70 2024-11-06T03:21:46Z- AHRS_M2 msg: 1730863306.08 epoch sec; seqNo:4528 2024-11-06T03:21:46Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.03 psi 2024-11-06T03:21:46Z- handleDepth()- Depth_Keller msg: 1730863306.08 epoch sec; seqNo:4817; depth 49.53 2024-11-06T03:21:46Z- handleDVL() - DVL msg epochMillisec: 1730863306.08 2024-11-06T03:21:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:46Z- handleDvl() - RDI_Pathfinder msg: 1730863306.08 sec; seqNo:1608; #beams:0 2024-11-06T03:21:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.14 , 0.01 2024-11-06T03:21:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863306.09 epoch sec; seqNo:4817; 4061565.91 north; 594766.36 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T03:21:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863302.86 2024-11-06T03:21:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:46Z- waiting for 1730863306.86, time is 1730863306.09 2024-11-06T03:21:46Z- AHRS_M2 msg: 1730863306.47 epoch sec; seqNo:4529 2024-11-06T03:21:46Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.03 psi 2024-11-06T03:21:46Z- handleDepth()- Depth_Keller msg: 1730863306.49 epoch sec; seqNo:4818; depth 49.53 2024-11-06T03:21:46Z- handleDVL() - DVL msg epochMillisec: 1730863306.51 2024-11-06T03:21:46Z- handleDvl() - RDI_Pathfinder msg: 1730863306.51 sec; seqNo:1609; #beams:4 2024-11-06T03:21:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.03 , 27.22 , 29.21 , 34.87 2024-11-06T03:21:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.12 , 0.04 2024-11-06T03:21:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863306.52 epoch sec; seqNo:4818; 4061566.29 north; 594766.35 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T03:21:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863302.86 2024-11-06T03:21:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:46Z- waiting for 1730863306.86, time is 1730863306.52 2024-11-06T03:21:46Z- AHRS_M2 msg: 1730863306.87 epoch sec; seqNo:4530 2024-11-06T03:21:46Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.03 psi 2024-11-06T03:21:46Z- handleDepth()- Depth_Keller msg: 1730863306.88 epoch sec; seqNo:4819; depth 49.55 2024-11-06T03:21:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863306.90 epoch sec; seqNo:4819; 4061566.67 north; 594766.35 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:21:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863302.86 2024-11-06T03:21:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:47Z- waiting for 1730863306.86, time is 1730863306.90 2024-11-06T03:21:47Z- LcmTrn::updateTrn() >>>> pose time 1730863306.90, meas time 1730863306.51 <<<< 2024-11-06T03:21:47Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863306.90 2024-11-06T03:21:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863306.51 sec, ping # 0; waiting for INS data more recent than 1730863302.86... 2024-11-06T03:21:47Z- TerrainNav::motionUpdate() - currEstimate = (4061567, 594766.35, 49.55) 2024-11-06T03:21:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:21:47Z- TerrainNav:: SubcloudNIS is 1.23, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:21:47Z- TNavPF::Projecting Measurements 2024-11-06T03:21:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:21:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:21:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:21:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:21:47Z- TNavFilter::4 good beams. 2024-11-06T03:21:47Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:21:47Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:21:47Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:21:47Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:21:47Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:21:47Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:21:47Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:21:47Z- TerrainMapOctree: (vehN, vehE ) = (4061566.30, 594766.47). 2024-11-06T03:21:47Z- TerrainMapOctree: Min Distance = 184.43. 2024-11-06T03:21:47Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:21:47Z- Forcing Subcloud Comparison 2024-11-06T03:21:47Z- Weighting particles with subcloud comparison 2024-11-06T03:21:47Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:47Z- meanExpectedMeasDiff: 3.666334 delta_rms_squared: 0.469406 alpha: 0.959134 2024-11-06T03:21:47Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:47Z- meanExpectedMeasDiff: -4.572589 delta_rms_squared: 1.924722 alpha: 0.989716 2024-11-06T03:21:47Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:47Z- meanExpectedMeasDiff: -4.907298 delta_rms_squared: 1.006897 alpha: 0.980524 2024-11-06T03:21:47Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:47Z- meanExpectedMeasDiff: 4.979876 delta_rms_squared: 3.160475 alpha: 0.993712 2024-11-06T03:21:47Z- Subcloud NIS: 1.265430 2024-11-06T03:21:47Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:21:47Z- TNavPF:: sumWeights = 0.000005 2024-11-06T03:21:47Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:21:47Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:21:47Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:21:47Z- TerrainNav::Resampling particle filter... 2024-11-06T03:21:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863306.51 sec. 2024-11-06T03:21:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:21:47Z- variances([0], [2], and psi_berg) = 0.665938 0.129655 0.000000 2024-11-06T03:21:47Z- publishEstimatesDecoder() - at 1730863306.90 sec 2024-11-06T03:21:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:21:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061597.31 594749.08 54.32 2024-11-06T03:21:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061597.07 594749.10 53.95 lat/lon: 36.70 -121.94 2024-11-06T03:21:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.67 0.13 0.88 0.00 2024-11-06T03:21:47Z- AHRS_M2 msg: 1730863307.28 epoch sec; seqNo:4531 2024-11-06T03:21:47Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.03 psi 2024-11-06T03:21:47Z- handleDepth()- Depth_Keller msg: 1730863307.29 epoch sec; seqNo:4820; depth 49.56 2024-11-06T03:21:47Z- handleDVL() - DVL msg epochMillisec: 1730863307.32 2024-11-06T03:21:47Z- handleDvl() - RDI_Pathfinder msg: 1730863307.32 sec; seqNo:1610; #beams:4 2024-11-06T03:21:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.88 , 27.22 , 29.06 , 35.02 2024-11-06T03:21:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.10 , 0.05 2024-11-06T03:21:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863307.35 epoch sec; seqNo:4820; 4061567.05 north; 594766.34 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:21:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863306.90 2024-11-06T03:21:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:47Z- waiting for 1730863310.90, time is 1730863307.35 2024-11-06T03:21:47Z- AHRS_M2 msg: 1730863307.69 epoch sec; seqNo:4532 2024-11-06T03:21:47Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.03 psi 2024-11-06T03:21:47Z- handleDepth()- Depth_Keller msg: 1730863307.69 epoch sec; seqNo:4821; depth 49.58 2024-11-06T03:21:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863307.71 epoch sec; seqNo:4821; 4061567.03 north; 594765.12 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:21:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863306.90 2024-11-06T03:21:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:47Z- waiting for 1730863310.90, time is 1730863307.71 2024-11-06T03:21:48Z- AHRS_M2 msg: 1730863308.09 epoch sec; seqNo:4533 2024-11-06T03:21:48Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.03 psi 2024-11-06T03:21:48Z- handleDepth()- Depth_Keller msg: 1730863308.09 epoch sec; seqNo:4822; depth 49.60 2024-11-06T03:21:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863308.12 epoch sec; seqNo:4822; 4061567.41 north; 594765.12 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:21:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863306.90 2024-11-06T03:21:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:48Z- waiting for 1730863310.90, time is 1730863308.12 2024-11-06T03:21:48Z- AHRS_M2 msg: 1730863308.49 epoch sec; seqNo:4534 2024-11-06T03:21:48Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.03 psi 2024-11-06T03:21:48Z- handleDepth()- Depth_Keller msg: 1730863308.49 epoch sec; seqNo:4823; depth 49.62 2024-11-06T03:21:48Z- handleDVL() - DVL msg epochMillisec: 1730863308.52 2024-11-06T03:21:48Z- handleDvl() - RDI_Pathfinder msg: 1730863308.52 sec; seqNo:1611; #beams:4 2024-11-06T03:21:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.42 , 27.38 , 29.21 , 35.48 2024-11-06T03:21:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , 0.05 2024-11-06T03:21:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863308.54 epoch sec; seqNo:4823; 4061567.79 north; 594765.12 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:21:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863306.90 2024-11-06T03:21:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:48Z- waiting for 1730863310.90, time is 1730863308.54 2024-11-06T03:21:48Z- AHRS_M2 msg: 1730863308.89 epoch sec; seqNo:4535 2024-11-06T03:21:48Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.03 psi 2024-11-06T03:21:48Z- handleDepth()- Depth_Keller msg: 1730863308.91 epoch sec; seqNo:4824; depth 49.63 2024-11-06T03:21:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863308.92 epoch sec; seqNo:4824; 4061568.17 north; 594765.11 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:21:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863306.90 2024-11-06T03:21:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:49Z- waiting for 1730863310.90, time is 1730863308.92 2024-11-06T03:21:49Z- AHRS_M2 msg: 1730863309.30 epoch sec; seqNo:4536 2024-11-06T03:21:49Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.03 psi 2024-11-06T03:21:49Z- handleDepth()- Depth_Keller msg: 1730863309.31 epoch sec; seqNo:4825; depth 49.65 2024-11-06T03:21:49Z- handleDVL() - DVL msg epochMillisec: 1730863309.33 2024-11-06T03:21:49Z- handleDvl() - RDI_Pathfinder msg: 1730863309.33 sec; seqNo:1612; #beams:4 2024-11-06T03:21:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.27 , 27.68 , 28.90 , 35.17 2024-11-06T03:21:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.03 2024-11-06T03:21:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863309.34 epoch sec; seqNo:4825; 4061568.55 north; 594765.11 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:21:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863306.90 2024-11-06T03:21:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:49Z- waiting for 1730863310.90, time is 1730863309.34 2024-11-06T03:21:49Z- AHRS_M2 msg: 1730863309.71 epoch sec; seqNo:4537 2024-11-06T03:21:49Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.04 psi 2024-11-06T03:21:49Z- handleDepth()- Depth_Keller msg: 1730863309.71 epoch sec; seqNo:4826; depth 49.67 2024-11-06T03:21:49Z- handleDVL() - DVL msg epochMillisec: 1730863309.71 2024-11-06T03:21:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:49Z- handleDvl() - RDI_Pathfinder msg: 1730863309.71 sec; seqNo:1613; #beams:0 2024-11-06T03:21:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.03 2024-11-06T03:21:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863309.73 epoch sec; seqNo:4826; 4061568.93 north; 594765.10 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:21:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863306.90 2024-11-06T03:21:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:49Z- waiting for 1730863310.90, time is 1730863309.73 2024-11-06T03:21:50Z- AHRS_M2 msg: 1730863310.11 epoch sec; seqNo:4538 2024-11-06T03:21:50Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.04 psi 2024-11-06T03:21:50Z- handleDepth()- Depth_Keller msg: 1730863310.11 epoch sec; seqNo:4827; depth 49.68 2024-11-06T03:21:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863310.15 epoch sec; seqNo:4827; 4061569.31 north; 594765.10 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:21:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863306.90 2024-11-06T03:21:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:50Z- waiting for 1730863310.90, time is 1730863310.15 2024-11-06T03:21:50Z- AHRS_M2 msg: 1730863310.51 epoch sec; seqNo:4539 2024-11-06T03:21:50Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.04 psi 2024-11-06T03:21:50Z- handleDepth()- Depth_Keller msg: 1730863310.52 epoch sec; seqNo:4828; depth 49.69 2024-11-06T03:21:50Z- handleDVL() - DVL msg epochMillisec: 1730863310.54 2024-11-06T03:21:50Z- handleDvl() - RDI_Pathfinder msg: 1730863310.54 sec; seqNo:1614; #beams:4 2024-11-06T03:21:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.27 , 27.83 , 28.90 , 35.02 2024-11-06T03:21:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , 0.00 2024-11-06T03:21:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863310.57 epoch sec; seqNo:4828; 4061569.69 north; 594765.09 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:21:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863306.90 2024-11-06T03:21:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:50Z- waiting for 1730863310.90, time is 1730863310.57 2024-11-06T03:21:50Z- AHRS_M2 msg: 1730863310.91 epoch sec; seqNo:4540 2024-11-06T03:21:50Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.04 psi 2024-11-06T03:21:50Z- handleDepth()- Depth_Keller msg: 1730863310.93 epoch sec; seqNo:4829; depth 49.70 2024-11-06T03:21:50Z- handleDVL() - DVL msg epochMillisec: 1730863310.93 2024-11-06T03:21:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:50Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:50Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:50Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:50Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:50Z- handleDvl() - RDI_Pathfinder msg: 1730863310.93 sec; seqNo:1615; #beams:0 2024-11-06T03:21:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , 0.00 2024-11-06T03:21:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863310.95 epoch sec; seqNo:4829; 4061570.06 north; 594765.09 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:21:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863306.90 2024-11-06T03:21:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:51Z- waiting for 1730863310.90, time is 1730863310.95 2024-11-06T03:21:51Z- LcmTrn::updateTrn() >>>> pose time 1730863310.95, meas time 1730863310.93 <<<< 2024-11-06T03:21:51Z- LcmTrn::updateTrn() equating pose 1730863310.93 and meas times 1730863310.93 2024-11-06T03:21:51Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863310.93 2024-11-06T03:21:51Z- TerrainNav::motionUpdate() - currEstimate = (4061570, 594765.09, 49.70) 2024-11-06T03:21:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:21:51Z- TerrainNav:: SubcloudNIS is 1.27, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:21:51Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:21:51Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:21:51Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:21:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:21:51Z- variances([0], [2], and psi_berg) = 0.666978 0.131689 0.000000 2024-11-06T03:21:51Z- publishEstimatesDecoder() - at 1730863310.93 sec 2024-11-06T03:21:51Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:21:51Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061600.70 594747.79 53.43 2024-11-06T03:21:51Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061600.46 594747.84 54.10 lat/lon: 36.70 -121.94 2024-11-06T03:21:51Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.67 0.13 1.13 0.00 2024-11-06T03:21:51Z- AHRS_M2 msg: 1730863311.33 epoch sec; seqNo:4541 2024-11-06T03:21:51Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.04 psi 2024-11-06T03:21:51Z- handleDepth()- Depth_Keller msg: 1730863311.34 epoch sec; seqNo:4830; depth 49.70 2024-11-06T03:21:51Z- handleDVL() - DVL msg epochMillisec: 1730863311.36 2024-11-06T03:21:51Z- handleDvl() - RDI_Pathfinder msg: 1730863311.36 sec; seqNo:1616; #beams:4 2024-11-06T03:21:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.12 , 27.68 , 28.90 , 34.87 2024-11-06T03:21:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , -0.01 2024-11-06T03:21:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863311.38 epoch sec; seqNo:4830; 4061570.44 north; 594765.09 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:21:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863310.93 2024-11-06T03:21:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:51Z- waiting for 1730863314.93, time is 1730863311.38 2024-11-06T03:21:51Z- AHRS_M2 msg: 1730863311.73 epoch sec; seqNo:4542 2024-11-06T03:21:51Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.04 psi 2024-11-06T03:21:51Z- handleDepth()- Depth_Keller msg: 1730863311.74 epoch sec; seqNo:4831; depth 49.71 2024-11-06T03:21:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863311.76 epoch sec; seqNo:4831; 4061570.44 north; 594765.09 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:21:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863310.93 2024-11-06T03:21:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:51Z- waiting for 1730863314.93, time is 1730863311.76 2024-11-06T03:21:52Z- AHRS_M2 msg: 1730863312.13 epoch sec; seqNo:4543 2024-11-06T03:21:52Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.04 psi 2024-11-06T03:21:52Z- handleDepth()- Depth_Keller msg: 1730863312.13 epoch sec; seqNo:4832; depth 49.73 2024-11-06T03:21:52Z- handleDVL() - DVL msg epochMillisec: 1730863312.13 2024-11-06T03:21:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:52Z- handleDvl() - RDI_Pathfinder msg: 1730863312.13 sec; seqNo:1617; #beams:0 2024-11-06T03:21:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , -0.01 2024-11-06T03:21:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863312.15 epoch sec; seqNo:4832; 4061570.81 north; 594763.86 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:21:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863310.93 2024-11-06T03:21:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:52Z- waiting for 1730863314.93, time is 1730863312.15 2024-11-06T03:21:52Z- AHRS_M2 msg: 1730863312.53 epoch sec; seqNo:4544 2024-11-06T03:21:52Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.04 psi 2024-11-06T03:21:52Z- handleDepth()- Depth_Keller msg: 1730863312.54 epoch sec; seqNo:4833; depth 49.75 2024-11-06T03:21:52Z- handleDVL() - DVL msg epochMillisec: 1730863312.57 2024-11-06T03:21:52Z- handleDvl() - RDI_Pathfinder msg: 1730863312.57 sec; seqNo:1618; #beams:4 2024-11-06T03:21:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.27 , 28.44 , 29.06 , 34.56 2024-11-06T03:21:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , -0.02 2024-11-06T03:21:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863312.58 epoch sec; seqNo:4833; 4061571.19 north; 594763.86 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:21:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863310.93 2024-11-06T03:21:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:52Z- waiting for 1730863314.93, time is 1730863312.58 2024-11-06T03:21:52Z- AHRS_M2 msg: 1730863312.94 epoch sec; seqNo:4545 2024-11-06T03:21:52Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.04 psi 2024-11-06T03:21:52Z- handleDepth()- Depth_Keller msg: 1730863312.94 epoch sec; seqNo:4834; depth 49.74 2024-11-06T03:21:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863312.95 epoch sec; seqNo:4834; 4061571.57 north; 594763.85 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:21:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863310.93 2024-11-06T03:21:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:53Z- waiting for 1730863314.93, time is 1730863312.95 2024-11-06T03:21:53Z- AHRS_M2 msg: 1730863313.36 epoch sec; seqNo:4546 2024-11-06T03:21:53Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.04 psi 2024-11-06T03:21:53Z- handleDepth()- Depth_Keller msg: 1730863313.36 epoch sec; seqNo:4835; depth 49.76 2024-11-06T03:21:53Z- handleDVL() - DVL msg epochMillisec: 1730863313.39 2024-11-06T03:21:53Z- handleDvl() - RDI_Pathfinder msg: 1730863313.39 sec; seqNo:1619; #beams:4 2024-11-06T03:21:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.57 , 28.60 , 28.90 , 34.56 2024-11-06T03:21:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , -0.02 2024-11-06T03:21:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863313.43 epoch sec; seqNo:4835; 4061571.95 north; 594763.85 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:21:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863310.93 2024-11-06T03:21:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:53Z- waiting for 1730863314.93, time is 1730863313.43 2024-11-06T03:21:53Z- AHRS_M2 msg: 1730863313.74 epoch sec; seqNo:4547 2024-11-06T03:21:53Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.04 psi 2024-11-06T03:21:53Z- handleDepth()- Depth_Keller msg: 1730863313.75 epoch sec; seqNo:4836; depth 49.76 2024-11-06T03:21:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863313.78 epoch sec; seqNo:4836; 4061572.32 north; 594763.84 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:21:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863310.93 2024-11-06T03:21:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:53Z- waiting for 1730863314.93, time is 1730863313.78 2024-11-06T03:21:54Z- AHRS_M2 msg: 1730863314.15 epoch sec; seqNo:4548 2024-11-06T03:21:54Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 6.04 psi 2024-11-06T03:21:54Z- handleDepth()- Depth_Keller msg: 1730863314.16 epoch sec; seqNo:4837; depth 49.76 2024-11-06T03:21:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863314.17 epoch sec; seqNo:4837; 4061572.70 north; 594763.84 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:21:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863310.93 2024-11-06T03:21:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:54Z- waiting for 1730863314.93, time is 1730863314.17 2024-11-06T03:21:54Z- AHRS_M2 msg: 1730863314.55 epoch sec; seqNo:4549 2024-11-06T03:21:54Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 6.04 psi 2024-11-06T03:21:54Z- handleDepth()- Depth_Keller msg: 1730863314.56 epoch sec; seqNo:4838; depth 49.78 2024-11-06T03:21:54Z- handleDVL() - DVL msg epochMillisec: 1730863314.58 2024-11-06T03:21:54Z- handleDvl() - RDI_Pathfinder msg: 1730863314.58 sec; seqNo:1620; #beams:4 2024-11-06T03:21:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.81 , 28.44 , 29.06 , 34.41 2024-11-06T03:21:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.09 , -0.01 2024-11-06T03:21:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863314.61 epoch sec; seqNo:4838; 4061573.08 north; 594763.84 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:21:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863310.93 2024-11-06T03:21:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:54Z- waiting for 1730863314.93, time is 1730863314.61 2024-11-06T03:21:54Z- AHRS_M2 msg: 1730863314.95 epoch sec; seqNo:4550 2024-11-06T03:21:54Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 6.04 psi 2024-11-06T03:21:54Z- handleDepth()- Depth_Keller msg: 1730863314.97 epoch sec; seqNo:4839; depth 49.78 2024-11-06T03:21:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863314.98 epoch sec; seqNo:4839; 4061573.46 north; 594763.83 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:21:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863310.93 2024-11-06T03:21:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:55Z- waiting for 1730863314.93, time is 1730863314.98 2024-11-06T03:21:55Z- LcmTrn::updateTrn() >>>> pose time 1730863314.98, meas time 1730863314.58 <<<< 2024-11-06T03:21:55Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863314.98 2024-11-06T03:21:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863314.58 sec, ping # 0; waiting for INS data more recent than 1730863310.93... 2024-11-06T03:21:55Z- TerrainNav::motionUpdate() - currEstimate = (4061573, 594763.83, 49.78) 2024-11-06T03:21:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:21:55Z- TerrainNav:: SubcloudNIS is 1.27, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:21:55Z- TNavPF::Projecting Measurements 2024-11-06T03:21:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:21:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:21:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:21:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:21:55Z- TNavFilter::4 good beams. 2024-11-06T03:21:55Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:21:55Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:21:55Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:21:55Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:21:55Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:21:55Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:21:55Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:21:55Z- TerrainMapOctree: (vehN, vehE ) = (4061573.12, 594763.96). 2024-11-06T03:21:55Z- TerrainMapOctree: Min Distance = 177.43. 2024-11-06T03:21:55Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:21:55Z- Forcing Subcloud Comparison 2024-11-06T03:21:55Z- Weighting particles with subcloud comparison 2024-11-06T03:21:55Z- beam number: 0 num in subcloud: 9987 num not in subcloud: 13 2024-11-06T03:21:55Z- meanExpectedMeasDiff: 0.979917 delta_rms_squared: 2.254941 alpha: 0.991209 2024-11-06T03:21:55Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:55Z- meanExpectedMeasDiff: -3.430066 delta_rms_squared: 1.698446 alpha: 0.988362 2024-11-06T03:21:55Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:55Z- meanExpectedMeasDiff: -3.917788 delta_rms_squared: 1.926659 alpha: 0.989726 2024-11-06T03:21:55Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:21:55Z- meanExpectedMeasDiff: 2.940273 delta_rms_squared: 1.101515 alpha: 0.982167 2024-11-06T03:21:55Z- Subcloud NIS: 1.213210 2024-11-06T03:21:55Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:21:55Z- TNavPF:: sumWeights = 0.003886 2024-11-06T03:21:55Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:21:55Z- TNavPF::First Term of Map Covariance Matrix is nan 2024-11-06T03:21:55Z- TNavPF::Calculated NIS Value : nan numBeams normalized NIS: nan 2024-11-06T03:21:55Z- TerrainNav::Resampling particle filter... 2024-11-06T03:21:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863314.58 sec. 2024-11-06T03:21:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:21:55Z- variances([0], [2], and psi_berg) = 0.617678 0.126422 0.000000 2024-11-06T03:21:55Z- publishEstimatesDecoder() - at 1730863314.98 sec 2024-11-06T03:21:55Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:21:55Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061604.16 594746.66 54.38 2024-11-06T03:21:55Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061603.96 594746.57 54.93 lat/lon: 36.70 -121.94 2024-11-06T03:21:55Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.62 0.13 0.69 0.00 2024-11-06T03:21:55Z- handleDepth()- Depth_Keller msg: 1730863315.41 epoch sec; seqNo:4840; depth 49.78 2024-11-06T03:21:55Z- handleDVL() - DVL msg epochMillisec: 1730863315.44 2024-11-06T03:21:55Z- handleDvl() - RDI_Pathfinder msg: 1730863315.44 sec; seqNo:1621; #beams:4 2024-11-06T03:21:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.81 , 28.44 , 28.90 , 34.26 2024-11-06T03:21:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.09 , 0.00 2024-11-06T03:21:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863315.46 epoch sec; seqNo:4840; 4061573.84 north; 594763.83 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:21:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863314.98 2024-11-06T03:21:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:55Z- waiting for 1730863318.98, time is 1730863315.46 2024-11-06T03:21:55Z- AHRS_M2 msg: 1730863315.77 epoch sec; seqNo:4551 2024-11-06T03:21:55Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 6.04 psi 2024-11-06T03:21:55Z- handleDepth()- Depth_Keller msg: 1730863315.77 epoch sec; seqNo:4841; depth 49.79 2024-11-06T03:21:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863315.79 epoch sec; seqNo:4841; 4061574.22 north; 594763.82 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:21:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863314.98 2024-11-06T03:21:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:55Z- waiting for 1730863318.98, time is 1730863315.79 2024-11-06T03:21:56Z- AHRS_M2 msg: 1730863316.17 epoch sec; seqNo:4552 2024-11-06T03:21:56Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 6.04 psi 2024-11-06T03:21:56Z- handleDepth()- Depth_Keller msg: 1730863316.17 epoch sec; seqNo:4842; depth 49.81 2024-11-06T03:21:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863316.19 epoch sec; seqNo:4842; 4061574.58 north; 594762.60 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:21:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863314.98 2024-11-06T03:21:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:56Z- waiting for 1730863318.98, time is 1730863316.19 2024-11-06T03:21:56Z- AHRS_M2 msg: 1730863316.57 epoch sec; seqNo:4553 2024-11-06T03:21:56Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 6.04 psi 2024-11-06T03:21:56Z- handleDepth()- Depth_Keller msg: 1730863316.58 epoch sec; seqNo:4843; depth 49.82 2024-11-06T03:21:56Z- handleDVL() - DVL msg epochMillisec: 1730863316.60 2024-11-06T03:21:56Z- handleDvl() - RDI_Pathfinder msg: 1730863316.60 sec; seqNo:1622; #beams:4 2024-11-06T03:21:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.20 , 28.44 , 28.75 , 34.10 2024-11-06T03:21:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.09 , 0.00 2024-11-06T03:21:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863316.62 epoch sec; seqNo:4843; 4061574.96 north; 594762.59 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:21:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863314.98 2024-11-06T03:21:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:56Z- waiting for 1730863318.98, time is 1730863316.62 2024-11-06T03:21:56Z- AHRS_M2 msg: 1730863316.98 epoch sec; seqNo:4554 2024-11-06T03:21:56Z- AHRS_M2 msg: 0.13 phi; -0.05 theta; 6.04 psi 2024-11-06T03:21:56Z- handleDepth()- Depth_Keller msg: 1730863316.98 epoch sec; seqNo:4844; depth 49.84 2024-11-06T03:21:56Z- handleDVL() - DVL msg epochMillisec: 1730863316.99 2024-11-06T03:21:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:57Z- handleDvl() - RDI_Pathfinder msg: 1730863316.99 sec; seqNo:1623; #beams:0 2024-11-06T03:21:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.09 , 0.00 2024-11-06T03:21:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863317.00 epoch sec; seqNo:4844; 4061575.34 north; 594762.59 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:21:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863314.98 2024-11-06T03:21:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:57Z- waiting for 1730863318.98, time is 1730863317.00 2024-11-06T03:21:57Z- AHRS_M2 msg: 1730863317.41 epoch sec; seqNo:4555 2024-11-06T03:21:57Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 6.04 psi 2024-11-06T03:21:57Z- handleDepth()- Depth_Keller msg: 1730863317.41 epoch sec; seqNo:4845; depth 49.83 2024-11-06T03:21:57Z- handleDVL() - DVL msg epochMillisec: 1730863317.44 2024-11-06T03:21:57Z- handleDvl() - RDI_Pathfinder msg: 1730863317.44 sec; seqNo:1624; #beams:4 2024-11-06T03:21:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 30.86 , 28.14 , 28.55 , 33.71 2024-11-06T03:21:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.09 , 0.00 2024-11-06T03:21:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863317.45 epoch sec; seqNo:4845; 4061575.72 north; 594762.59 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:21:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863314.98 2024-11-06T03:21:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:57Z- waiting for 1730863318.98, time is 1730863317.45 2024-11-06T03:21:57Z- AHRS_M2 msg: 1730863317.78 epoch sec; seqNo:4556 2024-11-06T03:21:57Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 6.05 psi 2024-11-06T03:21:57Z- handleDepth()- Depth_Keller msg: 1730863317.80 epoch sec; seqNo:4846; depth 49.85 2024-11-06T03:21:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863317.81 epoch sec; seqNo:4846; 4061576.10 north; 594762.58 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:21:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863314.98 2024-11-06T03:21:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:57Z- waiting for 1730863318.98, time is 1730863317.81 2024-11-06T03:21:58Z- AHRS_M2 msg: 1730863318.19 epoch sec; seqNo:4557 2024-11-06T03:21:58Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 6.05 psi 2024-11-06T03:21:58Z- handleDepth()- Depth_Keller msg: 1730863318.19 epoch sec; seqNo:4847; depth 49.87 2024-11-06T03:21:58Z- handleDVL() - DVL msg epochMillisec: 1730863318.19 2024-11-06T03:21:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:58Z- handleDvl() - RDI_Pathfinder msg: 1730863318.19 sec; seqNo:1625; #beams:0 2024-11-06T03:21:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.09 , 0.00 2024-11-06T03:21:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863318.20 epoch sec; seqNo:4847; 4061576.10 north; 594762.58 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:21:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863314.98 2024-11-06T03:21:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:58Z- waiting for 1730863318.98, time is 1730863318.20 2024-11-06T03:21:58Z- AHRS_M2 msg: 1730863318.59 epoch sec; seqNo:4558 2024-11-06T03:21:58Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 6.05 psi 2024-11-06T03:21:58Z- handleDepth()- Depth_Keller msg: 1730863318.60 epoch sec; seqNo:4848; depth 49.88 2024-11-06T03:21:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863318.61 epoch sec; seqNo:4848; 4061576.48 north; 594762.58 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:21:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863314.98 2024-11-06T03:21:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:58Z- waiting for 1730863318.98, time is 1730863318.61 2024-11-06T03:21:59Z- AHRS_M2 msg: 1730863319.00 epoch sec; seqNo:4559 2024-11-06T03:21:59Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 6.05 psi 2024-11-06T03:21:59Z- handleDepth()- Depth_Keller msg: 1730863319.00 epoch sec; seqNo:4849; depth 49.89 2024-11-06T03:21:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863319.01 epoch sec; seqNo:4849; 4061576.86 north; 594762.57 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:21:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863314.98 2024-11-06T03:21:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:59Z- waiting for 1730863318.98, time is 1730863319.01 2024-11-06T03:21:59Z- LcmTrn::updateTrn() >>>> pose time 1730863319.01, meas time 1730863318.19 <<<< 2024-11-06T03:21:59Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863319.01 2024-11-06T03:21:59Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:21:59Z- TerrainNav::motionUpdate() - currEstimate = (4061577, 594762.57, 49.89) 2024-11-06T03:21:59Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:21:59Z- TerrainNav:: SubcloudNIS is 1.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:21:59Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:21:59Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:21:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:21:59Z- variances([0], [2], and psi_berg) = 0.618959 0.128147 0.000000 2024-11-06T03:21:59Z- publishEstimatesDecoder() - at 1730863319.01 sec 2024-11-06T03:21:59Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:21:59Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061607.51 594745.47 53.76 2024-11-06T03:21:59Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061607.36 594745.32 55.04 lat/lon: 36.70 -121.94 2024-11-06T03:21:59Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.62 0.13 0.95 0.00 2024-11-06T03:21:59Z- AHRS_M2 msg: 1730863319.43 epoch sec; seqNo:4560 2024-11-06T03:21:59Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.05 psi 2024-11-06T03:21:59Z- handleDepth()- Depth_Keller msg: 1730863319.44 epoch sec; seqNo:4850; depth 49.90 2024-11-06T03:21:59Z- handleDVL() - DVL msg epochMillisec: 1730863319.44 2024-11-06T03:21:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:21:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:21:59Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:21:59Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:21:59Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:21:59Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:21:59Z- handleDvl() - RDI_Pathfinder msg: 1730863319.44 sec; seqNo:1626; #beams:0 2024-11-06T03:21:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:21:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.09 , 0.00 2024-11-06T03:21:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863319.49 epoch sec; seqNo:4850; 4061577.61 north; 594762.57 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:21:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:21:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863319.01 2024-11-06T03:21:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:59Z- waiting for 1730863323.01, time is 1730863319.49 2024-11-06T03:21:59Z- AHRS_M2 msg: 1730863319.80 epoch sec; seqNo:4561 2024-11-06T03:21:59Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.05 psi 2024-11-06T03:21:59Z- handleDepth()- Depth_Keller msg: 1730863319.81 epoch sec; seqNo:4851; depth 49.91 2024-11-06T03:21:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863319.82 epoch sec; seqNo:4851; 4061577.61 north; 594762.57 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:21:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:21:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863319.01 2024-11-06T03:21:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:21:59Z- waiting for 1730863323.01, time is 1730863319.82 2024-11-06T03:22:00Z- AHRS_M2 msg: 1730863320.21 epoch sec; seqNo:4562 2024-11-06T03:22:00Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.05 psi 2024-11-06T03:22:00Z- handleDepth()- Depth_Keller msg: 1730863320.22 epoch sec; seqNo:4852; depth 49.93 2024-11-06T03:22:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863320.22 epoch sec; seqNo:4852; 4061577.98 north; 594761.34 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:22:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863319.01 2024-11-06T03:22:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:00Z- waiting for 1730863323.01, time is 1730863320.22 2024-11-06T03:22:00Z- AHRS_M2 msg: 1730863320.61 epoch sec; seqNo:4563 2024-11-06T03:22:00Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.05 psi 2024-11-06T03:22:00Z- handleDepth()- Depth_Keller msg: 1730863320.62 epoch sec; seqNo:4853; depth 49.93 2024-11-06T03:22:00Z- handleDVL() - DVL msg epochMillisec: 1730863320.65 2024-11-06T03:22:00Z- handleDvl() - RDI_Pathfinder msg: 1730863320.65 sec; seqNo:1627; #beams:4 2024-11-06T03:22:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 30.99 , 27.59 , 28.14 , 29.91 2024-11-06T03:22:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , 0.01 2024-11-06T03:22:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863320.68 epoch sec; seqNo:4853; 4061578.36 north; 594761.34 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:22:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863319.01 2024-11-06T03:22:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:00Z- waiting for 1730863323.01, time is 1730863320.68 2024-11-06T03:22:01Z- AHRS_M2 msg: 1730863321.01 epoch sec; seqNo:4564 2024-11-06T03:22:01Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.05 psi 2024-11-06T03:22:01Z- handleDepth()- Depth_Keller msg: 1730863321.02 epoch sec; seqNo:4854; depth 49.95 2024-11-06T03:22:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863321.04 epoch sec; seqNo:4854; 4061578.74 north; 594761.33 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:22:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863319.01 2024-11-06T03:22:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:01Z- waiting for 1730863323.01, time is 1730863321.04 2024-11-06T03:22:01Z- AHRS_M2 msg: 1730863321.46 epoch sec; seqNo:4565 2024-11-06T03:22:01Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.05 psi 2024-11-06T03:22:01Z- handleDepth()- Depth_Keller msg: 1730863321.47 epoch sec; seqNo:4855; depth 49.97 2024-11-06T03:22:01Z- handleDVL() - DVL msg epochMillisec: 1730863321.50 2024-11-06T03:22:01Z- handleDvl() - RDI_Pathfinder msg: 1730863321.50 sec; seqNo:1628; #beams:4 2024-11-06T03:22:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.40 , 27.73 , 28.00 , 28.82 2024-11-06T03:22:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.10 , 0.02 2024-11-06T03:22:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863321.51 epoch sec; seqNo:4855; 4061579.12 north; 594761.33 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:22:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863319.01 2024-11-06T03:22:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:01Z- waiting for 1730863323.01, time is 1730863321.51 2024-11-06T03:22:01Z- AHRS_M2 msg: 1730863321.83 epoch sec; seqNo:4566 2024-11-06T03:22:01Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.06 psi 2024-11-06T03:22:01Z- handleDepth()- Depth_Keller msg: 1730863321.83 epoch sec; seqNo:4856; depth 50.00 2024-11-06T03:22:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863321.85 epoch sec; seqNo:4856; 4061579.50 north; 594761.32 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:22:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863319.01 2024-11-06T03:22:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:01Z- waiting for 1730863323.01, time is 1730863321.85 2024-11-06T03:22:02Z- AHRS_M2 msg: 1730863322.23 epoch sec; seqNo:4567 2024-11-06T03:22:02Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.06 psi 2024-11-06T03:22:02Z- handleDepth()- Depth_Keller msg: 1730863322.23 epoch sec; seqNo:4857; depth 49.99 2024-11-06T03:22:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863322.24 epoch sec; seqNo:4857; 4061579.87 north; 594761.32 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:22:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863319.01 2024-11-06T03:22:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:02Z- waiting for 1730863323.01, time is 1730863322.24 2024-11-06T03:22:02Z- AHRS_M2 msg: 1730863322.63 epoch sec; seqNo:4568 2024-11-06T03:22:02Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.06 psi 2024-11-06T03:22:02Z- handleDepth()- Depth_Keller msg: 1730863322.65 epoch sec; seqNo:4858; depth 50.01 2024-11-06T03:22:02Z- handleDVL() - DVL msg epochMillisec: 1730863322.67 2024-11-06T03:22:02Z- handleDvl() - RDI_Pathfinder msg: 1730863322.67 sec; seqNo:1629; #beams:4 2024-11-06T03:22:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.27 , 27.46 , 28.14 , 28.95 2024-11-06T03:22:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.09 , 0.03 2024-11-06T03:22:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863322.68 epoch sec; seqNo:4858; 4061580.25 north; 594761.32 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:22:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863319.01 2024-11-06T03:22:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:02Z- waiting for 1730863323.01, time is 1730863322.68 2024-11-06T03:22:03Z- AHRS_M2 msg: 1730863323.04 epoch sec; seqNo:4569 2024-11-06T03:22:03Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.06 psi 2024-11-06T03:22:03Z- handleDepth()- Depth_Keller msg: 1730863323.04 epoch sec; seqNo:4859; depth 50.03 2024-11-06T03:22:03Z- handleDVL() - DVL msg epochMillisec: 1730863323.04 2024-11-06T03:22:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:03Z- handleDvl() - RDI_Pathfinder msg: 1730863323.04 sec; seqNo:1630; #beams:0 2024-11-06T03:22:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.09 , 0.03 2024-11-06T03:22:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863323.06 epoch sec; seqNo:4859; 4061580.63 north; 594761.31 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:22:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863319.01 2024-11-06T03:22:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:03Z- waiting for 1730863323.01, time is 1730863323.06 2024-11-06T03:22:03Z- LcmTrn::updateTrn() >>>> pose time 1730863323.06, meas time 1730863323.04 <<<< 2024-11-06T03:22:03Z- LcmTrn::updateTrn() equating pose 1730863323.04 and meas times 1730863323.04 2024-11-06T03:22:03Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863323.04 2024-11-06T03:22:03Z- TerrainNav::motionUpdate() - currEstimate = (4061581, 594761.31, 50.03) 2024-11-06T03:22:03Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:22:03Z- TerrainNav:: SubcloudNIS is 1.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:22:03Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:22:03Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:22:03Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:22:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:22:03Z- variances([0], [2], and psi_berg) = 0.621262 0.130908 0.000000 2024-11-06T03:22:03Z- publishEstimatesDecoder() - at 1730863323.04 sec 2024-11-06T03:22:03Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:22:03Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061611.23 594744.22 53.72 2024-11-06T03:22:03Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061611.13 594744.05 55.17 lat/lon: 36.70 -121.94 2024-11-06T03:22:03Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.62 0.13 1.20 0.00 2024-11-06T03:22:03Z- AHRS_M2 msg: 1730863323.52 epoch sec; seqNo:4570 2024-11-06T03:22:03Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.06 psi 2024-11-06T03:22:03Z- handleDepth()- Depth_Keller msg: 1730863323.52 epoch sec; seqNo:4860; depth 50.05 2024-11-06T03:22:03Z- handleDVL() - DVL msg epochMillisec: 1730863323.55 2024-11-06T03:22:03Z- handleDvl() - RDI_Pathfinder msg: 1730863323.55 sec; seqNo:1631; #beams:4 2024-11-06T03:22:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.40 , 27.32 , 28.00 , 28.95 2024-11-06T03:22:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , 0.03 2024-11-06T03:22:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863323.56 epoch sec; seqNo:4860; 4061581.01 north; 594761.31 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:22:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863323.04 2024-11-06T03:22:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:03Z- waiting for 1730863327.04, time is 1730863323.56 2024-11-06T03:22:03Z- AHRS_M2 msg: 1730863323.84 epoch sec; seqNo:4571 2024-11-06T03:22:03Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.06 psi 2024-11-06T03:22:03Z- handleDepth()- Depth_Keller msg: 1730863323.86 epoch sec; seqNo:4861; depth 50.06 2024-11-06T03:22:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863323.88 epoch sec; seqNo:4861; 4061581.39 north; 594761.30 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:22:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863323.04 2024-11-06T03:22:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:03Z- waiting for 1730863327.04, time is 1730863323.88 2024-11-06T03:22:04Z- AHRS_M2 msg: 1730863324.25 epoch sec; seqNo:4572 2024-11-06T03:22:04Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.07 psi 2024-11-06T03:22:04Z- handleDepth()- Depth_Keller msg: 1730863324.26 epoch sec; seqNo:4862; depth 50.06 2024-11-06T03:22:04Z- handleDVL() - DVL msg epochMillisec: 1730863324.26 2024-11-06T03:22:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:04Z- handleDvl() - RDI_Pathfinder msg: 1730863324.26 sec; seqNo:1632; #beams:0 2024-11-06T03:22:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , 0.03 2024-11-06T03:22:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863324.29 epoch sec; seqNo:4862; 4061581.75 north; 594760.08 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:22:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863323.04 2024-11-06T03:22:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:04Z- waiting for 1730863327.04, time is 1730863324.29 2024-11-06T03:22:04Z- AHRS_M2 msg: 1730863324.65 epoch sec; seqNo:4573 2024-11-06T03:22:04Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.07 psi 2024-11-06T03:22:04Z- handleDepth()- Depth_Keller msg: 1730863324.66 epoch sec; seqNo:4863; depth 50.07 2024-11-06T03:22:04Z- handleDVL() - DVL msg epochMillisec: 1730863324.68 2024-11-06T03:22:04Z- handleDvl() - RDI_Pathfinder msg: 1730863324.68 sec; seqNo:1633; #beams:4 2024-11-06T03:22:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.40 , 27.46 , 27.73 , 29.23 2024-11-06T03:22:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , 0.03 2024-11-06T03:22:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863324.72 epoch sec; seqNo:4863; 4061582.13 north; 594760.07 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:22:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863323.04 2024-11-06T03:22:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:04Z- waiting for 1730863327.04, time is 1730863324.72 2024-11-06T03:22:05Z- AHRS_M2 msg: 1730863325.06 epoch sec; seqNo:4574 2024-11-06T03:22:05Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.07 psi 2024-11-06T03:22:05Z- handleDepth()- Depth_Keller msg: 1730863325.07 epoch sec; seqNo:4864; depth 50.08 2024-11-06T03:22:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863325.08 epoch sec; seqNo:4864; 4061582.13 north; 594760.07 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:22:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863323.04 2024-11-06T03:22:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:05Z- waiting for 1730863327.04, time is 1730863325.08 2024-11-06T03:22:05Z- handleDepth()- Depth_Keller msg: 1730863325.49 epoch sec; seqNo:4865; depth 50.11 2024-11-06T03:22:05Z- handleDVL() - DVL msg epochMillisec: 1730863325.50 2024-11-06T03:22:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:05Z- handleDvl() - RDI_Pathfinder msg: 1730863325.50 sec; seqNo:1634; #beams:0 2024-11-06T03:22:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , 0.03 2024-11-06T03:22:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863325.53 epoch sec; seqNo:4865; 4061582.51 north; 594760.07 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:22:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863323.04 2024-11-06T03:22:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:05Z- waiting for 1730863327.04, time is 1730863325.53 2024-11-06T03:22:05Z- AHRS_M2 msg: 1730863325.86 epoch sec; seqNo:4575 2024-11-06T03:22:05Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.07 psi 2024-11-06T03:22:05Z- handleDepth()- Depth_Keller msg: 1730863325.87 epoch sec; seqNo:4866; depth 50.11 2024-11-06T03:22:05Z- handleDVL() - DVL msg epochMillisec: 1730863325.90 2024-11-06T03:22:05Z- handleDvl() - RDI_Pathfinder msg: 1730863325.90 sec; seqNo:1635; #beams:4 2024-11-06T03:22:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.54 , 27.73 , 27.46 , 29.36 2024-11-06T03:22:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , 0.02 2024-11-06T03:22:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863325.92 epoch sec; seqNo:4866; 4061582.89 north; 594760.07 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:22:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863323.04 2024-11-06T03:22:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:06Z- waiting for 1730863327.04, time is 1730863325.92 2024-11-06T03:22:06Z- AHRS_M2 msg: 1730863326.27 epoch sec; seqNo:4576 2024-11-06T03:22:06Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.07 psi 2024-11-06T03:22:06Z- handleDepth()- Depth_Keller msg: 1730863326.28 epoch sec; seqNo:4867; depth 50.13 2024-11-06T03:22:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863326.29 epoch sec; seqNo:4867; 4061583.27 north; 594760.06 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:22:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863323.04 2024-11-06T03:22:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:06Z- waiting for 1730863327.04, time is 1730863326.29 2024-11-06T03:22:06Z- AHRS_M2 msg: 1730863326.67 epoch sec; seqNo:4577 2024-11-06T03:22:06Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.07 psi 2024-11-06T03:22:06Z- handleDepth()- Depth_Keller msg: 1730863326.69 epoch sec; seqNo:4868; depth 50.12 2024-11-06T03:22:06Z- handleDVL() - DVL msg epochMillisec: 1730863326.71 2024-11-06T03:22:06Z- handleDvl() - RDI_Pathfinder msg: 1730863326.71 sec; seqNo:1636; #beams:4 2024-11-06T03:22:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.54 , 27.73 , 27.59 , 29.36 2024-11-06T03:22:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.01 2024-11-06T03:22:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863326.72 epoch sec; seqNo:4868; 4061583.65 north; 594760.06 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:22:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863323.04 2024-11-06T03:22:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:06Z- waiting for 1730863327.04, time is 1730863326.72 2024-11-06T03:22:07Z- AHRS_M2 msg: 1730863327.07 epoch sec; seqNo:4578 2024-11-06T03:22:07Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.07 psi 2024-11-06T03:22:07Z- handleDepth()- Depth_Keller msg: 1730863327.08 epoch sec; seqNo:4869; depth 50.14 2024-11-06T03:22:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863327.13 epoch sec; seqNo:4869; 4061584.03 north; 594760.05 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:22:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863323.04 2024-11-06T03:22:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:07Z- waiting for 1730863327.04, time is 1730863327.13 2024-11-06T03:22:07Z- LcmTrn::updateTrn() >>>> pose time 1730863327.13, meas time 1730863326.71 <<<< 2024-11-06T03:22:07Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863327.13 2024-11-06T03:22:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863326.71 sec, ping # 0; waiting for INS data more recent than 1730863323.04... 2024-11-06T03:22:07Z- TerrainNav::motionUpdate() - currEstimate = (4061584, 594760.05, 50.14) 2024-11-06T03:22:07Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:22:07Z- TerrainNav:: SubcloudNIS is 1.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:22:07Z- TNavPF::Projecting Measurements 2024-11-06T03:22:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:22:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:22:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:22:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:22:07Z- TNavFilter::4 good beams. 2024-11-06T03:22:07Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:22:07Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:22:07Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:22:07Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:22:07Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:22:07Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:22:07Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:22:07Z- TerrainMapOctree: (vehN, vehE ) = (4061583.68, 594760.18). 2024-11-06T03:22:07Z- TerrainMapOctree: Min Distance = 166.63. 2024-11-06T03:22:07Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:22:07Z- Forcing Subcloud Comparison 2024-11-06T03:22:07Z- Weighting particles with subcloud comparison 2024-11-06T03:22:07Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:07Z- meanExpectedMeasDiff: -0.673596 delta_rms_squared: 1.658983 alpha: 0.988088 2024-11-06T03:22:07Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:07Z- meanExpectedMeasDiff: -3.826330 delta_rms_squared: 1.482290 alpha: 0.986687 2024-11-06T03:22:07Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:07Z- meanExpectedMeasDiff: -5.021112 delta_rms_squared: 2.873516 alpha: 0.993088 2024-11-06T03:22:07Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:07Z- meanExpectedMeasDiff: -0.796923 delta_rms_squared: 3.383511 alpha: 0.994124 2024-11-06T03:22:07Z- Subcloud NIS: 1.260457 2024-11-06T03:22:07Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:22:07Z- TNavPF:: sumWeights = 0.014380 2024-11-06T03:22:07Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:22:07Z- TNavPF::First Term of Map Covariance Matrix is 0.02 2024-11-06T03:22:07Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:22:07Z- TerrainNav::Resampling particle filter... 2024-11-06T03:22:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863326.71 sec. 2024-11-06T03:22:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:22:07Z- variances([0], [2], and psi_berg) = 0.521664 0.116958 0.000000 2024-11-06T03:22:07Z- publishEstimatesDecoder() - at 1730863327.13 sec 2024-11-06T03:22:07Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:22:07Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061614.78 594742.95 55.03 2024-11-06T03:22:07Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061614.51 594742.88 56.81 lat/lon: 36.70 -121.94 2024-11-06T03:22:07Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.52 0.12 0.66 0.00 2024-11-06T03:22:07Z- handleDepth()- Depth_Keller msg: 1730863327.52 epoch sec; seqNo:4870; depth 50.15 2024-11-06T03:22:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863327.56 epoch sec; seqNo:4870; 4061584.41 north; 594760.05 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:22:07Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:22:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863327.13 2024-11-06T03:22:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:07Z- waiting for 1730863331.13, time is 1730863327.56 2024-11-06T03:22:07Z- handleDepth()- Depth_Keller msg: 1730863327.91 epoch sec; seqNo:4871; depth 50.15 2024-11-06T03:22:07Z- handleDVL() - DVL msg epochMillisec: 1730863327.93 2024-11-06T03:22:07Z- handleDvl() - RDI_Pathfinder msg: 1730863327.93 sec; seqNo:1637; #beams:4 2024-11-06T03:22:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.54 , 28.00 , 26.92 , 29.50 2024-11-06T03:22:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , -0.01 2024-11-06T03:22:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863327.94 epoch sec; seqNo:4871; 4061584.79 north; 594760.05 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:22:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863327.13 2024-11-06T03:22:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:08Z- waiting for 1730863331.13, time is 1730863327.94 2024-11-06T03:22:08Z- handleDepth()- Depth_Keller msg: 1730863328.31 epoch sec; seqNo:4872; depth 50.16 2024-11-06T03:22:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863328.32 epoch sec; seqNo:4872; 4061585.16 north; 594760.04 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:22:08Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:22:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863327.13 2024-11-06T03:22:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:08Z- waiting for 1730863331.13, time is 1730863328.32 2024-11-06T03:22:08Z- AHRS_M2 msg: 1730863328.69 epoch sec; seqNo:4579 2024-11-06T03:22:08Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 6.07 psi 2024-11-06T03:22:08Z- handleDepth()- Depth_Keller msg: 1730863328.70 epoch sec; seqNo:4873; depth 50.17 2024-11-06T03:22:08Z- handleDVL() - DVL msg epochMillisec: 1730863328.73 2024-11-06T03:22:08Z- handleDvl() - RDI_Pathfinder msg: 1730863328.73 sec; seqNo:1638; #beams:4 2024-11-06T03:22:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.54 , 27.87 , 26.37 , 29.36 2024-11-06T03:22:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.02 2024-11-06T03:22:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863328.74 epoch sec; seqNo:4873; 4061585.54 north; 594760.04 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:22:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863327.13 2024-11-06T03:22:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:08Z- waiting for 1730863331.13, time is 1730863328.74 2024-11-06T03:22:09Z- AHRS_M2 msg: 1730863329.10 epoch sec; seqNo:4580 2024-11-06T03:22:09Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 6.07 psi 2024-11-06T03:22:09Z- handleDepth()- Depth_Keller msg: 1730863329.10 epoch sec; seqNo:4874; depth 50.18 2024-11-06T03:22:09Z- handleDVL() - DVL msg epochMillisec: 1730863329.10 2024-11-06T03:22:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:09Z- handleDvl() - RDI_Pathfinder msg: 1730863329.10 sec; seqNo:1639; #beams:0 2024-11-06T03:22:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.02 2024-11-06T03:22:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863329.13 epoch sec; seqNo:4874; 4061585.91 north; 594758.81 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:22:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863327.13 2024-11-06T03:22:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:09Z- waiting for 1730863331.13, time is 1730863329.13 2024-11-06T03:22:09Z- AHRS_M2 msg: 1730863329.50 epoch sec; seqNo:4581 2024-11-06T03:22:09Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 6.06 psi 2024-11-06T03:22:09Z- handleDepth()- Depth_Keller msg: 1730863329.51 epoch sec; seqNo:4875; depth 50.20 2024-11-06T03:22:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863329.58 epoch sec; seqNo:4875; 4061586.29 north; 594758.81 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:22:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863327.13 2024-11-06T03:22:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:09Z- waiting for 1730863331.13, time is 1730863329.58 2024-11-06T03:22:09Z- AHRS_M2 msg: 1730863329.90 epoch sec; seqNo:4582 2024-11-06T03:22:09Z- AHRS_M2 msg: 0.12 phi; -0.06 theta; 6.06 psi 2024-11-06T03:22:09Z- handleDepth()- Depth_Keller msg: 1730863329.91 epoch sec; seqNo:4876; depth 50.20 2024-11-06T03:22:09Z- handleDVL() - DVL msg epochMillisec: 1730863329.93 2024-11-06T03:22:09Z- handleDvl() - RDI_Pathfinder msg: 1730863329.93 sec; seqNo:1640; #beams:4 2024-11-06T03:22:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 30.99 , 27.73 , 26.10 , 28.95 2024-11-06T03:22:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.08 , -0.02 2024-11-06T03:22:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863329.96 epoch sec; seqNo:4876; 4061586.29 north; 594758.81 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:22:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863327.13 2024-11-06T03:22:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:10Z- waiting for 1730863331.13, time is 1730863329.96 2024-11-06T03:22:10Z- AHRS_M2 msg: 1730863330.31 epoch sec; seqNo:4583 2024-11-06T03:22:10Z- AHRS_M2 msg: 0.12 phi; -0.06 theta; 6.06 psi 2024-11-06T03:22:10Z- handleDepth()- Depth_Keller msg: 1730863330.31 epoch sec; seqNo:4877; depth 50.21 2024-11-06T03:22:10Z- handleDVL() - DVL msg epochMillisec: 1730863330.31 2024-11-06T03:22:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:10Z- handleDvl() - RDI_Pathfinder msg: 1730863330.31 sec; seqNo:1641; #beams:0 2024-11-06T03:22:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.08 , -0.02 2024-11-06T03:22:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863330.33 epoch sec; seqNo:4877; 4061586.67 north; 594758.80 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:22:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863327.13 2024-11-06T03:22:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:10Z- waiting for 1730863331.13, time is 1730863330.33 2024-11-06T03:22:10Z- AHRS_M2 msg: 1730863330.71 epoch sec; seqNo:4584 2024-11-06T03:22:10Z- AHRS_M2 msg: 0.12 phi; -0.06 theta; 6.06 psi 2024-11-06T03:22:10Z- handleDepth()- Depth_Keller msg: 1730863330.71 epoch sec; seqNo:4878; depth 50.22 2024-11-06T03:22:10Z- handleDVL() - DVL msg epochMillisec: 1730863330.76 2024-11-06T03:22:10Z- handleDvl() - RDI_Pathfinder msg: 1730863330.76 sec; seqNo:1642; #beams:4 2024-11-06T03:22:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.13 , 27.59 , 26.37 , 28.82 2024-11-06T03:22:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.10 , -0.00 2024-11-06T03:22:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863330.79 epoch sec; seqNo:4878; 4061587.05 north; 594758.80 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:22:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863327.13 2024-11-06T03:22:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:10Z- waiting for 1730863331.13, time is 1730863330.79 2024-11-06T03:22:11Z- AHRS_M2 msg: 1730863331.12 epoch sec; seqNo:4585 2024-11-06T03:22:11Z- AHRS_M2 msg: 0.12 phi; -0.05 theta; 6.06 psi 2024-11-06T03:22:11Z- handleDepth()- Depth_Keller msg: 1730863331.12 epoch sec; seqNo:4879; depth 50.22 2024-11-06T03:22:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863331.14 epoch sec; seqNo:4879; 4061587.42 north; 594758.80 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:22:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863327.13 2024-11-06T03:22:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:11Z- waiting for 1730863331.13, time is 1730863331.14 2024-11-06T03:22:11Z- LcmTrn::updateTrn() >>>> pose time 1730863331.14, meas time 1730863330.76 <<<< 2024-11-06T03:22:11Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863331.14 2024-11-06T03:22:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863330.76 sec, ping # 0; waiting for INS data more recent than 1730863327.13... 2024-11-06T03:22:11Z- TerrainNav::motionUpdate() - currEstimate = (4061587, 594758.80, 50.22) 2024-11-06T03:22:11Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:22:11Z- TerrainNav:: SubcloudNIS is 1.26, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:22:11Z- TNavPF::Projecting Measurements 2024-11-06T03:22:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:22:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:22:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:22:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:22:11Z- TNavFilter::4 good beams. 2024-11-06T03:22:11Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:22:11Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:22:11Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:22:11Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:22:11Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:22:11Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:22:11Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:22:11Z- TerrainMapOctree: (vehN, vehE ) = (4061587.11, 594758.91). 2024-11-06T03:22:11Z- TerrainMapOctree: Min Distance = 163.14. 2024-11-06T03:22:11Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:22:11Z- Forcing Subcloud Comparison 2024-11-06T03:22:11Z- Weighting particles with subcloud comparison 2024-11-06T03:22:11Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:11Z- meanExpectedMeasDiff: 1.333448 delta_rms_squared: 1.435311 alpha: 0.986257 2024-11-06T03:22:11Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:11Z- meanExpectedMeasDiff: -2.674314 delta_rms_squared: 1.083161 alpha: 0.981870 2024-11-06T03:22:11Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:11Z- meanExpectedMeasDiff: -4.995839 delta_rms_squared: 1.211319 alpha: 0.983757 2024-11-06T03:22:11Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:11Z- meanExpectedMeasDiff: -0.326918 delta_rms_squared: 0.735600 alpha: 0.973531 2024-11-06T03:22:11Z- Subcloud NIS: 1.296573 2024-11-06T03:22:11Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:22:11Z- TNavPF:: sumWeights = 0.006524 2024-11-06T03:22:11Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:22:11Z- TNavPF::First Term of Map Covariance Matrix is 0.05 2024-11-06T03:22:11Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:22:11Z- TerrainNav::Resampling particle filter... 2024-11-06T03:22:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863330.76 sec. 2024-11-06T03:22:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:22:11Z- variances([0], [2], and psi_berg) = 0.671610 0.131723 0.000000 2024-11-06T03:22:11Z- publishEstimatesDecoder() - at 1730863331.14 sec 2024-11-06T03:22:11Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:22:11Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061618.29 594741.71 56.84 2024-11-06T03:22:11Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061617.99 594741.60 57.87 lat/lon: 36.70 -121.94 2024-11-06T03:22:11Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.67 0.13 0.62 0.00 2024-11-06T03:22:11Z- AHRS_M2 msg: 1730863331.52 epoch sec; seqNo:4586 2024-11-06T03:22:11Z- AHRS_M2 msg: 0.12 phi; -0.05 theta; 6.06 psi 2024-11-06T03:22:11Z- handleDepth()- Depth_Keller msg: 1730863331.52 epoch sec; seqNo:4880; depth 50.24 2024-11-06T03:22:11Z- handleDVL() - DVL msg epochMillisec: 1730863331.53 2024-11-06T03:22:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:11Z- handleDvl() - RDI_Pathfinder msg: 1730863331.53 sec; seqNo:1643; #beams:0 2024-11-06T03:22:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.10 , -0.00 2024-11-06T03:22:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863331.54 epoch sec; seqNo:4880; 4061587.80 north; 594758.79 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:22:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863331.14 2024-11-06T03:22:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:11Z- waiting for 1730863335.14, time is 1730863331.54 2024-11-06T03:22:11Z- AHRS_M2 msg: 1730863331.92 epoch sec; seqNo:4587 2024-11-06T03:22:11Z- AHRS_M2 msg: 0.12 phi; -0.05 theta; 6.06 psi 2024-11-06T03:22:11Z- handleDepth()- Depth_Keller msg: 1730863331.94 epoch sec; seqNo:4881; depth 50.25 2024-11-06T03:22:11Z- handleDVL() - DVL msg epochMillisec: 1730863331.96 2024-11-06T03:22:11Z- handleDvl() - RDI_Pathfinder msg: 1730863331.96 sec; seqNo:1644; #beams:4 2024-11-06T03:22:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.13 , 27.19 , 26.10 , 28.14 2024-11-06T03:22:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.11 , -0.01 2024-11-06T03:22:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863332.00 epoch sec; seqNo:4881; 4061588.18 north; 594758.79 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:22:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863331.14 2024-11-06T03:22:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:12Z- waiting for 1730863335.14, time is 1730863332.00 2024-11-06T03:22:12Z- AHRS_M2 msg: 1730863332.33 epoch sec; seqNo:4588 2024-11-06T03:22:12Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 6.06 psi 2024-11-06T03:22:12Z- handleDepth()- Depth_Keller msg: 1730863332.34 epoch sec; seqNo:4882; depth 50.26 2024-11-06T03:22:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863332.36 epoch sec; seqNo:4882; 4061588.56 north; 594758.78 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:22:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863331.14 2024-11-06T03:22:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:12Z- waiting for 1730863335.14, time is 1730863332.36 2024-11-06T03:22:12Z- AHRS_M2 msg: 1730863332.75 epoch sec; seqNo:4589 2024-11-06T03:22:12Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.06 psi 2024-11-06T03:22:12Z- handleDepth()- Depth_Keller msg: 1730863332.76 epoch sec; seqNo:4883; depth 50.27 2024-11-06T03:22:12Z- handleDVL() - DVL msg epochMillisec: 1730863332.80 2024-11-06T03:22:12Z- handleDvl() - RDI_Pathfinder msg: 1730863332.80 sec; seqNo:1645; #beams:4 2024-11-06T03:22:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 30.72 , 27.19 , 27.32 , 28.14 2024-11-06T03:22:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.12 , 0.00 2024-11-06T03:22:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863332.81 epoch sec; seqNo:4883; 4061588.94 north; 594758.78 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:22:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863331.14 2024-11-06T03:22:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:12Z- waiting for 1730863335.14, time is 1730863332.81 2024-11-06T03:22:13Z- AHRS_M2 msg: 1730863333.14 epoch sec; seqNo:4590 2024-11-06T03:22:13Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.06 psi 2024-11-06T03:22:13Z- handleDepth()- Depth_Keller msg: 1730863333.14 epoch sec; seqNo:4884; depth 50.28 2024-11-06T03:22:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863333.16 epoch sec; seqNo:4884; 4061589.30 north; 594757.55 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:22:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863331.14 2024-11-06T03:22:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:13Z- waiting for 1730863335.14, time is 1730863333.16 2024-11-06T03:22:13Z- AHRS_M2 msg: 1730863333.54 epoch sec; seqNo:4591 2024-11-06T03:22:13Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.06 psi 2024-11-06T03:22:13Z- handleDepth()- Depth_Keller msg: 1730863333.54 epoch sec; seqNo:4885; depth 50.30 2024-11-06T03:22:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863333.56 epoch sec; seqNo:4885; 4061589.68 north; 594757.55 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:22:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863331.14 2024-11-06T03:22:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:13Z- waiting for 1730863335.14, time is 1730863333.56 2024-11-06T03:22:13Z- AHRS_M2 msg: 1730863333.94 epoch sec; seqNo:4592 2024-11-06T03:22:13Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 6.06 psi 2024-11-06T03:22:13Z- handleDepth()- Depth_Keller msg: 1730863333.95 epoch sec; seqNo:4886; depth 50.30 2024-11-06T03:22:13Z- handleDVL() - DVL msg epochMillisec: 1730863333.98 2024-11-06T03:22:13Z- handleDvl() - RDI_Pathfinder msg: 1730863333.98 sec; seqNo:1646; #beams:4 2024-11-06T03:22:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.40 , 27.05 , 27.46 , 28.41 2024-11-06T03:22:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.13 , 0.01 2024-11-06T03:22:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863333.99 epoch sec; seqNo:4886; 4061590.06 north; 594757.55 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:22:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863331.14 2024-11-06T03:22:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:14Z- waiting for 1730863335.14, time is 1730863333.99 2024-11-06T03:22:14Z- AHRS_M2 msg: 1730863334.35 epoch sec; seqNo:4593 2024-11-06T03:22:14Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 6.07 psi 2024-11-06T03:22:14Z- handleDepth()- Depth_Keller msg: 1730863334.35 epoch sec; seqNo:4887; depth 50.32 2024-11-06T03:22:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863334.37 epoch sec; seqNo:4887; 4061590.44 north; 594757.54 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:22:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863331.14 2024-11-06T03:22:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:14Z- waiting for 1730863335.14, time is 1730863334.37 2024-11-06T03:22:14Z- AHRS_M2 msg: 1730863334.75 epoch sec; seqNo:4594 2024-11-06T03:22:14Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 6.07 psi 2024-11-06T03:22:14Z- handleDepth()- Depth_Keller msg: 1730863334.77 epoch sec; seqNo:4888; depth 50.32 2024-11-06T03:22:14Z- handleDVL() - DVL msg epochMillisec: 1730863334.79 2024-11-06T03:22:14Z- handleDvl() - RDI_Pathfinder msg: 1730863334.79 sec; seqNo:1647; #beams:4 2024-11-06T03:22:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.54 , 27.05 , 27.73 , 28.82 2024-11-06T03:22:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.12 , 0.01 2024-11-06T03:22:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863334.80 epoch sec; seqNo:4888; 4061590.82 north; 594757.54 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:22:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863331.14 2024-11-06T03:22:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:14Z- waiting for 1730863335.14, time is 1730863334.80 2024-11-06T03:22:15Z- AHRS_M2 msg: 1730863335.15 epoch sec; seqNo:4595 2024-11-06T03:22:15Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 6.07 psi 2024-11-06T03:22:15Z- handleDepth()- Depth_Keller msg: 1730863335.16 epoch sec; seqNo:4889; depth 50.34 2024-11-06T03:22:15Z- handleDVL() - DVL msg epochMillisec: 1730863335.16 2024-11-06T03:22:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:15Z- handleDvl() - RDI_Pathfinder msg: 1730863335.16 sec; seqNo:1648; #beams:0 2024-11-06T03:22:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.12 , 0.01 2024-11-06T03:22:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863335.19 epoch sec; seqNo:4889; 4061591.20 north; 594757.53 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:22:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863331.14 2024-11-06T03:22:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:15Z- waiting for 1730863335.14, time is 1730863335.19 2024-11-06T03:22:15Z- LcmTrn::updateTrn() >>>> pose time 1730863335.19, meas time 1730863335.16 <<<< 2024-11-06T03:22:15Z- LcmTrn::updateTrn() equating pose 1730863335.16 and meas times 1730863335.16 2024-11-06T03:22:15Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863335.16 2024-11-06T03:22:15Z- TerrainNav::motionUpdate() - currEstimate = (4061591, 594757.53, 50.34) 2024-11-06T03:22:15Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:22:15Z- TerrainNav:: SubcloudNIS is 1.30, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:22:15Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:22:15Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:22:15Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:22:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:22:15Z- variances([0], [2], and psi_berg) = 0.673723 0.134695 0.000000 2024-11-06T03:22:15Z- publishEstimatesDecoder() - at 1730863335.16 sec 2024-11-06T03:22:15Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:22:15Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061622.07 594740.46 56.90 2024-11-06T03:22:15Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061621.76 594740.34 57.99 lat/lon: 36.70 -121.94 2024-11-06T03:22:15Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.67 0.13 0.86 0.00 2024-11-06T03:22:15Z- AHRS_M2 msg: 1730863335.56 epoch sec; seqNo:4596 2024-11-06T03:22:15Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.06 psi 2024-11-06T03:22:15Z- handleDepth()- Depth_Keller msg: 1730863335.57 epoch sec; seqNo:4890; depth 50.36 2024-11-06T03:22:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863335.58 epoch sec; seqNo:4890; 4061591.58 north; 594757.53 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:22:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863335.16 2024-11-06T03:22:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:15Z- waiting for 1730863339.16, time is 1730863335.58 2024-11-06T03:22:15Z- AHRS_M2 msg: 1730863335.96 epoch sec; seqNo:4597 2024-11-06T03:22:15Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.06 psi 2024-11-06T03:22:15Z- handleDepth()- Depth_Keller msg: 1730863335.97 epoch sec; seqNo:4891; depth 50.37 2024-11-06T03:22:16Z- handleDVL() - DVL msg epochMillisec: 1730863335.99 2024-11-06T03:22:16Z- handleDvl() - RDI_Pathfinder msg: 1730863335.99 sec; seqNo:1649; #beams:4 2024-11-06T03:22:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.67 , 27.05 , 28.00 , 29.23 2024-11-06T03:22:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.12 , 0.01 2024-11-06T03:22:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863336.00 epoch sec; seqNo:4891; 4061591.96 north; 594757.52 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:22:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863335.16 2024-11-06T03:22:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:16Z- waiting for 1730863339.16, time is 1730863336.00 2024-11-06T03:22:16Z- AHRS_M2 msg: 1730863336.37 epoch sec; seqNo:4598 2024-11-06T03:22:16Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.06 psi 2024-11-06T03:22:16Z- handleDepth()- Depth_Keller msg: 1730863336.37 epoch sec; seqNo:4892; depth 50.37 2024-11-06T03:22:16Z- handleDVL() - DVL msg epochMillisec: 1730863336.38 2024-11-06T03:22:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:16Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:16Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:16Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:16Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:16Z- handleDvl() - RDI_Pathfinder msg: 1730863336.38 sec; seqNo:1650; #beams:0 2024-11-06T03:22:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.12 , 0.01 2024-11-06T03:22:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863336.39 epoch sec; seqNo:4892; 4061592.34 north; 594757.52 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:22:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863335.16 2024-11-06T03:22:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:16Z- waiting for 1730863339.16, time is 1730863336.39 2024-11-06T03:22:16Z- handleDepth()- Depth_Keller msg: 1730863336.79 epoch sec; seqNo:4893; depth 50.39 2024-11-06T03:22:16Z- handleDVL() - DVL msg epochMillisec: 1730863336.82 2024-11-06T03:22:16Z- handleDvl() - RDI_Pathfinder msg: 1730863336.82 sec; seqNo:1651; #beams:4 2024-11-06T03:22:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.40 , 27.05 , 28.00 , 29.23 2024-11-06T03:22:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , 0.03 2024-11-06T03:22:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863336.83 epoch sec; seqNo:4893; 4061592.70 north; 594756.30 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:22:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863335.16 2024-11-06T03:22:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:16Z- waiting for 1730863339.16, time is 1730863336.83 2024-11-06T03:22:17Z- AHRS_M2 msg: 1730863337.17 epoch sec; seqNo:4599 2024-11-06T03:22:17Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.06 psi 2024-11-06T03:22:17Z- handleDepth()- Depth_Keller msg: 1730863337.19 epoch sec; seqNo:4894; depth 50.41 2024-11-06T03:22:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863337.20 epoch sec; seqNo:4894; 4061593.08 north; 594756.29 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:22:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863335.16 2024-11-06T03:22:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:17Z- waiting for 1730863339.16, time is 1730863337.20 2024-11-06T03:22:17Z- AHRS_M2 msg: 1730863337.58 epoch sec; seqNo:4600 2024-11-06T03:22:17Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.06 psi 2024-11-06T03:22:17Z- handleDepth()- Depth_Keller msg: 1730863337.59 epoch sec; seqNo:4895; depth 50.42 2024-11-06T03:22:17Z- handleDVL() - DVL msg epochMillisec: 1730863337.59 2024-11-06T03:22:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:17Z- handleDvl() - RDI_Pathfinder msg: 1730863337.59 sec; seqNo:1652; #beams:0 2024-11-06T03:22:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , 0.03 2024-11-06T03:22:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863337.60 epoch sec; seqNo:4895; 4061593.46 north; 594756.29 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:22:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863335.16 2024-11-06T03:22:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:17Z- waiting for 1730863339.16, time is 1730863337.60 2024-11-06T03:22:17Z- AHRS_M2 msg: 1730863337.98 epoch sec; seqNo:4601 2024-11-06T03:22:17Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.06 psi 2024-11-06T03:22:17Z- handleDepth()- Depth_Keller msg: 1730863337.99 epoch sec; seqNo:4896; depth 50.42 2024-11-06T03:22:18Z- handleDVL() - DVL msg epochMillisec: 1730863338.01 2024-11-06T03:22:18Z- handleDvl() - RDI_Pathfinder msg: 1730863338.01 sec; seqNo:1653; #beams:4 2024-11-06T03:22:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.40 , 26.92 , 27.87 , 29.50 2024-11-06T03:22:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , 0.04 2024-11-06T03:22:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863338.04 epoch sec; seqNo:4896; 4061593.84 north; 594756.28 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:22:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863335.16 2024-11-06T03:22:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:18Z- waiting for 1730863339.16, time is 1730863338.04 2024-11-06T03:22:18Z- AHRS_M2 msg: 1730863338.39 epoch sec; seqNo:4602 2024-11-06T03:22:18Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.06 psi 2024-11-06T03:22:18Z- handleDepth()- Depth_Keller msg: 1730863338.40 epoch sec; seqNo:4897; depth 50.41 2024-11-06T03:22:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863338.41 epoch sec; seqNo:4897; 4061593.84 north; 594756.28 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:22:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863335.16 2024-11-06T03:22:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:18Z- waiting for 1730863339.16, time is 1730863338.41 2024-11-06T03:22:18Z- AHRS_M2 msg: 1730863338.79 epoch sec; seqNo:4603 2024-11-06T03:22:18Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.06 psi 2024-11-06T03:22:18Z- handleDepth()- Depth_Keller msg: 1730863338.81 epoch sec; seqNo:4898; depth 50.42 2024-11-06T03:22:18Z- handleDVL() - DVL msg epochMillisec: 1730863338.84 2024-11-06T03:22:18Z- handleDvl() - RDI_Pathfinder msg: 1730863338.84 sec; seqNo:1654; #beams:4 2024-11-06T03:22:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.54 , 27.05 , 28.00 , 29.64 2024-11-06T03:22:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.03 2024-11-06T03:22:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863338.85 epoch sec; seqNo:4898; 4061594.22 north; 594756.28 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:22:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863335.16 2024-11-06T03:22:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:18Z- waiting for 1730863339.16, time is 1730863338.85 2024-11-06T03:22:19Z- AHRS_M2 msg: 1730863339.20 epoch sec; seqNo:4604 2024-11-06T03:22:19Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.06 psi 2024-11-06T03:22:19Z- handleDepth()- Depth_Keller msg: 1730863339.21 epoch sec; seqNo:4899; depth 50.43 2024-11-06T03:22:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863339.23 epoch sec; seqNo:4899; 4061594.60 north; 594756.28 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:22:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863335.16 2024-11-06T03:22:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:19Z- waiting for 1730863339.16, time is 1730863339.23 2024-11-06T03:22:19Z- LcmTrn::updateTrn() >>>> pose time 1730863339.23, meas time 1730863338.84 <<<< 2024-11-06T03:22:19Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863339.23 2024-11-06T03:22:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863338.84 sec, ping # 0; waiting for INS data more recent than 1730863335.16... 2024-11-06T03:22:19Z- TerrainNav::motionUpdate() - currEstimate = (4061595, 594756.28, 50.43) 2024-11-06T03:22:19Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:22:19Z- TerrainNav:: SubcloudNIS is 1.30, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:22:19Z- TNavPF::Projecting Measurements 2024-11-06T03:22:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:22:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:22:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:22:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:22:19Z- TNavFilter::4 good beams. 2024-11-06T03:22:19Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:22:19Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:22:19Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:22:19Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:22:19Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:22:19Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:22:19Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:22:19Z- TerrainMapOctree: (vehN, vehE ) = (4061594.26, 594756.40). 2024-11-06T03:22:19Z- TerrainMapOctree: Min Distance = 155.87. 2024-11-06T03:22:19Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:22:19Z- Forcing Subcloud Comparison 2024-11-06T03:22:19Z- Weighting particles with subcloud comparison 2024-11-06T03:22:19Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:19Z- meanExpectedMeasDiff: 2.653484 delta_rms_squared: 1.809653 alpha: 0.989069 2024-11-06T03:22:19Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:19Z- meanExpectedMeasDiff: -1.320071 delta_rms_squared: 1.427114 alpha: 0.986179 2024-11-06T03:22:19Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:19Z- meanExpectedMeasDiff: -2.701935 delta_rms_squared: 1.479954 alpha: 0.986666 2024-11-06T03:22:19Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:19Z- meanExpectedMeasDiff: 0.839378 delta_rms_squared: 1.252435 alpha: 0.984282 2024-11-06T03:22:19Z- Subcloud NIS: 1.288648 2024-11-06T03:22:19Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:22:19Z- TNavPF:: sumWeights = 0.041621 2024-11-06T03:22:19Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:22:19Z- TNavPF::First Term of Map Covariance Matrix is 0.32 2024-11-06T03:22:19Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:22:19Z- TerrainNav::Resampling particle filter... 2024-11-06T03:22:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863338.84 sec. 2024-11-06T03:22:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:22:19Z- variances([0], [2], and psi_berg) = 0.674046 0.127506 0.000000 2024-11-06T03:22:19Z- publishEstimatesDecoder() - at 1730863339.23 sec 2024-11-06T03:22:19Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:22:19Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061625.45 594739.19 57.41 2024-11-06T03:22:19Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061625.13 594739.10 58.19 lat/lon: 36.70 -121.94 2024-11-06T03:22:19Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.67 0.13 0.65 0.00 2024-11-06T03:22:19Z- AHRS_M2 msg: 1730863339.60 epoch sec; seqNo:4605 2024-11-06T03:22:19Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.06 psi 2024-11-06T03:22:19Z- handleDepth()- Depth_Keller msg: 1730863339.61 epoch sec; seqNo:4900; depth 50.42 2024-11-06T03:22:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863339.62 epoch sec; seqNo:4900; 4061594.97 north; 594756.27 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:22:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863339.23 2024-11-06T03:22:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:19Z- waiting for 1730863343.23, time is 1730863339.62 2024-11-06T03:22:20Z- AHRS_M2 msg: 1730863340.00 epoch sec; seqNo:4606 2024-11-06T03:22:20Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.07 psi 2024-11-06T03:22:20Z- handleDepth()- Depth_Keller msg: 1730863340.01 epoch sec; seqNo:4901; depth 50.41 2024-11-06T03:22:20Z- handleDVL() - DVL msg epochMillisec: 1730863340.03 2024-11-06T03:22:20Z- handleDvl() - RDI_Pathfinder msg: 1730863340.03 sec; seqNo:1655; #beams:4 2024-11-06T03:22:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.27 , 27.19 , 28.27 , 29.64 2024-11-06T03:22:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , 0.02 2024-11-06T03:22:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863340.04 epoch sec; seqNo:4901; 4061595.35 north; 594756.27 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:22:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863339.23 2024-11-06T03:22:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:20Z- waiting for 1730863343.23, time is 1730863340.04 2024-11-06T03:22:20Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:22:20Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:22:20Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:22:20Z- AHRS_M2 msg: 1730863340.40 epoch sec; seqNo:4607 2024-11-06T03:22:20Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.07 psi 2024-11-06T03:22:20Z- handleDepth()- Depth_Keller msg: 1730863340.43 epoch sec; seqNo:4902; depth 50.40 2024-11-06T03:22:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863340.44 epoch sec; seqNo:4902; 4061595.73 north; 594756.26 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:22:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863339.23 2024-11-06T03:22:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:20Z- waiting for 1730863343.23, time is 1730863340.44 2024-11-06T03:22:20Z- AHRS_M2 msg: 1730863340.81 epoch sec; seqNo:4608 2024-11-06T03:22:20Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.07 psi 2024-11-06T03:22:20Z- handleDepth()- Depth_Keller msg: 1730863340.83 epoch sec; seqNo:4903; depth 50.41 2024-11-06T03:22:20Z- handleDVL() - DVL msg epochMillisec: 1730863340.86 2024-11-06T03:22:20Z- handleDvl() - RDI_Pathfinder msg: 1730863340.86 sec; seqNo:1656; #beams:4 2024-11-06T03:22:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 30.72 , 26.64 , 28.27 , 29.50 2024-11-06T03:22:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , -0.00 2024-11-06T03:22:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863340.87 epoch sec; seqNo:4903; 4061596.11 north; 594756.26 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:22:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863339.23 2024-11-06T03:22:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:20Z- waiting for 1730863343.23, time is 1730863340.87 2024-11-06T03:22:21Z- AHRS_M2 msg: 1730863341.22 epoch sec; seqNo:4609 2024-11-06T03:22:21Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.07 psi 2024-11-06T03:22:21Z- handleDepth()- Depth_Keller msg: 1730863341.23 epoch sec; seqNo:4904; depth 50.39 2024-11-06T03:22:21Z- handleDVL() - DVL msg epochMillisec: 1730863341.23 2024-11-06T03:22:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:21Z- handleDvl() - RDI_Pathfinder msg: 1730863341.23 sec; seqNo:1657; #beams:0 2024-11-06T03:22:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , -0.00 2024-11-06T03:22:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863341.24 epoch sec; seqNo:4904; 4061596.49 north; 594756.25 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:22:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863339.23 2024-11-06T03:22:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:21Z- waiting for 1730863343.23, time is 1730863341.24 2024-11-06T03:22:21Z- AHRS_M2 msg: 1730863341.63 epoch sec; seqNo:4610 2024-11-06T03:22:21Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.07 psi 2024-11-06T03:22:21Z- handleDepth()- Depth_Keller msg: 1730863341.64 epoch sec; seqNo:4905; depth 50.39 2024-11-06T03:22:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863341.65 epoch sec; seqNo:4905; 4061596.48 north; 594755.03 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:22:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863339.23 2024-11-06T03:22:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:21Z- waiting for 1730863343.23, time is 1730863341.65 2024-11-06T03:22:22Z- AHRS_M2 msg: 1730863342.02 epoch sec; seqNo:4611 2024-11-06T03:22:22Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.07 psi 2024-11-06T03:22:22Z- handleDepth()- Depth_Keller msg: 1730863342.03 epoch sec; seqNo:4906; depth 50.38 2024-11-06T03:22:22Z- handleDVL() - DVL msg epochMillisec: 1730863342.05 2024-11-06T03:22:22Z- handleDvl() - RDI_Pathfinder msg: 1730863342.05 sec; seqNo:1658; #beams:4 2024-11-06T03:22:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 30.58 , 26.78 , 28.14 , 29.36 2024-11-06T03:22:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , -0.03 2024-11-06T03:22:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863342.07 epoch sec; seqNo:4906; 4061596.85 north; 594755.03 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:22:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863339.23 2024-11-06T03:22:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:22Z- waiting for 1730863343.23, time is 1730863342.07 2024-11-06T03:22:22Z- AHRS_M2 msg: 1730863342.43 epoch sec; seqNo:4612 2024-11-06T03:22:22Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.07 psi 2024-11-06T03:22:22Z- handleDepth()- Depth_Keller msg: 1730863342.44 epoch sec; seqNo:4907; depth 50.36 2024-11-06T03:22:22Z- handleDVL() - DVL msg epochMillisec: 1730863342.44 2024-11-06T03:22:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:22Z- handleDvl() - RDI_Pathfinder msg: 1730863342.44 sec; seqNo:1659; #beams:0 2024-11-06T03:22:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , -0.03 2024-11-06T03:22:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863342.45 epoch sec; seqNo:4907; 4061597.23 north; 594755.03 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:22:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863339.23 2024-11-06T03:22:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:22Z- waiting for 1730863343.23, time is 1730863342.45 2024-11-06T03:22:22Z- AHRS_M2 msg: 1730863342.83 epoch sec; seqNo:4613 2024-11-06T03:22:22Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.07 psi 2024-11-06T03:22:22Z- handleDepth()- Depth_Keller msg: 1730863342.84 epoch sec; seqNo:4908; depth 50.35 2024-11-06T03:22:22Z- handleDVL() - DVL msg epochMillisec: 1730863342.86 2024-11-06T03:22:22Z- handleDvl() - RDI_Pathfinder msg: 1730863342.86 sec; seqNo:1660; #beams:4 2024-11-06T03:22:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 30.85 , 26.64 , 28.00 , 29.36 2024-11-06T03:22:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.07 , -0.05 2024-11-06T03:22:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863342.88 epoch sec; seqNo:4908; 4061597.61 north; 594755.02 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:22:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863339.23 2024-11-06T03:22:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:22Z- waiting for 1730863343.23, time is 1730863342.88 2024-11-06T03:22:23Z- AHRS_M2 msg: 1730863343.23 epoch sec; seqNo:4614 2024-11-06T03:22:23Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.07 psi 2024-11-06T03:22:23Z- handleDepth()- Depth_Keller msg: 1730863343.25 epoch sec; seqNo:4909; depth 50.35 2024-11-06T03:22:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863343.27 epoch sec; seqNo:4909; 4061597.99 north; 594755.02 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:22:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863339.23 2024-11-06T03:22:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:23Z- waiting for 1730863343.23, time is 1730863343.27 2024-11-06T03:22:23Z- LcmTrn::updateTrn() >>>> pose time 1730863343.27, meas time 1730863342.86 <<<< 2024-11-06T03:22:23Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863343.27 2024-11-06T03:22:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863342.86 sec, ping # 0; waiting for INS data more recent than 1730863339.23... 2024-11-06T03:22:23Z- TerrainNav::motionUpdate() - currEstimate = (4061598, 594755.02, 50.35) 2024-11-06T03:22:23Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:22:23Z- TerrainNav:: SubcloudNIS is 1.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:22:23Z- TNavPF::Projecting Measurements 2024-11-06T03:22:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:22:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:22:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:22:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:22:23Z- TNavFilter::4 good beams. 2024-11-06T03:22:23Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:22:23Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:22:23Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:22:23Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:22:23Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:22:23Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:22:23Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:22:23Z- TerrainMapOctree: (vehN, vehE ) = (4061597.65, 594755.14). 2024-11-06T03:22:23Z- TerrainMapOctree: Min Distance = 152.44. 2024-11-06T03:22:23Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:22:23Z- Forcing Subcloud Comparison 2024-11-06T03:22:23Z- Weighting particles with subcloud comparison 2024-11-06T03:22:23Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:23Z- meanExpectedMeasDiff: 2.163163 delta_rms_squared: 1.428579 alpha: 0.986193 2024-11-06T03:22:23Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:23Z- meanExpectedMeasDiff: -1.994252 delta_rms_squared: 1.028829 alpha: 0.980931 2024-11-06T03:22:23Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:23Z- meanExpectedMeasDiff: -3.235656 delta_rms_squared: 1.247241 alpha: 0.984218 2024-11-06T03:22:23Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:23Z- meanExpectedMeasDiff: 0.976702 delta_rms_squared: 0.928531 alpha: 0.978915 2024-11-06T03:22:23Z- Subcloud NIS: 1.304043 2024-11-06T03:22:23Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:22:23Z- TNavPF:: sumWeights = 0.028177 2024-11-06T03:22:23Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:22:23Z- TNavPF::First Term of Map Covariance Matrix is 0.19 2024-11-06T03:22:23Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:22:23Z- TerrainNav::Resampling particle filter... 2024-11-06T03:22:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863342.86 sec. 2024-11-06T03:22:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:22:23Z- variances([0], [2], and psi_berg) = 0.911616 0.146478 0.000000 2024-11-06T03:22:23Z- publishEstimatesDecoder() - at 1730863343.27 sec 2024-11-06T03:22:23Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:22:23Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061628.68 594737.93 56.61 2024-11-06T03:22:23Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061628.58 594737.78 58.37 lat/lon: 36.70 -121.94 2024-11-06T03:22:23Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.91 0.15 0.65 0.00 2024-11-06T03:22:23Z- AHRS_M2 msg: 1730863343.64 epoch sec; seqNo:4615 2024-11-06T03:22:23Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.07 psi 2024-11-06T03:22:23Z- handleDepth()- Depth_Keller msg: 1730863343.65 epoch sec; seqNo:4910; depth 50.33 2024-11-06T03:22:23Z- handleDVL() - DVL msg epochMillisec: 1730863343.65 2024-11-06T03:22:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:23Z- handleDvl() - RDI_Pathfinder msg: 1730863343.65 sec; seqNo:1661; #beams:0 2024-11-06T03:22:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.07 , -0.05 2024-11-06T03:22:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863343.66 epoch sec; seqNo:4910; 4061598.37 north; 594755.01 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:22:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863343.27 2024-11-06T03:22:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:23Z- waiting for 1730863347.27, time is 1730863343.66 2024-11-06T03:22:24Z- AHRS_M2 msg: 1730863344.04 epoch sec; seqNo:4616 2024-11-06T03:22:24Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.07 psi 2024-11-06T03:22:24Z- handleDepth()- Depth_Keller msg: 1730863344.05 epoch sec; seqNo:4911; depth 50.31 2024-11-06T03:22:24Z- handleDVL() - DVL msg epochMillisec: 1730863344.08 2024-11-06T03:22:24Z- handleDvl() - RDI_Pathfinder msg: 1730863344.08 sec; seqNo:1662; #beams:4 2024-11-06T03:22:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 30.99 , 26.10 , 27.46 , 29.22 2024-11-06T03:22:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.10 , -0.05 2024-11-06T03:22:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863344.09 epoch sec; seqNo:4911; 4061598.75 north; 594755.01 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:22:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863343.27 2024-11-06T03:22:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:24Z- waiting for 1730863347.27, time is 1730863344.09 2024-11-06T03:22:24Z- AHRS_M2 msg: 1730863344.45 epoch sec; seqNo:4617 2024-11-06T03:22:24Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.07 psi 2024-11-06T03:22:24Z- handleDepth()- Depth_Keller msg: 1730863344.46 epoch sec; seqNo:4912; depth 50.32 2024-11-06T03:22:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863344.47 epoch sec; seqNo:4912; 4061599.13 north; 594755.00 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:22:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863343.27 2024-11-06T03:22:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:24Z- waiting for 1730863347.27, time is 1730863344.47 2024-11-06T03:22:24Z- AHRS_M2 msg: 1730863344.85 epoch sec; seqNo:4618 2024-11-06T03:22:24Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.07 psi 2024-11-06T03:22:24Z- handleDepth()- Depth_Keller msg: 1730863344.86 epoch sec; seqNo:4913; depth 50.31 2024-11-06T03:22:24Z- handleDVL() - DVL msg epochMillisec: 1730863344.89 2024-11-06T03:22:24Z- handleDvl() - RDI_Pathfinder msg: 1730863344.89 sec; seqNo:1663; #beams:4 2024-11-06T03:22:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.13 , 26.24 , 26.78 , 28.95 2024-11-06T03:22:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.12 , -0.03 2024-11-06T03:22:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863344.90 epoch sec; seqNo:4913; 4061599.51 north; 594755.00 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:22:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863343.27 2024-11-06T03:22:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:25Z- waiting for 1730863347.27, time is 1730863344.90 2024-11-06T03:22:25Z- AHRS_M2 msg: 1730863345.25 epoch sec; seqNo:4619 2024-11-06T03:22:25Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.07 psi 2024-11-06T03:22:25Z- handleDepth()- Depth_Keller msg: 1730863345.26 epoch sec; seqNo:4914; depth 50.29 2024-11-06T03:22:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863345.29 epoch sec; seqNo:4914; 4061599.89 north; 594755.00 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:22:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863343.27 2024-11-06T03:22:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:25Z- waiting for 1730863347.27, time is 1730863345.29 2024-11-06T03:22:25Z- AHRS_M2 msg: 1730863345.66 epoch sec; seqNo:4620 2024-11-06T03:22:25Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.06 psi 2024-11-06T03:22:25Z- handleDepth()- Depth_Keller msg: 1730863345.67 epoch sec; seqNo:4915; depth 50.30 2024-11-06T03:22:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863345.68 epoch sec; seqNo:4915; 4061600.26 north; 594754.99 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:22:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863343.27 2024-11-06T03:22:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:25Z- waiting for 1730863347.27, time is 1730863345.68 2024-11-06T03:22:26Z- AHRS_M2 msg: 1730863346.06 epoch sec; seqNo:4621 2024-11-06T03:22:26Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 6.06 psi 2024-11-06T03:22:26Z- handleDepth()- Depth_Keller msg: 1730863346.07 epoch sec; seqNo:4916; depth 50.29 2024-11-06T03:22:26Z- handleDVL() - DVL msg epochMillisec: 1730863346.09 2024-11-06T03:22:26Z- handleDvl() - RDI_Pathfinder msg: 1730863346.09 sec; seqNo:1664; #beams:4 2024-11-06T03:22:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 30.86 , 25.83 , 27.05 , 29.09 2024-11-06T03:22:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.13 , -0.01 2024-11-06T03:22:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863346.11 epoch sec; seqNo:4916; 4061600.63 north; 594753.77 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:22:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863343.27 2024-11-06T03:22:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:26Z- waiting for 1730863347.27, time is 1730863346.11 2024-11-06T03:22:26Z- AHRS_M2 msg: 1730863346.47 epoch sec; seqNo:4622 2024-11-06T03:22:26Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 6.06 psi 2024-11-06T03:22:26Z- handleDepth()- Depth_Keller msg: 1730863346.47 epoch sec; seqNo:4917; depth 50.29 2024-11-06T03:22:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863346.49 epoch sec; seqNo:4917; 4061601.01 north; 594753.76 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:22:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863343.27 2024-11-06T03:22:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:26Z- waiting for 1730863347.27, time is 1730863346.49 2024-11-06T03:22:26Z- AHRS_M2 msg: 1730863346.87 epoch sec; seqNo:4623 2024-11-06T03:22:26Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 6.06 psi 2024-11-06T03:22:26Z- handleDepth()- Depth_Keller msg: 1730863346.88 epoch sec; seqNo:4918; depth 50.31 2024-11-06T03:22:26Z- handleDVL() - DVL msg epochMillisec: 1730863346.90 2024-11-06T03:22:26Z- handleDvl() - RDI_Pathfinder msg: 1730863346.90 sec; seqNo:1665; #beams:4 2024-11-06T03:22:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 30.99 , 25.69 , 27.32 , 29.09 2024-11-06T03:22:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.14 , 0.02 2024-11-06T03:22:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863346.92 epoch sec; seqNo:4918; 4061601.39 north; 594753.76 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:22:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863343.27 2024-11-06T03:22:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:27Z- waiting for 1730863347.27, time is 1730863346.92 2024-11-06T03:22:27Z- AHRS_M2 msg: 1730863347.28 epoch sec; seqNo:4624 2024-11-06T03:22:27Z- AHRS_M2 msg: 0.12 phi; -0.00 theta; 6.05 psi 2024-11-06T03:22:27Z- handleDepth()- Depth_Keller msg: 1730863347.28 epoch sec; seqNo:4919; depth 50.31 2024-11-06T03:22:27Z- handleDVL() - DVL msg epochMillisec: 1730863347.28 2024-11-06T03:22:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:27Z- handleDvl() - RDI_Pathfinder msg: 1730863347.28 sec; seqNo:1666; #beams:0 2024-11-06T03:22:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.14 , 0.02 2024-11-06T03:22:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863347.30 epoch sec; seqNo:4919; 4061601.77 north; 594753.75 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:22:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863343.27 2024-11-06T03:22:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:27Z- waiting for 1730863347.27, time is 1730863347.30 2024-11-06T03:22:27Z- LcmTrn::updateTrn() >>>> pose time 1730863347.30, meas time 1730863347.28 <<<< 2024-11-06T03:22:27Z- LcmTrn::updateTrn() equating pose 1730863347.28 and meas times 1730863347.28 2024-11-06T03:22:27Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863347.28 2024-11-06T03:22:27Z- TerrainNav::motionUpdate() - currEstimate = (4061602, 594753.75, 50.31) 2024-11-06T03:22:27Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:22:27Z- TerrainNav:: SubcloudNIS is 1.30, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:22:27Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:22:27Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:22:27Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:22:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:22:27Z- variances([0], [2], and psi_berg) = 0.913369 0.148025 0.000000 2024-11-06T03:22:27Z- publishEstimatesDecoder() - at 1730863347.28 sec 2024-11-06T03:22:27Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:22:27Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061632.42 594736.66 57.16 2024-11-06T03:22:27Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061632.35 594736.52 58.33 lat/lon: 36.70 -121.94 2024-11-06T03:22:27Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.91 0.15 0.92 0.00 2024-11-06T03:22:27Z- AHRS_M2 msg: 1730863347.68 epoch sec; seqNo:4625 2024-11-06T03:22:27Z- AHRS_M2 msg: 0.12 phi; -0.00 theta; 6.05 psi 2024-11-06T03:22:27Z- handleDepth()- Depth_Keller msg: 1730863347.69 epoch sec; seqNo:4920; depth 50.33 2024-11-06T03:22:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863347.70 epoch sec; seqNo:4920; 4061602.14 north; 594753.75 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:22:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863347.28 2024-11-06T03:22:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:27Z- waiting for 1730863351.28, time is 1730863347.70 2024-11-06T03:22:28Z- AHRS_M2 msg: 1730863348.08 epoch sec; seqNo:4626 2024-11-06T03:22:28Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 6.05 psi 2024-11-06T03:22:28Z- handleDepth()- Depth_Keller msg: 1730863348.09 epoch sec; seqNo:4921; depth 50.33 2024-11-06T03:22:28Z- handleDVL() - DVL msg epochMillisec: 1730863348.12 2024-11-06T03:22:28Z- handleDvl() - RDI_Pathfinder msg: 1730863348.12 sec; seqNo:1667; #beams:4 2024-11-06T03:22:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 30.99 , 25.42 , 26.50 , 28.95 2024-11-06T03:22:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.11 , 0.05 2024-11-06T03:22:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863348.13 epoch sec; seqNo:4921; 4061602.52 north; 594753.75 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:22:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863347.28 2024-11-06T03:22:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:28Z- waiting for 1730863351.28, time is 1730863348.13 2024-11-06T03:22:28Z- AHRS_M2 msg: 1730863348.49 epoch sec; seqNo:4627 2024-11-06T03:22:28Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 6.05 psi 2024-11-06T03:22:28Z- handleDepth()- Depth_Keller msg: 1730863348.49 epoch sec; seqNo:4922; depth 50.33 2024-11-06T03:22:28Z- handleDVL() - DVL msg epochMillisec: 1730863348.49 2024-11-06T03:22:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:28Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:28Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:28Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:28Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:28Z- handleDvl() - RDI_Pathfinder msg: 1730863348.49 sec; seqNo:1668; #beams:0 2024-11-06T03:22:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.11 , 0.05 2024-11-06T03:22:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863348.51 epoch sec; seqNo:4922; 4061602.90 north; 594753.74 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:22:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863347.28 2024-11-06T03:22:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:28Z- waiting for 1730863351.28, time is 1730863348.51 2024-11-06T03:22:28Z- AHRS_M2 msg: 1730863348.91 epoch sec; seqNo:4628 2024-11-06T03:22:28Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 6.05 psi 2024-11-06T03:22:28Z- handleDepth()- Depth_Keller msg: 1730863348.91 epoch sec; seqNo:4923; depth 50.33 2024-11-06T03:22:28Z- handleDVL() - DVL msg epochMillisec: 1730863348.93 2024-11-06T03:22:28Z- handleDvl() - RDI_Pathfinder msg: 1730863348.93 sec; seqNo:1669; #beams:4 2024-11-06T03:22:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.40 , 24.60 , 27.87 , 28.95 2024-11-06T03:22:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.10 , 0.06 2024-11-06T03:22:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863348.96 epoch sec; seqNo:4923; 4061603.28 north; 594753.74 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:22:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863347.28 2024-11-06T03:22:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:29Z- waiting for 1730863351.28, time is 1730863348.96 2024-11-06T03:22:29Z- handleDepth()- Depth_Keller msg: 1730863349.31 epoch sec; seqNo:4924; depth 50.35 2024-11-06T03:22:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863349.32 epoch sec; seqNo:4924; 4061603.66 north; 594753.73 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:22:29Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:22:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863347.28 2024-11-06T03:22:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:29Z- waiting for 1730863351.28, time is 1730863349.32 2024-11-06T03:22:29Z- AHRS_M2 msg: 1730863349.70 epoch sec; seqNo:4629 2024-11-06T03:22:29Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 6.05 psi 2024-11-06T03:22:29Z- handleDepth()- Depth_Keller msg: 1730863349.71 epoch sec; seqNo:4925; depth 50.34 2024-11-06T03:22:29Z- handleDVL() - DVL msg epochMillisec: 1730863349.71 2024-11-06T03:22:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:29Z- handleDvl() - RDI_Pathfinder msg: 1730863349.71 sec; seqNo:1670; #beams:0 2024-11-06T03:22:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.10 , 0.06 2024-11-06T03:22:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863349.73 epoch sec; seqNo:4925; 4061604.03 north; 594752.51 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:22:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863347.28 2024-11-06T03:22:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:29Z- waiting for 1730863351.28, time is 1730863349.73 2024-11-06T03:22:30Z- AHRS_M2 msg: 1730863350.10 epoch sec; seqNo:4630 2024-11-06T03:22:30Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 6.05 psi 2024-11-06T03:22:30Z- handleDepth()- Depth_Keller msg: 1730863350.11 epoch sec; seqNo:4926; depth 50.34 2024-11-06T03:22:30Z- handleDVL() - DVL msg epochMillisec: 1730863350.14 2024-11-06T03:22:30Z- handleDvl() - RDI_Pathfinder msg: 1730863350.14 sec; seqNo:1671; #beams:4 2024-11-06T03:22:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.67 , 25.01 , 27.73 , 28.95 2024-11-06T03:22:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , 0.06 2024-11-06T03:22:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863350.15 epoch sec; seqNo:4926; 4061604.40 north; 594752.51 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:22:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863347.28 2024-11-06T03:22:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:30Z- waiting for 1730863351.28, time is 1730863350.15 2024-11-06T03:22:30Z- AHRS_M2 msg: 1730863350.51 epoch sec; seqNo:4631 2024-11-06T03:22:30Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 6.05 psi 2024-11-06T03:22:30Z- handleDepth()- Depth_Keller msg: 1730863350.52 epoch sec; seqNo:4927; depth 50.33 2024-11-06T03:22:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863350.53 epoch sec; seqNo:4927; 4061604.40 north; 594752.51 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:22:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863347.28 2024-11-06T03:22:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:30Z- waiting for 1730863351.28, time is 1730863350.53 2024-11-06T03:22:30Z- AHRS_M2 msg: 1730863350.94 epoch sec; seqNo:4632 2024-11-06T03:22:30Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 6.05 psi 2024-11-06T03:22:30Z- handleDepth()- Depth_Keller msg: 1730863350.94 epoch sec; seqNo:4928; depth 50.32 2024-11-06T03:22:30Z- handleDVL() - DVL msg epochMillisec: 1730863350.96 2024-11-06T03:22:30Z- handleDvl() - RDI_Pathfinder msg: 1730863350.96 sec; seqNo:1672; #beams:4 2024-11-06T03:22:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.81 , 24.47 , 28.00 , 28.41 2024-11-06T03:22:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , 0.05 2024-11-06T03:22:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863350.99 epoch sec; seqNo:4928; 4061605.16 north; 594752.50 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:22:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863347.28 2024-11-06T03:22:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:31Z- waiting for 1730863351.28, time is 1730863350.99 2024-11-06T03:22:31Z- AHRS_M2 msg: 1730863351.32 epoch sec; seqNo:4633 2024-11-06T03:22:31Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.05 psi 2024-11-06T03:22:31Z- handleDepth()- Depth_Keller msg: 1730863351.32 epoch sec; seqNo:4929; depth 50.32 2024-11-06T03:22:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863351.35 epoch sec; seqNo:4929; 4061605.16 north; 594752.50 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:22:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863347.28 2024-11-06T03:22:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:31Z- waiting for 1730863351.28, time is 1730863351.35 2024-11-06T03:22:31Z- LcmTrn::updateTrn() >>>> pose time 1730863351.35, meas time 1730863350.96 <<<< 2024-11-06T03:22:31Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863351.35 2024-11-06T03:22:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863350.96 sec, ping # 0; waiting for INS data more recent than 1730863347.28... 2024-11-06T03:22:31Z- TerrainNav::motionUpdate() - currEstimate = (4061605, 594752.50, 50.32) 2024-11-06T03:22:31Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:22:31Z- TerrainNav:: SubcloudNIS is 1.30, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:22:31Z- TNavPF::Projecting Measurements 2024-11-06T03:22:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:22:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:22:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:22:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:22:31Z- TNavFilter::4 good beams. 2024-11-06T03:22:31Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:22:31Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:22:31Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:22:31Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:22:31Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:22:31Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:22:31Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:22:31Z- TerrainMapOctree: (vehN, vehE ) = (4061604.84, 594752.62). 2024-11-06T03:22:31Z- TerrainMapOctree: Min Distance = 145.19. 2024-11-06T03:22:31Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:22:31Z- Forcing Subcloud Comparison 2024-11-06T03:22:31Z- Weighting particles with subcloud comparison 2024-11-06T03:22:31Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:31Z- meanExpectedMeasDiff: 1.099378 delta_rms_squared: 1.813253 alpha: 0.989090 2024-11-06T03:22:31Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:31Z- meanExpectedMeasDiff: -4.008850 delta_rms_squared: 1.361969 alpha: 0.985528 2024-11-06T03:22:31Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:31Z- meanExpectedMeasDiff: -3.924412 delta_rms_squared: 1.590605 alpha: 0.987582 2024-11-06T03:22:31Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:31Z- meanExpectedMeasDiff: -0.977106 delta_rms_squared: 1.494426 alpha: 0.986794 2024-11-06T03:22:31Z- Subcloud NIS: 1.255338 2024-11-06T03:22:31Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:22:31Z- TNavPF:: sumWeights = 0.010067 2024-11-06T03:22:31Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:22:31Z- TNavPF::First Term of Map Covariance Matrix is 0.07 2024-11-06T03:22:31Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:22:31Z- TerrainNav::Resampling particle filter... 2024-11-06T03:22:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863350.96 sec. 2024-11-06T03:22:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:22:31Z- variances([0], [2], and psi_berg) = 0.928910 0.155312 0.000000 2024-11-06T03:22:31Z- publishEstimatesDecoder() - at 1730863351.35 sec 2024-11-06T03:22:31Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:22:31Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061636.04 594735.58 60.06 2024-11-06T03:22:31Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061635.77 594735.28 59.51 lat/lon: 36.70 -121.94 2024-11-06T03:22:31Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.93 0.16 0.63 0.00 2024-11-06T03:22:31Z- AHRS_M2 msg: 1730863351.72 epoch sec; seqNo:4634 2024-11-06T03:22:31Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.05 psi 2024-11-06T03:22:31Z- handleDepth()- Depth_Keller msg: 1730863351.73 epoch sec; seqNo:4930; depth 50.30 2024-11-06T03:22:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863351.74 epoch sec; seqNo:4930; 4061605.54 north; 594752.49 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:22:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863351.35 2024-11-06T03:22:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:31Z- waiting for 1730863355.35, time is 1730863351.74 2024-11-06T03:22:32Z- AHRS_M2 msg: 1730863352.12 epoch sec; seqNo:4635 2024-11-06T03:22:32Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.05 psi 2024-11-06T03:22:32Z- handleDepth()- Depth_Keller msg: 1730863352.13 epoch sec; seqNo:4931; depth 50.29 2024-11-06T03:22:32Z- handleDVL() - DVL msg epochMillisec: 1730863352.15 2024-11-06T03:22:32Z- handleDvl() - RDI_Pathfinder msg: 1730863352.15 sec; seqNo:1673; #beams:4 2024-11-06T03:22:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.94 , 22.97 , 28.14 , 28.27 2024-11-06T03:22:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , 0.01 2024-11-06T03:22:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863352.16 epoch sec; seqNo:4931; 4061605.92 north; 594752.49 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:22:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863351.35 2024-11-06T03:22:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:32Z- waiting for 1730863355.35, time is 1730863352.16 2024-11-06T03:22:32Z- AHRS_M2 msg: 1730863352.53 epoch sec; seqNo:4636 2024-11-06T03:22:32Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.06 psi 2024-11-06T03:22:32Z- handleDepth()- Depth_Keller msg: 1730863352.53 epoch sec; seqNo:4932; depth 50.27 2024-11-06T03:22:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863352.56 epoch sec; seqNo:4932; 4061606.30 north; 594752.48 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:22:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863351.35 2024-11-06T03:22:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:32Z- waiting for 1730863355.35, time is 1730863352.56 2024-11-06T03:22:32Z- AHRS_M2 msg: 1730863352.95 epoch sec; seqNo:4637 2024-11-06T03:22:32Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.06 psi 2024-11-06T03:22:32Z- handleDepth()- Depth_Keller msg: 1730863352.96 epoch sec; seqNo:4933; depth 50.26 2024-11-06T03:22:32Z- handleDVL() - DVL msg epochMillisec: 1730863352.98 2024-11-06T03:22:32Z- handleDvl() - RDI_Pathfinder msg: 1730863352.98 sec; seqNo:1674; #beams:4 2024-11-06T03:22:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.21 , 23.38 , 28.00 , 28.54 2024-11-06T03:22:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , -0.02 2024-11-06T03:22:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863352.99 epoch sec; seqNo:4933; 4061606.68 north; 594752.48 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:22:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863351.35 2024-11-06T03:22:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:33Z- waiting for 1730863355.35, time is 1730863352.99 2024-11-06T03:22:33Z- AHRS_M2 msg: 1730863353.34 epoch sec; seqNo:4638 2024-11-06T03:22:33Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.06 psi 2024-11-06T03:22:33Z- handleDepth()- Depth_Keller msg: 1730863353.34 epoch sec; seqNo:4934; depth 50.23 2024-11-06T03:22:33Z- handleDVL() - DVL msg epochMillisec: 1730863353.34 2024-11-06T03:22:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:33Z- handleDvl() - RDI_Pathfinder msg: 1730863353.34 sec; seqNo:1675; #beams:0 2024-11-06T03:22:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , -0.02 2024-11-06T03:22:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863353.36 epoch sec; seqNo:4934; 4061607.06 north; 594752.48 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:22:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863351.35 2024-11-06T03:22:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:33Z- waiting for 1730863355.35, time is 1730863353.36 2024-11-06T03:22:33Z- AHRS_M2 msg: 1730863353.74 epoch sec; seqNo:4639 2024-11-06T03:22:33Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.06 psi 2024-11-06T03:22:33Z- handleDepth()- Depth_Keller msg: 1730863353.75 epoch sec; seqNo:4935; depth 50.22 2024-11-06T03:22:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863353.76 epoch sec; seqNo:4935; 4061607.44 north; 594752.47 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:22:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863351.35 2024-11-06T03:22:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:33Z- waiting for 1730863355.35, time is 1730863353.76 2024-11-06T03:22:34Z- handleDepth()- Depth_Keller msg: 1730863354.17 epoch sec; seqNo:4936; depth 50.19 2024-11-06T03:22:34Z- handleDVL() - DVL msg epochMillisec: 1730863354.19 2024-11-06T03:22:34Z- handleDvl() - RDI_Pathfinder msg: 1730863354.19 sec; seqNo:1676; #beams:4 2024-11-06T03:22:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.81 , 23.51 , 27.73 , 28.54 2024-11-06T03:22:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , -0.05 2024-11-06T03:22:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863354.21 epoch sec; seqNo:4936; 4061607.81 north; 594752.47 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:22:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863351.35 2024-11-06T03:22:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:34Z- waiting for 1730863355.35, time is 1730863354.21 2024-11-06T03:22:34Z- AHRS_M2 msg: 1730863354.56 epoch sec; seqNo:4640 2024-11-06T03:22:34Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.06 psi 2024-11-06T03:22:34Z- handleDepth()- Depth_Keller msg: 1730863354.56 epoch sec; seqNo:4937; depth 50.15 2024-11-06T03:22:34Z- handleDVL() - DVL msg epochMillisec: 1730863354.56 2024-11-06T03:22:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:34Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:34Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:34Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:34Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:34Z- handleDvl() - RDI_Pathfinder msg: 1730863354.56 sec; seqNo:1677; #beams:0 2024-11-06T03:22:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , -0.05 2024-11-06T03:22:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863354.59 epoch sec; seqNo:4937; 4061607.80 north; 594751.25 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:22:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863351.35 2024-11-06T03:22:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:34Z- waiting for 1730863355.35, time is 1730863354.59 2024-11-06T03:22:35Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:22:35Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:22:35Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:22:35Z- AHRS_M2 msg: 1730863354.96 epoch sec; seqNo:4641 2024-11-06T03:22:35Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.06 psi 2024-11-06T03:22:35Z- handleDepth()- Depth_Keller msg: 1730863355.01 epoch sec; seqNo:4938; depth 50.14 2024-11-06T03:22:35Z- handleDVL() - DVL msg epochMillisec: 1730863355.03 2024-11-06T03:22:35Z- handleDvl() - RDI_Pathfinder msg: 1730863355.03 sec; seqNo:1678; #beams:4 2024-11-06T03:22:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.94 , 23.11 , 27.46 , 28.27 2024-11-06T03:22:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , -0.08 2024-11-06T03:22:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863355.05 epoch sec; seqNo:4938; 4061608.18 north; 594751.24 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:22:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863351.35 2024-11-06T03:22:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:35Z- waiting for 1730863355.35, time is 1730863355.05 2024-11-06T03:22:35Z- AHRS_M2 msg: 1730863355.36 epoch sec; seqNo:4642 2024-11-06T03:22:35Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.06 psi 2024-11-06T03:22:35Z- handleDepth()- Depth_Keller msg: 1730863355.37 epoch sec; seqNo:4939; depth 50.10 2024-11-06T03:22:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863355.38 epoch sec; seqNo:4939; 4061608.56 north; 594751.24 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:22:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863351.35 2024-11-06T03:22:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:35Z- waiting for 1730863355.35, time is 1730863355.38 2024-11-06T03:22:35Z- LcmTrn::updateTrn() >>>> pose time 1730863355.38, meas time 1730863355.03 <<<< 2024-11-06T03:22:35Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863355.38 2024-11-06T03:22:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863355.03 sec, ping # 0; waiting for INS data more recent than 1730863351.35... 2024-11-06T03:22:35Z- TerrainNav::motionUpdate() - currEstimate = (4061609, 594751.24, 50.10) 2024-11-06T03:22:35Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:22:35Z- TerrainNav:: SubcloudNIS is 1.26, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:22:35Z- TNavPF::Projecting Measurements 2024-11-06T03:22:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:22:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:22:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:22:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:22:35Z- TNavFilter::4 good beams. 2024-11-06T03:22:35Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:22:35Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:22:35Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:22:35Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:22:35Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:22:35Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:22:35Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:22:35Z- TerrainMapOctree: (vehN, vehE ) = (4061608.26, 594751.35). 2024-11-06T03:22:35Z- TerrainMapOctree: Min Distance = 141.75. 2024-11-06T03:22:35Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:22:35Z- Forcing Subcloud Comparison 2024-11-06T03:22:35Z- Weighting particles with subcloud comparison 2024-11-06T03:22:35Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:35Z- meanExpectedMeasDiff: 1.248704 delta_rms_squared: 1.231831 alpha: 0.984023 2024-11-06T03:22:35Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:35Z- meanExpectedMeasDiff: -4.987953 delta_rms_squared: 0.841026 alpha: 0.976772 2024-11-06T03:22:35Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:35Z- meanExpectedMeasDiff: -3.470366 delta_rms_squared: 1.221740 alpha: 0.983894 2024-11-06T03:22:35Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:35Z- meanExpectedMeasDiff: 0.060331 delta_rms_squared: 1.189777 alpha: 0.983468 2024-11-06T03:22:35Z- Subcloud NIS: 1.274903 2024-11-06T03:22:35Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:22:35Z- TNavPF:: sumWeights = 0.002498 2024-11-06T03:22:35Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:22:35Z- TNavPF::First Term of Map Covariance Matrix is 0.01 2024-11-06T03:22:35Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:22:35Z- TerrainNav::Resampling particle filter... 2024-11-06T03:22:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863355.03 sec. 2024-11-06T03:22:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:22:35Z- variances([0], [2], and psi_berg) = 1.138872 0.177500 0.000000 2024-11-06T03:22:35Z- publishEstimatesDecoder() - at 1730863355.38 sec 2024-11-06T03:22:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:22:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061639.42 594734.33 59.99 2024-11-06T03:22:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061638.86 594734.16 60.23 lat/lon: 36.70 -121.94 2024-11-06T03:22:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.14 0.18 0.63 0.00 2024-11-06T03:22:35Z- AHRS_M2 msg: 1730863355.77 epoch sec; seqNo:4643 2024-11-06T03:22:35Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.06 psi 2024-11-06T03:22:35Z- handleDepth()- Depth_Keller msg: 1730863355.78 epoch sec; seqNo:4940; depth 50.08 2024-11-06T03:22:35Z- handleDVL() - DVL msg epochMillisec: 1730863355.79 2024-11-06T03:22:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:35Z- handleDvl() - RDI_Pathfinder msg: 1730863355.79 sec; seqNo:1679; #beams:0 2024-11-06T03:22:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , -0.08 2024-11-06T03:22:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863355.80 epoch sec; seqNo:4940; 4061608.94 north; 594751.23 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:22:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863355.38 2024-11-06T03:22:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:35Z- waiting for 1730863359.38, time is 1730863355.80 2024-11-06T03:22:36Z- AHRS_M2 msg: 1730863356.16 epoch sec; seqNo:4644 2024-11-06T03:22:36Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.06 psi 2024-11-06T03:22:36Z- handleDepth()- Depth_Keller msg: 1730863356.17 epoch sec; seqNo:4941; depth 50.06 2024-11-06T03:22:36Z- handleDVL() - DVL msg epochMillisec: 1730863356.19 2024-11-06T03:22:36Z- handleDvl() - RDI_Pathfinder msg: 1730863356.19 sec; seqNo:1680; #beams:4 2024-11-06T03:22:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.81 , 23.79 , 27.05 , 28.55 2024-11-06T03:22:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.12 , -0.07 2024-11-06T03:22:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863356.21 epoch sec; seqNo:4941; 4061609.32 north; 594751.23 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:22:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863355.38 2024-11-06T03:22:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:36Z- waiting for 1730863359.38, time is 1730863356.21 2024-11-06T03:22:36Z- AHRS_M2 msg: 1730863356.57 epoch sec; seqNo:4645 2024-11-06T03:22:36Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.05 psi 2024-11-06T03:22:36Z- handleDepth()- Depth_Keller msg: 1730863356.59 epoch sec; seqNo:4942; depth 50.03 2024-11-06T03:22:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863356.61 epoch sec; seqNo:4942; 4061609.69 north; 594751.23 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:22:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863355.38 2024-11-06T03:22:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:36Z- waiting for 1730863359.38, time is 1730863356.61 2024-11-06T03:22:37Z- AHRS_M2 msg: 1730863357.01 epoch sec; seqNo:4646 2024-11-06T03:22:37Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.05 psi 2024-11-06T03:22:37Z- handleDepth()- Depth_Keller msg: 1730863357.01 epoch sec; seqNo:4943; depth 50.03 2024-11-06T03:22:37Z- handleDVL() - DVL msg epochMillisec: 1730863357.03 2024-11-06T03:22:37Z- handleDvl() - RDI_Pathfinder msg: 1730863357.03 sec; seqNo:1681; #beams:4 2024-11-06T03:22:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.94 , 24.06 , 26.64 , 28.41 2024-11-06T03:22:37Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.15 , -0.04 2024-11-06T03:22:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863357.06 epoch sec; seqNo:4943; 4061610.07 north; 594751.22 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:22:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863355.38 2024-11-06T03:22:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:37Z- waiting for 1730863359.38, time is 1730863357.06 2024-11-06T03:22:37Z- AHRS_M2 msg: 1730863357.37 epoch sec; seqNo:4647 2024-11-06T03:22:37Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.05 psi 2024-11-06T03:22:37Z- handleDepth()- Depth_Keller msg: 1730863357.38 epoch sec; seqNo:4944; depth 50.02 2024-11-06T03:22:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863357.39 epoch sec; seqNo:4944; 4061610.45 north; 594751.22 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:22:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863355.38 2024-11-06T03:22:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:37Z- waiting for 1730863359.38, time is 1730863357.39 2024-11-06T03:22:37Z- AHRS_M2 msg: 1730863357.78 epoch sec; seqNo:4648 2024-11-06T03:22:37Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.04 psi 2024-11-06T03:22:37Z- handleDepth()- Depth_Keller msg: 1730863357.78 epoch sec; seqNo:4945; depth 50.01 2024-11-06T03:22:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863357.79 epoch sec; seqNo:4945; 4061610.83 north; 594751.21 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:22:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863355.38 2024-11-06T03:22:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:37Z- waiting for 1730863359.38, time is 1730863357.79 2024-11-06T03:22:38Z- AHRS_M2 msg: 1730863358.18 epoch sec; seqNo:4649 2024-11-06T03:22:38Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 6.04 psi 2024-11-06T03:22:38Z- handleDepth()- Depth_Keller msg: 1730863358.20 epoch sec; seqNo:4946; depth 50.01 2024-11-06T03:22:38Z- handleDVL() - DVL msg epochMillisec: 1730863358.22 2024-11-06T03:22:38Z- handleDvl() - RDI_Pathfinder msg: 1730863358.22 sec; seqNo:1682; #beams:4 2024-11-06T03:22:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.94 , 23.52 , 26.51 , 28.27 2024-11-06T03:22:38Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.18 , -0.00 2024-11-06T03:22:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863358.23 epoch sec; seqNo:4946; 4061611.20 north; 594749.99 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:22:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863355.38 2024-11-06T03:22:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:38Z- waiting for 1730863359.38, time is 1730863358.23 2024-11-06T03:22:38Z- AHRS_M2 msg: 1730863358.59 epoch sec; seqNo:4650 2024-11-06T03:22:38Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 6.04 psi 2024-11-06T03:22:38Z- handleDepth()- Depth_Keller msg: 1730863358.60 epoch sec; seqNo:4947; depth 50.01 2024-11-06T03:22:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863358.63 epoch sec; seqNo:4947; 4061611.58 north; 594749.98 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:22:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863355.38 2024-11-06T03:22:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:38Z- waiting for 1730863359.38, time is 1730863358.63 2024-11-06T03:22:39Z- AHRS_M2 msg: 1730863359.04 epoch sec; seqNo:4651 2024-11-06T03:22:39Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 6.04 psi 2024-11-06T03:22:39Z- handleDepth()- Depth_Keller msg: 1730863359.05 epoch sec; seqNo:4948; depth 50.01 2024-11-06T03:22:39Z- handleDVL() - DVL msg epochMillisec: 1730863359.07 2024-11-06T03:22:39Z- handleDvl() - RDI_Pathfinder msg: 1730863359.07 sec; seqNo:1683; #beams:4 2024-11-06T03:22:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.81 , 23.79 , 26.37 , 28.27 2024-11-06T03:22:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.17 , 0.05 2024-11-06T03:22:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863359.09 epoch sec; seqNo:4948; 4061611.95 north; 594749.98 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:22:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863355.38 2024-11-06T03:22:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:39Z- waiting for 1730863359.38, time is 1730863359.09 2024-11-06T03:22:39Z- AHRS_M2 msg: 1730863359.39 epoch sec; seqNo:4652 2024-11-06T03:22:39Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 6.03 psi 2024-11-06T03:22:39Z- handleDepth()- Depth_Keller msg: 1730863359.40 epoch sec; seqNo:4949; depth 50.00 2024-11-06T03:22:39Z- handleDVL() - DVL msg epochMillisec: 1730863359.40 2024-11-06T03:22:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:39Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:39Z- handleDvl() - RDI_Pathfinder msg: 1730863359.40 sec; seqNo:1684; #beams:0 2024-11-06T03:22:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.17 , 0.05 2024-11-06T03:22:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863359.43 epoch sec; seqNo:4949; 4061612.33 north; 594749.98 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:22:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863355.38 2024-11-06T03:22:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:39Z- waiting for 1730863359.38, time is 1730863359.43 2024-11-06T03:22:39Z- LcmTrn::updateTrn() >>>> pose time 1730863359.43, meas time 1730863359.40 <<<< 2024-11-06T03:22:39Z- LcmTrn::updateTrn() equating pose 1730863359.40 and meas times 1730863359.40 2024-11-06T03:22:39Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863359.40 2024-11-06T03:22:39Z- TerrainNav::motionUpdate() - currEstimate = (4061612, 594749.98, 50.00) 2024-11-06T03:22:39Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:22:39Z- TerrainNav:: SubcloudNIS is 1.27, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:22:39Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:22:39Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:22:39Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:22:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:22:39Z- variances([0], [2], and psi_berg) = 1.142307 0.180308 0.000000 2024-11-06T03:22:39Z- publishEstimatesDecoder() - at 1730863359.40 sec 2024-11-06T03:22:39Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:22:39Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061643.16 594732.99 60.14 2024-11-06T03:22:39Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061642.63 594732.89 60.13 lat/lon: 36.70 -121.94 2024-11-06T03:22:39Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.14 0.18 0.87 0.00 2024-11-06T03:22:39Z- AHRS_M2 msg: 1730863359.81 epoch sec; seqNo:4653 2024-11-06T03:22:39Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 6.03 psi 2024-11-06T03:22:39Z- handleDepth()- Depth_Keller msg: 1730863359.82 epoch sec; seqNo:4950; depth 50.00 2024-11-06T03:22:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863359.83 epoch sec; seqNo:4950; 4061612.71 north; 594749.97 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:22:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863359.40 2024-11-06T03:22:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:39Z- waiting for 1730863363.40, time is 1730863359.83 2024-11-06T03:22:40Z- AHRS_M2 msg: 1730863360.20 epoch sec; seqNo:4654 2024-11-06T03:22:40Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 6.03 psi 2024-11-06T03:22:40Z- handleDepth()- Depth_Keller msg: 1730863360.22 epoch sec; seqNo:4951; depth 50.02 2024-11-06T03:22:40Z- handleDVL() - DVL msg epochMillisec: 1730863360.24 2024-11-06T03:22:40Z- handleDvl() - RDI_Pathfinder msg: 1730863360.24 sec; seqNo:1685; #beams:4 2024-11-06T03:22:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.08 , 23.79 , 26.10 , 28.00 2024-11-06T03:22:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.13 , 0.07 2024-11-06T03:22:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863360.25 epoch sec; seqNo:4951; 4061613.09 north; 594749.97 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:22:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863359.40 2024-11-06T03:22:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:40Z- waiting for 1730863363.40, time is 1730863360.25 2024-11-06T03:22:40Z- AHRS_M2 msg: 1730863360.61 epoch sec; seqNo:4655 2024-11-06T03:22:40Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 6.03 psi 2024-11-06T03:22:40Z- handleDepth()- Depth_Keller msg: 1730863360.62 epoch sec; seqNo:4952; depth 50.02 2024-11-06T03:22:40Z- handleDVL() - DVL msg epochMillisec: 1730863360.62 2024-11-06T03:22:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:40Z- handleDvl() - RDI_Pathfinder msg: 1730863360.62 sec; seqNo:1686; #beams:0 2024-11-06T03:22:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.13 , 0.07 2024-11-06T03:22:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863360.66 epoch sec; seqNo:4952; 4061613.47 north; 594749.96 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:22:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863359.40 2024-11-06T03:22:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:40Z- waiting for 1730863363.40, time is 1730863360.66 2024-11-06T03:22:41Z- AHRS_M2 msg: 1730863361.07 epoch sec; seqNo:4656 2024-11-06T03:22:41Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 6.03 psi 2024-11-06T03:22:41Z- handleDepth()- Depth_Keller msg: 1730863361.08 epoch sec; seqNo:4953; depth 50.01 2024-11-06T03:22:41Z- handleDVL() - DVL msg epochMillisec: 1730863361.10 2024-11-06T03:22:41Z- handleDvl() - RDI_Pathfinder msg: 1730863361.10 sec; seqNo:1687; #beams:4 2024-11-06T03:22:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.08 , 24.06 , 26.10 , 28.00 2024-11-06T03:22:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.09 , 0.08 2024-11-06T03:22:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863361.12 epoch sec; seqNo:4953; 4061613.85 north; 594749.96 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:22:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863359.40 2024-11-06T03:22:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:41Z- waiting for 1730863363.40, time is 1730863361.12 2024-11-06T03:22:41Z- AHRS_M2 msg: 1730863361.42 epoch sec; seqNo:4657 2024-11-06T03:22:41Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 6.03 psi 2024-11-06T03:22:41Z- handleDepth()- Depth_Keller msg: 1730863361.42 epoch sec; seqNo:4954; depth 50.03 2024-11-06T03:22:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863361.44 epoch sec; seqNo:4954; 4061614.21 north; 594748.74 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:22:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863359.40 2024-11-06T03:22:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:41Z- waiting for 1730863363.40, time is 1730863361.44 2024-11-06T03:22:41Z- AHRS_M2 msg: 1730863361.82 epoch sec; seqNo:4658 2024-11-06T03:22:41Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 6.03 psi 2024-11-06T03:22:41Z- handleDepth()- Depth_Keller msg: 1730863361.83 epoch sec; seqNo:4955; depth 50.01 2024-11-06T03:22:41Z- handleDVL() - DVL msg epochMillisec: 1730863361.83 2024-11-06T03:22:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:41Z- handleDvl() - RDI_Pathfinder msg: 1730863361.83 sec; seqNo:1688; #beams:0 2024-11-06T03:22:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.09 , 0.08 2024-11-06T03:22:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863361.84 epoch sec; seqNo:4955; 4061614.59 north; 594748.73 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:22:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863359.40 2024-11-06T03:22:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:41Z- waiting for 1730863363.40, time is 1730863361.84 2024-11-06T03:22:42Z- AHRS_M2 msg: 1730863362.22 epoch sec; seqNo:4659 2024-11-06T03:22:42Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 6.03 psi 2024-11-06T03:22:42Z- handleDepth()- Depth_Keller msg: 1730863362.24 epoch sec; seqNo:4956; depth 50.01 2024-11-06T03:22:42Z- handleDVL() - DVL msg epochMillisec: 1730863362.26 2024-11-06T03:22:42Z- handleDvl() - RDI_Pathfinder msg: 1730863362.26 sec; seqNo:1689; #beams:4 2024-11-06T03:22:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.89 , 24.33 , 26.37 , 28.14 2024-11-06T03:22:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , 0.06 2024-11-06T03:22:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863362.27 epoch sec; seqNo:4956; 4061614.97 north; 594748.73 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:22:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863359.40 2024-11-06T03:22:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:42Z- waiting for 1730863363.40, time is 1730863362.27 2024-11-06T03:22:42Z- AHRS_M2 msg: 1730863362.63 epoch sec; seqNo:4660 2024-11-06T03:22:42Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 6.03 psi 2024-11-06T03:22:42Z- handleDepth()- Depth_Keller msg: 1730863362.64 epoch sec; seqNo:4957; depth 50.01 2024-11-06T03:22:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863362.65 epoch sec; seqNo:4957; 4061614.97 north; 594748.73 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:22:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863359.40 2024-11-06T03:22:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:42Z- waiting for 1730863363.40, time is 1730863362.65 2024-11-06T03:22:43Z- AHRS_M2 msg: 1730863363.04 epoch sec; seqNo:4661 2024-11-06T03:22:43Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.04 psi 2024-11-06T03:22:43Z- handleDepth()- Depth_Keller msg: 1730863363.07 epoch sec; seqNo:4958; depth 50.01 2024-11-06T03:22:43Z- handleDVL() - DVL msg epochMillisec: 1730863363.12 2024-11-06T03:22:43Z- handleDvl() - RDI_Pathfinder msg: 1730863363.12 sec; seqNo:1690; #beams:4 2024-11-06T03:22:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.35 , 24.47 , 27.05 , 28.27 2024-11-06T03:22:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.03 2024-11-06T03:22:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863363.14 epoch sec; seqNo:4958; 4061615.35 north; 594748.72 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:22:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863359.40 2024-11-06T03:22:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:43Z- waiting for 1730863363.40, time is 1730863363.14 2024-11-06T03:22:43Z- AHRS_M2 msg: 1730863363.43 epoch sec; seqNo:4662 2024-11-06T03:22:43Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.04 psi 2024-11-06T03:22:43Z- handleDepth()- Depth_Keller msg: 1730863363.44 epoch sec; seqNo:4959; depth 50.00 2024-11-06T03:22:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863363.49 epoch sec; seqNo:4959; 4061615.73 north; 594748.72 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:22:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863359.40 2024-11-06T03:22:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:43Z- waiting for 1730863363.40, time is 1730863363.49 2024-11-06T03:22:43Z- LcmTrn::updateTrn() >>>> pose time 1730863363.49, meas time 1730863363.12 <<<< 2024-11-06T03:22:43Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863363.49 2024-11-06T03:22:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863363.12 sec, ping # 0; waiting for INS data more recent than 1730863359.40... 2024-11-06T03:22:43Z- TerrainNav::motionUpdate() - currEstimate = (4061616, 594748.72, 50.00) 2024-11-06T03:22:43Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:22:43Z- TerrainNav:: SubcloudNIS is 1.27, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:22:43Z- TNavPF::Projecting Measurements 2024-11-06T03:22:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:22:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:22:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:22:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:22:43Z- TNavFilter::4 good beams. 2024-11-06T03:22:43Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:22:43Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:22:43Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:22:43Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:22:43Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:22:43Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:22:43Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:22:43Z- TerrainMapOctree: (vehN, vehE ) = (4061615.43, 594748.83). 2024-11-06T03:22:43Z- TerrainMapOctree: Min Distance = 134.58. 2024-11-06T03:22:43Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:22:43Z- Forcing Subcloud Comparison 2024-11-06T03:22:43Z- Weighting particles with subcloud comparison 2024-11-06T03:22:43Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:43Z- meanExpectedMeasDiff: 1.654777 delta_rms_squared: 1.371803 alpha: 0.985630 2024-11-06T03:22:43Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:43Z- meanExpectedMeasDiff: -1.918334 delta_rms_squared: 1.146240 alpha: 0.982851 2024-11-06T03:22:43Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:43Z- meanExpectedMeasDiff: -2.716669 delta_rms_squared: 1.513822 alpha: 0.986961 2024-11-06T03:22:43Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:43Z- meanExpectedMeasDiff: 0.265245 delta_rms_squared: 1.658924 alpha: 0.988088 2024-11-06T03:22:43Z- Subcloud NIS: 1.209690 2024-11-06T03:22:43Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:22:43Z- TNavPF:: sumWeights = 0.075717 2024-11-06T03:22:43Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:22:43Z- TNavPF::First Term of Map Covariance Matrix is 0.32 2024-11-06T03:22:43Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T03:22:43Z- TerrainNav::Resampling particle filter... 2024-11-06T03:22:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863363.12 sec. 2024-11-06T03:22:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:22:43Z- variances([0], [2], and psi_berg) = 1.076563 0.187793 0.000000 2024-11-06T03:22:43Z- publishEstimatesDecoder() - at 1730863363.49 sec 2024-11-06T03:22:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:22:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061646.55 594731.68 60.19 2024-11-06T03:22:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061646.14 594731.64 60.65 lat/lon: 36.70 -121.94 2024-11-06T03:22:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.08 0.19 0.67 0.00 2024-11-06T03:22:43Z- AHRS_M2 msg: 1730863363.84 epoch sec; seqNo:4663 2024-11-06T03:22:43Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.04 psi 2024-11-06T03:22:43Z- handleDepth()- Depth_Keller msg: 1730863363.85 epoch sec; seqNo:4960; depth 49.99 2024-11-06T03:22:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863363.86 epoch sec; seqNo:4960; 4061616.11 north; 594748.71 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:22:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863363.49 2024-11-06T03:22:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:43Z- waiting for 1730863367.49, time is 1730863363.86 2024-11-06T03:22:44Z- AHRS_M2 msg: 1730863364.24 epoch sec; seqNo:4664 2024-11-06T03:22:44Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 6.04 psi 2024-11-06T03:22:44Z- handleDepth()- Depth_Keller msg: 1730863364.25 epoch sec; seqNo:4961; depth 49.96 2024-11-06T03:22:44Z- handleDVL() - DVL msg epochMillisec: 1730863364.27 2024-11-06T03:22:44Z- handleDvl() - RDI_Pathfinder msg: 1730863364.27 sec; seqNo:1691; #beams:4 2024-11-06T03:22:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.17 , 24.60 , 27.18 , 28.00 2024-11-06T03:22:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , -0.01 2024-11-06T03:22:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863364.28 epoch sec; seqNo:4961; 4061616.49 north; 594748.71 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:22:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863363.49 2024-11-06T03:22:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:44Z- waiting for 1730863367.49, time is 1730863364.28 2024-11-06T03:22:44Z- AHRS_M2 msg: 1730863364.65 epoch sec; seqNo:4665 2024-11-06T03:22:44Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.04 psi 2024-11-06T03:22:44Z- handleDepth()- Depth_Keller msg: 1730863364.66 epoch sec; seqNo:4962; depth 49.96 2024-11-06T03:22:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863364.68 epoch sec; seqNo:4962; 4061616.87 north; 594748.71 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:22:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863363.49 2024-11-06T03:22:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:44Z- waiting for 1730863367.49, time is 1730863364.68 2024-11-06T03:22:45Z- AHRS_M2 msg: 1730863365.06 epoch sec; seqNo:4666 2024-11-06T03:22:45Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.04 psi 2024-11-06T03:22:45Z- handleDepth()- Depth_Keller msg: 1730863365.07 epoch sec; seqNo:4963; depth 49.95 2024-11-06T03:22:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863365.13 epoch sec; seqNo:4963; 4061617.24 north; 594748.70 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:22:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863363.49 2024-11-06T03:22:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:45Z- waiting for 1730863367.49, time is 1730863365.13 2024-11-06T03:22:45Z- AHRS_M2 msg: 1730863365.46 epoch sec; seqNo:4667 2024-11-06T03:22:45Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.04 psi 2024-11-06T03:22:45Z- handleDepth()- Depth_Keller msg: 1730863365.46 epoch sec; seqNo:4964; depth 49.93 2024-11-06T03:22:45Z- handleDVL() - DVL msg epochMillisec: 1730863365.50 2024-11-06T03:22:45Z- handleDvl() - RDI_Pathfinder msg: 1730863365.50 sec; seqNo:1692; #beams:4 2024-11-06T03:22:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.49 , 24.60 , 26.91 , 27.86 2024-11-06T03:22:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , -0.05 2024-11-06T03:22:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863365.51 epoch sec; seqNo:4964; 4061617.62 north; 594748.70 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:22:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863363.49 2024-11-06T03:22:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:45Z- waiting for 1730863367.49, time is 1730863365.51 2024-11-06T03:22:45Z- AHRS_M2 msg: 1730863365.86 epoch sec; seqNo:4668 2024-11-06T03:22:45Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.04 psi 2024-11-06T03:22:45Z- handleDepth()- Depth_Keller msg: 1730863365.87 epoch sec; seqNo:4965; depth 49.91 2024-11-06T03:22:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863365.89 epoch sec; seqNo:4965; 4061618.00 north; 594748.69 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:22:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863363.49 2024-11-06T03:22:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:46Z- waiting for 1730863367.49, time is 1730863365.89 2024-11-06T03:22:46Z- AHRS_M2 msg: 1730863366.27 epoch sec; seqNo:4669 2024-11-06T03:22:46Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.04 psi 2024-11-06T03:22:46Z- handleDepth()- Depth_Keller msg: 1730863366.28 epoch sec; seqNo:4966; depth 49.89 2024-11-06T03:22:46Z- handleDVL() - DVL msg epochMillisec: 1730863366.30 2024-11-06T03:22:46Z- handleDvl() - RDI_Pathfinder msg: 1730863366.30 sec; seqNo:1693; #beams:4 2024-11-06T03:22:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.17 , 24.47 , 26.78 , 28.00 2024-11-06T03:22:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , -0.07 2024-11-06T03:22:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863366.31 epoch sec; seqNo:4966; 4061618.38 north; 594748.69 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:22:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863363.49 2024-11-06T03:22:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:46Z- waiting for 1730863367.49, time is 1730863366.31 2024-11-06T03:22:46Z- AHRS_M2 msg: 1730863366.67 epoch sec; seqNo:4670 2024-11-06T03:22:46Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.04 psi 2024-11-06T03:22:46Z- handleDepth()- Depth_Keller msg: 1730863366.68 epoch sec; seqNo:4967; depth 49.87 2024-11-06T03:22:46Z- handleDVL() - DVL msg epochMillisec: 1730863366.68 2024-11-06T03:22:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:46Z- handleDvl() - RDI_Pathfinder msg: 1730863366.68 sec; seqNo:1694; #beams:0 2024-11-06T03:22:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , -0.07 2024-11-06T03:22:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863366.71 epoch sec; seqNo:4967; 4061618.75 north; 594747.46 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:22:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863363.49 2024-11-06T03:22:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:46Z- waiting for 1730863367.49, time is 1730863366.71 2024-11-06T03:22:47Z- AHRS_M2 msg: 1730863367.08 epoch sec; seqNo:4671 2024-11-06T03:22:47Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.04 psi 2024-11-06T03:22:47Z- handleDepth()- Depth_Keller msg: 1730863367.09 epoch sec; seqNo:4968; depth 49.87 2024-11-06T03:22:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863367.16 epoch sec; seqNo:4968; 4061619.13 north; 594747.46 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:22:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863363.49 2024-11-06T03:22:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:47Z- waiting for 1730863367.49, time is 1730863367.16 2024-11-06T03:22:47Z- AHRS_M2 msg: 1730863367.48 epoch sec; seqNo:4672 2024-11-06T03:22:47Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.03 psi 2024-11-06T03:22:47Z- handleDepth()- Depth_Keller msg: 1730863367.48 epoch sec; seqNo:4969; depth 49.84 2024-11-06T03:22:47Z- handleDVL() - DVL msg epochMillisec: 1730863367.50 2024-11-06T03:22:47Z- handleDvl() - RDI_Pathfinder msg: 1730863367.50 sec; seqNo:1695; #beams:4 2024-11-06T03:22:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.94 , 24.33 , 26.78 , 28.27 2024-11-06T03:22:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.10 , -0.05 2024-11-06T03:22:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863367.51 epoch sec; seqNo:4969; 4061619.50 north; 594747.46 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:22:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863363.49 2024-11-06T03:22:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:47Z- waiting for 1730863367.49, time is 1730863367.51 2024-11-06T03:22:47Z- LcmTrn::updateTrn() >>>> pose time 1730863367.51, meas time 1730863367.50 <<<< 2024-11-06T03:22:47Z- LcmTrn::updateTrn() equating pose 1730863367.50 and meas times 1730863367.50 2024-11-06T03:22:47Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863367.50 2024-11-06T03:22:47Z- TerrainNav::motionUpdate() - currEstimate = (4061620, 594747.46, 49.84) 2024-11-06T03:22:47Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:22:47Z- TerrainNav:: SubcloudNIS is 1.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:22:47Z- TNavPF::Projecting Measurements 2024-11-06T03:22:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:22:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:22:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:22:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:22:47Z- TNavFilter::4 good beams. 2024-11-06T03:22:47Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:22:47Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:22:47Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:22:47Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:22:47Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:22:47Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:22:47Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:22:47Z- TerrainMapOctree: (vehN, vehE ) = (4061619.50, 594747.46). 2024-11-06T03:22:47Z- TerrainMapOctree: Min Distance = 130.52. 2024-11-06T03:22:47Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:22:47Z- Forcing Subcloud Comparison 2024-11-06T03:22:47Z- Weighting particles with subcloud comparison 2024-11-06T03:22:47Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:47Z- meanExpectedMeasDiff: 1.294073 delta_rms_squared: 1.466438 alpha: 0.986545 2024-11-06T03:22:47Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:47Z- meanExpectedMeasDiff: -0.872257 delta_rms_squared: 0.860301 alpha: 0.977280 2024-11-06T03:22:47Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:47Z- meanExpectedMeasDiff: -2.508124 delta_rms_squared: 1.227059 alpha: 0.983962 2024-11-06T03:22:47Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:47Z- meanExpectedMeasDiff: 0.370616 delta_rms_squared: 1.019821 alpha: 0.980766 2024-11-06T03:22:47Z- Subcloud NIS: 1.229523 2024-11-06T03:22:47Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:22:47Z- TNavPF:: sumWeights = 0.157747 2024-11-06T03:22:47Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:22:47Z- TNavPF::First Term of Map Covariance Matrix is 0.39 2024-11-06T03:22:47Z- TNavPF::Calculated NIS Value : 0.09 numBeams normalized NIS: 0.02 2024-11-06T03:22:47Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863367.50 sec, ping # 0. 2024-11-06T03:22:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:22:47Z- variances([0], [2], and psi_berg) = 1.038874 0.201707 0.000000 2024-11-06T03:22:47Z- publishEstimatesDecoder() - at 1730863367.50 sec 2024-11-06T03:22:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:22:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061651.45 594730.68 60.20 2024-11-06T03:22:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061649.96 594730.39 60.74 lat/lon: 36.70 -121.94 2024-11-06T03:22:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.04 0.20 0.38 0.00 2024-11-06T03:22:47Z- AHRS_M2 msg: 1730863367.88 epoch sec; seqNo:4673 2024-11-06T03:22:47Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:22:47Z- handleDepth()- Depth_Keller msg: 1730863367.90 epoch sec; seqNo:4970; depth 49.84 2024-11-06T03:22:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863367.92 epoch sec; seqNo:4970; 4061619.88 north; 594747.45 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:22:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863367.50 2024-11-06T03:22:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:48Z- waiting for 1730863371.50, time is 1730863367.92 2024-11-06T03:22:48Z- AHRS_M2 msg: 1730863368.28 epoch sec; seqNo:4674 2024-11-06T03:22:48Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:22:48Z- handleDepth()- Depth_Keller msg: 1730863368.29 epoch sec; seqNo:4971; depth 49.84 2024-11-06T03:22:48Z- handleDVL() - DVL msg epochMillisec: 1730863368.32 2024-11-06T03:22:48Z- handleDvl() - RDI_Pathfinder msg: 1730863368.32 sec; seqNo:1696; #beams:4 2024-11-06T03:22:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.67 , 24.33 , 26.78 , 28.54 2024-11-06T03:22:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.13 , -0.02 2024-11-06T03:22:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863368.33 epoch sec; seqNo:4971; 4061619.88 north; 594747.45 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:22:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863367.50 2024-11-06T03:22:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:48Z- waiting for 1730863371.50, time is 1730863368.33 2024-11-06T03:22:48Z- AHRS_M2 msg: 1730863368.69 epoch sec; seqNo:4675 2024-11-06T03:22:48Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:22:48Z- handleDepth()- Depth_Keller msg: 1730863368.69 epoch sec; seqNo:4972; depth 49.82 2024-11-06T03:22:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863368.71 epoch sec; seqNo:4972; 4061620.26 north; 594747.45 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:22:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863367.50 2024-11-06T03:22:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:48Z- waiting for 1730863371.50, time is 1730863368.71 2024-11-06T03:22:49Z- AHRS_M2 msg: 1730863369.09 epoch sec; seqNo:4676 2024-11-06T03:22:49Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:22:49Z- handleDepth()- Depth_Keller msg: 1730863369.11 epoch sec; seqNo:4973; depth 49.81 2024-11-06T03:22:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863369.14 epoch sec; seqNo:4973; 4061620.64 north; 594747.44 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:22:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863367.50 2024-11-06T03:22:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:49Z- waiting for 1730863371.50, time is 1730863369.14 2024-11-06T03:22:49Z- AHRS_M2 msg: 1730863369.50 epoch sec; seqNo:4677 2024-11-06T03:22:49Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:22:49Z- handleDepth()- Depth_Keller msg: 1730863369.50 epoch sec; seqNo:4974; depth 49.81 2024-11-06T03:22:49Z- handleDVL() - DVL msg epochMillisec: 1730863369.53 2024-11-06T03:22:49Z- handleDvl() - RDI_Pathfinder msg: 1730863369.53 sec; seqNo:1697; #beams:4 2024-11-06T03:22:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.08 , 24.60 , 18.22 , 28.68 2024-11-06T03:22:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.14 , 0.01 2024-11-06T03:22:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863369.54 epoch sec; seqNo:4974; 4061621.02 north; 594747.44 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:22:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863367.50 2024-11-06T03:22:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:49Z- waiting for 1730863371.50, time is 1730863369.54 2024-11-06T03:22:49Z- AHRS_M2 msg: 1730863369.90 epoch sec; seqNo:4678 2024-11-06T03:22:49Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:22:49Z- handleDepth()- Depth_Keller msg: 1730863369.91 epoch sec; seqNo:4975; depth 49.80 2024-11-06T03:22:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863369.92 epoch sec; seqNo:4975; 4061621.40 north; 594747.44 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:22:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863367.50 2024-11-06T03:22:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:50Z- waiting for 1730863371.50, time is 1730863369.92 2024-11-06T03:22:50Z- AHRS_M2 msg: 1730863370.30 epoch sec; seqNo:4679 2024-11-06T03:22:50Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:22:50Z- handleDepth()- Depth_Keller msg: 1730863370.31 epoch sec; seqNo:4976; depth 49.80 2024-11-06T03:22:50Z- handleDVL() - DVL msg epochMillisec: 1730863370.33 2024-11-06T03:22:50Z- handleDvl() - RDI_Pathfinder msg: 1730863370.33 sec; seqNo:1698; #beams:4 2024-11-06T03:22:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.12 , 24.03 , 17.72 , 28.90 2024-11-06T03:22:50Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.13 , 0.03 2024-11-06T03:22:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863370.35 epoch sec; seqNo:4976; 4061621.76 north; 594746.21 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:22:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863367.50 2024-11-06T03:22:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:50Z- waiting for 1730863371.50, time is 1730863370.35 2024-11-06T03:22:50Z- AHRS_M2 msg: 1730863370.71 epoch sec; seqNo:4680 2024-11-06T03:22:50Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:22:50Z- handleDepth()- Depth_Keller msg: 1730863370.71 epoch sec; seqNo:4977; depth 49.79 2024-11-06T03:22:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863370.73 epoch sec; seqNo:4977; 4061622.14 north; 594746.21 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:22:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863367.50 2024-11-06T03:22:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:50Z- waiting for 1730863371.50, time is 1730863370.73 2024-11-06T03:22:51Z- AHRS_M2 msg: 1730863371.11 epoch sec; seqNo:4681 2024-11-06T03:22:51Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:22:51Z- handleDepth()- Depth_Keller msg: 1730863371.12 epoch sec; seqNo:4978; depth 49.78 2024-11-06T03:22:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863371.13 epoch sec; seqNo:4978; 4061622.52 north; 594746.20 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:22:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863367.50 2024-11-06T03:22:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:51Z- waiting for 1730863371.50, time is 1730863371.13 2024-11-06T03:22:51Z- AHRS_M2 msg: 1730863371.52 epoch sec; seqNo:4682 2024-11-06T03:22:51Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.03 psi 2024-11-06T03:22:51Z- handleDepth()- Depth_Keller msg: 1730863371.52 epoch sec; seqNo:4979; depth 49.78 2024-11-06T03:22:51Z- handleDVL() - DVL msg epochMillisec: 1730863371.54 2024-11-06T03:22:51Z- handleDvl() - RDI_Pathfinder msg: 1730863371.54 sec; seqNo:1699; #beams:4 2024-11-06T03:22:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.35 , 23.91 , 26.76 , 29.02 2024-11-06T03:22:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.10 , 0.06 2024-11-06T03:22:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863371.55 epoch sec; seqNo:4979; 4061622.90 north; 594746.20 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:22:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863367.50 2024-11-06T03:22:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:51Z- waiting for 1730863371.50, time is 1730863371.55 2024-11-06T03:22:51Z- LcmTrn::updateTrn() >>>> pose time 1730863371.55, meas time 1730863371.54 <<<< 2024-11-06T03:22:51Z- LcmTrn::updateTrn() equating pose 1730863371.54 and meas times 1730863371.54 2024-11-06T03:22:51Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863371.54 2024-11-06T03:22:51Z- TerrainNav::motionUpdate() - currEstimate = (4061623, 594746.20, 49.78) 2024-11-06T03:22:51Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:22:51Z- TerrainNav:: SubcloudNIS is 1.23, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:22:51Z- TNavPF::Projecting Measurements 2024-11-06T03:22:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:22:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:22:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:22:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:22:51Z- TNavFilter::4 good beams. 2024-11-06T03:22:51Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:22:51Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:22:51Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:22:51Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:22:51Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:22:51Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:22:51Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:22:51Z- TerrainMapOctree: (vehN, vehE ) = (4061622.90, 594746.20). 2024-11-06T03:22:51Z- TerrainMapOctree: Min Distance = 127.16. 2024-11-06T03:22:51Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:22:51Z- Forcing Subcloud Comparison 2024-11-06T03:22:51Z- Weighting particles with subcloud comparison 2024-11-06T03:22:51Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:51Z- meanExpectedMeasDiff: 1.784310 delta_rms_squared: 1.009412 alpha: 0.980571 2024-11-06T03:22:51Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:51Z- meanExpectedMeasDiff: -0.852602 delta_rms_squared: 0.723287 alpha: 0.973092 2024-11-06T03:22:51Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:51Z- meanExpectedMeasDiff: -2.269762 delta_rms_squared: 0.929200 alpha: 0.978930 2024-11-06T03:22:51Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:51Z- meanExpectedMeasDiff: 0.936361 delta_rms_squared: 0.856458 alpha: 0.977181 2024-11-06T03:22:51Z- Subcloud NIS: 1.239027 2024-11-06T03:22:51Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:22:51Z- TNavPF:: sumWeights = 0.137847 2024-11-06T03:22:51Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:22:51Z- TNavPF::First Term of Map Covariance Matrix is 0.43 2024-11-06T03:22:51Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T03:22:51Z- TerrainNav::Resampling particle filter... 2024-11-06T03:22:51Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863371.54 sec, ping # 0. 2024-11-06T03:22:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:22:51Z- variances([0], [2], and psi_berg) = 1.069381 0.207809 0.000000 2024-11-06T03:22:51Z- publishEstimatesDecoder() - at 1730863371.54 sec 2024-11-06T03:22:51Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:22:51Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061653.72 594729.06 59.86 2024-11-06T03:22:51Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061653.41 594729.14 60.76 lat/lon: 36.70 -121.94 2024-11-06T03:22:51Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.07 0.21 0.30 0.00 2024-11-06T03:22:51Z- AHRS_M2 msg: 1730863371.92 epoch sec; seqNo:4683 2024-11-06T03:22:51Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.03 psi 2024-11-06T03:22:51Z- handleDepth()- Depth_Keller msg: 1730863371.92 epoch sec; seqNo:4980; depth 49.76 2024-11-06T03:22:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863371.95 epoch sec; seqNo:4980; 4061623.28 north; 594746.19 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:22:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863371.54 2024-11-06T03:22:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:52Z- waiting for 1730863375.54, time is 1730863371.95 2024-11-06T03:22:52Z- AHRS_M2 msg: 1730863372.32 epoch sec; seqNo:4684 2024-11-06T03:22:52Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:22:52Z- handleDepth()- Depth_Keller msg: 1730863372.33 epoch sec; seqNo:4981; depth 49.75 2024-11-06T03:22:52Z- handleDVL() - DVL msg epochMillisec: 1730863372.36 2024-11-06T03:22:52Z- handleDvl() - RDI_Pathfinder msg: 1730863372.36 sec; seqNo:1700; #beams:4 2024-11-06T03:22:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.62 , 24.19 , 29.22 , 29.09 2024-11-06T03:22:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , 0.04 2024-11-06T03:22:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863372.37 epoch sec; seqNo:4981; 4061623.66 north; 594746.19 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:22:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863371.54 2024-11-06T03:22:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:52Z- waiting for 1730863375.54, time is 1730863372.37 2024-11-06T03:22:52Z- AHRS_M2 msg: 1730863372.73 epoch sec; seqNo:4685 2024-11-06T03:22:52Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:22:52Z- handleDepth()- Depth_Keller msg: 1730863372.74 epoch sec; seqNo:4982; depth 49.75 2024-11-06T03:22:52Z- handleDVL() - DVL msg epochMillisec: 1730863372.74 2024-11-06T03:22:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:22:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:22:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:22:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:22:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:22:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:22:52Z- handleDvl() - RDI_Pathfinder msg: 1730863372.74 sec; seqNo:1701; #beams:0 2024-11-06T03:22:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:22:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , 0.04 2024-11-06T03:22:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863372.76 epoch sec; seqNo:4982; 4061624.04 north; 594746.19 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:22:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863371.54 2024-11-06T03:22:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:52Z- waiting for 1730863375.54, time is 1730863372.76 2024-11-06T03:22:53Z- AHRS_M2 msg: 1730863373.13 epoch sec; seqNo:4686 2024-11-06T03:22:53Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:22:53Z- handleDepth()- Depth_Keller msg: 1730863373.14 epoch sec; seqNo:4983; depth 49.73 2024-11-06T03:22:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863373.15 epoch sec; seqNo:4983; 4061624.42 north; 594746.18 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:22:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863371.54 2024-11-06T03:22:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:53Z- waiting for 1730863375.54, time is 1730863373.15 2024-11-06T03:22:53Z- AHRS_M2 msg: 1730863373.54 epoch sec; seqNo:4687 2024-11-06T03:22:53Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:22:53Z- handleDepth()- Depth_Keller msg: 1730863373.55 epoch sec; seqNo:4984; depth 49.72 2024-11-06T03:22:53Z- handleDVL() - DVL msg epochMillisec: 1730863373.58 2024-11-06T03:22:53Z- handleDvl() - RDI_Pathfinder msg: 1730863373.58 sec; seqNo:1702; #beams:4 2024-11-06T03:22:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.62 , 24.33 , 29.63 , 29.09 2024-11-06T03:22:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , 0.00 2024-11-06T03:22:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863373.59 epoch sec; seqNo:4984; 4061624.79 north; 594746.18 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:22:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863371.54 2024-11-06T03:22:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:53Z- waiting for 1730863375.54, time is 1730863373.59 2024-11-06T03:22:53Z- AHRS_M2 msg: 1730863373.94 epoch sec; seqNo:4688 2024-11-06T03:22:53Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:22:53Z- handleDepth()- Depth_Keller msg: 1730863373.94 epoch sec; seqNo:4985; depth 49.71 2024-11-06T03:22:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863373.96 epoch sec; seqNo:4985; 4061625.16 north; 594744.95 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:22:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863371.54 2024-11-06T03:22:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:54Z- waiting for 1730863375.54, time is 1730863373.96 2024-11-06T03:22:54Z- AHRS_M2 msg: 1730863374.34 epoch sec; seqNo:4689 2024-11-06T03:22:54Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:22:54Z- handleDepth()- Depth_Keller msg: 1730863374.35 epoch sec; seqNo:4986; depth 49.71 2024-11-06T03:22:54Z- handleDVL() - DVL msg epochMillisec: 1730863374.37 2024-11-06T03:22:54Z- handleDvl() - RDI_Pathfinder msg: 1730863374.37 sec; seqNo:1703; #beams:4 2024-11-06T03:22:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.62 , 24.33 , 29.36 , 29.22 2024-11-06T03:22:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , -0.03 2024-11-06T03:22:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863374.38 epoch sec; seqNo:4986; 4061625.54 north; 594744.95 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:22:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863371.54 2024-11-06T03:22:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:54Z- waiting for 1730863375.54, time is 1730863374.38 2024-11-06T03:22:54Z- AHRS_M2 msg: 1730863374.75 epoch sec; seqNo:4690 2024-11-06T03:22:54Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:22:54Z- handleDepth()- Depth_Keller msg: 1730863374.77 epoch sec; seqNo:4987; depth 49.68 2024-11-06T03:22:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863374.78 epoch sec; seqNo:4987; 4061625.54 north; 594744.95 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:22:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863371.54 2024-11-06T03:22:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:54Z- waiting for 1730863375.54, time is 1730863374.78 2024-11-06T03:22:55Z- AHRS_M2 msg: 1730863375.15 epoch sec; seqNo:4691 2024-11-06T03:22:55Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:22:55Z- handleDepth()- Depth_Keller msg: 1730863375.16 epoch sec; seqNo:4988; depth 49.68 2024-11-06T03:22:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863375.17 epoch sec; seqNo:4988; 4061625.92 north; 594744.94 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:22:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863371.54 2024-11-06T03:22:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:55Z- waiting for 1730863375.54, time is 1730863375.17 2024-11-06T03:22:55Z- AHRS_M2 msg: 1730863375.56 epoch sec; seqNo:4692 2024-11-06T03:22:55Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:22:55Z- handleDepth()- Depth_Keller msg: 1730863375.56 epoch sec; seqNo:4989; depth 49.66 2024-11-06T03:22:55Z- handleDVL() - DVL msg epochMillisec: 1730863375.59 2024-11-06T03:22:55Z- handleDvl() - RDI_Pathfinder msg: 1730863375.59 sec; seqNo:1704; #beams:4 2024-11-06T03:22:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.49 , 24.33 , 29.36 , 29.36 2024-11-06T03:22:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.08 , -0.03 2024-11-06T03:22:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863375.62 epoch sec; seqNo:4989; 4061626.30 north; 594744.94 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:22:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863371.54 2024-11-06T03:22:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:55Z- waiting for 1730863375.54, time is 1730863375.62 2024-11-06T03:22:55Z- LcmTrn::updateTrn() >>>> pose time 1730863375.62, meas time 1730863375.59 <<<< 2024-11-06T03:22:55Z- LcmTrn::updateTrn() equating pose 1730863375.59 and meas times 1730863375.59 2024-11-06T03:22:55Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863375.59 2024-11-06T03:22:55Z- TerrainNav::motionUpdate() - currEstimate = (4061626, 594744.94, 49.66) 2024-11-06T03:22:55Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:22:55Z- TerrainNav:: SubcloudNIS is 1.24, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:22:55Z- TNavPF::Projecting Measurements 2024-11-06T03:22:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:22:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:22:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:22:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:22:55Z- TNavFilter::4 good beams. 2024-11-06T03:22:55Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:22:55Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:22:55Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:22:55Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:22:55Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:22:55Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:22:55Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:22:55Z- TerrainMapOctree: (vehN, vehE ) = (4061626.30, 594744.94). 2024-11-06T03:22:55Z- TerrainMapOctree: Min Distance = 123.81. 2024-11-06T03:22:55Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:22:55Z- Forcing Subcloud Comparison 2024-11-06T03:22:55Z- Weighting particles with subcloud comparison 2024-11-06T03:22:55Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:55Z- meanExpectedMeasDiff: 2.034588 delta_rms_squared: 0.784874 alpha: 0.975151 2024-11-06T03:22:55Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:55Z- meanExpectedMeasDiff: -0.559278 delta_rms_squared: 0.625256 alpha: 0.969005 2024-11-06T03:22:55Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:55Z- meanExpectedMeasDiff: 0.740737 delta_rms_squared: 0.995861 alpha: 0.980312 2024-11-06T03:22:55Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:55Z- meanExpectedMeasDiff: 1.090915 delta_rms_squared: 0.726472 alpha: 0.973207 2024-11-06T03:22:55Z- Subcloud NIS: 1.208096 2024-11-06T03:22:55Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:22:55Z- TNavPF:: sumWeights = 0.332830 2024-11-06T03:22:55Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:22:55Z- TNavPF::First Term of Map Covariance Matrix is 0.56 2024-11-06T03:22:55Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.06 2024-11-06T03:22:55Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863375.59 sec, ping # 0. 2024-11-06T03:22:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:22:55Z- variances([0], [2], and psi_berg) = 1.128127 0.216016 0.000000 2024-11-06T03:22:55Z- publishEstimatesDecoder() - at 1730863375.59 sec 2024-11-06T03:22:55Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:22:55Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061657.73 594725.95 60.17 2024-11-06T03:22:55Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061656.83 594727.86 60.31 lat/lon: 36.70 -121.94 2024-11-06T03:22:55Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.13 0.22 0.30 0.00 2024-11-06T03:22:55Z- AHRS_M2 msg: 1730863375.96 epoch sec; seqNo:4693 2024-11-06T03:22:55Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.02 psi 2024-11-06T03:22:55Z- handleDepth()- Depth_Keller msg: 1730863375.97 epoch sec; seqNo:4990; depth 49.64 2024-11-06T03:22:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863375.99 epoch sec; seqNo:4990; 4061626.68 north; 594744.94 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:22:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863375.59 2024-11-06T03:22:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:56Z- waiting for 1730863379.59, time is 1730863375.99 2024-11-06T03:22:56Z- AHRS_M2 msg: 1730863376.36 epoch sec; seqNo:4694 2024-11-06T03:22:56Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.02 psi 2024-11-06T03:22:56Z- handleDepth()- Depth_Keller msg: 1730863376.38 epoch sec; seqNo:4991; depth 49.64 2024-11-06T03:22:56Z- handleDVL() - DVL msg epochMillisec: 1730863376.41 2024-11-06T03:22:56Z- handleDvl() - RDI_Pathfinder msg: 1730863376.41 sec; seqNo:1705; #beams:4 2024-11-06T03:22:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.35 , 24.06 , 29.50 , 29.50 2024-11-06T03:22:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.09 , -0.02 2024-11-06T03:22:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863376.42 epoch sec; seqNo:4991; 4061627.05 north; 594744.93 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:22:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863375.59 2024-11-06T03:22:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:56Z- waiting for 1730863379.59, time is 1730863376.42 2024-11-06T03:22:56Z- AHRS_M2 msg: 1730863376.77 epoch sec; seqNo:4695 2024-11-06T03:22:56Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.02 psi 2024-11-06T03:22:56Z- handleDepth()- Depth_Keller msg: 1730863376.78 epoch sec; seqNo:4992; depth 49.62 2024-11-06T03:22:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863376.81 epoch sec; seqNo:4992; 4061627.43 north; 594744.93 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:22:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863375.59 2024-11-06T03:22:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:56Z- waiting for 1730863379.59, time is 1730863376.81 2024-11-06T03:22:57Z- AHRS_M2 msg: 1730863377.17 epoch sec; seqNo:4696 2024-11-06T03:22:57Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.02 psi 2024-11-06T03:22:57Z- handleDepth()- Depth_Keller msg: 1730863377.18 epoch sec; seqNo:4993; depth 49.63 2024-11-06T03:22:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863377.20 epoch sec; seqNo:4993; 4061627.81 north; 594744.92 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:22:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863375.59 2024-11-06T03:22:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:57Z- waiting for 1730863379.59, time is 1730863377.20 2024-11-06T03:22:57Z- AHRS_M2 msg: 1730863377.58 epoch sec; seqNo:4697 2024-11-06T03:22:57Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.02 psi 2024-11-06T03:22:57Z- handleDepth()- Depth_Keller msg: 1730863377.58 epoch sec; seqNo:4994; depth 49.63 2024-11-06T03:22:57Z- handleDVL() - DVL msg epochMillisec: 1730863377.60 2024-11-06T03:22:57Z- handleDvl() - RDI_Pathfinder msg: 1730863377.60 sec; seqNo:1706; #beams:4 2024-11-06T03:22:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.49 , 23.92 , 29.50 , 29.36 2024-11-06T03:22:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.11 , 0.01 2024-11-06T03:22:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863377.64 epoch sec; seqNo:4994; 4061628.18 north; 594743.70 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:22:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863375.59 2024-11-06T03:22:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:57Z- waiting for 1730863379.59, time is 1730863377.64 2024-11-06T03:22:57Z- AHRS_M2 msg: 1730863377.98 epoch sec; seqNo:4698 2024-11-06T03:22:57Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.02 psi 2024-11-06T03:22:57Z- handleDepth()- Depth_Keller msg: 1730863377.99 epoch sec; seqNo:4995; depth 49.62 2024-11-06T03:22:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863378.00 epoch sec; seqNo:4995; 4061628.56 north; 594743.69 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:22:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863375.59 2024-11-06T03:22:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:58Z- waiting for 1730863379.59, time is 1730863378.00 2024-11-06T03:22:58Z- AHRS_M2 msg: 1730863378.38 epoch sec; seqNo:4699 2024-11-06T03:22:58Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.01 psi 2024-11-06T03:22:58Z- handleDepth()- Depth_Keller msg: 1730863378.39 epoch sec; seqNo:4996; depth 49.62 2024-11-06T03:22:58Z- handleDVL() - DVL msg epochMillisec: 1730863378.41 2024-11-06T03:22:58Z- handleDvl() - RDI_Pathfinder msg: 1730863378.41 sec; seqNo:1707; #beams:4 2024-11-06T03:22:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.35 , 24.06 , 29.22 , 29.22 2024-11-06T03:22:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.09 , 0.03 2024-11-06T03:22:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863378.43 epoch sec; seqNo:4996; 4061628.94 north; 594743.69 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:22:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:22:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863375.59 2024-11-06T03:22:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:58Z- waiting for 1730863379.59, time is 1730863378.43 2024-11-06T03:22:58Z- AHRS_M2 msg: 1730863378.79 epoch sec; seqNo:4700 2024-11-06T03:22:58Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.01 psi 2024-11-06T03:22:58Z- handleDepth()- Depth_Keller msg: 1730863378.79 epoch sec; seqNo:4997; depth 49.63 2024-11-06T03:22:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863378.81 epoch sec; seqNo:4997; 4061629.31 north; 594743.69 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:22:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863375.59 2024-11-06T03:22:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:58Z- waiting for 1730863379.59, time is 1730863378.81 2024-11-06T03:22:59Z- AHRS_M2 msg: 1730863379.19 epoch sec; seqNo:4701 2024-11-06T03:22:59Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.01 psi 2024-11-06T03:22:59Z- handleDepth()- Depth_Keller msg: 1730863379.21 epoch sec; seqNo:4998; depth 49.64 2024-11-06T03:22:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863379.22 epoch sec; seqNo:4998; 4061629.69 north; 594743.68 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:22:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863375.59 2024-11-06T03:22:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:59Z- waiting for 1730863379.59, time is 1730863379.22 2024-11-06T03:22:59Z- handleDepth()- Depth_Keller msg: 1730863379.63 epoch sec; seqNo:4999; depth 49.63 2024-11-06T03:22:59Z- handleDVL() - DVL msg epochMillisec: 1730863379.66 2024-11-06T03:22:59Z- handleDvl() - RDI_Pathfinder msg: 1730863379.66 sec; seqNo:1708; #beams:4 2024-11-06T03:22:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.76 , 24.19 , 29.63 , 29.09 2024-11-06T03:22:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.09 , 0.05 2024-11-06T03:22:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863379.67 epoch sec; seqNo:4999; 4061630.07 north; 594743.68 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:22:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:22:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863375.59 2024-11-06T03:22:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:22:59Z- waiting for 1730863379.59, time is 1730863379.67 2024-11-06T03:22:59Z- LcmTrn::updateTrn() >>>> pose time 1730863379.67, meas time 1730863379.66 <<<< 2024-11-06T03:22:59Z- LcmTrn::updateTrn() equating pose 1730863379.66 and meas times 1730863379.66 2024-11-06T03:22:59Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863379.66 2024-11-06T03:22:59Z- TerrainNav::motionUpdate() - currEstimate = (4061630, 594743.68, 49.63) 2024-11-06T03:22:59Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:22:59Z- TerrainNav:: SubcloudNIS is 1.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:22:59Z- TNavPF::Projecting Measurements 2024-11-06T03:22:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:22:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:22:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:22:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:22:59Z- TNavFilter::4 good beams. 2024-11-06T03:22:59Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:22:59Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:22:59Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:22:59Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:22:59Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:22:59Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:22:59Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:22:59Z- TerrainMapOctree: (vehN, vehE ) = (4061630.07, 594743.68). 2024-11-06T03:22:59Z- TerrainMapOctree: Min Distance = 120.09. 2024-11-06T03:22:59Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:22:59Z- Forcing Subcloud Comparison 2024-11-06T03:22:59Z- Weighting particles with subcloud comparison 2024-11-06T03:22:59Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:59Z- meanExpectedMeasDiff: 1.550782 delta_rms_squared: 0.716418 alpha: 0.972842 2024-11-06T03:22:59Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:59Z- meanExpectedMeasDiff: -1.061592 delta_rms_squared: 0.630752 alpha: 0.969266 2024-11-06T03:22:59Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:59Z- meanExpectedMeasDiff: 1.133362 delta_rms_squared: 1.056376 alpha: 0.981419 2024-11-06T03:22:59Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:22:59Z- meanExpectedMeasDiff: -0.782761 delta_rms_squared: 0.699897 alpha: 0.972218 2024-11-06T03:22:59Z- Subcloud NIS: 1.146723 2024-11-06T03:22:59Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:22:59Z- TNavPF:: sumWeights = 0.297291 2024-11-06T03:22:59Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:22:59Z- TNavPF::First Term of Map Covariance Matrix is 0.46 2024-11-06T03:22:59Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.05 2024-11-06T03:22:59Z- TerrainNav::Resampling particle filter... 2024-11-06T03:22:59Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863379.66 sec, ping # 0. 2024-11-06T03:22:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:22:59Z- variances([0], [2], and psi_berg) = 1.183394 0.223285 0.000000 2024-11-06T03:22:59Z- publishEstimatesDecoder() - at 1730863379.66 sec 2024-11-06T03:22:59Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:22:59Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061660.76 594726.48 59.63 2024-11-06T03:22:59Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061660.63 594726.57 60.23 lat/lon: 36.70 -121.94 2024-11-06T03:22:59Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.18 0.22 0.32 0.00 2024-11-06T03:23:00Z- AHRS_M2 msg: 1730863380.03 epoch sec; seqNo:4702 2024-11-06T03:23:00Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.01 psi 2024-11-06T03:23:00Z- handleDepth()- Depth_Keller msg: 1730863380.05 epoch sec; seqNo:5000; depth 49.65 2024-11-06T03:23:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863380.07 epoch sec; seqNo:5000; 4061630.45 north; 594743.67 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:23:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863379.66 2024-11-06T03:23:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:00Z- waiting for 1730863383.66, time is 1730863380.07 2024-11-06T03:23:00Z- AHRS_M2 msg: 1730863380.40 epoch sec; seqNo:4703 2024-11-06T03:23:00Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 6.01 psi 2024-11-06T03:23:00Z- handleDepth()- Depth_Keller msg: 1730863380.41 epoch sec; seqNo:5001; depth 49.65 2024-11-06T03:23:00Z- handleDVL() - DVL msg epochMillisec: 1730863380.44 2024-11-06T03:23:00Z- handleDvl() - RDI_Pathfinder msg: 1730863380.44 sec; seqNo:1709; #beams:4 2024-11-06T03:23:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.03 , 24.47 , 29.50 , 29.50 2024-11-06T03:23:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , 0.05 2024-11-06T03:23:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863380.45 epoch sec; seqNo:5001; 4061630.83 north; 594743.67 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:23:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863379.66 2024-11-06T03:23:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:00Z- waiting for 1730863383.66, time is 1730863380.45 2024-11-06T03:23:00Z- handleDepth()- Depth_Keller msg: 1730863380.84 epoch sec; seqNo:5002; depth 49.65 2024-11-06T03:23:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863380.85 epoch sec; seqNo:5002; 4061631.21 north; 594743.67 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:23:00Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:23:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863379.66 2024-11-06T03:23:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:00Z- waiting for 1730863383.66, time is 1730863380.85 2024-11-06T03:23:01Z- AHRS_M2 msg: 1730863381.22 epoch sec; seqNo:4704 2024-11-06T03:23:01Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.01 psi 2024-11-06T03:23:01Z- handleDepth()- Depth_Keller msg: 1730863381.23 epoch sec; seqNo:5003; depth 49.67 2024-11-06T03:23:01Z- handleDVL() - DVL msg epochMillisec: 1730863381.23 2024-11-06T03:23:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:23:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:23:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:23:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:23:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:23:01Z- handleDvl() - RDI_Pathfinder msg: 1730863381.23 sec; seqNo:1710; #beams:0 2024-11-06T03:23:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:23:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , 0.05 2024-11-06T03:23:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863381.24 epoch sec; seqNo:5003; 4061631.57 north; 594742.44 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:23:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863379.66 2024-11-06T03:23:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:01Z- waiting for 1730863383.66, time is 1730863381.24 2024-11-06T03:23:01Z- AHRS_M2 msg: 1730863381.62 epoch sec; seqNo:4705 2024-11-06T03:23:01Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.01 psi 2024-11-06T03:23:01Z- handleDepth()- Depth_Keller msg: 1730863381.63 epoch sec; seqNo:5004; depth 49.66 2024-11-06T03:23:01Z- handleDVL() - DVL msg epochMillisec: 1730863381.65 2024-11-06T03:23:01Z- handleDvl() - RDI_Pathfinder msg: 1730863381.65 sec; seqNo:1711; #beams:4 2024-11-06T03:23:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.35 , 24.47 , 29.36 , 29.22 2024-11-06T03:23:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , 0.05 2024-11-06T03:23:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863381.69 epoch sec; seqNo:5004; 4061631.95 north; 594742.44 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:23:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863379.66 2024-11-06T03:23:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:01Z- waiting for 1730863383.66, time is 1730863381.69 2024-11-06T03:23:02Z- handleDepth()- Depth_Keller msg: 1730863382.05 epoch sec; seqNo:5005; depth 49.66 2024-11-06T03:23:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863382.07 epoch sec; seqNo:5005; 4061632.33 north; 594742.43 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:23:02Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:23:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863379.66 2024-11-06T03:23:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:02Z- waiting for 1730863383.66, time is 1730863382.07 2024-11-06T03:23:02Z- AHRS_M2 msg: 1730863382.42 epoch sec; seqNo:4706 2024-11-06T03:23:02Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.02 psi 2024-11-06T03:23:02Z- handleDepth()- Depth_Keller msg: 1730863382.43 epoch sec; seqNo:5006; depth 49.66 2024-11-06T03:23:02Z- handleDVL() - DVL msg epochMillisec: 1730863382.46 2024-11-06T03:23:02Z- handleDvl() - RDI_Pathfinder msg: 1730863382.46 sec; seqNo:1712; #beams:4 2024-11-06T03:23:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.49 , 24.33 , 29.63 , 29.36 2024-11-06T03:23:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.04 2024-11-06T03:23:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863382.48 epoch sec; seqNo:5006; 4061632.33 north; 594742.43 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:23:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863379.66 2024-11-06T03:23:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:02Z- waiting for 1730863383.66, time is 1730863382.48 2024-11-06T03:23:02Z- AHRS_M2 msg: 1730863382.83 epoch sec; seqNo:4707 2024-11-06T03:23:02Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.02 psi 2024-11-06T03:23:02Z- handleDepth()- Depth_Keller msg: 1730863382.84 epoch sec; seqNo:5007; depth 49.67 2024-11-06T03:23:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863382.85 epoch sec; seqNo:5007; 4061632.71 north; 594742.43 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:23:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863379.66 2024-11-06T03:23:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:02Z- waiting for 1730863383.66, time is 1730863382.85 2024-11-06T03:23:03Z- AHRS_M2 msg: 1730863383.24 epoch sec; seqNo:4708 2024-11-06T03:23:03Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.02 psi 2024-11-06T03:23:03Z- handleDepth()- Depth_Keller msg: 1730863383.25 epoch sec; seqNo:5008; depth 49.66 2024-11-06T03:23:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863383.26 epoch sec; seqNo:5008; 4061633.09 north; 594742.42 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:23:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863379.66 2024-11-06T03:23:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:03Z- waiting for 1730863383.66, time is 1730863383.26 2024-11-06T03:23:03Z- AHRS_M2 msg: 1730863383.64 epoch sec; seqNo:4709 2024-11-06T03:23:03Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.02 psi 2024-11-06T03:23:03Z- handleDepth()- Depth_Keller msg: 1730863383.65 epoch sec; seqNo:5009; depth 49.65 2024-11-06T03:23:03Z- handleDVL() - DVL msg epochMillisec: 1730863383.68 2024-11-06T03:23:03Z- handleDvl() - RDI_Pathfinder msg: 1730863383.68 sec; seqNo:1713; #beams:4 2024-11-06T03:23:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.76 , 24.19 , 29.36 , 29.50 2024-11-06T03:23:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.02 2024-11-06T03:23:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863383.69 epoch sec; seqNo:5009; 4061633.47 north; 594742.42 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:23:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863379.66 2024-11-06T03:23:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:03Z- waiting for 1730863383.66, time is 1730863383.69 2024-11-06T03:23:03Z- LcmTrn::updateTrn() >>>> pose time 1730863383.69, meas time 1730863383.68 <<<< 2024-11-06T03:23:03Z- LcmTrn::updateTrn() equating pose 1730863383.68 and meas times 1730863383.68 2024-11-06T03:23:03Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863383.68 2024-11-06T03:23:03Z- TerrainNav::motionUpdate() - currEstimate = (4061633, 594742.42, 49.65) 2024-11-06T03:23:03Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:23:03Z- TerrainNav:: SubcloudNIS is 1.15, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:23:03Z- TNavPF::Projecting Measurements 2024-11-06T03:23:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:23:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:23:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:23:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:23:03Z- TNavFilter::4 good beams. 2024-11-06T03:23:03Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:23:03Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:23:03Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:23:03Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:23:03Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:23:03Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:23:03Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:23:03Z- TerrainMapOctree: (vehN, vehE ) = (4061633.47, 594742.42). 2024-11-06T03:23:03Z- TerrainMapOctree: Min Distance = 116.78. 2024-11-06T03:23:03Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:23:03Z- Forcing Subcloud Comparison 2024-11-06T03:23:03Z- Weighting particles with subcloud comparison 2024-11-06T03:23:03Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:03Z- meanExpectedMeasDiff: 0.649768 delta_rms_squared: 0.640321 alpha: 0.969712 2024-11-06T03:23:03Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:03Z- meanExpectedMeasDiff: -0.978642 delta_rms_squared: 0.638114 alpha: 0.969610 2024-11-06T03:23:03Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:03Z- meanExpectedMeasDiff: 0.489473 delta_rms_squared: 1.052585 alpha: 0.981353 2024-11-06T03:23:03Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:03Z- meanExpectedMeasDiff: -0.954457 delta_rms_squared: 0.795014 alpha: 0.975461 2024-11-06T03:23:03Z- Subcloud NIS: 1.079490 2024-11-06T03:23:03Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:23:03Z- TNavPF:: sumWeights = 0.475703 2024-11-06T03:23:03Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:23:03Z- TNavPF::First Term of Map Covariance Matrix is 0.25 2024-11-06T03:23:03Z- TNavPF::Calculated NIS Value : 0.31 numBeams normalized NIS: 0.08 2024-11-06T03:23:03Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863383.68 sec, ping # 0. 2024-11-06T03:23:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:23:03Z- variances([0], [2], and psi_berg) = 1.179623 0.212834 0.000000 2024-11-06T03:23:03Z- publishEstimatesDecoder() - at 1730863383.68 sec 2024-11-06T03:23:03Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:23:03Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061663.75 594725.23 60.58 2024-11-06T03:23:03Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061664.17 594725.26 60.38 lat/lon: 36.70 -121.94 2024-11-06T03:23:03Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.18 0.21 0.30 0.00 2024-11-06T03:23:04Z- AHRS_M2 msg: 1730863384.05 epoch sec; seqNo:4710 2024-11-06T03:23:04Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.02 psi 2024-11-06T03:23:04Z- handleDepth()- Depth_Keller msg: 1730863384.05 epoch sec; seqNo:5010; depth 49.66 2024-11-06T03:23:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863384.07 epoch sec; seqNo:5010; 4061633.85 north; 594742.42 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:23:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863383.68 2024-11-06T03:23:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:04Z- waiting for 1730863387.68, time is 1730863384.07 2024-11-06T03:23:04Z- AHRS_M2 msg: 1730863384.44 epoch sec; seqNo:4711 2024-11-06T03:23:04Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.02 psi 2024-11-06T03:23:04Z- handleDepth()- Depth_Keller msg: 1730863384.45 epoch sec; seqNo:5011; depth 49.66 2024-11-06T03:23:04Z- handleDVL() - DVL msg epochMillisec: 1730863384.48 2024-11-06T03:23:04Z- handleDvl() - RDI_Pathfinder msg: 1730863384.48 sec; seqNo:1714; #beams:4 2024-11-06T03:23:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.03 , 24.60 , 29.36 , 29.36 2024-11-06T03:23:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , -0.00 2024-11-06T03:23:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863384.49 epoch sec; seqNo:5011; 4061634.23 north; 594742.41 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:23:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863383.68 2024-11-06T03:23:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:04Z- waiting for 1730863387.68, time is 1730863384.49 2024-11-06T03:23:04Z- AHRS_M2 msg: 1730863384.85 epoch sec; seqNo:4712 2024-11-06T03:23:04Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.02 psi 2024-11-06T03:23:04Z- handleDepth()- Depth_Keller msg: 1730863384.86 epoch sec; seqNo:5012; depth 49.65 2024-11-06T03:23:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863384.87 epoch sec; seqNo:5012; 4061634.60 north; 594742.41 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:23:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863383.68 2024-11-06T03:23:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:05Z- waiting for 1730863387.68, time is 1730863384.87 2024-11-06T03:23:05Z- AHRS_M2 msg: 1730863385.25 epoch sec; seqNo:4713 2024-11-06T03:23:05Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.02 psi 2024-11-06T03:23:05Z- handleDepth()- Depth_Keller msg: 1730863385.26 epoch sec; seqNo:5013; depth 49.64 2024-11-06T03:23:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863385.27 epoch sec; seqNo:5013; 4061634.98 north; 594742.40 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:23:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863383.68 2024-11-06T03:23:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:05Z- waiting for 1730863387.68, time is 1730863385.27 2024-11-06T03:23:05Z- AHRS_M2 msg: 1730863385.66 epoch sec; seqNo:4714 2024-11-06T03:23:05Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.02 psi 2024-11-06T03:23:05Z- handleDepth()- Depth_Keller msg: 1730863385.66 epoch sec; seqNo:5014; depth 49.63 2024-11-06T03:23:05Z- handleDVL() - DVL msg epochMillisec: 1730863385.68 2024-11-06T03:23:05Z- handleDvl() - RDI_Pathfinder msg: 1730863385.68 sec; seqNo:1715; #beams:4 2024-11-06T03:23:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.62 , 24.87 , 29.22 , 28.95 2024-11-06T03:23:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , -0.03 2024-11-06T03:23:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863385.69 epoch sec; seqNo:5014; 4061635.35 north; 594741.18 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:23:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863383.68 2024-11-06T03:23:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:05Z- waiting for 1730863387.68, time is 1730863385.69 2024-11-06T03:23:06Z- AHRS_M2 msg: 1730863386.06 epoch sec; seqNo:4715 2024-11-06T03:23:06Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.02 psi 2024-11-06T03:23:06Z- handleDepth()- Depth_Keller msg: 1730863386.07 epoch sec; seqNo:5015; depth 49.63 2024-11-06T03:23:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863386.09 epoch sec; seqNo:5015; 4061635.35 north; 594741.18 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:23:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863383.68 2024-11-06T03:23:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:06Z- waiting for 1730863387.68, time is 1730863386.09 2024-11-06T03:23:06Z- AHRS_M2 msg: 1730863386.46 epoch sec; seqNo:4716 2024-11-06T03:23:06Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.02 psi 2024-11-06T03:23:06Z- handleDepth()- Depth_Keller msg: 1730863386.47 epoch sec; seqNo:5016; depth 49.63 2024-11-06T03:23:06Z- handleDVL() - DVL msg epochMillisec: 1730863386.49 2024-11-06T03:23:06Z- handleDvl() - RDI_Pathfinder msg: 1730863386.49 sec; seqNo:1716; #beams:4 2024-11-06T03:23:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.17 , 25.01 , 29.22 , 29.22 2024-11-06T03:23:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.09 , -0.05 2024-11-06T03:23:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863386.51 epoch sec; seqNo:5016; 4061635.73 north; 594741.17 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:23:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863383.68 2024-11-06T03:23:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:06Z- waiting for 1730863387.68, time is 1730863386.51 2024-11-06T03:23:06Z- AHRS_M2 msg: 1730863386.88 epoch sec; seqNo:4717 2024-11-06T03:23:06Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.02 psi 2024-11-06T03:23:06Z- handleDepth()- Depth_Keller msg: 1730863386.89 epoch sec; seqNo:5017; depth 49.62 2024-11-06T03:23:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863386.90 epoch sec; seqNo:5017; 4061636.11 north; 594741.17 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:23:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863383.68 2024-11-06T03:23:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:07Z- waiting for 1730863387.68, time is 1730863386.90 2024-11-06T03:23:07Z- AHRS_M2 msg: 1730863387.27 epoch sec; seqNo:4718 2024-11-06T03:23:07Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.02 psi 2024-11-06T03:23:07Z- handleDepth()- Depth_Keller msg: 1730863387.28 epoch sec; seqNo:5018; depth 49.62 2024-11-06T03:23:07Z- handleDVL() - DVL msg epochMillisec: 1730863387.28 2024-11-06T03:23:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:23:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:23:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:23:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:23:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:23:07Z- handleDvl() - RDI_Pathfinder msg: 1730863387.28 sec; seqNo:1717; #beams:0 2024-11-06T03:23:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:23:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.09 , -0.05 2024-11-06T03:23:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863387.31 epoch sec; seqNo:5018; 4061636.49 north; 594741.17 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:23:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863383.68 2024-11-06T03:23:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:07Z- waiting for 1730863387.68, time is 1730863387.31 2024-11-06T03:23:07Z- AHRS_M2 msg: 1730863387.68 epoch sec; seqNo:4719 2024-11-06T03:23:07Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.02 psi 2024-11-06T03:23:07Z- handleDepth()- Depth_Keller msg: 1730863387.68 epoch sec; seqNo:5019; depth 49.62 2024-11-06T03:23:07Z- handleDVL() - DVL msg epochMillisec: 1730863387.70 2024-11-06T03:23:07Z- handleDvl() - RDI_Pathfinder msg: 1730863387.70 sec; seqNo:1718; #beams:4 2024-11-06T03:23:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.21 , 24.47 , 29.09 , 28.82 2024-11-06T03:23:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.10 , -0.06 2024-11-06T03:23:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863387.72 epoch sec; seqNo:5019; 4061636.86 north; 594741.16 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:23:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863383.68 2024-11-06T03:23:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:07Z- waiting for 1730863387.68, time is 1730863387.72 2024-11-06T03:23:07Z- LcmTrn::updateTrn() >>>> pose time 1730863387.72, meas time 1730863387.70 <<<< 2024-11-06T03:23:07Z- LcmTrn::updateTrn() equating pose 1730863387.70 and meas times 1730863387.70 2024-11-06T03:23:07Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863387.70 2024-11-06T03:23:07Z- TerrainNav::motionUpdate() - currEstimate = (4061637, 594741.16, 49.62) 2024-11-06T03:23:07Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:23:07Z- TerrainNav:: SubcloudNIS is 1.08, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:23:07Z- TNavPF::Projecting Measurements 2024-11-06T03:23:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:23:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:23:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:23:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:23:07Z- TNavFilter::4 good beams. 2024-11-06T03:23:07Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:23:07Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:23:07Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:23:07Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:23:07Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:23:07Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:23:07Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:23:07Z- TerrainMapOctree: (vehN, vehE ) = (4061636.86, 594741.16). 2024-11-06T03:23:07Z- TerrainMapOctree: Min Distance = 113.48. 2024-11-06T03:23:07Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:23:07Z- Forcing Subcloud Comparison 2024-11-06T03:23:07Z- Weighting particles with subcloud comparison 2024-11-06T03:23:07Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:07Z- meanExpectedMeasDiff: -0.219442 delta_rms_squared: 0.871341 alpha: 0.977562 2024-11-06T03:23:07Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:07Z- meanExpectedMeasDiff: -0.650641 delta_rms_squared: 0.615735 alpha: 0.968540 2024-11-06T03:23:07Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:07Z- meanExpectedMeasDiff: 0.349764 delta_rms_squared: 0.686027 alpha: 0.971672 2024-11-06T03:23:07Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:07Z- meanExpectedMeasDiff: -1.758664 delta_rms_squared: 0.776644 alpha: 0.974895 2024-11-06T03:23:07Z- Subcloud NIS: 1.014944 2024-11-06T03:23:07Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:23:07Z- TNavPF:: sumWeights = 0.440522 2024-11-06T03:23:07Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:23:07Z- TNavPF::First Term of Map Covariance Matrix is 0.20 2024-11-06T03:23:07Z- TNavPF::Calculated NIS Value : 0.30 numBeams normalized NIS: 0.07 2024-11-06T03:23:07Z- TerrainNav::Resampling particle filter... 2024-11-06T03:23:08Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863387.70 sec, ping # 0. 2024-11-06T03:23:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:23:08Z- variances([0], [2], and psi_berg) = 1.184156 0.210621 0.000000 2024-11-06T03:23:08Z- publishEstimatesDecoder() - at 1730863387.70 sec 2024-11-06T03:23:08Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:23:08Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061667.85 594724.11 60.07 2024-11-06T03:23:08Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061667.60 594723.98 60.58 lat/lon: 36.70 -121.94 2024-11-06T03:23:08Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.18 0.21 0.28 0.00 2024-11-06T03:23:08Z- AHRS_M2 msg: 1730863388.08 epoch sec; seqNo:4720 2024-11-06T03:23:08Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.02 psi 2024-11-06T03:23:08Z- handleDepth()- Depth_Keller msg: 1730863388.10 epoch sec; seqNo:5020; depth 49.61 2024-11-06T03:23:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863388.14 epoch sec; seqNo:5020; 4061637.24 north; 594741.16 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:23:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863387.70 2024-11-06T03:23:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:08Z- waiting for 1730863391.70, time is 1730863388.14 2024-11-06T03:23:08Z- AHRS_M2 msg: 1730863388.51 epoch sec; seqNo:4721 2024-11-06T03:23:08Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.02 psi 2024-11-06T03:23:08Z- handleDepth()- Depth_Keller msg: 1730863388.52 epoch sec; seqNo:5021; depth 49.62 2024-11-06T03:23:08Z- handleDVL() - DVL msg epochMillisec: 1730863388.54 2024-11-06T03:23:08Z- handleDvl() - RDI_Pathfinder msg: 1730863388.54 sec; seqNo:1719; #beams:4 2024-11-06T03:23:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.67 , 24.47 , 29.36 , 28.82 2024-11-06T03:23:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.13 , -0.05 2024-11-06T03:23:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863388.56 epoch sec; seqNo:5021; 4061637.62 north; 594741.15 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:23:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863387.70 2024-11-06T03:23:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:08Z- waiting for 1730863391.70, time is 1730863388.56 2024-11-06T03:23:08Z- AHRS_M2 msg: 1730863388.89 epoch sec; seqNo:4722 2024-11-06T03:23:08Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.01 psi 2024-11-06T03:23:08Z- handleDepth()- Depth_Keller msg: 1730863388.90 epoch sec; seqNo:5022; depth 49.62 2024-11-06T03:23:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863388.91 epoch sec; seqNo:5022; 4061638.00 north; 594741.15 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:23:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863387.70 2024-11-06T03:23:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:09Z- waiting for 1730863391.70, time is 1730863388.91 2024-11-06T03:23:09Z- AHRS_M2 msg: 1730863389.29 epoch sec; seqNo:4723 2024-11-06T03:23:09Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.01 psi 2024-11-06T03:23:09Z- handleDepth()- Depth_Keller msg: 1730863389.30 epoch sec; seqNo:5023; depth 49.65 2024-11-06T03:23:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863389.31 epoch sec; seqNo:5023; 4061638.37 north; 594739.92 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:23:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863387.70 2024-11-06T03:23:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:09Z- waiting for 1730863391.70, time is 1730863389.31 2024-11-06T03:23:09Z- AHRS_M2 msg: 1730863389.69 epoch sec; seqNo:4724 2024-11-06T03:23:09Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.01 psi 2024-11-06T03:23:09Z- handleDepth()- Depth_Keller msg: 1730863389.70 epoch sec; seqNo:5024; depth 49.64 2024-11-06T03:23:09Z- handleDVL() - DVL msg epochMillisec: 1730863389.73 2024-11-06T03:23:09Z- handleDvl() - RDI_Pathfinder msg: 1730863389.73 sec; seqNo:1720; #beams:4 2024-11-06T03:23:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 16.99 , 24.47 , 29.22 , 28.95 2024-11-06T03:23:09Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.18 , -0.04 2024-11-06T03:23:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863389.74 epoch sec; seqNo:5024; 4061638.74 north; 594739.92 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:23:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863387.70 2024-11-06T03:23:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:09Z- waiting for 1730863391.70, time is 1730863389.74 2024-11-06T03:23:10Z- AHRS_M2 msg: 1730863390.10 epoch sec; seqNo:4725 2024-11-06T03:23:10Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.01 psi 2024-11-06T03:23:10Z- handleDepth()- Depth_Keller msg: 1730863390.11 epoch sec; seqNo:5025; depth 49.65 2024-11-06T03:23:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863390.14 epoch sec; seqNo:5025; 4061639.12 north; 594739.92 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:23:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863387.70 2024-11-06T03:23:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:10Z- waiting for 1730863391.70, time is 1730863390.14 2024-11-06T03:23:10Z- AHRS_M2 msg: 1730863390.50 epoch sec; seqNo:4726 2024-11-06T03:23:10Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.01 psi 2024-11-06T03:23:10Z- handleDepth()- Depth_Keller msg: 1730863390.51 epoch sec; seqNo:5026; depth 49.67 2024-11-06T03:23:10Z- handleDVL() - DVL msg epochMillisec: 1730863390.53 2024-11-06T03:23:10Z- handleDvl() - RDI_Pathfinder msg: 1730863390.53 sec; seqNo:1721; #beams:4 2024-11-06T03:23:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 29.14 , 24.86 , 29.14 , 29.26 2024-11-06T03:23:10Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.15 , 0.02 2024-11-06T03:23:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863390.54 epoch sec; seqNo:5026; 4061639.50 north; 594739.91 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:23:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863387.70 2024-11-06T03:23:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:10Z- waiting for 1730863391.70, time is 1730863390.54 2024-11-06T03:23:10Z- handleDepth()- Depth_Keller msg: 1730863390.93 epoch sec; seqNo:5027; depth 49.71 2024-11-06T03:23:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863390.94 epoch sec; seqNo:5027; 4061639.88 north; 594739.91 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:23:11Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:23:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863387.70 2024-11-06T03:23:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:11Z- waiting for 1730863391.70, time is 1730863390.94 2024-11-06T03:23:11Z- AHRS_M2 msg: 1730863391.31 epoch sec; seqNo:4727 2024-11-06T03:23:11Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.01 psi 2024-11-06T03:23:11Z- handleDepth()- Depth_Keller msg: 1730863391.32 epoch sec; seqNo:5028; depth 49.73 2024-11-06T03:23:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863391.33 epoch sec; seqNo:5028; 4061640.26 north; 594739.90 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:23:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863387.70 2024-11-06T03:23:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:11Z- waiting for 1730863391.70, time is 1730863391.33 2024-11-06T03:23:11Z- AHRS_M2 msg: 1730863391.73 epoch sec; seqNo:4728 2024-11-06T03:23:11Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.01 psi 2024-11-06T03:23:11Z- handleDepth()- Depth_Keller msg: 1730863391.75 epoch sec; seqNo:5029; depth 49.75 2024-11-06T03:23:11Z- handleDVL() - DVL msg epochMillisec: 1730863391.78 2024-11-06T03:23:11Z- handleDvl() - RDI_Pathfinder msg: 1730863391.78 sec; seqNo:1722; #beams:4 2024-11-06T03:23:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 28.54 , 23.79 , 29.22 , 28.82 2024-11-06T03:23:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.15 , 0.05 2024-11-06T03:23:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863391.79 epoch sec; seqNo:5029; 4061640.64 north; 594739.90 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:23:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863387.70 2024-11-06T03:23:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:11Z- waiting for 1730863391.70, time is 1730863391.79 2024-11-06T03:23:11Z- LcmTrn::updateTrn() >>>> pose time 1730863391.79, meas time 1730863391.78 <<<< 2024-11-06T03:23:11Z- LcmTrn::updateTrn() equating pose 1730863391.78 and meas times 1730863391.78 2024-11-06T03:23:11Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863391.78 2024-11-06T03:23:11Z- TerrainNav::motionUpdate() - currEstimate = (4061641, 594739.90, 49.75) 2024-11-06T03:23:11Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:23:11Z- TerrainNav:: SubcloudNIS is 1.01, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:23:11Z- TNavPF::Projecting Measurements 2024-11-06T03:23:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:23:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:23:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:23:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:23:11Z- TNavFilter::4 good beams. 2024-11-06T03:23:11Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:23:11Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:23:11Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:23:11Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:23:11Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:23:11Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:23:11Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:23:11Z- TerrainMapOctree: (vehN, vehE ) = (4061640.64, 594739.90). 2024-11-06T03:23:11Z- TerrainMapOctree: Min Distance = 109.83. 2024-11-06T03:23:11Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:23:12Z- Forcing Subcloud Comparison 2024-11-06T03:23:12Z- Weighting particles with subcloud comparison 2024-11-06T03:23:12Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:12Z- meanExpectedMeasDiff: -3.442988 delta_rms_squared: 0.836116 alpha: 0.976639 2024-11-06T03:23:12Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:12Z- meanExpectedMeasDiff: -0.935287 delta_rms_squared: 0.601764 alpha: 0.967833 2024-11-06T03:23:12Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:12Z- meanExpectedMeasDiff: 0.604968 delta_rms_squared: 1.167869 alpha: 0.983163 2024-11-06T03:23:12Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:12Z- meanExpectedMeasDiff: -1.499832 delta_rms_squared: 0.768404 alpha: 0.974632 2024-11-06T03:23:12Z- Subcloud NIS: 0.946863 2024-11-06T03:23:12Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:23:12Z- TNavPF:: sumWeights = 0.101378 2024-11-06T03:23:12Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:23:12Z- TNavPF::First Term of Map Covariance Matrix is 0.66 2024-11-06T03:23:12Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T03:23:12Z- TerrainNav::Resampling particle filter... 2024-11-06T03:23:12Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863391.78 sec, ping # 0. 2024-11-06T03:23:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:23:12Z- variances([0], [2], and psi_berg) = 1.202874 0.221332 0.000000 2024-11-06T03:23:12Z- publishEstimatesDecoder() - at 1730863391.78 sec 2024-11-06T03:23:12Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:23:12Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061671.68 594722.86 60.76 2024-11-06T03:23:12Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061671.52 594722.65 61.27 lat/lon: 36.70 -121.94 2024-11-06T03:23:12Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.20 0.22 0.25 0.00 2024-11-06T03:23:12Z- AHRS_M2 msg: 1730863392.12 epoch sec; seqNo:4729 2024-11-06T03:23:12Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.01 psi 2024-11-06T03:23:12Z- handleDepth()- Depth_Keller msg: 1730863392.13 epoch sec; seqNo:5030; depth 49.77 2024-11-06T03:23:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863392.14 epoch sec; seqNo:5030; 4061641.00 north; 594738.67 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:23:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863391.78 2024-11-06T03:23:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:12Z- waiting for 1730863395.78, time is 1730863392.14 2024-11-06T03:23:12Z- AHRS_M2 msg: 1730863392.53 epoch sec; seqNo:4730 2024-11-06T03:23:12Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.01 psi 2024-11-06T03:23:12Z- handleDepth()- Depth_Keller msg: 1730863392.54 epoch sec; seqNo:5031; depth 49.81 2024-11-06T03:23:12Z- handleDVL() - DVL msg epochMillisec: 1730863392.56 2024-11-06T03:23:12Z- handleDvl() - RDI_Pathfinder msg: 1730863392.56 sec; seqNo:1723; #beams:4 2024-11-06T03:23:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 28.95 , 23.51 , 29.22 , 28.54 2024-11-06T03:23:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.10 , 0.07 2024-11-06T03:23:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863392.57 epoch sec; seqNo:5031; 4061641.38 north; 594738.67 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:23:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863391.78 2024-11-06T03:23:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:12Z- waiting for 1730863395.78, time is 1730863392.57 2024-11-06T03:23:12Z- AHRS_M2 msg: 1730863392.93 epoch sec; seqNo:4731 2024-11-06T03:23:12Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 6.01 psi 2024-11-06T03:23:12Z- handleDepth()- Depth_Keller msg: 1730863392.93 epoch sec; seqNo:5032; depth 49.84 2024-11-06T03:23:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863392.95 epoch sec; seqNo:5032; 4061641.76 north; 594738.67 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:23:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863391.78 2024-11-06T03:23:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:13Z- waiting for 1730863395.78, time is 1730863392.95 2024-11-06T03:23:13Z- AHRS_M2 msg: 1730863393.33 epoch sec; seqNo:4732 2024-11-06T03:23:13Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 6.02 psi 2024-11-06T03:23:13Z- handleDepth()- Depth_Keller msg: 1730863393.34 epoch sec; seqNo:5033; depth 49.88 2024-11-06T03:23:13Z- handleDVL() - DVL msg epochMillisec: 1730863393.34 2024-11-06T03:23:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:23:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:23:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:23:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:23:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:23:13Z- handleDvl() - RDI_Pathfinder msg: 1730863393.34 sec; seqNo:1724; #beams:0 2024-11-06T03:23:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:23:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.10 , 0.07 2024-11-06T03:23:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863393.35 epoch sec; seqNo:5033; 4061642.14 north; 594738.66 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:23:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863391.78 2024-11-06T03:23:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:13Z- waiting for 1730863395.78, time is 1730863393.35 2024-11-06T03:23:13Z- handleDepth()- Depth_Keller msg: 1730863393.76 epoch sec; seqNo:5034; depth 49.89 2024-11-06T03:23:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863393.77 epoch sec; seqNo:5034; 4061642.52 north; 594738.66 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:23:13Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:23:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863391.78 2024-11-06T03:23:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:13Z- waiting for 1730863395.78, time is 1730863393.77 2024-11-06T03:23:14Z- AHRS_M2 msg: 1730863394.14 epoch sec; seqNo:4733 2024-11-06T03:23:14Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 6.02 psi 2024-11-06T03:23:14Z- handleDepth()- Depth_Keller msg: 1730863394.16 epoch sec; seqNo:5035; depth 49.92 2024-11-06T03:23:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863394.17 epoch sec; seqNo:5035; 4061642.90 north; 594738.65 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:23:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863391.78 2024-11-06T03:23:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:14Z- waiting for 1730863395.78, time is 1730863394.17 2024-11-06T03:23:14Z- AHRS_M2 msg: 1730863394.56 epoch sec; seqNo:4734 2024-11-06T03:23:14Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 6.02 psi 2024-11-06T03:23:14Z- handleDepth()- Depth_Keller msg: 1730863394.56 epoch sec; seqNo:5036; depth 49.94 2024-11-06T03:23:14Z- handleDVL() - DVL msg epochMillisec: 1730863394.59 2024-11-06T03:23:14Z- handleDvl() - RDI_Pathfinder msg: 1730863394.59 sec; seqNo:1725; #beams:4 2024-11-06T03:23:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 28.54 , 22.29 , 29.22 , 27.18 2024-11-06T03:23:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , 0.06 2024-11-06T03:23:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863394.60 epoch sec; seqNo:5036; 4061643.28 north; 594738.65 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:23:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863391.78 2024-11-06T03:23:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:14Z- waiting for 1730863395.78, time is 1730863394.60 2024-11-06T03:23:14Z- AHRS_M2 msg: 1730863394.95 epoch sec; seqNo:4735 2024-11-06T03:23:14Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 6.03 psi 2024-11-06T03:23:14Z- handleDepth()- Depth_Keller msg: 1730863394.96 epoch sec; seqNo:5037; depth 49.96 2024-11-06T03:23:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863394.97 epoch sec; seqNo:5037; 4061643.28 north; 594738.65 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:23:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863391.78 2024-11-06T03:23:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:15Z- waiting for 1730863395.78, time is 1730863394.97 2024-11-06T03:23:15Z- handleDepth()- Depth_Keller msg: 1730863395.38 epoch sec; seqNo:5038; depth 49.99 2024-11-06T03:23:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863395.39 epoch sec; seqNo:5038; 4061643.66 north; 594738.65 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:23:15Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:23:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863391.78 2024-11-06T03:23:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:15Z- waiting for 1730863395.78, time is 1730863395.39 2024-11-06T03:23:15Z- AHRS_M2 msg: 1730863395.76 epoch sec; seqNo:4736 2024-11-06T03:23:15Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 6.04 psi 2024-11-06T03:23:15Z- handleDepth()- Depth_Keller msg: 1730863395.77 epoch sec; seqNo:5039; depth 50.00 2024-11-06T03:23:15Z- handleDVL() - DVL msg epochMillisec: 1730863395.79 2024-11-06T03:23:15Z- handleDvl() - RDI_Pathfinder msg: 1730863395.79 sec; seqNo:1726; #beams:4 2024-11-06T03:23:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 15.58 , 22.24 , 29.50 , 26.88 2024-11-06T03:23:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , 0.03 2024-11-06T03:23:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863395.81 epoch sec; seqNo:5039; 4061644.02 north; 594737.42 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:23:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863391.78 2024-11-06T03:23:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:15Z- waiting for 1730863395.78, time is 1730863395.81 2024-11-06T03:23:15Z- LcmTrn::updateTrn() >>>> pose time 1730863395.81, meas time 1730863395.79 <<<< 2024-11-06T03:23:15Z- LcmTrn::updateTrn() equating pose 1730863395.79 and meas times 1730863395.79 2024-11-06T03:23:15Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863395.79 2024-11-06T03:23:15Z- TerrainNav::motionUpdate() - currEstimate = (4061644, 594737.42, 50.00) 2024-11-06T03:23:15Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:23:15Z- TerrainNav:: SubcloudNIS is 0.95, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:23:15Z- TNavPF::Projecting Measurements 2024-11-06T03:23:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:23:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:23:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:23:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:23:15Z- TNavFilter::4 good beams. 2024-11-06T03:23:15Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:23:15Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:23:15Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:23:15Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:23:15Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:23:15Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:23:15Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:23:15Z- TerrainMapOctree: (vehN, vehE ) = (4061644.02, 594737.42). 2024-11-06T03:23:15Z- TerrainMapOctree: Min Distance = 106.72. 2024-11-06T03:23:15Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:23:16Z- Forcing Subcloud Comparison 2024-11-06T03:23:16Z- Weighting particles with subcloud comparison 2024-11-06T03:23:16Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:16Z- meanExpectedMeasDiff: -16.728020 delta_rms_squared: 0.782516 alpha: 0.975078 2024-11-06T03:23:16Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:16Z- meanExpectedMeasDiff: -1.495516 delta_rms_squared: 0.554628 alpha: 0.965195 2024-11-06T03:23:16Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:16Z- meanExpectedMeasDiff: 1.125411 delta_rms_squared: 0.629822 alpha: 0.969222 2024-11-06T03:23:16Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:16Z- meanExpectedMeasDiff: -2.648392 delta_rms_squared: 0.741618 alpha: 0.973740 2024-11-06T03:23:16Z- Subcloud NIS: 0.905950 2024-11-06T03:23:16Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:23:16Z- TNavPF:: sumWeights = 0.000000 2024-11-06T03:23:16Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:23:16Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:23:16Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:23:16Z- TerrainNav::Resampling particle filter... 2024-11-06T03:23:16Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863395.79 sec, ping # 0. 2024-11-06T03:23:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:23:16Z- variances([0], [2], and psi_berg) = 1.214718 0.135373 0.000000 2024-11-06T03:23:16Z- publishEstimatesDecoder() - at 1730863395.79 sec 2024-11-06T03:23:16Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:23:16Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061674.85 594720.09 63.22 2024-11-06T03:23:16Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061675.68 594720.02 63.82 lat/lon: 36.70 -121.94 2024-11-06T03:23:16Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.21 0.14 0.19 0.00 2024-11-06T03:23:16Z- AHRS_M2 msg: 1730863396.16 epoch sec; seqNo:4737 2024-11-06T03:23:16Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 6.04 psi 2024-11-06T03:23:16Z- handleDepth()- Depth_Keller msg: 1730863396.17 epoch sec; seqNo:5040; depth 50.04 2024-11-06T03:23:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863396.19 epoch sec; seqNo:5040; 4061644.40 north; 594737.42 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:23:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863395.79 2024-11-06T03:23:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:16Z- waiting for 1730863399.79, time is 1730863396.19 2024-11-06T03:23:16Z- AHRS_M2 msg: 1730863396.58 epoch sec; seqNo:4738 2024-11-06T03:23:16Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 6.04 psi 2024-11-06T03:23:16Z- handleDepth()- Depth_Keller msg: 1730863396.59 epoch sec; seqNo:5041; depth 50.03 2024-11-06T03:23:16Z- handleDVL() - DVL msg epochMillisec: 1730863396.62 2024-11-06T03:23:16Z- handleDvl() - RDI_Pathfinder msg: 1730863396.62 sec; seqNo:1727; #beams:4 2024-11-06T03:23:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 25.79 , 29.46 , 26.91 2024-11-06T03:23:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , -0.00 2024-11-06T03:23:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863396.63 epoch sec; seqNo:5041; 4061644.78 north; 594737.41 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:23:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863395.79 2024-11-06T03:23:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:16Z- waiting for 1730863399.79, time is 1730863396.63 2024-11-06T03:23:16Z- AHRS_M2 msg: 1730863396.97 epoch sec; seqNo:4739 2024-11-06T03:23:16Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 6.05 psi 2024-11-06T03:23:16Z- handleDepth()- Depth_Keller msg: 1730863396.98 epoch sec; seqNo:5042; depth 50.05 2024-11-06T03:23:16Z- handleDVL() - DVL msg epochMillisec: 1730863396.98 2024-11-06T03:23:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:23:16Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:23:16Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:23:16Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:23:16Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:23:16Z- handleDvl() - RDI_Pathfinder msg: 1730863396.98 sec; seqNo:1728; #beams:0 2024-11-06T03:23:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:23:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , -0.00 2024-11-06T03:23:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863397.01 epoch sec; seqNo:5042; 4061645.16 north; 594737.41 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:23:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863395.79 2024-11-06T03:23:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:17Z- waiting for 1730863399.79, time is 1730863397.01 2024-11-06T03:23:17Z- handleDepth()- Depth_Keller msg: 1730863397.39 epoch sec; seqNo:5043; depth 50.06 2024-11-06T03:23:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863397.40 epoch sec; seqNo:5043; 4061645.54 north; 594737.40 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:23:17Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:23:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863395.79 2024-11-06T03:23:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:17Z- waiting for 1730863399.79, time is 1730863397.40 2024-11-06T03:23:17Z- AHRS_M2 msg: 1730863397.78 epoch sec; seqNo:4740 2024-11-06T03:23:17Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 6.05 psi 2024-11-06T03:23:17Z- handleDepth()- Depth_Keller msg: 1730863397.79 epoch sec; seqNo:5044; depth 50.06 2024-11-06T03:23:17Z- handleDVL() - DVL msg epochMillisec: 1730863397.81 2024-11-06T03:23:17Z- handleDvl() - RDI_Pathfinder msg: 1730863397.81 sec; seqNo:1729; #beams:4 2024-11-06T03:23:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.67 , 25.28 , 29.22 , 26.78 2024-11-06T03:23:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , -0.03 2024-11-06T03:23:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863397.83 epoch sec; seqNo:5044; 4061645.92 north; 594737.40 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:23:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863395.79 2024-11-06T03:23:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:17Z- waiting for 1730863399.79, time is 1730863397.83 2024-11-06T03:23:18Z- AHRS_M2 msg: 1730863398.18 epoch sec; seqNo:4741 2024-11-06T03:23:18Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 6.05 psi 2024-11-06T03:23:18Z- handleDepth()- Depth_Keller msg: 1730863398.19 epoch sec; seqNo:5045; depth 50.08 2024-11-06T03:23:18Z- handleDVL() - DVL msg epochMillisec: 1730863398.19 2024-11-06T03:23:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:23:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:23:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:23:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:23:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:23:18Z- handleDvl() - RDI_Pathfinder msg: 1730863398.19 sec; seqNo:1730; #beams:0 2024-11-06T03:23:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:23:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , -0.03 2024-11-06T03:23:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863398.21 epoch sec; seqNo:5045; 4061646.29 north; 594737.40 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:23:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863395.79 2024-11-06T03:23:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:18Z- waiting for 1730863399.79, time is 1730863398.21 2024-11-06T03:23:18Z- AHRS_M2 msg: 1730863398.62 epoch sec; seqNo:4742 2024-11-06T03:23:18Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 6.05 psi 2024-11-06T03:23:18Z- handleDepth()- Depth_Keller msg: 1730863398.63 epoch sec; seqNo:5046; depth 50.09 2024-11-06T03:23:18Z- handleDVL() - DVL msg epochMillisec: 1730863398.66 2024-11-06T03:23:18Z- handleDvl() - RDI_Pathfinder msg: 1730863398.66 sec; seqNo:1731; #beams:4 2024-11-06T03:23:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 28.82 , 25.55 , 29.09 , 26.78 2024-11-06T03:23:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , -0.04 2024-11-06T03:23:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863398.68 epoch sec; seqNo:5046; 4061646.67 north; 594737.39 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:23:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863395.79 2024-11-06T03:23:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:18Z- waiting for 1730863399.79, time is 1730863398.68 2024-11-06T03:23:18Z- AHRS_M2 msg: 1730863398.99 epoch sec; seqNo:4743 2024-11-06T03:23:18Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 6.05 psi 2024-11-06T03:23:18Z- handleDepth()- Depth_Keller msg: 1730863398.99 epoch sec; seqNo:5047; depth 50.09 2024-11-06T03:23:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863399.02 epoch sec; seqNo:5047; 4061647.05 north; 594737.39 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:23:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863395.79 2024-11-06T03:23:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:19Z- waiting for 1730863399.79, time is 1730863399.02 2024-11-06T03:23:19Z- AHRS_M2 msg: 1730863399.39 epoch sec; seqNo:4744 2024-11-06T03:23:19Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 6.05 psi 2024-11-06T03:23:19Z- handleDepth()- Depth_Keller msg: 1730863399.41 epoch sec; seqNo:5048; depth 50.11 2024-11-06T03:23:19Z- handleDVL() - DVL msg epochMillisec: 1730863399.41 2024-11-06T03:23:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:23:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:23:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:23:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:23:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:23:19Z- handleDvl() - RDI_Pathfinder msg: 1730863399.41 sec; seqNo:1732; #beams:0 2024-11-06T03:23:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:23:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , -0.04 2024-11-06T03:23:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863399.42 epoch sec; seqNo:5048; 4061647.43 north; 594737.38 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:23:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863395.79 2024-11-06T03:23:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:19Z- waiting for 1730863399.79, time is 1730863399.42 2024-11-06T03:23:19Z- AHRS_M2 msg: 1730863399.80 epoch sec; seqNo:4745 2024-11-06T03:23:19Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 6.06 psi 2024-11-06T03:23:19Z- handleDepth()- Depth_Keller msg: 1730863399.81 epoch sec; seqNo:5049; depth 50.13 2024-11-06T03:23:19Z- handleDVL() - DVL msg epochMillisec: 1730863399.83 2024-11-06T03:23:19Z- handleDvl() - RDI_Pathfinder msg: 1730863399.83 sec; seqNo:1733; #beams:4 2024-11-06T03:23:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 27.32 , 25.55 , 29.09 , 26.23 2024-11-06T03:23:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.07 , -0.04 2024-11-06T03:23:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863399.86 epoch sec; seqNo:5049; 4061647.81 north; 594737.38 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:23:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863395.79 2024-11-06T03:23:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:19Z- waiting for 1730863399.79, time is 1730863399.86 2024-11-06T03:23:19Z- LcmTrn::updateTrn() >>>> pose time 1730863399.86, meas time 1730863399.83 <<<< 2024-11-06T03:23:19Z- LcmTrn::updateTrn() equating pose 1730863399.83 and meas times 1730863399.83 2024-11-06T03:23:19Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863399.83 2024-11-06T03:23:19Z- TerrainNav::motionUpdate() - currEstimate = (4061648, 594737.38, 50.13) 2024-11-06T03:23:19Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:23:19Z- TerrainNav:: SubcloudNIS is 0.91, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:23:19Z- TNavPF::Projecting Measurements 2024-11-06T03:23:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:23:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:23:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:23:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:23:19Z- TNavFilter::4 good beams. 2024-11-06T03:23:19Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:23:19Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:23:19Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:23:19Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:23:19Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:23:19Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:23:19Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:23:19Z- TerrainMapOctree: (vehN, vehE ) = (4061647.81, 594737.38). 2024-11-06T03:23:19Z- TerrainMapOctree: Min Distance = 102.97. 2024-11-06T03:23:19Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:23:20Z- Forcing Subcloud Comparison 2024-11-06T03:23:20Z- Weighting particles with subcloud comparison 2024-11-06T03:23:20Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:20Z- meanExpectedMeasDiff: -1.970028 delta_rms_squared: 0.709420 alpha: 0.972581 2024-11-06T03:23:20Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:20Z- meanExpectedMeasDiff: 4.539875 delta_rms_squared: 0.363932 alpha: 0.947907 2024-11-06T03:23:20Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:20Z- meanExpectedMeasDiff: 3.533685 delta_rms_squared: 0.538970 alpha: 0.964220 2024-11-06T03:23:20Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:20Z- meanExpectedMeasDiff: -0.375292 delta_rms_squared: 0.639553 alpha: 0.969676 2024-11-06T03:23:20Z- Subcloud NIS: 0.732247 2024-11-06T03:23:20Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:23:20Z- TNavPF:: sumWeights = 0.002286 2024-11-06T03:23:20Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:23:20Z- TNavPF::First Term of Map Covariance Matrix is 0.01 2024-11-06T03:23:20Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:23:20Z- TerrainNav::Resampling particle filter... 2024-11-06T03:23:20Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863399.83 sec, ping # 0. 2024-11-06T03:23:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:23:20Z- variances([0], [2], and psi_berg) = 1.238626 0.132082 0.000000 2024-11-06T03:23:20Z- publishEstimatesDecoder() - at 1730863399.83 sec 2024-11-06T03:23:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:23:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061678.66 594720.03 63.01 2024-11-06T03:23:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061679.41 594719.87 63.61 lat/lon: 36.70 -121.94 2024-11-06T03:23:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.24 0.13 0.27 0.00 2024-11-06T03:23:20Z- AHRS_M2 msg: 1730863400.21 epoch sec; seqNo:4746 2024-11-06T03:23:20Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 6.06 psi 2024-11-06T03:23:20Z- handleDepth()- Depth_Keller msg: 1730863400.22 epoch sec; seqNo:5050; depth 50.14 2024-11-06T03:23:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863400.23 epoch sec; seqNo:5050; 4061647.81 north; 594737.38 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:23:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863399.83 2024-11-06T03:23:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:20Z- waiting for 1730863403.83, time is 1730863400.23 2024-11-06T03:23:20Z- handleDepth()- Depth_Keller msg: 1730863400.67 epoch sec; seqNo:5051; depth 50.14 2024-11-06T03:23:20Z- handleDVL() - DVL msg epochMillisec: 1730863400.69 2024-11-06T03:23:20Z- handleDvl() - RDI_Pathfinder msg: 1730863400.69 sec; seqNo:1734; #beams:4 2024-11-06T03:23:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 27.35 , 24.98 , 29.02 , 26.40 2024-11-06T03:23:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.09 , -0.03 2024-11-06T03:23:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863400.70 epoch sec; seqNo:5051; 4061648.55 north; 594736.15 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:23:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863399.83 2024-11-06T03:23:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:20Z- waiting for 1730863403.83, time is 1730863400.70 2024-11-06T03:23:21Z- AHRS_M2 msg: 1730863401.02 epoch sec; seqNo:4747 2024-11-06T03:23:21Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 6.06 psi 2024-11-06T03:23:21Z- handleDepth()- Depth_Keller msg: 1730863401.03 epoch sec; seqNo:5052; depth 50.15 2024-11-06T03:23:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863401.05 epoch sec; seqNo:5052; 4061648.55 north; 594736.15 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:23:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863399.83 2024-11-06T03:23:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:21Z- waiting for 1730863403.83, time is 1730863401.05 2024-11-06T03:23:21Z- AHRS_M2 msg: 1730863401.42 epoch sec; seqNo:4748 2024-11-06T03:23:21Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 6.06 psi 2024-11-06T03:23:21Z- handleDepth()- Depth_Keller msg: 1730863401.42 epoch sec; seqNo:5053; depth 50.17 2024-11-06T03:23:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863401.43 epoch sec; seqNo:5053; 4061648.93 north; 594736.15 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:23:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863399.83 2024-11-06T03:23:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:21Z- waiting for 1730863403.83, time is 1730863401.43 2024-11-06T03:23:21Z- AHRS_M2 msg: 1730863401.82 epoch sec; seqNo:4749 2024-11-06T03:23:21Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 6.06 psi 2024-11-06T03:23:21Z- handleDepth()- Depth_Keller msg: 1730863401.84 epoch sec; seqNo:5054; depth 50.19 2024-11-06T03:23:21Z- handleDVL() - DVL msg epochMillisec: 1730863401.86 2024-11-06T03:23:21Z- handleDvl() - RDI_Pathfinder msg: 1730863401.86 sec; seqNo:1735; #beams:4 2024-11-06T03:23:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 27.12 , 24.86 , 29.02 , 26.52 2024-11-06T03:23:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.12 , -0.02 2024-11-06T03:23:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863401.87 epoch sec; seqNo:5054; 4061649.31 north; 594736.14 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:23:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863399.83 2024-11-06T03:23:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:21Z- waiting for 1730863403.83, time is 1730863401.87 2024-11-06T03:23:22Z- AHRS_M2 msg: 1730863402.23 epoch sec; seqNo:4750 2024-11-06T03:23:22Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 6.06 psi 2024-11-06T03:23:22Z- handleDepth()- Depth_Keller msg: 1730863402.24 epoch sec; seqNo:5055; depth 50.23 2024-11-06T03:23:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863402.26 epoch sec; seqNo:5055; 4061649.69 north; 594736.14 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:23:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863399.83 2024-11-06T03:23:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:22Z- waiting for 1730863403.83, time is 1730863402.26 2024-11-06T03:23:22Z- AHRS_M2 msg: 1730863402.67 epoch sec; seqNo:4751 2024-11-06T03:23:22Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 6.06 psi 2024-11-06T03:23:22Z- handleDepth()- Depth_Keller msg: 1730863402.68 epoch sec; seqNo:5056; depth 50.23 2024-11-06T03:23:22Z- handleDVL() - DVL msg epochMillisec: 1730863402.70 2024-11-06T03:23:22Z- handleDvl() - RDI_Pathfinder msg: 1730863402.70 sec; seqNo:1736; #beams:4 2024-11-06T03:23:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 27.12 , 24.86 , 29.14 , 26.88 2024-11-06T03:23:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.11 , -0.01 2024-11-06T03:23:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863402.71 epoch sec; seqNo:5056; 4061650.07 north; 594736.13 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:23:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863399.83 2024-11-06T03:23:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:22Z- waiting for 1730863403.83, time is 1730863402.71 2024-11-06T03:23:23Z- AHRS_M2 msg: 1730863403.03 epoch sec; seqNo:4752 2024-11-06T03:23:23Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 6.06 psi 2024-11-06T03:23:23Z- handleDepth()- Depth_Keller msg: 1730863403.05 epoch sec; seqNo:5057; depth 50.25 2024-11-06T03:23:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863403.06 epoch sec; seqNo:5057; 4061650.45 north; 594736.13 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:23:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863399.83 2024-11-06T03:23:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:23Z- waiting for 1730863403.83, time is 1730863403.06 2024-11-06T03:23:23Z- AHRS_M2 msg: 1730863403.43 epoch sec; seqNo:4753 2024-11-06T03:23:23Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.06 psi 2024-11-06T03:23:23Z- handleDepth()- Depth_Keller msg: 1730863403.44 epoch sec; seqNo:5058; depth 50.27 2024-11-06T03:23:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863403.45 epoch sec; seqNo:5058; 4061650.83 north; 594736.13 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:23:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863399.83 2024-11-06T03:23:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:23Z- waiting for 1730863403.83, time is 1730863403.45 2024-11-06T03:23:23Z- handleDepth()- Depth_Keller msg: 1730863403.87 epoch sec; seqNo:5059; depth 50.31 2024-11-06T03:23:23Z- handleDVL() - DVL msg epochMillisec: 1730863403.89 2024-11-06T03:23:23Z- handleDvl() - RDI_Pathfinder msg: 1730863403.89 sec; seqNo:1737; #beams:4 2024-11-06T03:23:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 27.00 , 24.98 , 29.14 , 27.12 2024-11-06T03:23:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.11 , -0.01 2024-11-06T03:23:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863403.90 epoch sec; seqNo:5059; 4061651.21 north; 594736.12 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:23:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863399.83 2024-11-06T03:23:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:24Z- waiting for 1730863403.83, time is 1730863403.90 2024-11-06T03:23:24Z- LcmTrn::updateTrn() >>>> pose time 1730863403.90, meas time 1730863403.89 <<<< 2024-11-06T03:23:24Z- LcmTrn::updateTrn() equating pose 1730863403.89 and meas times 1730863403.89 2024-11-06T03:23:24Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863403.89 2024-11-06T03:23:24Z- TerrainNav::motionUpdate() - currEstimate = (4061651, 594736.12, 50.31) 2024-11-06T03:23:24Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:23:24Z- TerrainNav:: SubcloudNIS is 0.73, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:23:24Z- TNavPF::Projecting Measurements 2024-11-06T03:23:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:23:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:23:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:23:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:23:24Z- TNavFilter::4 good beams. 2024-11-06T03:23:24Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:23:24Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:23:24Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:23:24Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:23:24Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:23:24Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:23:24Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:23:24Z- TerrainMapOctree: (vehN, vehE ) = (4061651.21, 594736.12). 2024-11-06T03:23:24Z- TerrainMapOctree: Min Distance = 99.76. 2024-11-06T03:23:24Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:23:24Z- Forcing Subcloud Comparison 2024-11-06T03:23:24Z- Weighting particles with subcloud comparison 2024-11-06T03:23:24Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:24Z- meanExpectedMeasDiff: -2.320948 delta_rms_squared: 0.837997 alpha: 0.976690 2024-11-06T03:23:24Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:24Z- meanExpectedMeasDiff: 3.585234 delta_rms_squared: 0.579301 alpha: 0.966628 2024-11-06T03:23:24Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:24Z- meanExpectedMeasDiff: 2.266018 delta_rms_squared: 0.693819 alpha: 0.971982 2024-11-06T03:23:24Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:24Z- meanExpectedMeasDiff: 0.535784 delta_rms_squared: 0.752205 alpha: 0.974100 2024-11-06T03:23:24Z- Subcloud NIS: 0.713557 2024-11-06T03:23:24Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:23:24Z- TNavPF:: sumWeights = 0.016085 2024-11-06T03:23:24Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:23:24Z- TNavPF::First Term of Map Covariance Matrix is 0.13 2024-11-06T03:23:24Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:23:24Z- TerrainNav::Resampling particle filter... 2024-11-06T03:23:24Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863403.89 sec, ping # 0. 2024-11-06T03:23:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:23:24Z- variances([0], [2], and psi_berg) = 1.175011 0.126655 0.000000 2024-11-06T03:23:24Z- publishEstimatesDecoder() - at 1730863403.89 sec 2024-11-06T03:23:24Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:23:24Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061682.09 594718.83 62.53 2024-11-06T03:23:24Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061682.76 594718.57 63.46 lat/lon: 36.70 -121.94 2024-11-06T03:23:24Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.18 0.13 0.27 0.00 2024-11-06T03:23:24Z- AHRS_M2 msg: 1730863404.24 epoch sec; seqNo:4754 2024-11-06T03:23:24Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.06 psi 2024-11-06T03:23:24Z- handleDepth()- Depth_Keller msg: 1730863404.26 epoch sec; seqNo:5060; depth 50.34 2024-11-06T03:23:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863404.29 epoch sec; seqNo:5060; 4061651.58 north; 594736.12 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:23:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863403.89 2024-11-06T03:23:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:24Z- waiting for 1730863407.89, time is 1730863404.29 2024-11-06T03:23:24Z- AHRS_M2 msg: 1730863404.69 epoch sec; seqNo:4755 2024-11-06T03:23:24Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.06 psi 2024-11-06T03:23:24Z- handleDepth()- Depth_Keller msg: 1730863404.71 epoch sec; seqNo:5061; depth 50.35 2024-11-06T03:23:24Z- handleDVL() - DVL msg epochMillisec: 1730863404.74 2024-11-06T03:23:24Z- handleDvl() - RDI_Pathfinder msg: 1730863404.74 sec; seqNo:1738; #beams:4 2024-11-06T03:23:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 26.88 , 25.45 , 29.14 , 27.47 2024-11-06T03:23:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.11 , -0.01 2024-11-06T03:23:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863404.75 epoch sec; seqNo:5061; 4061651.95 north; 594734.89 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:23:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863403.89 2024-11-06T03:23:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:24Z- waiting for 1730863407.89, time is 1730863404.75 2024-11-06T03:23:25Z- AHRS_M2 msg: 1730863405.06 epoch sec; seqNo:4756 2024-11-06T03:23:25Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 6.06 psi 2024-11-06T03:23:25Z- handleDepth()- Depth_Keller msg: 1730863405.07 epoch sec; seqNo:5062; depth 50.38 2024-11-06T03:23:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863405.08 epoch sec; seqNo:5062; 4061652.33 north; 594734.89 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:23:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863403.89 2024-11-06T03:23:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:25Z- waiting for 1730863407.89, time is 1730863405.08 2024-11-06T03:23:25Z- AHRS_M2 msg: 1730863405.45 epoch sec; seqNo:4757 2024-11-06T03:23:25Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.06 psi 2024-11-06T03:23:25Z- handleDepth()- Depth_Keller msg: 1730863405.47 epoch sec; seqNo:5063; depth 50.41 2024-11-06T03:23:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863405.48 epoch sec; seqNo:5063; 4061652.71 north; 594734.88 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:23:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863403.89 2024-11-06T03:23:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:25Z- waiting for 1730863407.89, time is 1730863405.48 2024-11-06T03:23:25Z- AHRS_M2 msg: 1730863405.86 epoch sec; seqNo:4758 2024-11-06T03:23:25Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.06 psi 2024-11-06T03:23:25Z- handleDepth()- Depth_Keller msg: 1730863405.87 epoch sec; seqNo:5064; depth 50.44 2024-11-06T03:23:25Z- handleDVL() - DVL msg epochMillisec: 1730863405.89 2024-11-06T03:23:25Z- handleDvl() - RDI_Pathfinder msg: 1730863405.89 sec; seqNo:1739; #beams:4 2024-11-06T03:23:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 26.78 , 25.28 , 29.50 , 27.05 2024-11-06T03:23:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.09 , -0.01 2024-11-06T03:23:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863405.90 epoch sec; seqNo:5064; 4061653.09 north; 594734.88 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:23:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863403.89 2024-11-06T03:23:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:26Z- waiting for 1730863407.89, time is 1730863405.90 2024-11-06T03:23:26Z- AHRS_M2 msg: 1730863406.26 epoch sec; seqNo:4759 2024-11-06T03:23:26Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.06 psi 2024-11-06T03:23:26Z- handleDepth()- Depth_Keller msg: 1730863406.27 epoch sec; seqNo:5065; depth 50.46 2024-11-06T03:23:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863406.29 epoch sec; seqNo:5065; 4061653.47 north; 594734.88 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:23:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863403.89 2024-11-06T03:23:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:26Z- waiting for 1730863407.89, time is 1730863406.29 2024-11-06T03:23:26Z- AHRS_M2 msg: 1730863406.67 epoch sec; seqNo:4760 2024-11-06T03:23:26Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.06 psi 2024-11-06T03:23:26Z- handleDepth()- Depth_Keller msg: 1730863406.69 epoch sec; seqNo:5066; depth 50.49 2024-11-06T03:23:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863406.74 epoch sec; seqNo:5066; 4061653.84 north; 594734.87 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:23:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863403.89 2024-11-06T03:23:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:26Z- waiting for 1730863407.89, time is 1730863406.74 2024-11-06T03:23:27Z- AHRS_M2 msg: 1730863407.07 epoch sec; seqNo:4761 2024-11-06T03:23:27Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.06 psi 2024-11-06T03:23:27Z- handleDepth()- Depth_Keller msg: 1730863407.08 epoch sec; seqNo:5067; depth 50.52 2024-11-06T03:23:27Z- handleDVL() - DVL msg epochMillisec: 1730863407.11 2024-11-06T03:23:27Z- handleDvl() - RDI_Pathfinder msg: 1730863407.11 sec; seqNo:1740; #beams:4 2024-11-06T03:23:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 26.88 , 25.33 , 29.50 , 27.12 2024-11-06T03:23:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.07 , -0.01 2024-11-06T03:23:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863407.12 epoch sec; seqNo:5067; 4061654.22 north; 594734.87 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:23:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863403.89 2024-11-06T03:23:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:27Z- waiting for 1730863407.89, time is 1730863407.12 2024-11-06T03:23:27Z- handleDepth()- Depth_Keller msg: 1730863407.49 epoch sec; seqNo:5068; depth 50.54 2024-11-06T03:23:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863407.51 epoch sec; seqNo:5068; 4061654.60 north; 594734.86 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:23:27Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:23:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863403.89 2024-11-06T03:23:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:27Z- waiting for 1730863407.89, time is 1730863407.51 2024-11-06T03:23:27Z- AHRS_M2 msg: 1730863407.88 epoch sec; seqNo:4762 2024-11-06T03:23:27Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.06 psi 2024-11-06T03:23:27Z- handleDepth()- Depth_Keller msg: 1730863407.89 epoch sec; seqNo:5069; depth 50.55 2024-11-06T03:23:27Z- handleDVL() - DVL msg epochMillisec: 1730863407.91 2024-11-06T03:23:27Z- handleDvl() - RDI_Pathfinder msg: 1730863407.91 sec; seqNo:1741; #beams:4 2024-11-06T03:23:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 26.91 , 25.28 , 29.90 , 27.46 2024-11-06T03:23:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.08 , 0.00 2024-11-06T03:23:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863407.94 epoch sec; seqNo:5069; 4061654.98 north; 594734.86 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:23:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863403.89 2024-11-06T03:23:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:28Z- waiting for 1730863407.89, time is 1730863407.94 2024-11-06T03:23:28Z- LcmTrn::updateTrn() >>>> pose time 1730863407.94, meas time 1730863407.91 <<<< 2024-11-06T03:23:28Z- LcmTrn::updateTrn() equating pose 1730863407.91 and meas times 1730863407.91 2024-11-06T03:23:28Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863407.91 2024-11-06T03:23:28Z- TerrainNav::motionUpdate() - currEstimate = (4061655, 594734.86, 50.55) 2024-11-06T03:23:28Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:23:28Z- TerrainNav:: SubcloudNIS is 0.71, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:23:28Z- TNavPF::Projecting Measurements 2024-11-06T03:23:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:23:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:23:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:23:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:23:28Z- TNavFilter::4 good beams. 2024-11-06T03:23:28Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:23:28Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:23:28Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:23:28Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:23:28Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:23:28Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:23:28Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:23:28Z- TerrainMapOctree: (vehN, vehE ) = (4061654.98, 594734.86). 2024-11-06T03:23:28Z- TerrainMapOctree: Min Distance = 96.22. 2024-11-06T03:23:28Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:23:28Z- Forcing Subcloud Comparison 2024-11-06T03:23:28Z- Weighting particles with subcloud comparison 2024-11-06T03:23:28Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:28Z- meanExpectedMeasDiff: -2.439882 delta_rms_squared: 0.834464 alpha: 0.976594 2024-11-06T03:23:28Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:28Z- meanExpectedMeasDiff: 3.428475 delta_rms_squared: 0.584551 alpha: 0.966918 2024-11-06T03:23:28Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:28Z- meanExpectedMeasDiff: 2.531390 delta_rms_squared: 0.664319 alpha: 0.970774 2024-11-06T03:23:28Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:28Z- meanExpectedMeasDiff: 1.184015 delta_rms_squared: 0.729540 alpha: 0.973317 2024-11-06T03:23:28Z- Subcloud NIS: 0.708015 2024-11-06T03:23:28Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:23:28Z- TNavPF:: sumWeights = 0.013811 2024-11-06T03:23:28Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:23:28Z- TNavPF::First Term of Map Covariance Matrix is 0.12 2024-11-06T03:23:28Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:23:28Z- TerrainNav::Resampling particle filter... 2024-11-06T03:23:28Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863407.91 sec, ping # 0. 2024-11-06T03:23:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:23:28Z- variances([0], [2], and psi_berg) = 1.111223 0.132617 0.000000 2024-11-06T03:23:28Z- publishEstimatesDecoder() - at 1730863407.91 sec 2024-11-06T03:23:28Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:23:28Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061685.93 594717.53 62.53 2024-11-06T03:23:28Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061686.72 594717.31 63.37 lat/lon: 36.70 -121.94 2024-11-06T03:23:28Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.11 0.13 0.29 0.00 2024-11-06T03:23:28Z- AHRS_M2 msg: 1730863408.30 epoch sec; seqNo:4763 2024-11-06T03:23:28Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.06 psi 2024-11-06T03:23:28Z- handleDepth()- Depth_Keller msg: 1730863408.31 epoch sec; seqNo:5070; depth 50.58 2024-11-06T03:23:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863408.33 epoch sec; seqNo:5070; 4061655.36 north; 594734.85 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T03:23:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863407.91 2024-11-06T03:23:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:28Z- waiting for 1730863411.91, time is 1730863408.33 2024-11-06T03:23:28Z- handleDepth()- Depth_Keller msg: 1730863408.72 epoch sec; seqNo:5071; depth 50.62 2024-11-06T03:23:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863408.75 epoch sec; seqNo:5071; 4061655.73 north; 594733.63 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:23:28Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:23:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863407.91 2024-11-06T03:23:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:28Z- waiting for 1730863411.91, time is 1730863408.75 2024-11-06T03:23:29Z- AHRS_M2 msg: 1730863409.09 epoch sec; seqNo:4764 2024-11-06T03:23:29Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.06 psi 2024-11-06T03:23:29Z- handleDepth()- Depth_Keller msg: 1730863409.10 epoch sec; seqNo:5072; depth 50.64 2024-11-06T03:23:29Z- handleDVL() - DVL msg epochMillisec: 1730863409.12 2024-11-06T03:23:29Z- handleDvl() - RDI_Pathfinder msg: 1730863409.12 sec; seqNo:1742; #beams:4 2024-11-06T03:23:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 27.05 , 25.28 , 30.17 , 27.73 2024-11-06T03:23:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.07 , 0.01 2024-11-06T03:23:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863409.14 epoch sec; seqNo:5072; 4061656.10 north; 594733.63 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:23:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863407.91 2024-11-06T03:23:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:29Z- waiting for 1730863411.91, time is 1730863409.14 2024-11-06T03:23:29Z- AHRS_M2 msg: 1730863409.50 epoch sec; seqNo:4765 2024-11-06T03:23:29Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.06 psi 2024-11-06T03:23:29Z- handleDepth()- Depth_Keller msg: 1730863409.50 epoch sec; seqNo:5073; depth 50.65 2024-11-06T03:23:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863409.51 epoch sec; seqNo:5073; 4061656.48 north; 594733.62 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:23:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863407.91 2024-11-06T03:23:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:29Z- waiting for 1730863411.91, time is 1730863409.51 2024-11-06T03:23:29Z- AHRS_M2 msg: 1730863409.90 epoch sec; seqNo:4766 2024-11-06T03:23:29Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.06 psi 2024-11-06T03:23:29Z- handleDepth()- Depth_Keller msg: 1730863409.90 epoch sec; seqNo:5074; depth 50.69 2024-11-06T03:23:29Z- handleDVL() - DVL msg epochMillisec: 1730863409.94 2024-11-06T03:23:29Z- handleDvl() - RDI_Pathfinder msg: 1730863409.94 sec; seqNo:1743; #beams:4 2024-11-06T03:23:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 27.32 , 25.55 , 30.04 , 28.00 2024-11-06T03:23:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , 0.01 2024-11-06T03:23:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863409.95 epoch sec; seqNo:5074; 4061656.86 north; 594733.62 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:23:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863407.91 2024-11-06T03:23:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:30Z- waiting for 1730863411.91, time is 1730863409.95 2024-11-06T03:23:30Z- AHRS_M2 msg: 1730863410.30 epoch sec; seqNo:4767 2024-11-06T03:23:30Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.06 psi 2024-11-06T03:23:30Z- handleDepth()- Depth_Keller msg: 1730863410.31 epoch sec; seqNo:5075; depth 50.68 2024-11-06T03:23:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863410.34 epoch sec; seqNo:5075; 4061657.24 north; 594733.61 east; 36.70 lat; -121.94 lon; 50.68 depth; utmZone: 10 2024-11-06T03:23:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863407.91 2024-11-06T03:23:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:30Z- waiting for 1730863411.91, time is 1730863410.34 2024-11-06T03:23:30Z- AHRS_M2 msg: 1730863410.71 epoch sec; seqNo:4768 2024-11-06T03:23:30Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.06 psi 2024-11-06T03:23:30Z- handleDepth()- Depth_Keller msg: 1730863410.72 epoch sec; seqNo:5076; depth 50.69 2024-11-06T03:23:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863410.73 epoch sec; seqNo:5076; 4061657.62 north; 594733.61 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:23:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863407.91 2024-11-06T03:23:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:30Z- waiting for 1730863411.91, time is 1730863410.73 2024-11-06T03:23:31Z- AHRS_M2 msg: 1730863411.11 epoch sec; seqNo:4769 2024-11-06T03:23:31Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.06 psi 2024-11-06T03:23:31Z- handleDepth()- Depth_Keller msg: 1730863411.12 epoch sec; seqNo:5077; depth 50.73 2024-11-06T03:23:31Z- handleDVL() - DVL msg epochMillisec: 1730863411.14 2024-11-06T03:23:31Z- handleDvl() - RDI_Pathfinder msg: 1730863411.14 sec; seqNo:1744; #beams:4 2024-11-06T03:23:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 27.73 , 25.69 , 30.04 , 28.54 2024-11-06T03:23:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , 0.01 2024-11-06T03:23:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863411.15 epoch sec; seqNo:5077; 4061658.00 north; 594733.60 east; 36.70 lat; -121.94 lon; 50.73 depth; utmZone: 10 2024-11-06T03:23:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863407.91 2024-11-06T03:23:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:31Z- waiting for 1730863411.91, time is 1730863411.15 2024-11-06T03:23:31Z- AHRS_M2 msg: 1730863411.51 epoch sec; seqNo:4770 2024-11-06T03:23:31Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.07 psi 2024-11-06T03:23:31Z- handleDepth()- Depth_Keller msg: 1730863411.52 epoch sec; seqNo:5078; depth 50.74 2024-11-06T03:23:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863411.54 epoch sec; seqNo:5078; 4061658.00 north; 594733.60 east; 36.70 lat; -121.94 lon; 50.74 depth; utmZone: 10 2024-11-06T03:23:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863407.91 2024-11-06T03:23:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:31Z- waiting for 1730863411.91, time is 1730863411.54 2024-11-06T03:23:31Z- AHRS_M2 msg: 1730863411.92 epoch sec; seqNo:4771 2024-11-06T03:23:31Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.07 psi 2024-11-06T03:23:31Z- handleDepth()- Depth_Keller msg: 1730863411.93 epoch sec; seqNo:5079; depth 50.74 2024-11-06T03:23:31Z- handleDVL() - DVL msg epochMillisec: 1730863411.96 2024-11-06T03:23:31Z- handleDvl() - RDI_Pathfinder msg: 1730863411.96 sec; seqNo:1745; #beams:4 2024-11-06T03:23:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 16.99 , 25.28 , 29.90 , 28.68 2024-11-06T03:23:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , -0.01 2024-11-06T03:23:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863411.97 epoch sec; seqNo:5079; 4061658.38 north; 594733.60 east; 36.70 lat; -121.94 lon; 50.74 depth; utmZone: 10 2024-11-06T03:23:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863407.91 2024-11-06T03:23:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:32Z- waiting for 1730863411.91, time is 1730863411.97 2024-11-06T03:23:32Z- LcmTrn::updateTrn() >>>> pose time 1730863411.97, meas time 1730863411.96 <<<< 2024-11-06T03:23:32Z- LcmTrn::updateTrn() equating pose 1730863411.96 and meas times 1730863411.96 2024-11-06T03:23:32Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863411.96 2024-11-06T03:23:32Z- TerrainNav::motionUpdate() - currEstimate = (4061658, 594733.60, 50.74) 2024-11-06T03:23:32Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:23:32Z- TerrainNav:: SubcloudNIS is 0.71, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:23:32Z- TNavPF::Projecting Measurements 2024-11-06T03:23:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:23:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:23:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:23:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:23:32Z- TNavFilter::4 good beams. 2024-11-06T03:23:32Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:23:32Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:23:32Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:23:32Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:23:32Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:23:32Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:23:32Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:23:32Z- TerrainMapOctree: (vehN, vehE ) = (4061658.38, 594733.60). 2024-11-06T03:23:32Z- TerrainMapOctree: Min Distance = 93.08. 2024-11-06T03:23:32Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:23:32Z- Forcing Subcloud Comparison 2024-11-06T03:23:32Z- Weighting particles with subcloud comparison 2024-11-06T03:23:32Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:32Z- meanExpectedMeasDiff: -13.815423 delta_rms_squared: 0.991968 alpha: 0.980237 2024-11-06T03:23:32Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:32Z- meanExpectedMeasDiff: 1.727332 delta_rms_squared: 0.683416 alpha: 0.971567 2024-11-06T03:23:32Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:32Z- meanExpectedMeasDiff: 2.101640 delta_rms_squared: 0.720548 alpha: 0.972993 2024-11-06T03:23:32Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:32Z- meanExpectedMeasDiff: 2.464318 delta_rms_squared: 0.762307 alpha: 0.974435 2024-11-06T03:23:32Z- Subcloud NIS: 0.704288 2024-11-06T03:23:32Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:23:32Z- TNavPF:: sumWeights = 0.000000 2024-11-06T03:23:32Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:23:32Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:23:32Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:23:32Z- TerrainNav::Resampling particle filter... 2024-11-06T03:23:32Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863411.96 sec, ping # 0. 2024-11-06T03:23:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:23:32Z- variances([0], [2], and psi_berg) = 0.518940 0.109397 0.000000 2024-11-06T03:23:32Z- publishEstimatesDecoder() - at 1730863411.96 sec 2024-11-06T03:23:32Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:23:32Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061689.15 594716.18 63.89 2024-11-06T03:23:32Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061688.35 594716.30 65.09 lat/lon: 36.70 -121.94 2024-11-06T03:23:32Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.52 0.11 0.45 0.00 2024-11-06T03:23:32Z- AHRS_M2 msg: 1730863412.34 epoch sec; seqNo:4772 2024-11-06T03:23:32Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.07 psi 2024-11-06T03:23:32Z- handleDepth()- Depth_Keller msg: 1730863412.35 epoch sec; seqNo:5080; depth 50.76 2024-11-06T03:23:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863412.38 epoch sec; seqNo:5080; 4061658.76 north; 594733.60 east; 36.70 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T03:23:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863411.96 2024-11-06T03:23:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:32Z- waiting for 1730863415.96, time is 1730863412.38 2024-11-06T03:23:32Z- AHRS_M2 msg: 1730863412.73 epoch sec; seqNo:4773 2024-11-06T03:23:32Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.07 psi 2024-11-06T03:23:32Z- handleDepth()- Depth_Keller msg: 1730863412.74 epoch sec; seqNo:5081; depth 50.76 2024-11-06T03:23:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863412.75 epoch sec; seqNo:5081; 4061659.13 north; 594733.59 east; 36.70 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T03:23:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863411.96 2024-11-06T03:23:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:32Z- waiting for 1730863415.96, time is 1730863412.75 2024-11-06T03:23:33Z- AHRS_M2 msg: 1730863413.13 epoch sec; seqNo:4774 2024-11-06T03:23:33Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.07 psi 2024-11-06T03:23:33Z- handleDepth()- Depth_Keller msg: 1730863413.15 epoch sec; seqNo:5082; depth 50.77 2024-11-06T03:23:33Z- handleDVL() - DVL msg epochMillisec: 1730863413.17 2024-11-06T03:23:33Z- handleDvl() - RDI_Pathfinder msg: 1730863413.17 sec; seqNo:1746; #beams:4 2024-11-06T03:23:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.28 , 25.33 , 29.85 , 28.42 2024-11-06T03:23:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , -0.02 2024-11-06T03:23:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863413.18 epoch sec; seqNo:5082; 4061659.51 north; 594733.59 east; 36.70 lat; -121.94 lon; 50.77 depth; utmZone: 10 2024-11-06T03:23:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863411.96 2024-11-06T03:23:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:33Z- waiting for 1730863415.96, time is 1730863413.18 2024-11-06T03:23:33Z- handleDepth()- Depth_Keller msg: 1730863413.56 epoch sec; seqNo:5083; depth 50.77 2024-11-06T03:23:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863413.58 epoch sec; seqNo:5083; 4061659.89 north; 594733.58 east; 36.70 lat; -121.94 lon; 50.77 depth; utmZone: 10 2024-11-06T03:23:33Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:23:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863411.96 2024-11-06T03:23:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:33Z- waiting for 1730863415.96, time is 1730863413.58 2024-11-06T03:23:33Z- AHRS_M2 msg: 1730863413.95 epoch sec; seqNo:4775 2024-11-06T03:23:33Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.07 psi 2024-11-06T03:23:33Z- handleDepth()- Depth_Keller msg: 1730863413.96 epoch sec; seqNo:5084; depth 50.77 2024-11-06T03:23:33Z- handleDVL() - DVL msg epochMillisec: 1730863413.98 2024-11-06T03:23:33Z- handleDvl() - RDI_Pathfinder msg: 1730863413.98 sec; seqNo:1747; #beams:4 2024-11-06T03:23:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 30.45 , 25.01 , 29.77 , 28.68 2024-11-06T03:23:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , -0.04 2024-11-06T03:23:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863414.01 epoch sec; seqNo:5084; 4061660.26 north; 594732.36 east; 36.70 lat; -121.94 lon; 50.77 depth; utmZone: 10 2024-11-06T03:23:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863411.96 2024-11-06T03:23:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:34Z- waiting for 1730863415.96, time is 1730863414.01 2024-11-06T03:23:34Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:23:34Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:23:34Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:23:34Z- AHRS_M2 msg: 1730863414.34 epoch sec; seqNo:4776 2024-11-06T03:23:34Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.07 psi 2024-11-06T03:23:34Z- handleDepth()- Depth_Keller msg: 1730863414.36 epoch sec; seqNo:5085; depth 50.76 2024-11-06T03:23:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863414.38 epoch sec; seqNo:5085; 4061660.64 north; 594732.36 east; 36.70 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T03:23:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863411.96 2024-11-06T03:23:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:34Z- waiting for 1730863415.96, time is 1730863414.38 2024-11-06T03:23:34Z- AHRS_M2 msg: 1730863414.75 epoch sec; seqNo:4777 2024-11-06T03:23:34Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.07 psi 2024-11-06T03:23:34Z- handleDepth()- Depth_Keller msg: 1730863414.75 epoch sec; seqNo:5086; depth 50.77 2024-11-06T03:23:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863414.76 epoch sec; seqNo:5086; 4061661.02 north; 594732.35 east; 36.70 lat; -121.94 lon; 50.77 depth; utmZone: 10 2024-11-06T03:23:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863411.96 2024-11-06T03:23:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:34Z- waiting for 1730863415.96, time is 1730863414.76 2024-11-06T03:23:35Z- handleDepth()- Depth_Keller msg: 1730863415.17 epoch sec; seqNo:5087; depth 50.78 2024-11-06T03:23:35Z- handleDVL() - DVL msg epochMillisec: 1730863415.19 2024-11-06T03:23:35Z- handleDvl() - RDI_Pathfinder msg: 1730863415.19 sec; seqNo:1748; #beams:4 2024-11-06T03:23:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 30.72 , 25.15 , 29.50 , 28.68 2024-11-06T03:23:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.05 2024-11-06T03:23:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863415.21 epoch sec; seqNo:5087; 4061661.39 north; 594732.35 east; 36.70 lat; -121.94 lon; 50.78 depth; utmZone: 10 2024-11-06T03:23:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863411.96 2024-11-06T03:23:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:35Z- waiting for 1730863415.96, time is 1730863415.21 2024-11-06T03:23:35Z- AHRS_M2 msg: 1730863415.56 epoch sec; seqNo:4778 2024-11-06T03:23:35Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.07 psi 2024-11-06T03:23:35Z- handleDepth()- Depth_Keller msg: 1730863415.57 epoch sec; seqNo:5088; depth 50.78 2024-11-06T03:23:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863415.59 epoch sec; seqNo:5088; 4061661.77 north; 594732.34 east; 36.70 lat; -121.94 lon; 50.78 depth; utmZone: 10 2024-11-06T03:23:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863411.96 2024-11-06T03:23:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:35Z- waiting for 1730863415.96, time is 1730863415.59 2024-11-06T03:23:35Z- AHRS_M2 msg: 1730863415.96 epoch sec; seqNo:4779 2024-11-06T03:23:35Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.06 psi 2024-11-06T03:23:35Z- handleDepth()- Depth_Keller msg: 1730863415.96 epoch sec; seqNo:5089; depth 50.79 2024-11-06T03:23:36Z- handleDVL() - DVL msg epochMillisec: 1730863415.99 2024-11-06T03:23:36Z- handleDvl() - RDI_Pathfinder msg: 1730863415.99 sec; seqNo:1749; #beams:4 2024-11-06T03:23:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 30.31 , 25.69 , 29.50 , 28.68 2024-11-06T03:23:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.08 , -0.04 2024-11-06T03:23:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863416.01 epoch sec; seqNo:5089; 4061662.15 north; 594732.34 east; 36.70 lat; -121.94 lon; 50.79 depth; utmZone: 10 2024-11-06T03:23:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863411.96 2024-11-06T03:23:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:36Z- waiting for 1730863415.96, time is 1730863416.01 2024-11-06T03:23:36Z- LcmTrn::updateTrn() >>>> pose time 1730863416.01, meas time 1730863415.99 <<<< 2024-11-06T03:23:36Z- LcmTrn::updateTrn() equating pose 1730863415.99 and meas times 1730863415.99 2024-11-06T03:23:36Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863415.99 2024-11-06T03:23:36Z- TerrainNav::motionUpdate() - currEstimate = (4061662, 594732.34, 50.79) 2024-11-06T03:23:36Z- TerrainNav:: Windowed NIS is nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:23:36Z- TerrainNav:: SubcloudNIS is 0.70, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:23:36Z- TNavPF::Projecting Measurements 2024-11-06T03:23:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:23:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:23:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:23:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:23:36Z- TNavFilter::4 good beams. 2024-11-06T03:23:36Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:23:36Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:23:36Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:23:36Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:23:36Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:23:36Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:23:36Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:23:36Z- TerrainMapOctree: (vehN, vehE ) = (4061662.15, 594732.34). 2024-11-06T03:23:36Z- TerrainMapOctree: Min Distance = 89.61. 2024-11-06T03:23:36Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:23:36Z- Forcing Subcloud Comparison 2024-11-06T03:23:36Z- Weighting particles with subcloud comparison 2024-11-06T03:23:36Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:36Z- meanExpectedMeasDiff: 1.775262 delta_rms_squared: 1.122677 alpha: 0.982497 2024-11-06T03:23:36Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:36Z- meanExpectedMeasDiff: 3.326659 delta_rms_squared: 0.512860 alpha: 0.962467 2024-11-06T03:23:36Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:36Z- meanExpectedMeasDiff: 3.358452 delta_rms_squared: 0.940293 alpha: 0.979173 2024-11-06T03:23:36Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:36Z- meanExpectedMeasDiff: 4.159650 delta_rms_squared: 0.526262 alpha: 0.963388 2024-11-06T03:23:36Z- Subcloud NIS: 1.025716 2024-11-06T03:23:36Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:23:36Z- TNavPF:: sumWeights = 0.013797 2024-11-06T03:23:36Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:23:36Z- TNavPF::First Term of Map Covariance Matrix is 0.01 2024-11-06T03:23:36Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:23:36Z- TerrainNav::Resampling particle filter... 2024-11-06T03:23:36Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863415.99 sec, ping # 0. 2024-11-06T03:23:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:23:36Z- variances([0], [2], and psi_berg) = 0.805892 0.142705 0.000000 2024-11-06T03:23:36Z- publishEstimatesDecoder() - at 1730863415.99 sec 2024-11-06T03:23:36Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:23:36Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061692.93 594714.87 64.51 2024-11-06T03:23:36Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061691.89 594715.19 63.69 lat/lon: 36.70 -121.94 2024-11-06T03:23:36Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.81 0.14 0.30 0.00 2024-11-06T03:23:36Z- handleDepth()- Depth_Keller msg: 1730863416.40 epoch sec; seqNo:5090; depth 50.79 2024-11-06T03:23:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863416.42 epoch sec; seqNo:5090; 4061662.53 north; 594732.33 east; 36.70 lat; -121.94 lon; 50.79 depth; utmZone: 10 2024-11-06T03:23:36Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:23:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863415.99 2024-11-06T03:23:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:36Z- waiting for 1730863419.99, time is 1730863416.42 2024-11-06T03:23:36Z- AHRS_M2 msg: 1730863416.77 epoch sec; seqNo:4780 2024-11-06T03:23:36Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 6.06 psi 2024-11-06T03:23:36Z- handleDepth()- Depth_Keller msg: 1730863416.78 epoch sec; seqNo:5091; depth 50.79 2024-11-06T03:23:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863416.79 epoch sec; seqNo:5091; 4061662.91 north; 594732.33 east; 36.70 lat; -121.94 lon; 50.79 depth; utmZone: 10 2024-11-06T03:23:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863415.99 2024-11-06T03:23:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:36Z- waiting for 1730863419.99, time is 1730863416.79 2024-11-06T03:23:37Z- AHRS_M2 msg: 1730863417.17 epoch sec; seqNo:4781 2024-11-06T03:23:37Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 6.06 psi 2024-11-06T03:23:37Z- handleDepth()- Depth_Keller msg: 1730863417.18 epoch sec; seqNo:5092; depth 50.79 2024-11-06T03:23:37Z- handleDVL() - DVL msg epochMillisec: 1730863417.21 2024-11-06T03:23:37Z- handleDvl() - RDI_Pathfinder msg: 1730863417.21 sec; seqNo:1750; #beams:4 2024-11-06T03:23:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 30.17 , 25.83 , 29.63 , 28.54 2024-11-06T03:23:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.10 , -0.03 2024-11-06T03:23:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863417.22 epoch sec; seqNo:5092; 4061663.29 north; 594732.33 east; 36.70 lat; -121.94 lon; 50.79 depth; utmZone: 10 2024-11-06T03:23:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863415.99 2024-11-06T03:23:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:37Z- waiting for 1730863419.99, time is 1730863417.22 2024-11-06T03:23:37Z- AHRS_M2 msg: 1730863417.57 epoch sec; seqNo:4782 2024-11-06T03:23:37Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 6.06 psi 2024-11-06T03:23:37Z- handleDepth()- Depth_Keller msg: 1730863417.59 epoch sec; seqNo:5093; depth 50.80 2024-11-06T03:23:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863417.62 epoch sec; seqNo:5093; 4061663.67 north; 594732.32 east; 36.70 lat; -121.94 lon; 50.80 depth; utmZone: 10 2024-11-06T03:23:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863415.99 2024-11-06T03:23:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:37Z- waiting for 1730863419.99, time is 1730863417.62 2024-11-06T03:23:37Z- AHRS_M2 msg: 1730863417.99 epoch sec; seqNo:4783 2024-11-06T03:23:37Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.06 psi 2024-11-06T03:23:37Z- handleDepth()- Depth_Keller msg: 1730863417.99 epoch sec; seqNo:5094; depth 50.79 2024-11-06T03:23:38Z- handleDVL() - DVL msg epochMillisec: 1730863418.01 2024-11-06T03:23:38Z- handleDvl() - RDI_Pathfinder msg: 1730863418.01 sec; seqNo:1751; #beams:4 2024-11-06T03:23:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.13 , 25.83 , 30.04 , 28.54 2024-11-06T03:23:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.09 , -0.00 2024-11-06T03:23:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863418.02 epoch sec; seqNo:5094; 4061664.05 north; 594732.32 east; 36.70 lat; -121.94 lon; 50.79 depth; utmZone: 10 2024-11-06T03:23:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863415.99 2024-11-06T03:23:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:38Z- waiting for 1730863419.99, time is 1730863418.02 2024-11-06T03:23:38Z- AHRS_M2 msg: 1730863418.39 epoch sec; seqNo:4784 2024-11-06T03:23:38Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.06 psi 2024-11-06T03:23:38Z- handleDepth()- Depth_Keller msg: 1730863418.39 epoch sec; seqNo:5095; depth 50.80 2024-11-06T03:23:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863418.41 epoch sec; seqNo:5095; 4061664.03 north; 594731.10 east; 36.70 lat; -121.94 lon; 50.80 depth; utmZone: 10 2024-11-06T03:23:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863415.99 2024-11-06T03:23:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:38Z- waiting for 1730863419.99, time is 1730863418.41 2024-11-06T03:23:38Z- AHRS_M2 msg: 1730863418.79 epoch sec; seqNo:4785 2024-11-06T03:23:38Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.06 psi 2024-11-06T03:23:38Z- handleDepth()- Depth_Keller msg: 1730863418.80 epoch sec; seqNo:5096; depth 50.80 2024-11-06T03:23:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863418.82 epoch sec; seqNo:5096; 4061664.41 north; 594731.09 east; 36.70 lat; -121.94 lon; 50.80 depth; utmZone: 10 2024-11-06T03:23:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863415.99 2024-11-06T03:23:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:38Z- waiting for 1730863419.99, time is 1730863418.82 2024-11-06T03:23:39Z- AHRS_M2 msg: 1730863419.19 epoch sec; seqNo:4786 2024-11-06T03:23:39Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.06 psi 2024-11-06T03:23:39Z- handleDepth()- Depth_Keller msg: 1730863419.20 epoch sec; seqNo:5097; depth 50.80 2024-11-06T03:23:39Z- handleDVL() - DVL msg epochMillisec: 1730863419.22 2024-11-06T03:23:39Z- handleDvl() - RDI_Pathfinder msg: 1730863419.22 sec; seqNo:1752; #beams:4 2024-11-06T03:23:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.53 , 25.69 , 30.31 , 28.54 2024-11-06T03:23:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.07 , -0.01 2024-11-06T03:23:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863419.24 epoch sec; seqNo:5097; 4061664.79 north; 594731.09 east; 36.70 lat; -121.94 lon; 50.80 depth; utmZone: 10 2024-11-06T03:23:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863415.99 2024-11-06T03:23:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:39Z- waiting for 1730863419.99, time is 1730863419.24 2024-11-06T03:23:39Z- AHRS_M2 msg: 1730863419.60 epoch sec; seqNo:4787 2024-11-06T03:23:39Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.06 psi 2024-11-06T03:23:39Z- handleDepth()- Depth_Keller msg: 1730863419.60 epoch sec; seqNo:5098; depth 50.81 2024-11-06T03:23:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863419.62 epoch sec; seqNo:5098; 4061665.17 north; 594731.08 east; 36.70 lat; -121.94 lon; 50.81 depth; utmZone: 10 2024-11-06T03:23:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863415.99 2024-11-06T03:23:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:39Z- waiting for 1730863419.99, time is 1730863419.62 2024-11-06T03:23:40Z- AHRS_M2 msg: 1730863420.01 epoch sec; seqNo:4788 2024-11-06T03:23:40Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.06 psi 2024-11-06T03:23:40Z- handleDepth()- Depth_Keller msg: 1730863420.02 epoch sec; seqNo:5099; depth 50.79 2024-11-06T03:23:40Z- handleDVL() - DVL msg epochMillisec: 1730863420.04 2024-11-06T03:23:40Z- handleDvl() - RDI_Pathfinder msg: 1730863420.04 sec; seqNo:1753; #beams:4 2024-11-06T03:23:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.13 , 25.69 , 30.31 , 28.68 2024-11-06T03:23:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , -0.01 2024-11-06T03:23:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863420.07 epoch sec; seqNo:5099; 4061665.55 north; 594731.08 east; 36.70 lat; -121.94 lon; 50.79 depth; utmZone: 10 2024-11-06T03:23:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863415.99 2024-11-06T03:23:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:40Z- waiting for 1730863419.99, time is 1730863420.07 2024-11-06T03:23:40Z- LcmTrn::updateTrn() >>>> pose time 1730863420.07, meas time 1730863420.04 <<<< 2024-11-06T03:23:40Z- LcmTrn::updateTrn() equating pose 1730863420.04 and meas times 1730863420.04 2024-11-06T03:23:40Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863420.04 2024-11-06T03:23:40Z- TerrainNav::motionUpdate() - currEstimate = (4061666, 594731.08, 50.79) 2024-11-06T03:23:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:23:40Z- TerrainNav:: SubcloudNIS is 1.03, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:23:40Z- TNavPF::Projecting Measurements 2024-11-06T03:23:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:23:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:23:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:23:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:23:40Z- TNavFilter::4 good beams. 2024-11-06T03:23:40Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:23:40Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:23:40Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:23:40Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:23:40Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:23:40Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:23:40Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:23:40Z- TerrainMapOctree: (vehN, vehE ) = (4061665.55, 594731.08). 2024-11-06T03:23:40Z- TerrainMapOctree: Min Distance = 86.55. 2024-11-06T03:23:40Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:23:40Z- Forcing Subcloud Comparison 2024-11-06T03:23:40Z- Weighting particles with subcloud comparison 2024-11-06T03:23:40Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:40Z- meanExpectedMeasDiff: 0.903517 delta_rms_squared: 0.861477 alpha: 0.977311 2024-11-06T03:23:40Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:40Z- meanExpectedMeasDiff: 1.019699 delta_rms_squared: 0.586215 alpha: 0.967008 2024-11-06T03:23:40Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:40Z- meanExpectedMeasDiff: 2.055722 delta_rms_squared: 0.749839 alpha: 0.974021 2024-11-06T03:23:40Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:40Z- meanExpectedMeasDiff: 2.201219 delta_rms_squared: 0.720028 alpha: 0.972974 2024-11-06T03:23:40Z- Subcloud NIS: 0.926587 2024-11-06T03:23:40Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:23:40Z- TNavPF:: sumWeights = 0.259927 2024-11-06T03:23:40Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:23:40Z- TNavPF::First Term of Map Covariance Matrix is 0.12 2024-11-06T03:23:40Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T03:23:40Z- TerrainNav::Resampling particle filter... 2024-11-06T03:23:40Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863420.04 sec, ping # 0. 2024-11-06T03:23:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:23:40Z- variances([0], [2], and psi_berg) = 0.653053 0.130606 0.000000 2024-11-06T03:23:40Z- publishEstimatesDecoder() - at 1730863420.04 sec 2024-11-06T03:23:40Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:23:40Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061696.35 594713.56 63.70 2024-11-06T03:23:40Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061695.17 594713.96 63.06 lat/lon: 36.70 -121.94 2024-11-06T03:23:40Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.65 0.13 0.28 0.00 2024-11-06T03:23:40Z- AHRS_M2 msg: 1730863420.40 epoch sec; seqNo:4789 2024-11-06T03:23:40Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.06 psi 2024-11-06T03:23:40Z- handleDepth()- Depth_Keller msg: 1730863420.41 epoch sec; seqNo:5100; depth 50.80 2024-11-06T03:23:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863420.44 epoch sec; seqNo:5100; 4061665.93 north; 594731.08 east; 36.70 lat; -121.94 lon; 50.80 depth; utmZone: 10 2024-11-06T03:23:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863420.04 2024-11-06T03:23:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:40Z- waiting for 1730863424.04, time is 1730863420.44 2024-11-06T03:23:40Z- AHRS_M2 msg: 1730863420.81 epoch sec; seqNo:4790 2024-11-06T03:23:40Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.06 psi 2024-11-06T03:23:40Z- handleDepth()- Depth_Keller msg: 1730863420.81 epoch sec; seqNo:5101; depth 50.78 2024-11-06T03:23:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863420.82 epoch sec; seqNo:5101; 4061666.31 north; 594731.07 east; 36.70 lat; -121.94 lon; 50.78 depth; utmZone: 10 2024-11-06T03:23:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863420.04 2024-11-06T03:23:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:40Z- waiting for 1730863424.04, time is 1730863420.82 2024-11-06T03:23:41Z- AHRS_M2 msg: 1730863421.21 epoch sec; seqNo:4791 2024-11-06T03:23:41Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 6.06 psi 2024-11-06T03:23:41Z- handleDepth()- Depth_Keller msg: 1730863421.22 epoch sec; seqNo:5102; depth 50.79 2024-11-06T03:23:41Z- handleDVL() - DVL msg epochMillisec: 1730863421.24 2024-11-06T03:23:41Z- handleDvl() - RDI_Pathfinder msg: 1730863421.24 sec; seqNo:1754; #beams:4 2024-11-06T03:23:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.13 , 26.23 , 30.45 , 28.68 2024-11-06T03:23:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , -0.03 2024-11-06T03:23:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863421.25 epoch sec; seqNo:5102; 4061666.68 north; 594731.07 east; 36.70 lat; -121.94 lon; 50.79 depth; utmZone: 10 2024-11-06T03:23:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863420.04 2024-11-06T03:23:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:41Z- waiting for 1730863424.04, time is 1730863421.25 2024-11-06T03:23:41Z- AHRS_M2 msg: 1730863421.62 epoch sec; seqNo:4792 2024-11-06T03:23:41Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 6.06 psi 2024-11-06T03:23:41Z- handleDepth()- Depth_Keller msg: 1730863421.63 epoch sec; seqNo:5103; depth 50.79 2024-11-06T03:23:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863421.66 epoch sec; seqNo:5103; 4061667.06 north; 594731.06 east; 36.70 lat; -121.94 lon; 50.79 depth; utmZone: 10 2024-11-06T03:23:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863420.04 2024-11-06T03:23:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:41Z- waiting for 1730863424.04, time is 1730863421.66 2024-11-06T03:23:42Z- AHRS_M2 msg: 1730863422.02 epoch sec; seqNo:4793 2024-11-06T03:23:42Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 6.06 psi 2024-11-06T03:23:42Z- handleDepth()- Depth_Keller msg: 1730863422.02 epoch sec; seqNo:5104; depth 50.78 2024-11-06T03:23:42Z- handleDVL() - DVL msg epochMillisec: 1730863422.05 2024-11-06T03:23:42Z- handleDvl() - RDI_Pathfinder msg: 1730863422.05 sec; seqNo:1755; #beams:4 2024-11-06T03:23:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.21 , 26.23 , 30.45 , 28.68 2024-11-06T03:23:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , -0.04 2024-11-06T03:23:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863422.07 epoch sec; seqNo:5104; 4061667.44 north; 594731.06 east; 36.70 lat; -121.94 lon; 50.78 depth; utmZone: 10 2024-11-06T03:23:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863420.04 2024-11-06T03:23:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:42Z- waiting for 1730863424.04, time is 1730863422.07 2024-11-06T03:23:42Z- AHRS_M2 msg: 1730863422.42 epoch sec; seqNo:4794 2024-11-06T03:23:42Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 6.06 psi 2024-11-06T03:23:42Z- handleDepth()- Depth_Keller msg: 1730863422.43 epoch sec; seqNo:5105; depth 50.77 2024-11-06T03:23:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863422.44 epoch sec; seqNo:5105; 4061667.82 north; 594731.06 east; 36.70 lat; -121.94 lon; 50.77 depth; utmZone: 10 2024-11-06T03:23:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863420.04 2024-11-06T03:23:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:42Z- waiting for 1730863424.04, time is 1730863422.44 2024-11-06T03:23:42Z- AHRS_M2 msg: 1730863422.83 epoch sec; seqNo:4795 2024-11-06T03:23:42Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.06 psi 2024-11-06T03:23:42Z- handleDepth()- Depth_Keller msg: 1730863422.83 epoch sec; seqNo:5106; depth 50.75 2024-11-06T03:23:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863422.85 epoch sec; seqNo:5106; 4061668.20 north; 594731.05 east; 36.70 lat; -121.94 lon; 50.75 depth; utmZone: 10 2024-11-06T03:23:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863420.04 2024-11-06T03:23:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:42Z- waiting for 1730863424.04, time is 1730863422.85 2024-11-06T03:23:43Z- AHRS_M2 msg: 1730863423.23 epoch sec; seqNo:4796 2024-11-06T03:23:43Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.05 psi 2024-11-06T03:23:43Z- handleDepth()- Depth_Keller msg: 1730863423.23 epoch sec; seqNo:5107; depth 50.76 2024-11-06T03:23:43Z- handleDVL() - DVL msg epochMillisec: 1730863423.26 2024-11-06T03:23:43Z- handleDvl() - RDI_Pathfinder msg: 1730863423.26 sec; seqNo:1756; #beams:4 2024-11-06T03:23:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.62 , 26.23 , 30.58 , 28.41 2024-11-06T03:23:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.07 , -0.05 2024-11-06T03:23:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863423.28 epoch sec; seqNo:5107; 4061668.57 north; 594729.83 east; 36.70 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T03:23:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863420.04 2024-11-06T03:23:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:43Z- waiting for 1730863424.04, time is 1730863423.28 2024-11-06T03:23:43Z- AHRS_M2 msg: 1730863423.64 epoch sec; seqNo:4797 2024-11-06T03:23:43Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.05 psi 2024-11-06T03:23:43Z- handleDepth()- Depth_Keller msg: 1730863423.65 epoch sec; seqNo:5108; depth 50.75 2024-11-06T03:23:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863423.66 epoch sec; seqNo:5108; 4061668.94 north; 594729.82 east; 36.70 lat; -121.94 lon; 50.75 depth; utmZone: 10 2024-11-06T03:23:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863420.04 2024-11-06T03:23:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:43Z- waiting for 1730863424.04, time is 1730863423.66 2024-11-06T03:23:44Z- AHRS_M2 msg: 1730863424.04 epoch sec; seqNo:4798 2024-11-06T03:23:44Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.05 psi 2024-11-06T03:23:44Z- handleDepth()- Depth_Keller msg: 1730863424.05 epoch sec; seqNo:5109; depth 50.75 2024-11-06T03:23:44Z- handleDVL() - DVL msg epochMillisec: 1730863424.08 2024-11-06T03:23:44Z- handleDvl() - RDI_Pathfinder msg: 1730863424.08 sec; seqNo:1757; #beams:4 2024-11-06T03:23:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.62 , 26.64 , 30.85 , 28.54 2024-11-06T03:23:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.10 , -0.05 2024-11-06T03:23:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863424.11 epoch sec; seqNo:5109; 4061669.32 north; 594729.82 east; 36.70 lat; -121.94 lon; 50.75 depth; utmZone: 10 2024-11-06T03:23:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863420.04 2024-11-06T03:23:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:44Z- waiting for 1730863424.04, time is 1730863424.11 2024-11-06T03:23:44Z- LcmTrn::updateTrn() >>>> pose time 1730863424.11, meas time 1730863424.08 <<<< 2024-11-06T03:23:44Z- LcmTrn::updateTrn() equating pose 1730863424.08 and meas times 1730863424.08 2024-11-06T03:23:44Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863424.08 2024-11-06T03:23:44Z- TerrainNav::motionUpdate() - currEstimate = (4061669, 594729.82, 50.75) 2024-11-06T03:23:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:23:44Z- TerrainNav:: SubcloudNIS is 0.93, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:23:44Z- TNavPF::Projecting Measurements 2024-11-06T03:23:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:23:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:23:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:23:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:23:44Z- TNavFilter::4 good beams. 2024-11-06T03:23:44Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:23:44Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:23:44Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:23:44Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:23:44Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:23:44Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:23:44Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:23:44Z- TerrainMapOctree: (vehN, vehE ) = (4061669.32, 594729.82). 2024-11-06T03:23:44Z- TerrainMapOctree: Min Distance = 83.16. 2024-11-06T03:23:44Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:23:44Z- Forcing Subcloud Comparison 2024-11-06T03:23:44Z- Weighting particles with subcloud comparison 2024-11-06T03:23:44Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:44Z- meanExpectedMeasDiff: 1.636815 delta_rms_squared: 0.825388 alpha: 0.976342 2024-11-06T03:23:44Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:44Z- meanExpectedMeasDiff: 0.946352 delta_rms_squared: 0.614053 alpha: 0.968457 2024-11-06T03:23:44Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:44Z- meanExpectedMeasDiff: 1.848868 delta_rms_squared: 0.903281 alpha: 0.978338 2024-11-06T03:23:44Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:44Z- meanExpectedMeasDiff: 1.677801 delta_rms_squared: 0.671002 alpha: 0.971057 2024-11-06T03:23:44Z- Subcloud NIS: 0.891421 2024-11-06T03:23:44Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:23:44Z- TNavPF:: sumWeights = 0.310268 2024-11-06T03:23:44Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:23:44Z- TNavPF::First Term of Map Covariance Matrix is 0.25 2024-11-06T03:23:44Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T03:23:44Z- TerrainNav::Resampling particle filter... 2024-11-06T03:23:44Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863424.08 sec, ping # 0. 2024-11-06T03:23:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:23:44Z- variances([0], [2], and psi_berg) = 0.521919 0.123588 0.000000 2024-11-06T03:23:44Z- publishEstimatesDecoder() - at 1730863424.08 sec 2024-11-06T03:23:44Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:23:44Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061700.06 594712.24 63.40 2024-11-06T03:23:44Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061698.77 594712.70 62.42 lat/lon: 36.70 -121.94 2024-11-06T03:23:44Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.52 0.12 0.29 0.00 2024-11-06T03:23:44Z- AHRS_M2 msg: 1730863424.45 epoch sec; seqNo:4799 2024-11-06T03:23:44Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 6.05 psi 2024-11-06T03:23:44Z- handleDepth()- Depth_Keller msg: 1730863424.46 epoch sec; seqNo:5110; depth 50.74 2024-11-06T03:23:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863424.48 epoch sec; seqNo:5110; 4061669.70 north; 594729.81 east; 36.70 lat; -121.94 lon; 50.74 depth; utmZone: 10 2024-11-06T03:23:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863424.08 2024-11-06T03:23:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:44Z- waiting for 1730863428.08, time is 1730863424.48 2024-11-06T03:23:44Z- AHRS_M2 msg: 1730863424.85 epoch sec; seqNo:4800 2024-11-06T03:23:44Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 6.05 psi 2024-11-06T03:23:44Z- handleDepth()- Depth_Keller msg: 1730863424.85 epoch sec; seqNo:5111; depth 50.73 2024-11-06T03:23:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863424.86 epoch sec; seqNo:5111; 4061670.08 north; 594729.81 east; 36.70 lat; -121.94 lon; 50.73 depth; utmZone: 10 2024-11-06T03:23:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863424.08 2024-11-06T03:23:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:45Z- waiting for 1730863428.08, time is 1730863424.86 2024-11-06T03:23:45Z- AHRS_M2 msg: 1730863425.25 epoch sec; seqNo:4801 2024-11-06T03:23:45Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 6.05 psi 2024-11-06T03:23:45Z- handleDepth()- Depth_Keller msg: 1730863425.26 epoch sec; seqNo:5112; depth 50.73 2024-11-06T03:23:45Z- handleDVL() - DVL msg epochMillisec: 1730863425.28 2024-11-06T03:23:45Z- handleDvl() - RDI_Pathfinder msg: 1730863425.28 sec; seqNo:1758; #beams:4 2024-11-06T03:23:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.76 , 26.64 , 30.99 , 28.41 2024-11-06T03:23:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.09 , -0.03 2024-11-06T03:23:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863425.29 epoch sec; seqNo:5112; 4061670.46 north; 594729.81 east; 36.70 lat; -121.94 lon; 50.73 depth; utmZone: 10 2024-11-06T03:23:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863424.08 2024-11-06T03:23:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:45Z- waiting for 1730863428.08, time is 1730863425.29 2024-11-06T03:23:45Z- AHRS_M2 msg: 1730863425.66 epoch sec; seqNo:4802 2024-11-06T03:23:45Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 6.04 psi 2024-11-06T03:23:45Z- handleDepth()- Depth_Keller msg: 1730863425.67 epoch sec; seqNo:5113; depth 50.72 2024-11-06T03:23:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863425.68 epoch sec; seqNo:5113; 4061670.84 north; 594729.80 east; 36.70 lat; -121.94 lon; 50.72 depth; utmZone: 10 2024-11-06T03:23:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863424.08 2024-11-06T03:23:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:45Z- waiting for 1730863428.08, time is 1730863425.68 2024-11-06T03:23:46Z- AHRS_M2 msg: 1730863426.06 epoch sec; seqNo:4803 2024-11-06T03:23:46Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 6.04 psi 2024-11-06T03:23:46Z- handleDepth()- Depth_Keller msg: 1730863426.07 epoch sec; seqNo:5114; depth 50.71 2024-11-06T03:23:46Z- handleDVL() - DVL msg epochMillisec: 1730863426.10 2024-11-06T03:23:46Z- handleDvl() - RDI_Pathfinder msg: 1730863426.10 sec; seqNo:1759; #beams:4 2024-11-06T03:23:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.03 , 26.64 , 30.85 , 28.41 2024-11-06T03:23:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.10 , -0.01 2024-11-06T03:23:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863426.11 epoch sec; seqNo:5114; 4061671.22 north; 594729.80 east; 36.70 lat; -121.94 lon; 50.71 depth; utmZone: 10 2024-11-06T03:23:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863424.08 2024-11-06T03:23:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:46Z- waiting for 1730863428.08, time is 1730863426.11 2024-11-06T03:23:46Z- AHRS_M2 msg: 1730863426.46 epoch sec; seqNo:4804 2024-11-06T03:23:46Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 6.04 psi 2024-11-06T03:23:46Z- handleDepth()- Depth_Keller msg: 1730863426.47 epoch sec; seqNo:5115; depth 50.69 2024-11-06T03:23:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863426.48 epoch sec; seqNo:5115; 4061671.60 north; 594729.79 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:23:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863424.08 2024-11-06T03:23:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:46Z- waiting for 1730863428.08, time is 1730863426.48 2024-11-06T03:23:46Z- AHRS_M2 msg: 1730863426.87 epoch sec; seqNo:4805 2024-11-06T03:23:46Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.04 psi 2024-11-06T03:23:46Z- handleDepth()- Depth_Keller msg: 1730863426.87 epoch sec; seqNo:5116; depth 50.69 2024-11-06T03:23:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863426.89 epoch sec; seqNo:5116; 4061671.60 north; 594729.79 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:23:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863424.08 2024-11-06T03:23:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:47Z- waiting for 1730863428.08, time is 1730863426.89 2024-11-06T03:23:47Z- AHRS_M2 msg: 1730863427.27 epoch sec; seqNo:4806 2024-11-06T03:23:47Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.04 psi 2024-11-06T03:23:47Z- handleDepth()- Depth_Keller msg: 1730863427.28 epoch sec; seqNo:5117; depth 50.69 2024-11-06T03:23:47Z- handleDVL() - DVL msg epochMillisec: 1730863427.30 2024-11-06T03:23:47Z- handleDvl() - RDI_Pathfinder msg: 1730863427.30 sec; seqNo:1760; #beams:4 2024-11-06T03:23:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.03 , 26.64 , 31.13 , 28.54 2024-11-06T03:23:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.09 , 0.01 2024-11-06T03:23:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863427.31 epoch sec; seqNo:5117; 4061671.96 north; 594728.57 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:23:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863424.08 2024-11-06T03:23:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:47Z- waiting for 1730863428.08, time is 1730863427.31 2024-11-06T03:23:47Z- AHRS_M2 msg: 1730863427.67 epoch sec; seqNo:4807 2024-11-06T03:23:47Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.04 psi 2024-11-06T03:23:47Z- handleDepth()- Depth_Keller msg: 1730863427.69 epoch sec; seqNo:5118; depth 50.68 2024-11-06T03:23:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863427.71 epoch sec; seqNo:5118; 4061672.34 north; 594728.56 east; 36.70 lat; -121.94 lon; 50.68 depth; utmZone: 10 2024-11-06T03:23:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863424.08 2024-11-06T03:23:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:47Z- waiting for 1730863428.08, time is 1730863427.71 2024-11-06T03:23:48Z- AHRS_M2 msg: 1730863428.08 epoch sec; seqNo:4808 2024-11-06T03:23:48Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.04 psi 2024-11-06T03:23:48Z- handleDepth()- Depth_Keller msg: 1730863428.09 epoch sec; seqNo:5119; depth 50.66 2024-11-06T03:23:48Z- handleDVL() - DVL msg epochMillisec: 1730863428.11 2024-11-06T03:23:48Z- handleDvl() - RDI_Pathfinder msg: 1730863428.11 sec; seqNo:1761; #beams:4 2024-11-06T03:23:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.30 , 26.78 , 31.13 , 28.68 2024-11-06T03:23:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , 0.00 2024-11-06T03:23:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863428.14 epoch sec; seqNo:5119; 4061672.72 north; 594728.56 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T03:23:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863424.08 2024-11-06T03:23:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:48Z- waiting for 1730863428.08, time is 1730863428.14 2024-11-06T03:23:48Z- LcmTrn::updateTrn() >>>> pose time 1730863428.14, meas time 1730863428.11 <<<< 2024-11-06T03:23:48Z- LcmTrn::updateTrn() equating pose 1730863428.11 and meas times 1730863428.11 2024-11-06T03:23:48Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863428.11 2024-11-06T03:23:48Z- TerrainNav::motionUpdate() - currEstimate = (4061673, 594728.56, 50.66) 2024-11-06T03:23:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:23:48Z- TerrainNav:: SubcloudNIS is 0.89, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:23:48Z- TNavPF::Projecting Measurements 2024-11-06T03:23:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:23:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:23:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:23:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:23:48Z- TNavFilter::4 good beams. 2024-11-06T03:23:48Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:23:48Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:23:48Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:23:48Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:23:48Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:23:48Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:23:48Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:23:48Z- TerrainMapOctree: (vehN, vehE ) = (4061672.72, 594728.56). 2024-11-06T03:23:48Z- TerrainMapOctree: Min Distance = 80.20. 2024-11-06T03:23:48Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:23:48Z- Forcing Subcloud Comparison 2024-11-06T03:23:48Z- Weighting particles with subcloud comparison 2024-11-06T03:23:48Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:48Z- meanExpectedMeasDiff: 1.290236 delta_rms_squared: 0.844425 alpha: 0.976863 2024-11-06T03:23:48Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:48Z- meanExpectedMeasDiff: 0.364195 delta_rms_squared: 0.620461 alpha: 0.968772 2024-11-06T03:23:48Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:48Z- meanExpectedMeasDiff: 0.608539 delta_rms_squared: 1.095345 alpha: 0.982068 2024-11-06T03:23:48Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:48Z- meanExpectedMeasDiff: 0.852208 delta_rms_squared: 0.632872 alpha: 0.969366 2024-11-06T03:23:48Z- Subcloud NIS: 0.824423 2024-11-06T03:23:48Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:23:48Z- TNavPF:: sumWeights = 0.550540 2024-11-06T03:23:48Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:23:48Z- TNavPF::First Term of Map Covariance Matrix is 0.34 2024-11-06T03:23:48Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T03:23:48Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863428.11 sec, ping # 0. 2024-11-06T03:23:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:23:48Z- variances([0], [2], and psi_berg) = 0.516395 0.114712 0.000000 2024-11-06T03:23:48Z- publishEstimatesDecoder() - at 1730863428.11 sec 2024-11-06T03:23:48Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:23:48Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061702.64 594711.19 61.73 2024-11-06T03:23:48Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061702.19 594711.45 62.01 lat/lon: 36.70 -121.94 2024-11-06T03:23:48Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.52 0.11 0.29 0.00 2024-11-06T03:23:48Z- AHRS_M2 msg: 1730863428.48 epoch sec; seqNo:4809 2024-11-06T03:23:48Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.04 psi 2024-11-06T03:23:48Z- handleDepth()- Depth_Keller msg: 1730863428.49 epoch sec; seqNo:5120; depth 50.64 2024-11-06T03:23:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863428.50 epoch sec; seqNo:5120; 4061673.10 north; 594728.56 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:23:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863428.11 2024-11-06T03:23:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:48Z- waiting for 1730863432.11, time is 1730863428.50 2024-11-06T03:23:48Z- AHRS_M2 msg: 1730863428.89 epoch sec; seqNo:4810 2024-11-06T03:23:48Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.04 psi 2024-11-06T03:23:48Z- handleDepth()- Depth_Keller msg: 1730863428.90 epoch sec; seqNo:5121; depth 50.64 2024-11-06T03:23:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863428.91 epoch sec; seqNo:5121; 4061673.48 north; 594728.55 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:23:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863428.11 2024-11-06T03:23:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:49Z- waiting for 1730863432.11, time is 1730863428.91 2024-11-06T03:23:49Z- AHRS_M2 msg: 1730863429.29 epoch sec; seqNo:4811 2024-11-06T03:23:49Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.04 psi 2024-11-06T03:23:49Z- handleDepth()- Depth_Keller msg: 1730863429.30 epoch sec; seqNo:5122; depth 50.62 2024-11-06T03:23:49Z- handleDVL() - DVL msg epochMillisec: 1730863429.32 2024-11-06T03:23:49Z- handleDvl() - RDI_Pathfinder msg: 1730863429.32 sec; seqNo:1762; #beams:4 2024-11-06T03:23:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.17 , 26.91 , 31.53 , 28.82 2024-11-06T03:23:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.01 2024-11-06T03:23:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863429.34 epoch sec; seqNo:5122; 4061673.86 north; 594728.55 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:23:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863428.11 2024-11-06T03:23:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:49Z- waiting for 1730863432.11, time is 1730863429.34 2024-11-06T03:23:49Z- AHRS_M2 msg: 1730863429.71 epoch sec; seqNo:4812 2024-11-06T03:23:49Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.04 psi 2024-11-06T03:23:49Z- handleDepth()- Depth_Keller msg: 1730863429.71 epoch sec; seqNo:5123; depth 50.60 2024-11-06T03:23:49Z- handleDVL() - DVL msg epochMillisec: 1730863429.72 2024-11-06T03:23:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:23:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:23:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:23:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:23:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:23:49Z- handleDvl() - RDI_Pathfinder msg: 1730863429.72 sec; seqNo:1763; #beams:0 2024-11-06T03:23:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:23:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.01 2024-11-06T03:23:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863429.73 epoch sec; seqNo:5123; 4061674.23 north; 594728.54 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:23:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863428.11 2024-11-06T03:23:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:49Z- waiting for 1730863432.11, time is 1730863429.73 2024-11-06T03:23:50Z- AHRS_M2 msg: 1730863430.11 epoch sec; seqNo:4813 2024-11-06T03:23:50Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.04 psi 2024-11-06T03:23:50Z- handleDepth()- Depth_Keller msg: 1730863430.12 epoch sec; seqNo:5124; depth 50.59 2024-11-06T03:23:50Z- handleDVL() - DVL msg epochMillisec: 1730863430.15 2024-11-06T03:23:50Z- handleDvl() - RDI_Pathfinder msg: 1730863430.15 sec; seqNo:1764; #beams:4 2024-11-06T03:23:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.17 , 26.91 , 31.53 , 28.95 2024-11-06T03:23:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , -0.02 2024-11-06T03:23:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863430.17 epoch sec; seqNo:5124; 4061674.61 north; 594728.54 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:23:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863428.11 2024-11-06T03:23:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:50Z- waiting for 1730863432.11, time is 1730863430.17 2024-11-06T03:23:50Z- AHRS_M2 msg: 1730863430.50 epoch sec; seqNo:4814 2024-11-06T03:23:50Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.04 psi 2024-11-06T03:23:50Z- handleDepth()- Depth_Keller msg: 1730863430.51 epoch sec; seqNo:5125; depth 50.57 2024-11-06T03:23:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863430.52 epoch sec; seqNo:5125; 4061674.99 north; 594728.54 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T03:23:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863428.11 2024-11-06T03:23:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:50Z- waiting for 1730863432.11, time is 1730863430.52 2024-11-06T03:23:50Z- handleDepth()- Depth_Keller msg: 1730863430.93 epoch sec; seqNo:5126; depth 50.57 2024-11-06T03:23:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863430.94 epoch sec; seqNo:5126; 4061675.37 north; 594728.53 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T03:23:51Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:23:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863428.11 2024-11-06T03:23:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:51Z- waiting for 1730863432.11, time is 1730863430.94 2024-11-06T03:23:51Z- AHRS_M2 msg: 1730863431.31 epoch sec; seqNo:4815 2024-11-06T03:23:51Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.04 psi 2024-11-06T03:23:51Z- handleDepth()- Depth_Keller msg: 1730863431.32 epoch sec; seqNo:5127; depth 50.53 2024-11-06T03:23:51Z- handleDVL() - DVL msg epochMillisec: 1730863431.34 2024-11-06T03:23:51Z- handleDvl() - RDI_Pathfinder msg: 1730863431.34 sec; seqNo:1765; #beams:4 2024-11-06T03:23:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.85 , 27.18 , 31.53 , 28.95 2024-11-06T03:23:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.04 2024-11-06T03:23:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863431.36 epoch sec; seqNo:5127; 4061675.74 north; 594727.31 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:23:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863428.11 2024-11-06T03:23:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:51Z- waiting for 1730863432.11, time is 1730863431.36 2024-11-06T03:23:51Z- handleDepth()- Depth_Keller msg: 1730863431.74 epoch sec; seqNo:5128; depth 50.52 2024-11-06T03:23:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863431.76 epoch sec; seqNo:5128; 4061675.74 north; 594727.31 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:23:51Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:23:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863428.11 2024-11-06T03:23:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:51Z- waiting for 1730863432.11, time is 1730863431.76 2024-11-06T03:23:52Z- AHRS_M2 msg: 1730863432.14 epoch sec; seqNo:4816 2024-11-06T03:23:52Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.04 psi 2024-11-06T03:23:52Z- handleDepth()- Depth_Keller msg: 1730863432.15 epoch sec; seqNo:5129; depth 50.52 2024-11-06T03:23:52Z- handleDVL() - DVL msg epochMillisec: 1730863432.18 2024-11-06T03:23:52Z- handleDvl() - RDI_Pathfinder msg: 1730863432.18 sec; seqNo:1766; #beams:4 2024-11-06T03:23:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.85 , 27.05 , 31.67 , 28.95 2024-11-06T03:23:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.07 , -0.03 2024-11-06T03:23:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863432.20 epoch sec; seqNo:5129; 4061676.12 north; 594727.30 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:23:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863428.11 2024-11-06T03:23:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:52Z- waiting for 1730863432.11, time is 1730863432.20 2024-11-06T03:23:52Z- LcmTrn::updateTrn() >>>> pose time 1730863432.20, meas time 1730863432.18 <<<< 2024-11-06T03:23:52Z- LcmTrn::updateTrn() equating pose 1730863432.18 and meas times 1730863432.18 2024-11-06T03:23:52Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863432.18 2024-11-06T03:23:52Z- TerrainNav::motionUpdate() - currEstimate = (4061676, 594727.30, 50.52) 2024-11-06T03:23:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:23:52Z- TerrainNav:: SubcloudNIS is 0.82, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:23:52Z- TNavPF::Projecting Measurements 2024-11-06T03:23:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:23:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:23:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:23:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:23:52Z- TNavFilter::4 good beams. 2024-11-06T03:23:52Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:23:52Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:23:52Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:23:52Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:23:52Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:23:52Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:23:52Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:23:52Z- TerrainMapOctree: (vehN, vehE ) = (4061676.12, 594727.30). 2024-11-06T03:23:52Z- TerrainMapOctree: Min Distance = 77.29. 2024-11-06T03:23:52Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:23:52Z- Forcing Subcloud Comparison 2024-11-06T03:23:52Z- Weighting particles with subcloud comparison 2024-11-06T03:23:52Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:52Z- meanExpectedMeasDiff: 1.346160 delta_rms_squared: 0.854185 alpha: 0.977122 2024-11-06T03:23:52Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:52Z- meanExpectedMeasDiff: 0.129843 delta_rms_squared: 0.719078 alpha: 0.972939 2024-11-06T03:23:52Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:52Z- meanExpectedMeasDiff: -0.510928 delta_rms_squared: 1.352932 alpha: 0.985433 2024-11-06T03:23:52Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:52Z- meanExpectedMeasDiff: 0.832164 delta_rms_squared: 0.646943 alpha: 0.970012 2024-11-06T03:23:52Z- Subcloud NIS: 0.788476 2024-11-06T03:23:52Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:23:52Z- TNavPF:: sumWeights = 0.476567 2024-11-06T03:23:52Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:23:52Z- TNavPF::First Term of Map Covariance Matrix is 0.38 2024-11-06T03:23:52Z- TNavPF::Calculated NIS Value : 0.27 numBeams normalized NIS: 0.07 2024-11-06T03:23:52Z- TerrainNav::Resampling particle filter... 2024-11-06T03:23:52Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863432.18 sec, ping # 0. 2024-11-06T03:23:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:23:52Z- variances([0], [2], and psi_berg) = 0.555281 0.118490 0.000000 2024-11-06T03:23:52Z- publishEstimatesDecoder() - at 1730863432.18 sec 2024-11-06T03:23:52Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:23:52Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061706.24 594709.83 61.77 2024-11-06T03:23:52Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061705.61 594710.19 61.70 lat/lon: 36.70 -121.94 2024-11-06T03:23:52Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.56 0.12 0.28 0.00 2024-11-06T03:23:52Z- AHRS_M2 msg: 1730863432.54 epoch sec; seqNo:4817 2024-11-06T03:23:52Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.03 psi 2024-11-06T03:23:52Z- handleDepth()- Depth_Keller msg: 1730863432.55 epoch sec; seqNo:5130; depth 50.47 2024-11-06T03:23:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863432.60 epoch sec; seqNo:5130; 4061676.49 north; 594727.30 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:23:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863432.18 2024-11-06T03:23:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:52Z- waiting for 1730863436.18, time is 1730863432.60 2024-11-06T03:23:52Z- AHRS_M2 msg: 1730863432.93 epoch sec; seqNo:4818 2024-11-06T03:23:52Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.03 psi 2024-11-06T03:23:52Z- handleDepth()- Depth_Keller msg: 1730863432.93 epoch sec; seqNo:5131; depth 50.46 2024-11-06T03:23:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863432.95 epoch sec; seqNo:5131; 4061676.87 north; 594727.29 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:23:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863432.18 2024-11-06T03:23:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:53Z- waiting for 1730863436.18, time is 1730863432.95 2024-11-06T03:23:53Z- AHRS_M2 msg: 1730863433.33 epoch sec; seqNo:4819 2024-11-06T03:23:53Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.03 psi 2024-11-06T03:23:53Z- handleDepth()- Depth_Keller msg: 1730863433.34 epoch sec; seqNo:5132; depth 50.43 2024-11-06T03:23:53Z- handleDVL() - DVL msg epochMillisec: 1730863433.36 2024-11-06T03:23:53Z- handleDvl() - RDI_Pathfinder msg: 1730863433.36 sec; seqNo:1767; #beams:4 2024-11-06T03:23:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.57 , 27.05 , 31.67 , 28.95 2024-11-06T03:23:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.06 , -0.02 2024-11-06T03:23:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863433.38 epoch sec; seqNo:5132; 4061677.25 north; 594727.29 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:23:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863432.18 2024-11-06T03:23:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:53Z- waiting for 1730863436.18, time is 1730863433.38 2024-11-06T03:23:53Z- AHRS_M2 msg: 1730863433.74 epoch sec; seqNo:4820 2024-11-06T03:23:53Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.03 psi 2024-11-06T03:23:53Z- handleDepth()- Depth_Keller msg: 1730863433.74 epoch sec; seqNo:5133; depth 50.42 2024-11-06T03:23:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863433.77 epoch sec; seqNo:5133; 4061677.63 north; 594727.29 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:23:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863432.18 2024-11-06T03:23:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:53Z- waiting for 1730863436.18, time is 1730863433.77 2024-11-06T03:23:54Z- AHRS_M2 msg: 1730863434.17 epoch sec; seqNo:4821 2024-11-06T03:23:54Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.03 psi 2024-11-06T03:23:54Z- handleDepth()- Depth_Keller msg: 1730863434.18 epoch sec; seqNo:5134; depth 50.40 2024-11-06T03:23:54Z- handleDVL() - DVL msg epochMillisec: 1730863434.22 2024-11-06T03:23:54Z- handleDvl() - RDI_Pathfinder msg: 1730863434.22 sec; seqNo:1768; #beams:4 2024-11-06T03:23:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.39 , 27.05 , 31.81 , 29.22 2024-11-06T03:23:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.06 , -0.02 2024-11-06T03:23:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863434.23 epoch sec; seqNo:5134; 4061678.01 north; 594727.28 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:23:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863432.18 2024-11-06T03:23:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:54Z- waiting for 1730863436.18, time is 1730863434.23 2024-11-06T03:23:54Z- handleDepth()- Depth_Keller msg: 1730863434.56 epoch sec; seqNo:5135; depth 50.36 2024-11-06T03:23:54Z- handleDVL() - DVL msg epochMillisec: 1730863434.57 2024-11-06T03:23:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:23:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:23:54Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:23:54Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:23:54Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:23:54Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:23:54Z- handleDvl() - RDI_Pathfinder msg: 1730863434.57 sec; seqNo:1769; #beams:0 2024-11-06T03:23:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:23:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.06 , -0.02 2024-11-06T03:23:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863434.58 epoch sec; seqNo:5135; 4061678.39 north; 594727.28 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:23:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863432.18 2024-11-06T03:23:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:54Z- waiting for 1730863436.18, time is 1730863434.58 2024-11-06T03:23:54Z- handleDepth()- Depth_Keller msg: 1730863434.97 epoch sec; seqNo:5136; depth 50.35 2024-11-06T03:23:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863434.99 epoch sec; seqNo:5136; 4061678.77 north; 594727.27 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:23:55Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:23:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863432.18 2024-11-06T03:23:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:55Z- waiting for 1730863436.18, time is 1730863434.99 2024-11-06T03:23:55Z- AHRS_M2 msg: 1730863435.36 epoch sec; seqNo:4822 2024-11-06T03:23:55Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.03 psi 2024-11-06T03:23:55Z- handleDepth()- Depth_Keller msg: 1730863435.37 epoch sec; seqNo:5137; depth 50.33 2024-11-06T03:23:55Z- handleDVL() - DVL msg epochMillisec: 1730863435.39 2024-11-06T03:23:55Z- handleDvl() - RDI_Pathfinder msg: 1730863435.39 sec; seqNo:1770; #beams:4 2024-11-06T03:23:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.66 , 27.46 , 31.81 , 29.09 2024-11-06T03:23:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , -0.03 2024-11-06T03:23:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863435.41 epoch sec; seqNo:5137; 4061679.15 north; 594727.27 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:23:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863432.18 2024-11-06T03:23:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:55Z- waiting for 1730863436.18, time is 1730863435.41 2024-11-06T03:23:55Z- AHRS_M2 msg: 1730863435.76 epoch sec; seqNo:4823 2024-11-06T03:23:55Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.03 psi 2024-11-06T03:23:55Z- handleDepth()- Depth_Keller msg: 1730863435.77 epoch sec; seqNo:5138; depth 50.31 2024-11-06T03:23:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863435.78 epoch sec; seqNo:5138; 4061679.51 north; 594726.04 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:23:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863432.18 2024-11-06T03:23:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:55Z- waiting for 1730863436.18, time is 1730863435.78 2024-11-06T03:23:56Z- AHRS_M2 msg: 1730863436.20 epoch sec; seqNo:4824 2024-11-06T03:23:56Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.03 psi 2024-11-06T03:23:56Z- handleDepth()- Depth_Keller msg: 1730863436.20 epoch sec; seqNo:5139; depth 50.29 2024-11-06T03:23:56Z- handleDVL() - DVL msg epochMillisec: 1730863436.22 2024-11-06T03:23:56Z- handleDvl() - RDI_Pathfinder msg: 1730863436.22 sec; seqNo:1771; #beams:4 2024-11-06T03:23:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.07 , 27.46 , 31.53 , 29.36 2024-11-06T03:23:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , -0.05 2024-11-06T03:23:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863436.24 epoch sec; seqNo:5139; 4061679.89 north; 594726.04 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:23:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863432.18 2024-11-06T03:23:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:56Z- waiting for 1730863436.18, time is 1730863436.24 2024-11-06T03:23:56Z- LcmTrn::updateTrn() >>>> pose time 1730863436.24, meas time 1730863436.22 <<<< 2024-11-06T03:23:56Z- LcmTrn::updateTrn() equating pose 1730863436.22 and meas times 1730863436.22 2024-11-06T03:23:56Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863436.22 2024-11-06T03:23:56Z- TerrainNav::motionUpdate() - currEstimate = (4061680, 594726.04, 50.29) 2024-11-06T03:23:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:23:56Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:23:56Z- TNavPF::Projecting Measurements 2024-11-06T03:23:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:23:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:23:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:23:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:23:56Z- TNavFilter::4 good beams. 2024-11-06T03:23:56Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:23:56Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:23:56Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:23:56Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:23:56Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:23:56Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:23:56Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:23:56Z- TerrainMapOctree: (vehN, vehE ) = (4061679.89, 594726.04). 2024-11-06T03:23:56Z- TerrainMapOctree: Min Distance = 74.09. 2024-11-06T03:23:56Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:23:56Z- Forcing Subcloud Comparison 2024-11-06T03:23:56Z- Weighting particles with subcloud comparison 2024-11-06T03:23:56Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:56Z- meanExpectedMeasDiff: 2.003372 delta_rms_squared: 0.847999 alpha: 0.976958 2024-11-06T03:23:56Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:56Z- meanExpectedMeasDiff: 0.292688 delta_rms_squared: 1.066617 alpha: 0.981594 2024-11-06T03:23:56Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:56Z- meanExpectedMeasDiff: -1.372037 delta_rms_squared: 0.778409 alpha: 0.974950 2024-11-06T03:23:56Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:23:56Z- meanExpectedMeasDiff: -0.160770 delta_rms_squared: 0.673572 alpha: 0.971164 2024-11-06T03:23:56Z- Subcloud NIS: 0.759092 2024-11-06T03:23:56Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:23:56Z- TNavPF:: sumWeights = 0.312124 2024-11-06T03:23:56Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:23:56Z- TNavPF::First Term of Map Covariance Matrix is 0.67 2024-11-06T03:23:56Z- TNavPF::Calculated NIS Value : 0.21 numBeams normalized NIS: 0.05 2024-11-06T03:23:56Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863436.22 sec, ping # 0. 2024-11-06T03:23:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:23:56Z- variances([0], [2], and psi_berg) = 0.564072 0.117918 0.000000 2024-11-06T03:23:56Z- publishEstimatesDecoder() - at 1730863436.22 sec 2024-11-06T03:23:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:23:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061709.20 594709.15 61.38 2024-11-06T03:23:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061709.38 594708.95 61.40 lat/lon: 36.70 -121.94 2024-11-06T03:23:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.56 0.12 0.27 0.00 2024-11-06T03:23:56Z- AHRS_M2 msg: 1730863436.56 epoch sec; seqNo:4825 2024-11-06T03:23:56Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.03 psi 2024-11-06T03:23:56Z- handleDepth()- Depth_Keller msg: 1730863436.57 epoch sec; seqNo:5140; depth 50.26 2024-11-06T03:23:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863436.59 epoch sec; seqNo:5140; 4061680.27 north; 594726.04 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:23:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863436.22 2024-11-06T03:23:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:56Z- waiting for 1730863440.22, time is 1730863436.59 2024-11-06T03:23:56Z- AHRS_M2 msg: 1730863436.97 epoch sec; seqNo:4826 2024-11-06T03:23:56Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.03 psi 2024-11-06T03:23:56Z- handleDepth()- Depth_Keller msg: 1730863436.97 epoch sec; seqNo:5141; depth 50.24 2024-11-06T03:23:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863436.98 epoch sec; seqNo:5141; 4061680.65 north; 594726.03 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:23:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863436.22 2024-11-06T03:23:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:57Z- waiting for 1730863440.22, time is 1730863436.98 2024-11-06T03:23:57Z- AHRS_M2 msg: 1730863437.37 epoch sec; seqNo:4827 2024-11-06T03:23:57Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.03 psi 2024-11-06T03:23:57Z- handleDepth()- Depth_Keller msg: 1730863437.38 epoch sec; seqNo:5142; depth 50.22 2024-11-06T03:23:57Z- handleDVL() - DVL msg epochMillisec: 1730863437.40 2024-11-06T03:23:57Z- handleDvl() - RDI_Pathfinder msg: 1730863437.40 sec; seqNo:1772; #beams:4 2024-11-06T03:23:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.71 , 27.83 , 31.81 , 29.51 2024-11-06T03:23:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.07 , -0.07 2024-11-06T03:23:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863437.42 epoch sec; seqNo:5142; 4061681.03 north; 594726.03 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:23:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863436.22 2024-11-06T03:23:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:57Z- waiting for 1730863440.22, time is 1730863437.42 2024-11-06T03:23:57Z- AHRS_M2 msg: 1730863437.78 epoch sec; seqNo:4828 2024-11-06T03:23:57Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.03 psi 2024-11-06T03:23:57Z- handleDepth()- Depth_Keller msg: 1730863437.78 epoch sec; seqNo:5143; depth 50.21 2024-11-06T03:23:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863437.80 epoch sec; seqNo:5143; 4061681.41 north; 594726.02 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:23:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863436.22 2024-11-06T03:23:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:57Z- waiting for 1730863440.22, time is 1730863437.80 2024-11-06T03:23:58Z- AHRS_M2 msg: 1730863438.22 epoch sec; seqNo:4829 2024-11-06T03:23:58Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 6.03 psi 2024-11-06T03:23:58Z- handleDepth()- Depth_Keller msg: 1730863438.23 epoch sec; seqNo:5144; depth 50.19 2024-11-06T03:23:58Z- handleDVL() - DVL msg epochMillisec: 1730863438.26 2024-11-06T03:23:58Z- handleDvl() - RDI_Pathfinder msg: 1730863438.26 sec; seqNo:1773; #beams:4 2024-11-06T03:23:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.01 , 28.13 , 31.80 , 29.66 2024-11-06T03:23:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.10 , -0.07 2024-11-06T03:23:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863438.27 epoch sec; seqNo:5144; 4061681.78 north; 594726.02 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:23:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863436.22 2024-11-06T03:23:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:58Z- waiting for 1730863440.22, time is 1730863438.27 2024-11-06T03:23:58Z- AHRS_M2 msg: 1730863438.59 epoch sec; seqNo:4830 2024-11-06T03:23:58Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 6.03 psi 2024-11-06T03:23:58Z- handleDepth()- Depth_Keller msg: 1730863438.60 epoch sec; seqNo:5145; depth 50.16 2024-11-06T03:23:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863438.62 epoch sec; seqNo:5145; 4061682.16 north; 594726.01 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:23:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863436.22 2024-11-06T03:23:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:58Z- waiting for 1730863440.22, time is 1730863438.62 2024-11-06T03:23:59Z- AHRS_M2 msg: 1730863438.99 epoch sec; seqNo:4831 2024-11-06T03:23:59Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 6.03 psi 2024-11-06T03:23:59Z- handleDepth()- Depth_Keller msg: 1730863439.01 epoch sec; seqNo:5146; depth 50.15 2024-11-06T03:23:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863439.01 epoch sec; seqNo:5146; 4061682.16 north; 594726.01 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:23:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863436.22 2024-11-06T03:23:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:59Z- waiting for 1730863440.22, time is 1730863439.01 2024-11-06T03:23:59Z- AHRS_M2 msg: 1730863439.40 epoch sec; seqNo:4832 2024-11-06T03:23:59Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 6.03 psi 2024-11-06T03:23:59Z- handleDepth()- Depth_Keller msg: 1730863439.41 epoch sec; seqNo:5147; depth 50.13 2024-11-06T03:23:59Z- handleDVL() - DVL msg epochMillisec: 1730863439.43 2024-11-06T03:23:59Z- handleDvl() - RDI_Pathfinder msg: 1730863439.43 sec; seqNo:1774; #beams:4 2024-11-06T03:23:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.17 , 28.44 , 31.80 , 29.66 2024-11-06T03:23:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.11 , -0.06 2024-11-06T03:23:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863439.44 epoch sec; seqNo:5147; 4061682.54 north; 594726.01 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:23:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:23:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863436.22 2024-11-06T03:23:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:59Z- waiting for 1730863440.22, time is 1730863439.44 2024-11-06T03:23:59Z- AHRS_M2 msg: 1730863439.81 epoch sec; seqNo:4833 2024-11-06T03:23:59Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 6.02 psi 2024-11-06T03:23:59Z- handleDepth()- Depth_Keller msg: 1730863439.81 epoch sec; seqNo:5148; depth 50.12 2024-11-06T03:23:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863439.88 epoch sec; seqNo:5148; 4061682.91 north; 594724.79 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:23:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:23:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863436.22 2024-11-06T03:23:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:23:59Z- waiting for 1730863440.22, time is 1730863439.88 2024-11-06T03:24:00Z- AHRS_M2 msg: 1730863440.25 epoch sec; seqNo:4834 2024-11-06T03:24:00Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 6.02 psi 2024-11-06T03:24:00Z- handleDepth()- Depth_Keller msg: 1730863440.26 epoch sec; seqNo:5149; depth 50.11 2024-11-06T03:24:00Z- handleDVL() - DVL msg epochMillisec: 1730863440.29 2024-11-06T03:24:00Z- handleDvl() - RDI_Pathfinder msg: 1730863440.29 sec; seqNo:1775; #beams:4 2024-11-06T03:24:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.17 , 28.44 , 31.80 , 29.66 2024-11-06T03:24:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.13 , -0.04 2024-11-06T03:24:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863440.30 epoch sec; seqNo:5149; 4061683.29 north; 594724.78 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:24:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863436.22 2024-11-06T03:24:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:00Z- waiting for 1730863440.22, time is 1730863440.30 2024-11-06T03:24:00Z- LcmTrn::updateTrn() >>>> pose time 1730863440.30, meas time 1730863440.29 <<<< 2024-11-06T03:24:00Z- LcmTrn::updateTrn() equating pose 1730863440.29 and meas times 1730863440.29 2024-11-06T03:24:00Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863440.29 2024-11-06T03:24:00Z- TerrainNav::motionUpdate() - currEstimate = (4061683, 594724.78, 50.11) 2024-11-06T03:24:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:24:00Z- TerrainNav:: SubcloudNIS is 0.76, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:24:00Z- TNavPF::Projecting Measurements 2024-11-06T03:24:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:24:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:24:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:24:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:24:00Z- TNavFilter::4 good beams. 2024-11-06T03:24:00Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:24:00Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:24:00Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:24:00Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:24:00Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:24:00Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:24:00Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:24:00Z- TerrainMapOctree: (vehN, vehE ) = (4061683.29, 594724.78). 2024-11-06T03:24:00Z- TerrainMapOctree: Min Distance = 71.32. 2024-11-06T03:24:00Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:24:00Z- Forcing Subcloud Comparison 2024-11-06T03:24:00Z- Weighting particles with subcloud comparison 2024-11-06T03:24:00Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:00Z- meanExpectedMeasDiff: 1.911699 delta_rms_squared: 0.811136 alpha: 0.975937 2024-11-06T03:24:00Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:00Z- meanExpectedMeasDiff: 0.798568 delta_rms_squared: 1.006840 alpha: 0.980523 2024-11-06T03:24:00Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:00Z- meanExpectedMeasDiff: -1.133216 delta_rms_squared: 0.737842 alpha: 0.973609 2024-11-06T03:24:00Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:00Z- meanExpectedMeasDiff: -0.302841 delta_rms_squared: 0.656680 alpha: 0.970444 2024-11-06T03:24:00Z- Subcloud NIS: 0.726242 2024-11-06T03:24:00Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:24:00Z- TNavPF:: sumWeights = 0.335679 2024-11-06T03:24:00Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:24:00Z- TNavPF::First Term of Map Covariance Matrix is 0.60 2024-11-06T03:24:00Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T03:24:00Z- TerrainNav::Resampling particle filter... 2024-11-06T03:24:00Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863440.29 sec, ping # 0. 2024-11-06T03:24:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:24:00Z- variances([0], [2], and psi_berg) = 0.571661 0.118083 0.000000 2024-11-06T03:24:00Z- publishEstimatesDecoder() - at 1730863440.29 sec 2024-11-06T03:24:00Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:24:00Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061713.75 594707.58 62.00 2024-11-06T03:24:00Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061712.77 594707.70 61.11 lat/lon: 36.70 -121.94 2024-11-06T03:24:00Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.12 0.27 0.00 2024-11-06T03:24:00Z- AHRS_M2 msg: 1730863440.63 epoch sec; seqNo:4835 2024-11-06T03:24:00Z- AHRS_M2 msg: 0.12 phi; 0.05 theta; 6.02 psi 2024-11-06T03:24:00Z- handleDepth()- Depth_Keller msg: 1730863440.63 epoch sec; seqNo:5150; depth 50.10 2024-11-06T03:24:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863440.67 epoch sec; seqNo:5150; 4061683.67 north; 594724.78 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:24:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863440.29 2024-11-06T03:24:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:00Z- waiting for 1730863444.29, time is 1730863440.67 2024-11-06T03:24:01Z- AHRS_M2 msg: 1730863441.01 epoch sec; seqNo:4836 2024-11-06T03:24:01Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 6.02 psi 2024-11-06T03:24:01Z- handleDepth()- Depth_Keller msg: 1730863441.03 epoch sec; seqNo:5151; depth 50.09 2024-11-06T03:24:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863441.04 epoch sec; seqNo:5151; 4061684.04 north; 594724.77 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:24:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863440.29 2024-11-06T03:24:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:01Z- waiting for 1730863444.29, time is 1730863441.04 2024-11-06T03:24:01Z- AHRS_M2 msg: 1730863441.42 epoch sec; seqNo:4837 2024-11-06T03:24:01Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.02 psi 2024-11-06T03:24:01Z- handleDepth()- Depth_Keller msg: 1730863441.43 epoch sec; seqNo:5152; depth 50.09 2024-11-06T03:24:01Z- handleDVL() - DVL msg epochMillisec: 1730863441.45 2024-11-06T03:24:01Z- handleDvl() - RDI_Pathfinder msg: 1730863441.45 sec; seqNo:1776; #beams:4 2024-11-06T03:24:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.79 , 28.29 , 31.96 , 29.97 2024-11-06T03:24:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.12 , -0.00 2024-11-06T03:24:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863441.46 epoch sec; seqNo:5152; 4061684.42 north; 594724.77 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:24:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863440.29 2024-11-06T03:24:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:01Z- waiting for 1730863444.29, time is 1730863441.46 2024-11-06T03:24:01Z- AHRS_M2 msg: 1730863441.83 epoch sec; seqNo:4838 2024-11-06T03:24:01Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.02 psi 2024-11-06T03:24:01Z- handleDepth()- Depth_Keller msg: 1730863441.83 epoch sec; seqNo:5153; depth 50.07 2024-11-06T03:24:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863441.84 epoch sec; seqNo:5153; 4061684.80 north; 594724.77 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:24:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863440.29 2024-11-06T03:24:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:01Z- waiting for 1730863444.29, time is 1730863441.84 2024-11-06T03:24:02Z- AHRS_M2 msg: 1730863442.28 epoch sec; seqNo:4839 2024-11-06T03:24:02Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.02 psi 2024-11-06T03:24:02Z- handleDepth()- Depth_Keller msg: 1730863442.28 epoch sec; seqNo:5154; depth 50.07 2024-11-06T03:24:02Z- handleDVL() - DVL msg epochMillisec: 1730863442.31 2024-11-06T03:24:02Z- handleDvl() - RDI_Pathfinder msg: 1730863442.31 sec; seqNo:1777; #beams:4 2024-11-06T03:24:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.03 , 28.44 , 31.80 , 29.97 2024-11-06T03:24:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.10 , 0.02 2024-11-06T03:24:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863442.32 epoch sec; seqNo:5154; 4061685.18 north; 594724.76 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:24:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863440.29 2024-11-06T03:24:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:02Z- waiting for 1730863444.29, time is 1730863442.32 2024-11-06T03:24:02Z- AHRS_M2 msg: 1730863442.63 epoch sec; seqNo:4840 2024-11-06T03:24:02Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.02 psi 2024-11-06T03:24:02Z- handleDepth()- Depth_Keller msg: 1730863442.64 epoch sec; seqNo:5155; depth 50.07 2024-11-06T03:24:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863442.66 epoch sec; seqNo:5155; 4061685.56 north; 594724.76 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:24:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863440.29 2024-11-06T03:24:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:02Z- waiting for 1730863444.29, time is 1730863442.66 2024-11-06T03:24:03Z- AHRS_M2 msg: 1730863443.04 epoch sec; seqNo:4841 2024-11-06T03:24:03Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.02 psi 2024-11-06T03:24:03Z- handleDepth()- Depth_Keller msg: 1730863443.04 epoch sec; seqNo:5156; depth 50.07 2024-11-06T03:24:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863443.05 epoch sec; seqNo:5156; 4061685.94 north; 594724.75 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:24:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863440.29 2024-11-06T03:24:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:03Z- waiting for 1730863444.29, time is 1730863443.05 2024-11-06T03:24:03Z- handleDepth()- Depth_Keller msg: 1730863443.46 epoch sec; seqNo:5157; depth 50.05 2024-11-06T03:24:03Z- handleDVL() - DVL msg epochMillisec: 1730863443.48 2024-11-06T03:24:03Z- handleDvl() - RDI_Pathfinder msg: 1730863443.48 sec; seqNo:1778; #beams:4 2024-11-06T03:24:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.42 , 28.29 , 31.96 , 30.12 2024-11-06T03:24:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , 0.02 2024-11-06T03:24:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863443.50 epoch sec; seqNo:5157; 4061686.30 north; 594723.53 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:24:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863440.29 2024-11-06T03:24:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:03Z- waiting for 1730863444.29, time is 1730863443.50 2024-11-06T03:24:03Z- AHRS_M2 msg: 1730863443.84 epoch sec; seqNo:4842 2024-11-06T03:24:03Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.02 psi 2024-11-06T03:24:03Z- handleDepth()- Depth_Keller msg: 1730863443.85 epoch sec; seqNo:5158; depth 50.06 2024-11-06T03:24:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863443.88 epoch sec; seqNo:5158; 4061686.68 north; 594723.52 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:24:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863440.29 2024-11-06T03:24:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:04Z- waiting for 1730863444.29, time is 1730863443.88 2024-11-06T03:24:04Z- AHRS_M2 msg: 1730863444.29 epoch sec; seqNo:4843 2024-11-06T03:24:04Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.02 psi 2024-11-06T03:24:04Z- handleDepth()- Depth_Keller msg: 1730863444.31 epoch sec; seqNo:5159; depth 50.05 2024-11-06T03:24:04Z- handleDVL() - DVL msg epochMillisec: 1730863444.33 2024-11-06T03:24:04Z- handleDvl() - RDI_Pathfinder msg: 1730863444.33 sec; seqNo:1779; #beams:4 2024-11-06T03:24:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.94 , 28.41 , 31.94 , 30.31 2024-11-06T03:24:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , 0.02 2024-11-06T03:24:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863444.36 epoch sec; seqNo:5159; 4061687.06 north; 594723.52 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:24:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863440.29 2024-11-06T03:24:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:04Z- waiting for 1730863444.29, time is 1730863444.36 2024-11-06T03:24:04Z- LcmTrn::updateTrn() >>>> pose time 1730863444.36, meas time 1730863444.33 <<<< 2024-11-06T03:24:04Z- LcmTrn::updateTrn() equating pose 1730863444.33 and meas times 1730863444.33 2024-11-06T03:24:04Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863444.33 2024-11-06T03:24:04Z- TerrainNav::motionUpdate() - currEstimate = (4061687, 594723.52, 50.05) 2024-11-06T03:24:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:24:04Z- TerrainNav:: SubcloudNIS is 0.73, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:24:04Z- TNavPF::Projecting Measurements 2024-11-06T03:24:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:24:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:24:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:24:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:24:04Z- TNavFilter::4 good beams. 2024-11-06T03:24:04Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:24:04Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:24:04Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:24:04Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:24:04Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:24:04Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:24:04Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:24:04Z- TerrainMapOctree: (vehN, vehE ) = (4061687.06, 594723.52). 2024-11-06T03:24:04Z- TerrainMapOctree: Min Distance = 68.28. 2024-11-06T03:24:04Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:24:04Z- Forcing Subcloud Comparison 2024-11-06T03:24:04Z- Weighting particles with subcloud comparison 2024-11-06T03:24:04Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:04Z- meanExpectedMeasDiff: -1.713993 delta_rms_squared: 0.812699 alpha: 0.975982 2024-11-06T03:24:04Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:04Z- meanExpectedMeasDiff: -0.292146 delta_rms_squared: 0.900778 alpha: 0.978279 2024-11-06T03:24:04Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:04Z- meanExpectedMeasDiff: -1.102069 delta_rms_squared: 0.727780 alpha: 0.973254 2024-11-06T03:24:04Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:04Z- meanExpectedMeasDiff: 0.005160 delta_rms_squared: 0.658038 alpha: 0.970503 2024-11-06T03:24:04Z- Subcloud NIS: 0.706737 2024-11-06T03:24:04Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:24:04Z- TNavPF:: sumWeights = 0.452330 2024-11-06T03:24:04Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:24:04Z- TNavPF::First Term of Map Covariance Matrix is 0.44 2024-11-06T03:24:04Z- TNavPF::Calculated NIS Value : 0.24 numBeams normalized NIS: 0.06 2024-11-06T03:24:04Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863444.33 sec, ping # 0. 2024-11-06T03:24:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:24:04Z- variances([0], [2], and psi_berg) = 0.602928 0.120334 0.000000 2024-11-06T03:24:04Z- publishEstimatesDecoder() - at 1730863444.33 sec 2024-11-06T03:24:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:24:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061715.79 594706.79 61.75 2024-11-06T03:24:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061716.59 594706.44 61.35 lat/lon: 36.70 -121.94 2024-11-06T03:24:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.60 0.12 0.27 0.00 2024-11-06T03:24:04Z- AHRS_M2 msg: 1730863444.66 epoch sec; seqNo:4844 2024-11-06T03:24:04Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.02 psi 2024-11-06T03:24:04Z- handleDepth()- Depth_Keller msg: 1730863444.67 epoch sec; seqNo:5160; depth 50.03 2024-11-06T03:24:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863444.70 epoch sec; seqNo:5160; 4061687.06 north; 594723.52 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:24:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863444.33 2024-11-06T03:24:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:04Z- waiting for 1730863448.33, time is 1730863444.70 2024-11-06T03:24:05Z- AHRS_M2 msg: 1730863445.06 epoch sec; seqNo:4845 2024-11-06T03:24:05Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.02 psi 2024-11-06T03:24:05Z- handleDepth()- Depth_Keller msg: 1730863445.07 epoch sec; seqNo:5161; depth 50.02 2024-11-06T03:24:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863445.08 epoch sec; seqNo:5161; 4061687.44 north; 594723.52 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:24:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863444.33 2024-11-06T03:24:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:05Z- waiting for 1730863448.33, time is 1730863445.08 2024-11-06T03:24:05Z- AHRS_M2 msg: 1730863445.46 epoch sec; seqNo:4846 2024-11-06T03:24:05Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.02 psi 2024-11-06T03:24:05Z- handleDepth()- Depth_Keller msg: 1730863445.47 epoch sec; seqNo:5162; depth 50.02 2024-11-06T03:24:05Z- handleDVL() - DVL msg epochMillisec: 1730863445.51 2024-11-06T03:24:05Z- handleDvl() - RDI_Pathfinder msg: 1730863445.51 sec; seqNo:1780; #beams:4 2024-11-06T03:24:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.08 , 28.00 , 32.08 , 30.44 2024-11-06T03:24:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , -0.00 2024-11-06T03:24:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863445.52 epoch sec; seqNo:5162; 4061687.82 north; 594723.51 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:24:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863444.33 2024-11-06T03:24:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:05Z- waiting for 1730863448.33, time is 1730863445.52 2024-11-06T03:24:05Z- AHRS_M2 msg: 1730863445.87 epoch sec; seqNo:4847 2024-11-06T03:24:05Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.02 psi 2024-11-06T03:24:05Z- handleDepth()- Depth_Keller msg: 1730863445.88 epoch sec; seqNo:5163; depth 50.00 2024-11-06T03:24:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863445.90 epoch sec; seqNo:5163; 4061688.20 north; 594723.51 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:24:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863444.33 2024-11-06T03:24:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:06Z- waiting for 1730863448.33, time is 1730863445.90 2024-11-06T03:24:06Z- AHRS_M2 msg: 1730863446.27 epoch sec; seqNo:4848 2024-11-06T03:24:06Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.02 psi 2024-11-06T03:24:06Z- handleDepth()- Depth_Keller msg: 1730863446.29 epoch sec; seqNo:5164; depth 50.00 2024-11-06T03:24:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863446.34 epoch sec; seqNo:5164; 4061688.58 north; 594723.50 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:24:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863444.33 2024-11-06T03:24:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:06Z- waiting for 1730863448.33, time is 1730863446.34 2024-11-06T03:24:06Z- AHRS_M2 msg: 1730863446.67 epoch sec; seqNo:4849 2024-11-06T03:24:06Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.02 psi 2024-11-06T03:24:06Z- handleDepth()- Depth_Keller msg: 1730863446.68 epoch sec; seqNo:5165; depth 49.98 2024-11-06T03:24:06Z- handleDVL() - DVL msg epochMillisec: 1730863446.70 2024-11-06T03:24:06Z- handleDvl() - RDI_Pathfinder msg: 1730863446.70 sec; seqNo:1781; #beams:4 2024-11-06T03:24:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.49 , 28.14 , 32.08 , 30.72 2024-11-06T03:24:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , -0.02 2024-11-06T03:24:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863446.71 epoch sec; seqNo:5165; 4061688.96 north; 594723.50 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:24:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863444.33 2024-11-06T03:24:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:06Z- waiting for 1730863448.33, time is 1730863446.71 2024-11-06T03:24:07Z- AHRS_M2 msg: 1730863447.07 epoch sec; seqNo:4850 2024-11-06T03:24:07Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.02 psi 2024-11-06T03:24:07Z- handleDepth()- Depth_Keller msg: 1730863447.08 epoch sec; seqNo:5166; depth 49.96 2024-11-06T03:24:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863447.11 epoch sec; seqNo:5166; 4061689.33 north; 594723.49 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:24:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863444.33 2024-11-06T03:24:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:07Z- waiting for 1730863448.33, time is 1730863447.11 2024-11-06T03:24:07Z- AHRS_M2 msg: 1730863447.48 epoch sec; seqNo:4851 2024-11-06T03:24:07Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.02 psi 2024-11-06T03:24:07Z- handleDepth()- Depth_Keller msg: 1730863447.48 epoch sec; seqNo:5167; depth 49.95 2024-11-06T03:24:07Z- handleDVL() - DVL msg epochMillisec: 1730863447.50 2024-11-06T03:24:07Z- handleDvl() - RDI_Pathfinder msg: 1730863447.50 sec; seqNo:1782; #beams:4 2024-11-06T03:24:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.42 , 27.98 , 31.96 , 30.74 2024-11-06T03:24:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , -0.04 2024-11-06T03:24:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863447.52 epoch sec; seqNo:5167; 4061689.71 north; 594723.49 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:24:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863444.33 2024-11-06T03:24:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:07Z- waiting for 1730863448.33, time is 1730863447.52 2024-11-06T03:24:07Z- AHRS_M2 msg: 1730863447.89 epoch sec; seqNo:4852 2024-11-06T03:24:07Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.02 psi 2024-11-06T03:24:07Z- handleDepth()- Depth_Keller msg: 1730863447.89 epoch sec; seqNo:5168; depth 49.93 2024-11-06T03:24:07Z- handleDVL() - DVL msg epochMillisec: 1730863447.89 2024-11-06T03:24:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:24:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:24:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:24:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:24:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:24:07Z- handleDvl() - RDI_Pathfinder msg: 1730863447.89 sec; seqNo:1783; #beams:0 2024-11-06T03:24:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:24:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , -0.04 2024-11-06T03:24:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863447.91 epoch sec; seqNo:5168; 4061690.08 north; 594722.27 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:24:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863444.33 2024-11-06T03:24:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:08Z- waiting for 1730863448.33, time is 1730863447.91 2024-11-06T03:24:08Z- AHRS_M2 msg: 1730863448.29 epoch sec; seqNo:4853 2024-11-06T03:24:08Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.02 psi 2024-11-06T03:24:08Z- handleDepth()- Depth_Keller msg: 1730863448.30 epoch sec; seqNo:5169; depth 49.91 2024-11-06T03:24:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863448.37 epoch sec; seqNo:5169; 4061690.46 north; 594722.26 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:24:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863444.33 2024-11-06T03:24:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:08Z- waiting for 1730863448.33, time is 1730863448.37 2024-11-06T03:24:08Z- LcmTrn::updateTrn() >>>> pose time 1730863448.37, meas time 1730863447.89 <<<< 2024-11-06T03:24:08Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863448.37 2024-11-06T03:24:08Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:24:08Z- TerrainNav::motionUpdate() - currEstimate = (4061690, 594722.26, 49.91) 2024-11-06T03:24:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:24:08Z- TerrainNav:: SubcloudNIS is 0.71, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:24:08Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:24:08Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:24:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:24:08Z- variances([0], [2], and psi_berg) = 0.604151 0.122441 0.000000 2024-11-06T03:24:08Z- publishEstimatesDecoder() - at 1730863448.37 sec 2024-11-06T03:24:08Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:24:08Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061719.19 594705.56 60.59 2024-11-06T03:24:08Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061719.99 594705.19 61.22 lat/lon: 36.70 -121.94 2024-11-06T03:24:08Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.60 0.12 0.53 0.00 2024-11-06T03:24:08Z- AHRS_M2 msg: 1730863448.69 epoch sec; seqNo:4854 2024-11-06T03:24:08Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.02 psi 2024-11-06T03:24:08Z- handleDepth()- Depth_Keller msg: 1730863448.70 epoch sec; seqNo:5170; depth 49.90 2024-11-06T03:24:08Z- handleDVL() - DVL msg epochMillisec: 1730863448.73 2024-11-06T03:24:08Z- handleDvl() - RDI_Pathfinder msg: 1730863448.73 sec; seqNo:1784; #beams:4 2024-11-06T03:24:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.08 , 27.86 , 32.08 , 30.58 2024-11-06T03:24:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , -0.06 2024-11-06T03:24:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863448.74 epoch sec; seqNo:5170; 4061690.84 north; 594722.26 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:24:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863448.37 2024-11-06T03:24:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:08Z- waiting for 1730863452.37, time is 1730863448.74 2024-11-06T03:24:09Z- AHRS_M2 msg: 1730863449.09 epoch sec; seqNo:4855 2024-11-06T03:24:09Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.01 psi 2024-11-06T03:24:09Z- handleDepth()- Depth_Keller msg: 1730863449.10 epoch sec; seqNo:5171; depth 49.86 2024-11-06T03:24:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863449.12 epoch sec; seqNo:5171; 4061691.22 north; 594722.25 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:24:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863448.37 2024-11-06T03:24:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:09Z- waiting for 1730863452.37, time is 1730863449.12 2024-11-06T03:24:09Z- AHRS_M2 msg: 1730863449.50 epoch sec; seqNo:4856 2024-11-06T03:24:09Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.01 psi 2024-11-06T03:24:09Z- handleDepth()- Depth_Keller msg: 1730863449.51 epoch sec; seqNo:5172; depth 49.86 2024-11-06T03:24:09Z- handleDVL() - DVL msg epochMillisec: 1730863449.53 2024-11-06T03:24:09Z- handleDvl() - RDI_Pathfinder msg: 1730863449.53 sec; seqNo:1785; #beams:4 2024-11-06T03:24:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.21 , 28.14 , 31.81 , 30.72 2024-11-06T03:24:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.07 , -0.06 2024-11-06T03:24:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863449.54 epoch sec; seqNo:5172; 4061691.59 north; 594722.25 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:24:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863448.37 2024-11-06T03:24:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:09Z- waiting for 1730863452.37, time is 1730863449.54 2024-11-06T03:24:09Z- AHRS_M2 msg: 1730863449.90 epoch sec; seqNo:4857 2024-11-06T03:24:09Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.01 psi 2024-11-06T03:24:09Z- handleDepth()- Depth_Keller msg: 1730863449.91 epoch sec; seqNo:5173; depth 49.85 2024-11-06T03:24:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863449.92 epoch sec; seqNo:5173; 4061691.59 north; 594722.25 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:24:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863448.37 2024-11-06T03:24:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:10Z- waiting for 1730863452.37, time is 1730863449.92 2024-11-06T03:24:10Z- handleDepth()- Depth_Keller msg: 1730863450.33 epoch sec; seqNo:5174; depth 49.81 2024-11-06T03:24:10Z- handleDVL() - DVL msg epochMillisec: 1730863450.33 2024-11-06T03:24:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:24:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:24:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:24:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:24:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:24:10Z- handleDvl() - RDI_Pathfinder msg: 1730863450.33 sec; seqNo:1786; #beams:0 2024-11-06T03:24:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:24:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.07 , -0.06 2024-11-06T03:24:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863450.40 epoch sec; seqNo:5174; 4061692.35 north; 594722.24 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:24:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863448.37 2024-11-06T03:24:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:10Z- waiting for 1730863452.37, time is 1730863450.40 2024-11-06T03:24:10Z- AHRS_M2 msg: 1730863450.71 epoch sec; seqNo:4858 2024-11-06T03:24:10Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.01 psi 2024-11-06T03:24:10Z- handleDepth()- Depth_Keller msg: 1730863450.72 epoch sec; seqNo:5175; depth 49.81 2024-11-06T03:24:10Z- handleDVL() - DVL msg epochMillisec: 1730863450.74 2024-11-06T03:24:10Z- handleDvl() - RDI_Pathfinder msg: 1730863450.74 sec; seqNo:1787; #beams:4 2024-11-06T03:24:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.21 , 28.14 , 31.67 , 30.58 2024-11-06T03:24:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.09 , -0.07 2024-11-06T03:24:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863450.77 epoch sec; seqNo:5175; 4061692.35 north; 594722.24 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:24:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863448.37 2024-11-06T03:24:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:10Z- waiting for 1730863452.37, time is 1730863450.77 2024-11-06T03:24:11Z- AHRS_M2 msg: 1730863451.12 epoch sec; seqNo:4859 2024-11-06T03:24:11Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.01 psi 2024-11-06T03:24:11Z- handleDepth()- Depth_Keller msg: 1730863451.14 epoch sec; seqNo:5176; depth 49.79 2024-11-06T03:24:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863451.16 epoch sec; seqNo:5176; 4061692.73 north; 594722.24 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:24:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863448.37 2024-11-06T03:24:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:11Z- waiting for 1730863452.37, time is 1730863451.16 2024-11-06T03:24:11Z- AHRS_M2 msg: 1730863451.52 epoch sec; seqNo:4860 2024-11-06T03:24:11Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.01 psi 2024-11-06T03:24:11Z- handleDepth()- Depth_Keller msg: 1730863451.53 epoch sec; seqNo:5177; depth 49.78 2024-11-06T03:24:11Z- handleDVL() - DVL msg epochMillisec: 1730863451.56 2024-11-06T03:24:11Z- handleDvl() - RDI_Pathfinder msg: 1730863451.56 sec; seqNo:1788; #beams:4 2024-11-06T03:24:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.21 , 28.14 , 31.81 , 30.04 2024-11-06T03:24:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.11 , -0.05 2024-11-06T03:24:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863451.57 epoch sec; seqNo:5177; 4061693.10 north; 594721.01 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:24:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863448.37 2024-11-06T03:24:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:11Z- waiting for 1730863452.37, time is 1730863451.57 2024-11-06T03:24:11Z- AHRS_M2 msg: 1730863451.93 epoch sec; seqNo:4861 2024-11-06T03:24:11Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 6.01 psi 2024-11-06T03:24:11Z- handleDepth()- Depth_Keller msg: 1730863451.94 epoch sec; seqNo:5178; depth 49.76 2024-11-06T03:24:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863451.95 epoch sec; seqNo:5178; 4061693.47 north; 594721.01 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:24:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863448.37 2024-11-06T03:24:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:12Z- waiting for 1730863452.37, time is 1730863451.95 2024-11-06T03:24:12Z- AHRS_M2 msg: 1730863452.33 epoch sec; seqNo:4862 2024-11-06T03:24:12Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 6.00 psi 2024-11-06T03:24:12Z- handleDepth()- Depth_Keller msg: 1730863452.34 epoch sec; seqNo:5179; depth 49.75 2024-11-06T03:24:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863452.36 epoch sec; seqNo:5179; 4061693.85 north; 594721.00 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:24:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863448.37 2024-11-06T03:24:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:12Z- waiting for 1730863452.37, time is 1730863452.36 2024-11-06T03:24:12Z- AHRS_M2 msg: 1730863452.73 epoch sec; seqNo:4863 2024-11-06T03:24:12Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 6.00 psi 2024-11-06T03:24:12Z- handleDepth()- Depth_Keller msg: 1730863452.74 epoch sec; seqNo:5180; depth 49.76 2024-11-06T03:24:12Z- handleDVL() - DVL msg epochMillisec: 1730863452.76 2024-11-06T03:24:12Z- handleDvl() - RDI_Pathfinder msg: 1730863452.76 sec; seqNo:1789; #beams:4 2024-11-06T03:24:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.49 , 28.14 , 31.67 , 30.31 2024-11-06T03:24:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.11 , -0.04 2024-11-06T03:24:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863452.77 epoch sec; seqNo:5180; 4061694.23 north; 594721.00 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:24:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863448.37 2024-11-06T03:24:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:12Z- waiting for 1730863452.37, time is 1730863452.77 2024-11-06T03:24:12Z- LcmTrn::updateTrn() >>>> pose time 1730863452.77, meas time 1730863452.76 <<<< 2024-11-06T03:24:12Z- LcmTrn::updateTrn() equating pose 1730863452.76 and meas times 1730863452.76 2024-11-06T03:24:12Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863452.76 2024-11-06T03:24:12Z- TerrainNav::motionUpdate() - currEstimate = (4061694, 594721.00, 49.76) 2024-11-06T03:24:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:24:12Z- TerrainNav:: SubcloudNIS is 0.71, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:24:12Z- TNavPF::Projecting Measurements 2024-11-06T03:24:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:24:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:24:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:24:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:24:12Z- TNavFilter::4 good beams. 2024-11-06T03:24:12Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:24:12Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:24:12Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:24:12Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:24:12Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:24:12Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:24:12Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:24:12Z- TerrainMapOctree: (vehN, vehE ) = (4061694.23, 594721.00). 2024-11-06T03:24:12Z- TerrainMapOctree: Min Distance = 62.86. 2024-11-06T03:24:12Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:24:13Z- Forcing Subcloud Comparison 2024-11-06T03:24:13Z- Weighting particles with subcloud comparison 2024-11-06T03:24:13Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:13Z- meanExpectedMeasDiff: -2.315045 delta_rms_squared: 1.209872 alpha: 0.983738 2024-11-06T03:24:13Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:13Z- meanExpectedMeasDiff: -0.736196 delta_rms_squared: 0.913877 alpha: 0.978584 2024-11-06T03:24:13Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:13Z- meanExpectedMeasDiff: -1.152463 delta_rms_squared: 1.075106 alpha: 0.981737 2024-11-06T03:24:13Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:13Z- meanExpectedMeasDiff: -0.102044 delta_rms_squared: 0.992966 alpha: 0.980256 2024-11-06T03:24:13Z- Subcloud NIS: 0.701650 2024-11-06T03:24:13Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:24:13Z- TNavPF:: sumWeights = 0.341051 2024-11-06T03:24:13Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:24:13Z- TNavPF::First Term of Map Covariance Matrix is 0.60 2024-11-06T03:24:13Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T03:24:13Z- TerrainNav::Resampling particle filter... 2024-11-06T03:24:13Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863452.76 sec, ping # 0. 2024-11-06T03:24:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:24:13Z- variances([0], [2], and psi_berg) = 0.585580 0.121365 0.000000 2024-11-06T03:24:13Z- publishEstimatesDecoder() - at 1730863452.76 sec 2024-11-06T03:24:13Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:24:13Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061724.78 594703.89 61.65 2024-11-06T03:24:13Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061723.78 594703.93 61.58 lat/lon: 36.70 -121.94 2024-11-06T03:24:13Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.59 0.12 0.35 0.00 2024-11-06T03:24:13Z- AHRS_M2 msg: 1730863453.15 epoch sec; seqNo:4864 2024-11-06T03:24:13Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 6.00 psi 2024-11-06T03:24:13Z- handleDepth()- Depth_Keller msg: 1730863453.16 epoch sec; seqNo:5181; depth 49.74 2024-11-06T03:24:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863453.17 epoch sec; seqNo:5181; 4061694.61 north; 594721.00 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:24:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863452.76 2024-11-06T03:24:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:13Z- waiting for 1730863456.76, time is 1730863453.17 2024-11-06T03:24:13Z- AHRS_M2 msg: 1730863453.57 epoch sec; seqNo:4865 2024-11-06T03:24:13Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 6.00 psi 2024-11-06T03:24:13Z- handleDepth()- Depth_Keller msg: 1730863453.57 epoch sec; seqNo:5182; depth 49.73 2024-11-06T03:24:13Z- handleDVL() - DVL msg epochMillisec: 1730863453.59 2024-11-06T03:24:13Z- handleDvl() - RDI_Pathfinder msg: 1730863453.59 sec; seqNo:1790; #beams:4 2024-11-06T03:24:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.76 , 28.27 , 31.81 , 30.45 2024-11-06T03:24:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.12 , -0.02 2024-11-06T03:24:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863453.61 epoch sec; seqNo:5182; 4061694.99 north; 594720.99 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:24:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863452.76 2024-11-06T03:24:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:13Z- waiting for 1730863456.76, time is 1730863453.61 2024-11-06T03:24:13Z- AHRS_M2 msg: 1730863453.94 epoch sec; seqNo:4866 2024-11-06T03:24:13Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 6.00 psi 2024-11-06T03:24:13Z- handleDepth()- Depth_Keller msg: 1730863453.95 epoch sec; seqNo:5183; depth 49.72 2024-11-06T03:24:13Z- handleDVL() - DVL msg epochMillisec: 1730863453.95 2024-11-06T03:24:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:24:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:24:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:24:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:24:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:24:13Z- handleDvl() - RDI_Pathfinder msg: 1730863453.95 sec; seqNo:1791; #beams:0 2024-11-06T03:24:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:24:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.12 , -0.02 2024-11-06T03:24:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863453.97 epoch sec; seqNo:5183; 4061695.37 north; 594720.99 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:24:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863452.76 2024-11-06T03:24:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:14Z- waiting for 1730863456.76, time is 1730863453.97 2024-11-06T03:24:14Z- AHRS_M2 msg: 1730863454.35 epoch sec; seqNo:4867 2024-11-06T03:24:14Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 6.00 psi 2024-11-06T03:24:14Z- handleDepth()- Depth_Keller msg: 1730863454.35 epoch sec; seqNo:5184; depth 49.72 2024-11-06T03:24:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863454.36 epoch sec; seqNo:5184; 4061695.75 north; 594720.98 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:24:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863452.76 2024-11-06T03:24:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:14Z- waiting for 1730863456.76, time is 1730863454.36 2024-11-06T03:24:14Z- AHRS_M2 msg: 1730863454.75 epoch sec; seqNo:4868 2024-11-06T03:24:14Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 6.00 psi 2024-11-06T03:24:14Z- handleDepth()- Depth_Keller msg: 1730863454.76 epoch sec; seqNo:5185; depth 49.73 2024-11-06T03:24:14Z- handleDVL() - DVL msg epochMillisec: 1730863454.79 2024-11-06T03:24:14Z- handleDvl() - RDI_Pathfinder msg: 1730863454.79 sec; seqNo:1792; #beams:4 2024-11-06T03:24:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.18 , 28.44 , 31.96 , 30.43 2024-11-06T03:24:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.12 , -0.00 2024-11-06T03:24:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863454.80 epoch sec; seqNo:5185; 4061696.13 north; 594720.98 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:24:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863452.76 2024-11-06T03:24:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:14Z- waiting for 1730863456.76, time is 1730863454.80 2024-11-06T03:24:15Z- AHRS_M2 msg: 1730863455.16 epoch sec; seqNo:4869 2024-11-06T03:24:15Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 6.00 psi 2024-11-06T03:24:15Z- handleDepth()- Depth_Keller msg: 1730863455.16 epoch sec; seqNo:5186; depth 49.71 2024-11-06T03:24:15Z- handleDVL() - DVL msg epochMillisec: 1730863455.17 2024-11-06T03:24:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:24:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:24:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:24:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:24:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:24:15Z- handleDvl() - RDI_Pathfinder msg: 1730863455.17 sec; seqNo:1793; #beams:0 2024-11-06T03:24:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:24:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.12 , -0.00 2024-11-06T03:24:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863455.18 epoch sec; seqNo:5186; 4061696.49 north; 594719.75 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:24:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863452.76 2024-11-06T03:24:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:15Z- waiting for 1730863456.76, time is 1730863455.18 2024-11-06T03:24:15Z- AHRS_M2 msg: 1730863455.56 epoch sec; seqNo:4870 2024-11-06T03:24:15Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 6.01 psi 2024-11-06T03:24:15Z- handleDepth()- Depth_Keller msg: 1730863455.57 epoch sec; seqNo:5187; depth 49.70 2024-11-06T03:24:15Z- handleDVL() - DVL msg epochMillisec: 1730863455.59 2024-11-06T03:24:15Z- handleDvl() - RDI_Pathfinder msg: 1730863455.59 sec; seqNo:1794; #beams:4 2024-11-06T03:24:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.49 , 28.59 , 31.96 , 30.28 2024-11-06T03:24:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.10 , 0.00 2024-11-06T03:24:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863455.61 epoch sec; seqNo:5187; 4061696.87 north; 594719.75 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:24:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863452.76 2024-11-06T03:24:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:15Z- waiting for 1730863456.76, time is 1730863455.61 2024-11-06T03:24:15Z- AHRS_M2 msg: 1730863455.96 epoch sec; seqNo:4871 2024-11-06T03:24:15Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 6.01 psi 2024-11-06T03:24:15Z- handleDepth()- Depth_Keller msg: 1730863455.97 epoch sec; seqNo:5188; depth 49.70 2024-11-06T03:24:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863455.99 epoch sec; seqNo:5188; 4061697.25 north; 594719.75 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:24:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863452.76 2024-11-06T03:24:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:16Z- waiting for 1730863456.76, time is 1730863455.99 2024-11-06T03:24:16Z- AHRS_M2 msg: 1730863456.37 epoch sec; seqNo:4872 2024-11-06T03:24:16Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 6.01 psi 2024-11-06T03:24:16Z- handleDepth()- Depth_Keller msg: 1730863456.37 epoch sec; seqNo:5189; depth 49.70 2024-11-06T03:24:16Z- handleDVL() - DVL msg epochMillisec: 1730863456.38 2024-11-06T03:24:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:24:16Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:24:16Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:24:16Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:24:16Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:24:16Z- handleDvl() - RDI_Pathfinder msg: 1730863456.38 sec; seqNo:1795; #beams:0 2024-11-06T03:24:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:24:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.10 , 0.00 2024-11-06T03:24:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863456.38 epoch sec; seqNo:5189; 4061697.25 north; 594719.75 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:24:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863452.76 2024-11-06T03:24:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:16Z- waiting for 1730863456.76, time is 1730863456.38 2024-11-06T03:24:16Z- AHRS_M2 msg: 1730863456.79 epoch sec; seqNo:4873 2024-11-06T03:24:16Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 6.01 psi 2024-11-06T03:24:16Z- handleDepth()- Depth_Keller msg: 1730863456.80 epoch sec; seqNo:5190; depth 49.69 2024-11-06T03:24:16Z- handleDVL() - DVL msg epochMillisec: 1730863456.82 2024-11-06T03:24:16Z- handleDvl() - RDI_Pathfinder msg: 1730863456.82 sec; seqNo:1796; #beams:4 2024-11-06T03:24:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.79 , 28.59 , 31.81 , 30.58 2024-11-06T03:24:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.07 , 0.00 2024-11-06T03:24:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863456.84 epoch sec; seqNo:5190; 4061697.63 north; 594719.74 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:24:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863452.76 2024-11-06T03:24:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:16Z- waiting for 1730863456.76, time is 1730863456.84 2024-11-06T03:24:16Z- LcmTrn::updateTrn() >>>> pose time 1730863456.84, meas time 1730863456.82 <<<< 2024-11-06T03:24:16Z- LcmTrn::updateTrn() equating pose 1730863456.82 and meas times 1730863456.82 2024-11-06T03:24:16Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863456.82 2024-11-06T03:24:16Z- TerrainNav::motionUpdate() - currEstimate = (4061698, 594719.74, 49.69) 2024-11-06T03:24:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:24:16Z- TerrainNav:: SubcloudNIS is 0.70, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:24:16Z- TNavPF::Projecting Measurements 2024-11-06T03:24:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:24:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:24:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:24:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:24:16Z- TNavFilter::4 good beams. 2024-11-06T03:24:16Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:24:16Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:24:16Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:24:16Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:24:16Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:24:16Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:24:16Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:24:16Z- TerrainMapOctree: (vehN, vehE ) = (4061697.63, 594719.74). 2024-11-06T03:24:16Z- TerrainMapOctree: Min Distance = 60.48. 2024-11-06T03:24:16Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:24:17Z- Forcing Subcloud Comparison 2024-11-06T03:24:17Z- Weighting particles with subcloud comparison 2024-11-06T03:24:17Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:17Z- meanExpectedMeasDiff: -0.795628 delta_rms_squared: 0.944205 alpha: 0.979258 2024-11-06T03:24:17Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:17Z- meanExpectedMeasDiff: -0.673215 delta_rms_squared: 0.674496 alpha: 0.971202 2024-11-06T03:24:17Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:17Z- meanExpectedMeasDiff: -0.311930 delta_rms_squared: 0.812707 alpha: 0.975982 2024-11-06T03:24:17Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:17Z- meanExpectedMeasDiff: 0.418013 delta_rms_squared: 0.772203 alpha: 0.974754 2024-11-06T03:24:17Z- Subcloud NIS: 0.674868 2024-11-06T03:24:17Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:24:17Z- TNavPF:: sumWeights = 0.604634 2024-11-06T03:24:17Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:24:17Z- TNavPF::First Term of Map Covariance Matrix is 0.33 2024-11-06T03:24:17Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.05 2024-11-06T03:24:17Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863456.82 sec, ping # 0. 2024-11-06T03:24:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:24:17Z- variances([0], [2], and psi_berg) = 0.586359 0.123271 0.000000 2024-11-06T03:24:17Z- publishEstimatesDecoder() - at 1730863456.82 sec 2024-11-06T03:24:17Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:24:17Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061725.70 594702.79 61.77 2024-11-06T03:24:17Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061727.18 594702.68 61.64 lat/lon: 36.70 -121.94 2024-11-06T03:24:17Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.59 0.12 0.30 0.00 2024-11-06T03:24:17Z- AHRS_M2 msg: 1730863457.17 epoch sec; seqNo:4874 2024-11-06T03:24:17Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 6.01 psi 2024-11-06T03:24:17Z- handleDepth()- Depth_Keller msg: 1730863457.18 epoch sec; seqNo:5191; depth 49.68 2024-11-06T03:24:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863457.20 epoch sec; seqNo:5191; 4061698.01 north; 594719.74 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:24:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863456.82 2024-11-06T03:24:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:17Z- waiting for 1730863460.82, time is 1730863457.20 2024-11-06T03:24:17Z- AHRS_M2 msg: 1730863457.58 epoch sec; seqNo:4875 2024-11-06T03:24:17Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 6.01 psi 2024-11-06T03:24:17Z- handleDepth()- Depth_Keller msg: 1730863457.59 epoch sec; seqNo:5192; depth 49.68 2024-11-06T03:24:17Z- handleDVL() - DVL msg epochMillisec: 1730863457.61 2024-11-06T03:24:17Z- handleDvl() - RDI_Pathfinder msg: 1730863457.61 sec; seqNo:1797; #beams:4 2024-11-06T03:24:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.25 , 28.59 , 31.65 , 30.58 2024-11-06T03:24:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.08 , -0.01 2024-11-06T03:24:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863457.64 epoch sec; seqNo:5192; 4061698.39 north; 594719.73 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:24:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863456.82 2024-11-06T03:24:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:17Z- waiting for 1730863460.82, time is 1730863457.64 2024-11-06T03:24:17Z- AHRS_M2 msg: 1730863457.98 epoch sec; seqNo:4876 2024-11-06T03:24:17Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 6.01 psi 2024-11-06T03:24:17Z- handleDepth()- Depth_Keller msg: 1730863457.99 epoch sec; seqNo:5193; depth 49.68 2024-11-06T03:24:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863458.01 epoch sec; seqNo:5193; 4061698.77 north; 594719.73 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:24:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863456.82 2024-11-06T03:24:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:18Z- waiting for 1730863460.82, time is 1730863458.01 2024-11-06T03:24:18Z- AHRS_M2 msg: 1730863458.39 epoch sec; seqNo:4877 2024-11-06T03:24:18Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.01 psi 2024-11-06T03:24:18Z- handleDepth()- Depth_Keller msg: 1730863458.40 epoch sec; seqNo:5194; depth 49.68 2024-11-06T03:24:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863458.41 epoch sec; seqNo:5194; 4061699.14 north; 594719.72 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:24:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863456.82 2024-11-06T03:24:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:18Z- waiting for 1730863460.82, time is 1730863458.41 2024-11-06T03:24:18Z- AHRS_M2 msg: 1730863458.79 epoch sec; seqNo:4878 2024-11-06T03:24:18Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.01 psi 2024-11-06T03:24:18Z- handleDepth()- Depth_Keller msg: 1730863458.80 epoch sec; seqNo:5195; depth 49.67 2024-11-06T03:24:18Z- handleDVL() - DVL msg epochMillisec: 1730863458.83 2024-11-06T03:24:18Z- handleDvl() - RDI_Pathfinder msg: 1730863458.83 sec; seqNo:1798; #beams:4 2024-11-06T03:24:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.02 , 28.29 , 31.50 , 30.89 2024-11-06T03:24:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , -0.02 2024-11-06T03:24:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863458.84 epoch sec; seqNo:5195; 4061699.51 north; 594718.50 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:24:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863456.82 2024-11-06T03:24:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:18Z- waiting for 1730863460.82, time is 1730863458.84 2024-11-06T03:24:19Z- AHRS_M2 msg: 1730863459.20 epoch sec; seqNo:4879 2024-11-06T03:24:19Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.01 psi 2024-11-06T03:24:19Z- handleDepth()- Depth_Keller msg: 1730863459.21 epoch sec; seqNo:5196; depth 49.65 2024-11-06T03:24:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863459.22 epoch sec; seqNo:5196; 4061699.89 north; 594718.50 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:24:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863456.82 2024-11-06T03:24:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:19Z- waiting for 1730863460.82, time is 1730863459.22 2024-11-06T03:24:19Z- AHRS_M2 msg: 1730863459.60 epoch sec; seqNo:4880 2024-11-06T03:24:19Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.01 psi 2024-11-06T03:24:19Z- handleDepth()- Depth_Keller msg: 1730863459.60 epoch sec; seqNo:5197; depth 49.66 2024-11-06T03:24:19Z- handleDVL() - DVL msg epochMillisec: 1730863459.64 2024-11-06T03:24:19Z- handleDvl() - RDI_Pathfinder msg: 1730863459.64 sec; seqNo:1799; #beams:4 2024-11-06T03:24:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 28.14 , 31.35 , 31.04 2024-11-06T03:24:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , -0.03 2024-11-06T03:24:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863459.65 epoch sec; seqNo:5197; 4061700.27 north; 594718.49 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:24:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863456.82 2024-11-06T03:24:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:19Z- waiting for 1730863460.82, time is 1730863459.65 2024-11-06T03:24:20Z- AHRS_M2 msg: 1730863460.01 epoch sec; seqNo:4881 2024-11-06T03:24:20Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.01 psi 2024-11-06T03:24:20Z- handleDepth()- Depth_Keller msg: 1730863460.01 epoch sec; seqNo:5198; depth 49.65 2024-11-06T03:24:20Z- handleDVL() - DVL msg epochMillisec: 1730863460.02 2024-11-06T03:24:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:24:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:24:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:24:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:24:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:24:20Z- handleDvl() - RDI_Pathfinder msg: 1730863460.02 sec; seqNo:1800; #beams:0 2024-11-06T03:24:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:24:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , -0.03 2024-11-06T03:24:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863460.03 epoch sec; seqNo:5198; 4061700.65 north; 594718.49 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:24:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863456.82 2024-11-06T03:24:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:20Z- waiting for 1730863460.82, time is 1730863460.03 2024-11-06T03:24:20Z- AHRS_M2 msg: 1730863460.41 epoch sec; seqNo:4882 2024-11-06T03:24:20Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.01 psi 2024-11-06T03:24:20Z- handleDepth()- Depth_Keller msg: 1730863460.41 epoch sec; seqNo:5199; depth 49.65 2024-11-06T03:24:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863460.43 epoch sec; seqNo:5199; 4061701.02 north; 594718.48 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:24:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863456.82 2024-11-06T03:24:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:20Z- waiting for 1730863460.82, time is 1730863460.43 2024-11-06T03:24:20Z- AHRS_M2 msg: 1730863460.81 epoch sec; seqNo:4883 2024-11-06T03:24:20Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.01 psi 2024-11-06T03:24:20Z- handleDepth()- Depth_Keller msg: 1730863460.82 epoch sec; seqNo:5200; depth 49.65 2024-11-06T03:24:20Z- handleDVL() - DVL msg epochMillisec: 1730863460.84 2024-11-06T03:24:20Z- handleDvl() - RDI_Pathfinder msg: 1730863460.84 sec; seqNo:1801; #beams:4 2024-11-06T03:24:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 27.83 , 31.04 , 31.04 2024-11-06T03:24:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , -0.04 2024-11-06T03:24:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863460.86 epoch sec; seqNo:5200; 4061701.40 north; 594718.48 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:24:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863456.82 2024-11-06T03:24:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:20Z- waiting for 1730863460.82, time is 1730863460.86 2024-11-06T03:24:20Z- LcmTrn::updateTrn() >>>> pose time 1730863460.86, meas time 1730863460.84 <<<< 2024-11-06T03:24:20Z- LcmTrn::updateTrn() equating pose 1730863460.84 and meas times 1730863460.84 2024-11-06T03:24:20Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863460.84 2024-11-06T03:24:20Z- TerrainNav::motionUpdate() - currEstimate = (4061701, 594718.48, 49.65) 2024-11-06T03:24:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:24:20Z- TerrainNav:: SubcloudNIS is 0.67, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:24:20Z- TNavPF::Projecting Measurements 2024-11-06T03:24:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:24:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:24:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:24:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:24:20Z- TNavFilter::4 good beams. 2024-11-06T03:24:20Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:24:20Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:24:20Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:24:20Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:24:20Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:24:20Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:24:20Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:24:20Z- TerrainMapOctree: (vehN, vehE ) = (4061701.40, 594718.48). 2024-11-06T03:24:20Z- TerrainMapOctree: Min Distance = 57.92. 2024-11-06T03:24:20Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:24:21Z- Forcing Subcloud Comparison 2024-11-06T03:24:21Z- Weighting particles with subcloud comparison 2024-11-06T03:24:21Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:21Z- meanExpectedMeasDiff: 1.006224 delta_rms_squared: 0.870015 alpha: 0.977528 2024-11-06T03:24:21Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:21Z- meanExpectedMeasDiff: -1.370904 delta_rms_squared: 0.627215 alpha: 0.969098 2024-11-06T03:24:21Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:21Z- meanExpectedMeasDiff: -0.502732 delta_rms_squared: 0.733574 alpha: 0.973460 2024-11-06T03:24:21Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:21Z- meanExpectedMeasDiff: 0.595694 delta_rms_squared: 0.741855 alpha: 0.973748 2024-11-06T03:24:21Z- Subcloud NIS: 0.667983 2024-11-06T03:24:21Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:24:21Z- TNavPF:: sumWeights = 0.428373 2024-11-06T03:24:21Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:24:21Z- TNavPF::First Term of Map Covariance Matrix is 0.36 2024-11-06T03:24:21Z- TNavPF::Calculated NIS Value : 0.33 numBeams normalized NIS: 0.08 2024-11-06T03:24:21Z- TerrainNav::Resampling particle filter... 2024-11-06T03:24:21Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863460.84 sec, ping # 0. 2024-11-06T03:24:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:24:21Z- variances([0], [2], and psi_berg) = 0.598584 0.125754 0.000000 2024-11-06T03:24:21Z- publishEstimatesDecoder() - at 1730863460.84 sec 2024-11-06T03:24:21Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:24:21Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061731.85 594701.38 60.98 2024-11-06T03:24:21Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061730.96 594701.41 61.65 lat/lon: 36.70 -121.94 2024-11-06T03:24:21Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.60 0.13 0.29 0.00 2024-11-06T03:24:21Z- AHRS_M2 msg: 1730863461.22 epoch sec; seqNo:4884 2024-11-06T03:24:21Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.01 psi 2024-11-06T03:24:21Z- handleDepth()- Depth_Keller msg: 1730863461.23 epoch sec; seqNo:5201; depth 49.64 2024-11-06T03:24:21Z- handleDVL() - DVL msg epochMillisec: 1730863461.23 2024-11-06T03:24:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:24:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:24:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:24:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:24:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:24:21Z- handleDvl() - RDI_Pathfinder msg: 1730863461.23 sec; seqNo:1802; #beams:0 2024-11-06T03:24:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:24:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.05 , -0.04 2024-11-06T03:24:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863461.25 epoch sec; seqNo:5201; 4061701.78 north; 594718.47 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:24:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863460.84 2024-11-06T03:24:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:21Z- waiting for 1730863464.84, time is 1730863461.25 2024-11-06T03:24:21Z- AHRS_M2 msg: 1730863461.62 epoch sec; seqNo:4885 2024-11-06T03:24:21Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.01 psi 2024-11-06T03:24:21Z- handleDepth()- Depth_Keller msg: 1730863461.63 epoch sec; seqNo:5202; depth 49.65 2024-11-06T03:24:21Z- handleDVL() - DVL msg epochMillisec: 1730863461.66 2024-11-06T03:24:21Z- handleDvl() - RDI_Pathfinder msg: 1730863461.66 sec; seqNo:1803; #beams:4 2024-11-06T03:24:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.17 , 27.98 , 30.73 , 31.04 2024-11-06T03:24:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.07 , -0.04 2024-11-06T03:24:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863461.67 epoch sec; seqNo:5202; 4061702.16 north; 594718.47 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:24:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863460.84 2024-11-06T03:24:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:21Z- waiting for 1730863464.84, time is 1730863461.67 2024-11-06T03:24:22Z- AHRS_M2 msg: 1730863462.02 epoch sec; seqNo:4886 2024-11-06T03:24:22Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.01 psi 2024-11-06T03:24:22Z- handleDepth()- Depth_Keller msg: 1730863462.03 epoch sec; seqNo:5203; depth 49.64 2024-11-06T03:24:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863462.04 epoch sec; seqNo:5203; 4061702.16 north; 594718.47 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:24:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863460.84 2024-11-06T03:24:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:22Z- waiting for 1730863464.84, time is 1730863462.04 2024-11-06T03:24:22Z- AHRS_M2 msg: 1730863462.43 epoch sec; seqNo:4887 2024-11-06T03:24:22Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.01 psi 2024-11-06T03:24:22Z- handleDepth()- Depth_Keller msg: 1730863462.43 epoch sec; seqNo:5204; depth 49.64 2024-11-06T03:24:22Z- handleDVL() - DVL msg epochMillisec: 1730863462.44 2024-11-06T03:24:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:24:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:24:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:24:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:24:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:24:22Z- handleDvl() - RDI_Pathfinder msg: 1730863462.44 sec; seqNo:1804; #beams:0 2024-11-06T03:24:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:24:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.07 , -0.04 2024-11-06T03:24:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863462.45 epoch sec; seqNo:5204; 4061702.54 north; 594718.47 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:24:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863460.84 2024-11-06T03:24:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:22Z- waiting for 1730863464.84, time is 1730863462.45 2024-11-06T03:24:22Z- AHRS_M2 msg: 1730863462.83 epoch sec; seqNo:4888 2024-11-06T03:24:22Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.01 psi 2024-11-06T03:24:22Z- handleDepth()- Depth_Keller msg: 1730863462.84 epoch sec; seqNo:5205; depth 49.65 2024-11-06T03:24:22Z- handleDVL() - DVL msg epochMillisec: 1730863462.87 2024-11-06T03:24:22Z- handleDvl() - RDI_Pathfinder msg: 1730863462.87 sec; seqNo:1805; #beams:4 2024-11-06T03:24:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.17 , 28.29 , 30.43 , 31.04 2024-11-06T03:24:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.08 , -0.03 2024-11-06T03:24:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863462.88 epoch sec; seqNo:5205; 4061702.91 north; 594717.24 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:24:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863460.84 2024-11-06T03:24:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:22Z- waiting for 1730863464.84, time is 1730863462.88 2024-11-06T03:24:23Z- handleDepth()- Depth_Keller msg: 1730863463.26 epoch sec; seqNo:5206; depth 49.64 2024-11-06T03:24:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863463.28 epoch sec; seqNo:5206; 4061703.28 north; 594717.24 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:24:23Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:24:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863460.84 2024-11-06T03:24:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:23Z- waiting for 1730863464.84, time is 1730863463.28 2024-11-06T03:24:23Z- AHRS_M2 msg: 1730863463.64 epoch sec; seqNo:4889 2024-11-06T03:24:23Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.01 psi 2024-11-06T03:24:23Z- handleDepth()- Depth_Keller msg: 1730863463.65 epoch sec; seqNo:5207; depth 49.65 2024-11-06T03:24:23Z- handleDVL() - DVL msg epochMillisec: 1730863463.67 2024-11-06T03:24:23Z- handleDvl() - RDI_Pathfinder msg: 1730863463.67 sec; seqNo:1806; #beams:4 2024-11-06T03:24:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.02 , 28.14 , 30.28 , 31.19 2024-11-06T03:24:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.08 , -0.02 2024-11-06T03:24:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863463.69 epoch sec; seqNo:5207; 4061703.66 north; 594717.23 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:24:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863460.84 2024-11-06T03:24:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:23Z- waiting for 1730863464.84, time is 1730863463.69 2024-11-06T03:24:24Z- AHRS_M2 msg: 1730863464.04 epoch sec; seqNo:4890 2024-11-06T03:24:24Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.01 psi 2024-11-06T03:24:24Z- handleDepth()- Depth_Keller msg: 1730863464.05 epoch sec; seqNo:5208; depth 49.65 2024-11-06T03:24:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863464.08 epoch sec; seqNo:5208; 4061704.04 north; 594717.23 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:24:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863460.84 2024-11-06T03:24:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:24Z- waiting for 1730863464.84, time is 1730863464.08 2024-11-06T03:24:24Z- AHRS_M2 msg: 1730863464.45 epoch sec; seqNo:4891 2024-11-06T03:24:24Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.01 psi 2024-11-06T03:24:24Z- handleDepth()- Depth_Keller msg: 1730863464.46 epoch sec; seqNo:5209; depth 49.65 2024-11-06T03:24:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863464.47 epoch sec; seqNo:5209; 4061704.42 north; 594717.23 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:24:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863460.84 2024-11-06T03:24:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:24Z- waiting for 1730863464.84, time is 1730863464.47 2024-11-06T03:24:24Z- AHRS_M2 msg: 1730863464.86 epoch sec; seqNo:4892 2024-11-06T03:24:24Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 6.01 psi 2024-11-06T03:24:24Z- handleDepth()- Depth_Keller msg: 1730863464.87 epoch sec; seqNo:5210; depth 49.66 2024-11-06T03:24:24Z- handleDVL() - DVL msg epochMillisec: 1730863464.89 2024-11-06T03:24:24Z- handleDvl() - RDI_Pathfinder msg: 1730863464.89 sec; seqNo:1807; #beams:4 2024-11-06T03:24:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.71 , 28.90 , 30.12 , 31.35 2024-11-06T03:24:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.08 , -0.00 2024-11-06T03:24:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863464.92 epoch sec; seqNo:5210; 4061704.80 north; 594717.22 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:24:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863460.84 2024-11-06T03:24:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:25Z- waiting for 1730863464.84, time is 1730863464.92 2024-11-06T03:24:25Z- LcmTrn::updateTrn() >>>> pose time 1730863464.92, meas time 1730863464.89 <<<< 2024-11-06T03:24:25Z- LcmTrn::updateTrn() equating pose 1730863464.89 and meas times 1730863464.89 2024-11-06T03:24:25Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863464.89 2024-11-06T03:24:25Z- TerrainNav::motionUpdate() - currEstimate = (4061705, 594717.22, 49.66) 2024-11-06T03:24:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:24:25Z- TerrainNav:: SubcloudNIS is 0.67, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:24:25Z- TNavPF::Projecting Measurements 2024-11-06T03:24:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:24:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:24:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:24:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:24:25Z- TNavFilter::4 good beams. 2024-11-06T03:24:25Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:24:25Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:24:25Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:24:25Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:24:25Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:24:25Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:24:25Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:24:25Z- TerrainMapOctree: (vehN, vehE ) = (4061704.80, 594717.22). 2024-11-06T03:24:25Z- TerrainMapOctree: Min Distance = 55.83. 2024-11-06T03:24:25Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:24:25Z- Forcing Subcloud Comparison 2024-11-06T03:24:25Z- Weighting particles with subcloud comparison 2024-11-06T03:24:25Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:25Z- meanExpectedMeasDiff: 0.652368 delta_rms_squared: 0.846377 alpha: 0.976915 2024-11-06T03:24:25Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:25Z- meanExpectedMeasDiff: -0.382732 delta_rms_squared: 0.623050 alpha: 0.968898 2024-11-06T03:24:25Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:25Z- meanExpectedMeasDiff: -1.086939 delta_rms_squared: 0.709116 alpha: 0.972570 2024-11-06T03:24:25Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:25Z- meanExpectedMeasDiff: -0.382715 delta_rms_squared: 0.698754 alpha: 0.972174 2024-11-06T03:24:25Z- Subcloud NIS: 0.651811 2024-11-06T03:24:25Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:24:25Z- TNavPF:: sumWeights = 0.566582 2024-11-06T03:24:25Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:24:25Z- TNavPF::First Term of Map Covariance Matrix is 0.33 2024-11-06T03:24:25Z- TNavPF::Calculated NIS Value : 0.29 numBeams normalized NIS: 0.07 2024-11-06T03:24:25Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863464.89 sec, ping # 0. 2024-11-06T03:24:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:24:25Z- variances([0], [2], and psi_berg) = 0.603745 0.127939 0.000000 2024-11-06T03:24:25Z- publishEstimatesDecoder() - at 1730863464.89 sec 2024-11-06T03:24:25Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:24:25Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061732.79 594700.26 61.90 2024-11-06T03:24:25Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061734.36 594700.16 61.77 lat/lon: 36.70 -121.94 2024-11-06T03:24:25Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.60 0.13 0.29 0.00 2024-11-06T03:24:25Z- handleDepth()- Depth_Keller msg: 1730863465.28 epoch sec; seqNo:5211; depth 49.68 2024-11-06T03:24:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863465.30 epoch sec; seqNo:5211; 4061705.18 north; 594717.22 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:24:25Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:24:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863464.89 2024-11-06T03:24:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:25Z- waiting for 1730863468.89, time is 1730863465.30 2024-11-06T03:24:25Z- AHRS_M2 msg: 1730863465.66 epoch sec; seqNo:4893 2024-11-06T03:24:25Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 6.01 psi 2024-11-06T03:24:25Z- handleDepth()- Depth_Keller msg: 1730863465.66 epoch sec; seqNo:5212; depth 49.67 2024-11-06T03:24:25Z- handleDVL() - DVL msg epochMillisec: 1730863465.69 2024-11-06T03:24:25Z- handleDvl() - RDI_Pathfinder msg: 1730863465.69 sec; seqNo:1808; #beams:4 2024-11-06T03:24:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.71 , 28.90 , 30.28 , 31.04 2024-11-06T03:24:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.09 , 0.01 2024-11-06T03:24:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863465.71 epoch sec; seqNo:5212; 4061705.56 north; 594717.21 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:24:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863464.89 2024-11-06T03:24:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:25Z- waiting for 1730863468.89, time is 1730863465.71 2024-11-06T03:24:26Z- AHRS_M2 msg: 1730863466.06 epoch sec; seqNo:4894 2024-11-06T03:24:26Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.01 psi 2024-11-06T03:24:26Z- handleDepth()- Depth_Keller msg: 1730863466.07 epoch sec; seqNo:5213; depth 49.67 2024-11-06T03:24:26Z- handleDVL() - DVL msg epochMillisec: 1730863466.08 2024-11-06T03:24:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:24:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:24:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:24:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:24:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:24:26Z- handleDvl() - RDI_Pathfinder msg: 1730863466.08 sec; seqNo:1809; #beams:0 2024-11-06T03:24:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:24:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.09 , 0.01 2024-11-06T03:24:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863466.09 epoch sec; seqNo:5213; 4061705.94 north; 594717.21 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:24:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863464.89 2024-11-06T03:24:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:26Z- waiting for 1730863468.89, time is 1730863466.09 2024-11-06T03:24:26Z- AHRS_M2 msg: 1730863466.47 epoch sec; seqNo:4895 2024-11-06T03:24:26Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.01 psi 2024-11-06T03:24:26Z- handleDepth()- Depth_Keller msg: 1730863466.47 epoch sec; seqNo:5214; depth 49.68 2024-11-06T03:24:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863466.48 epoch sec; seqNo:5214; 4061706.30 north; 594715.98 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:24:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863464.89 2024-11-06T03:24:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:26Z- waiting for 1730863468.89, time is 1730863466.48 2024-11-06T03:24:26Z- AHRS_M2 msg: 1730863466.87 epoch sec; seqNo:4896 2024-11-06T03:24:26Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.01 psi 2024-11-06T03:24:26Z- handleDepth()- Depth_Keller msg: 1730863466.88 epoch sec; seqNo:5215; depth 49.69 2024-11-06T03:24:26Z- handleDVL() - DVL msg epochMillisec: 1730863466.91 2024-11-06T03:24:26Z- handleDvl() - RDI_Pathfinder msg: 1730863466.91 sec; seqNo:1810; #beams:4 2024-11-06T03:24:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.17 , 28.44 , 30.12 , 31.35 2024-11-06T03:24:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , 0.00 2024-11-06T03:24:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863466.94 epoch sec; seqNo:5215; 4061706.68 north; 594715.98 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:24:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863464.89 2024-11-06T03:24:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:27Z- waiting for 1730863468.89, time is 1730863466.94 2024-11-06T03:24:27Z- AHRS_M2 msg: 1730863467.28 epoch sec; seqNo:4897 2024-11-06T03:24:27Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.01 psi 2024-11-06T03:24:27Z- handleDepth()- Depth_Keller msg: 1730863467.29 epoch sec; seqNo:5216; depth 49.68 2024-11-06T03:24:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863467.33 epoch sec; seqNo:5216; 4061707.06 north; 594715.98 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:24:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863464.89 2024-11-06T03:24:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:27Z- waiting for 1730863468.89, time is 1730863467.33 2024-11-06T03:24:27Z- AHRS_M2 msg: 1730863467.68 epoch sec; seqNo:4898 2024-11-06T03:24:27Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.02 psi 2024-11-06T03:24:27Z- handleDepth()- Depth_Keller msg: 1730863467.68 epoch sec; seqNo:5217; depth 49.69 2024-11-06T03:24:27Z- handleDVL() - DVL msg epochMillisec: 1730863467.70 2024-11-06T03:24:27Z- handleDvl() - RDI_Pathfinder msg: 1730863467.70 sec; seqNo:1811; #beams:4 2024-11-06T03:24:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.17 , 28.59 , 30.43 , 31.35 2024-11-06T03:24:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , -0.01 2024-11-06T03:24:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863467.74 epoch sec; seqNo:5217; 4061707.44 north; 594715.97 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:24:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863464.89 2024-11-06T03:24:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:27Z- waiting for 1730863468.89, time is 1730863467.74 2024-11-06T03:24:28Z- AHRS_M2 msg: 1730863468.08 epoch sec; seqNo:4899 2024-11-06T03:24:28Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.02 psi 2024-11-06T03:24:28Z- handleDepth()- Depth_Keller msg: 1730863468.09 epoch sec; seqNo:5218; depth 49.70 2024-11-06T03:24:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863468.11 epoch sec; seqNo:5218; 4061707.82 north; 594715.97 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:24:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863464.89 2024-11-06T03:24:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:28Z- waiting for 1730863468.89, time is 1730863468.11 2024-11-06T03:24:28Z- handleDepth()- Depth_Keller msg: 1730863468.51 epoch sec; seqNo:5219; depth 49.70 2024-11-06T03:24:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863468.52 epoch sec; seqNo:5219; 4061708.20 north; 594715.96 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:24:28Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:24:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863464.89 2024-11-06T03:24:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:28Z- waiting for 1730863468.89, time is 1730863468.52 2024-11-06T03:24:28Z- AHRS_M2 msg: 1730863468.90 epoch sec; seqNo:4900 2024-11-06T03:24:28Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.02 psi 2024-11-06T03:24:28Z- handleDepth()- Depth_Keller msg: 1730863468.90 epoch sec; seqNo:5220; depth 49.71 2024-11-06T03:24:28Z- handleDVL() - DVL msg epochMillisec: 1730863468.92 2024-11-06T03:24:28Z- handleDvl() - RDI_Pathfinder msg: 1730863468.92 sec; seqNo:1812; #beams:4 2024-11-06T03:24:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.71 , 28.59 , 30.43 , 31.65 2024-11-06T03:24:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , -0.03 2024-11-06T03:24:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863468.94 epoch sec; seqNo:5220; 4061708.57 north; 594715.96 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:24:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863464.89 2024-11-06T03:24:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:29Z- waiting for 1730863468.89, time is 1730863468.94 2024-11-06T03:24:29Z- LcmTrn::updateTrn() >>>> pose time 1730863468.94, meas time 1730863468.92 <<<< 2024-11-06T03:24:29Z- LcmTrn::updateTrn() equating pose 1730863468.92 and meas times 1730863468.92 2024-11-06T03:24:29Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863468.92 2024-11-06T03:24:29Z- TerrainNav::motionUpdate() - currEstimate = (4061709, 594715.96, 49.71) 2024-11-06T03:24:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:24:29Z- TerrainNav:: SubcloudNIS is 0.65, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:24:29Z- TNavPF::Projecting Measurements 2024-11-06T03:24:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:24:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:24:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:24:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:24:29Z- TNavFilter::4 good beams. 2024-11-06T03:24:29Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:24:29Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:24:29Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:24:29Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:24:29Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:24:29Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:24:29Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:24:29Z- TerrainMapOctree: (vehN, vehE ) = (4061708.57, 594715.96). 2024-11-06T03:24:29Z- TerrainMapOctree: Min Distance = 53.62. 2024-11-06T03:24:29Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:24:29Z- Forcing Subcloud Comparison 2024-11-06T03:24:29Z- Weighting particles with subcloud comparison 2024-11-06T03:24:29Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:29Z- meanExpectedMeasDiff: 0.357338 delta_rms_squared: 0.864939 alpha: 0.977400 2024-11-06T03:24:29Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:29Z- meanExpectedMeasDiff: -0.268007 delta_rms_squared: 0.607575 alpha: 0.968131 2024-11-06T03:24:29Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:29Z- meanExpectedMeasDiff: -0.393512 delta_rms_squared: 0.694321 alpha: 0.972001 2024-11-06T03:24:29Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:29Z- meanExpectedMeasDiff: -0.283852 delta_rms_squared: 0.746120 alpha: 0.973894 2024-11-06T03:24:29Z- Subcloud NIS: 0.640467 2024-11-06T03:24:29Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:24:29Z- TNavPF:: sumWeights = 0.691901 2024-11-06T03:24:29Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:24:29Z- TNavPF::First Term of Map Covariance Matrix is 0.34 2024-11-06T03:24:29Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.03 2024-11-06T03:24:29Z- TerrainNav::Resampling particle filter... 2024-11-06T03:24:29Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863468.92 sec, ping # 0. 2024-11-06T03:24:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:24:29Z- variances([0], [2], and psi_berg) = 0.605507 0.131319 0.000000 2024-11-06T03:24:29Z- publishEstimatesDecoder() - at 1730863468.92 sec 2024-11-06T03:24:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:24:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061738.97 594698.66 61.51 2024-11-06T03:24:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061738.14 594698.89 61.88 lat/lon: 36.70 -121.94 2024-11-06T03:24:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.61 0.13 0.29 0.00 2024-11-06T03:24:29Z- AHRS_M2 msg: 1730863469.30 epoch sec; seqNo:4901 2024-11-06T03:24:29Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.02 psi 2024-11-06T03:24:29Z- handleDepth()- Depth_Keller msg: 1730863469.32 epoch sec; seqNo:5221; depth 49.71 2024-11-06T03:24:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863469.34 epoch sec; seqNo:5221; 4061708.57 north; 594715.96 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:24:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863468.92 2024-11-06T03:24:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:29Z- waiting for 1730863472.92, time is 1730863469.34 2024-11-06T03:24:29Z- AHRS_M2 msg: 1730863469.70 epoch sec; seqNo:4902 2024-11-06T03:24:29Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.02 psi 2024-11-06T03:24:29Z- handleDepth()- Depth_Keller msg: 1730863469.71 epoch sec; seqNo:5222; depth 49.71 2024-11-06T03:24:29Z- handleDVL() - DVL msg epochMillisec: 1730863469.74 2024-11-06T03:24:29Z- handleDvl() - RDI_Pathfinder msg: 1730863469.74 sec; seqNo:1813; #beams:4 2024-11-06T03:24:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.71 , 28.59 , 30.28 , 31.65 2024-11-06T03:24:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , -0.04 2024-11-06T03:24:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863469.75 epoch sec; seqNo:5222; 4061708.95 north; 594715.95 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:24:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863468.92 2024-11-06T03:24:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:29Z- waiting for 1730863472.92, time is 1730863469.75 2024-11-06T03:24:30Z- AHRS_M2 msg: 1730863470.10 epoch sec; seqNo:4903 2024-11-06T03:24:30Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.02 psi 2024-11-06T03:24:30Z- handleDepth()- Depth_Keller msg: 1730863470.11 epoch sec; seqNo:5223; depth 49.71 2024-11-06T03:24:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863470.12 epoch sec; seqNo:5223; 4061709.33 north; 594715.95 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:24:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863468.92 2024-11-06T03:24:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:30Z- waiting for 1730863472.92, time is 1730863470.12 2024-11-06T03:24:30Z- AHRS_M2 msg: 1730863470.51 epoch sec; seqNo:4904 2024-11-06T03:24:30Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.02 psi 2024-11-06T03:24:30Z- handleDepth()- Depth_Keller msg: 1730863470.51 epoch sec; seqNo:5224; depth 49.72 2024-11-06T03:24:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863470.52 epoch sec; seqNo:5224; 4061709.70 north; 594714.73 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:24:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863468.92 2024-11-06T03:24:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:30Z- waiting for 1730863472.92, time is 1730863470.52 2024-11-06T03:24:30Z- AHRS_M2 msg: 1730863470.91 epoch sec; seqNo:4905 2024-11-06T03:24:30Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.02 psi 2024-11-06T03:24:30Z- handleDepth()- Depth_Keller msg: 1730863470.92 epoch sec; seqNo:5225; depth 49.72 2024-11-06T03:24:30Z- handleDVL() - DVL msg epochMillisec: 1730863470.95 2024-11-06T03:24:30Z- handleDvl() - RDI_Pathfinder msg: 1730863470.95 sec; seqNo:1814; #beams:4 2024-11-06T03:24:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.41 , 28.59 , 29.97 , 31.81 2024-11-06T03:24:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.09 , -0.03 2024-11-06T03:24:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863470.96 epoch sec; seqNo:5225; 4061710.08 north; 594714.72 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:24:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863468.92 2024-11-06T03:24:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:31Z- waiting for 1730863472.92, time is 1730863470.96 2024-11-06T03:24:31Z- AHRS_M2 msg: 1730863471.32 epoch sec; seqNo:4906 2024-11-06T03:24:31Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.02 psi 2024-11-06T03:24:31Z- handleDepth()- Depth_Keller msg: 1730863471.33 epoch sec; seqNo:5226; depth 49.73 2024-11-06T03:24:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863471.34 epoch sec; seqNo:5226; 4061710.46 north; 594714.72 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:24:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863468.92 2024-11-06T03:24:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:31Z- waiting for 1730863472.92, time is 1730863471.34 2024-11-06T03:24:31Z- handleDepth()- Depth_Keller msg: 1730863471.74 epoch sec; seqNo:5227; depth 49.74 2024-11-06T03:24:31Z- handleDVL() - DVL msg epochMillisec: 1730863471.76 2024-11-06T03:24:31Z- handleDvl() - RDI_Pathfinder msg: 1730863471.76 sec; seqNo:1815; #beams:4 2024-11-06T03:24:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.41 , 29.05 , 30.28 , 31.81 2024-11-06T03:24:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.09 , -0.02 2024-11-06T03:24:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863471.77 epoch sec; seqNo:5227; 4061710.83 north; 594714.71 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:24:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863468.92 2024-11-06T03:24:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:31Z- waiting for 1730863472.92, time is 1730863471.77 2024-11-06T03:24:32Z- AHRS_M2 msg: 1730863472.12 epoch sec; seqNo:4907 2024-11-06T03:24:32Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.02 psi 2024-11-06T03:24:32Z- handleDepth()- Depth_Keller msg: 1730863472.13 epoch sec; seqNo:5228; depth 49.75 2024-11-06T03:24:32Z- handleDVL() - DVL msg epochMillisec: 1730863472.13 2024-11-06T03:24:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:24:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:24:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:24:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:24:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:24:32Z- handleDvl() - RDI_Pathfinder msg: 1730863472.13 sec; seqNo:1816; #beams:0 2024-11-06T03:24:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:24:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.09 , -0.02 2024-11-06T03:24:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863472.15 epoch sec; seqNo:5228; 4061711.21 north; 594714.71 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:24:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863468.92 2024-11-06T03:24:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:32Z- waiting for 1730863472.92, time is 1730863472.15 2024-11-06T03:24:32Z- AHRS_M2 msg: 1730863472.53 epoch sec; seqNo:4908 2024-11-06T03:24:32Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.02 psi 2024-11-06T03:24:32Z- handleDepth()- Depth_Keller msg: 1730863472.55 epoch sec; seqNo:5229; depth 49.78 2024-11-06T03:24:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863472.56 epoch sec; seqNo:5229; 4061711.59 north; 594714.71 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:24:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863468.92 2024-11-06T03:24:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:32Z- waiting for 1730863472.92, time is 1730863472.56 2024-11-06T03:24:32Z- AHRS_M2 msg: 1730863472.93 epoch sec; seqNo:4909 2024-11-06T03:24:32Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.02 psi 2024-11-06T03:24:32Z- handleDepth()- Depth_Keller msg: 1730863472.94 epoch sec; seqNo:5230; depth 49.79 2024-11-06T03:24:32Z- handleDVL() - DVL msg epochMillisec: 1730863472.97 2024-11-06T03:24:32Z- handleDvl() - RDI_Pathfinder msg: 1730863472.97 sec; seqNo:1817; #beams:4 2024-11-06T03:24:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.87 , 28.75 , 30.28 , 31.81 2024-11-06T03:24:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.10 , -0.01 2024-11-06T03:24:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863472.98 epoch sec; seqNo:5230; 4061711.97 north; 594714.70 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:24:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863468.92 2024-11-06T03:24:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:33Z- waiting for 1730863472.92, time is 1730863472.98 2024-11-06T03:24:33Z- LcmTrn::updateTrn() >>>> pose time 1730863472.98, meas time 1730863472.97 <<<< 2024-11-06T03:24:33Z- LcmTrn::updateTrn() equating pose 1730863472.97 and meas times 1730863472.97 2024-11-06T03:24:33Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863472.97 2024-11-06T03:24:33Z- TerrainNav::motionUpdate() - currEstimate = (4061712, 594714.70, 49.79) 2024-11-06T03:24:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:24:33Z- TerrainNav:: SubcloudNIS is 0.64, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:24:33Z- TNavPF::Projecting Measurements 2024-11-06T03:24:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:24:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:24:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:24:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:24:33Z- TNavFilter::4 good beams. 2024-11-06T03:24:33Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:24:33Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:24:33Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:24:33Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:24:33Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:24:33Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:24:33Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:24:33Z- TerrainMapOctree: (vehN, vehE ) = (4061711.97, 594714.70). 2024-11-06T03:24:33Z- TerrainMapOctree: Min Distance = 51.89. 2024-11-06T03:24:33Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:24:33Z- Forcing Subcloud Comparison 2024-11-06T03:24:33Z- Weighting particles with subcloud comparison 2024-11-06T03:24:33Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:33Z- meanExpectedMeasDiff: 0.537224 delta_rms_squared: 0.881280 alpha: 0.977809 2024-11-06T03:24:33Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:33Z- meanExpectedMeasDiff: 0.070878 delta_rms_squared: 0.612005 alpha: 0.968355 2024-11-06T03:24:33Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:33Z- meanExpectedMeasDiff: -0.240215 delta_rms_squared: 0.694389 alpha: 0.972004 2024-11-06T03:24:33Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:33Z- meanExpectedMeasDiff: -0.210863 delta_rms_squared: 0.765319 alpha: 0.974533 2024-11-06T03:24:33Z- Subcloud NIS: 0.628076 2024-11-06T03:24:33Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:24:33Z- TNavPF:: sumWeights = 0.690562 2024-11-06T03:24:33Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:24:33Z- TNavPF::First Term of Map Covariance Matrix is 0.35 2024-11-06T03:24:33Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.03 2024-11-06T03:24:33Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863472.97 sec, ping # 0. 2024-11-06T03:24:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:24:33Z- variances([0], [2], and psi_berg) = 0.609188 0.133448 0.000000 2024-11-06T03:24:33Z- publishEstimatesDecoder() - at 1730863472.97 sec 2024-11-06T03:24:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:24:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061742.04 594697.36 61.93 2024-11-06T03:24:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061741.54 594697.63 61.95 lat/lon: 36.70 -121.94 2024-11-06T03:24:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.61 0.13 0.29 0.00 2024-11-06T03:24:33Z- handleDepth()- Depth_Keller msg: 1730863473.36 epoch sec; seqNo:5231; depth 49.79 2024-11-06T03:24:33Z- handleDVL() - DVL msg epochMillisec: 1730863473.36 2024-11-06T03:24:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:24:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:24:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:24:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:24:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:24:33Z- handleDvl() - RDI_Pathfinder msg: 1730863473.36 sec; seqNo:1818; #beams:0 2024-11-06T03:24:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:24:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.10 , -0.01 2024-11-06T03:24:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863473.39 epoch sec; seqNo:5231; 4061712.35 north; 594714.70 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:24:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863472.97 2024-11-06T03:24:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:33Z- waiting for 1730863476.97, time is 1730863473.39 2024-11-06T03:24:33Z- AHRS_M2 msg: 1730863473.74 epoch sec; seqNo:4910 2024-11-06T03:24:33Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 6.03 psi 2024-11-06T03:24:33Z- handleDepth()- Depth_Keller msg: 1730863473.75 epoch sec; seqNo:5232; depth 49.82 2024-11-06T03:24:33Z- handleDVL() - DVL msg epochMillisec: 1730863473.77 2024-11-06T03:24:33Z- handleDvl() - RDI_Pathfinder msg: 1730863473.77 sec; seqNo:1819; #beams:4 2024-11-06T03:24:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.02 , 28.44 , 30.58 , 31.96 2024-11-06T03:24:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.09 , 0.00 2024-11-06T03:24:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863473.78 epoch sec; seqNo:5232; 4061712.73 north; 594714.69 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:24:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863472.97 2024-11-06T03:24:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:33Z- waiting for 1730863476.97, time is 1730863473.78 2024-11-06T03:24:34Z- AHRS_M2 msg: 1730863474.14 epoch sec; seqNo:4911 2024-11-06T03:24:34Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 6.03 psi 2024-11-06T03:24:34Z- handleDepth()- Depth_Keller msg: 1730863474.15 epoch sec; seqNo:5233; depth 49.83 2024-11-06T03:24:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863474.17 epoch sec; seqNo:5233; 4061713.09 north; 594713.47 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:24:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863472.97 2024-11-06T03:24:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:34Z- waiting for 1730863476.97, time is 1730863474.17 2024-11-06T03:24:34Z- AHRS_M2 msg: 1730863474.55 epoch sec; seqNo:4912 2024-11-06T03:24:34Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 6.03 psi 2024-11-06T03:24:34Z- handleDepth()- Depth_Keller msg: 1730863474.56 epoch sec; seqNo:5234; depth 49.84 2024-11-06T03:24:34Z- handleDVL() - DVL msg epochMillisec: 1730863474.56 2024-11-06T03:24:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:24:34Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:24:34Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:24:34Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:24:34Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:24:34Z- handleDvl() - RDI_Pathfinder msg: 1730863474.56 sec; seqNo:1820; #beams:0 2024-11-06T03:24:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:24:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.09 , 0.00 2024-11-06T03:24:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863474.58 epoch sec; seqNo:5234; 4061713.47 north; 594713.46 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:24:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863472.97 2024-11-06T03:24:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:34Z- waiting for 1730863476.97, time is 1730863474.58 2024-11-06T03:24:34Z- AHRS_M2 msg: 1730863474.95 epoch sec; seqNo:4913 2024-11-06T03:24:34Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 6.03 psi 2024-11-06T03:24:34Z- handleDepth()- Depth_Keller msg: 1730863474.96 epoch sec; seqNo:5235; depth 49.86 2024-11-06T03:24:35Z- handleDVL() - DVL msg epochMillisec: 1730863474.99 2024-11-06T03:24:35Z- handleDvl() - RDI_Pathfinder msg: 1730863474.99 sec; seqNo:1821; #beams:4 2024-11-06T03:24:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.87 , 28.29 , 30.58 , 32.26 2024-11-06T03:24:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.09 , -0.00 2024-11-06T03:24:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863475.01 epoch sec; seqNo:5235; 4061713.85 north; 594713.46 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:24:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863472.97 2024-11-06T03:24:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:35Z- waiting for 1730863476.97, time is 1730863475.01 2024-11-06T03:24:35Z- AHRS_M2 msg: 1730863475.36 epoch sec; seqNo:4914 2024-11-06T03:24:35Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 6.03 psi 2024-11-06T03:24:35Z- handleDepth()- Depth_Keller msg: 1730863475.37 epoch sec; seqNo:5236; depth 49.89 2024-11-06T03:24:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863475.39 epoch sec; seqNo:5236; 4061714.23 north; 594713.46 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:24:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863472.97 2024-11-06T03:24:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:35Z- waiting for 1730863476.97, time is 1730863475.39 2024-11-06T03:24:35Z- AHRS_M2 msg: 1730863475.77 epoch sec; seqNo:4915 2024-11-06T03:24:35Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.03 psi 2024-11-06T03:24:35Z- handleDepth()- Depth_Keller msg: 1730863475.78 epoch sec; seqNo:5237; depth 49.90 2024-11-06T03:24:35Z- handleDVL() - DVL msg epochMillisec: 1730863475.80 2024-11-06T03:24:35Z- handleDvl() - RDI_Pathfinder msg: 1730863475.80 sec; seqNo:1822; #beams:4 2024-11-06T03:24:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.17 , 28.29 , 31.65 , 32.11 2024-11-06T03:24:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.08 , -0.01 2024-11-06T03:24:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863475.81 epoch sec; seqNo:5237; 4061714.61 north; 594713.45 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:24:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863472.97 2024-11-06T03:24:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:35Z- waiting for 1730863476.97, time is 1730863475.81 2024-11-06T03:24:36Z- AHRS_M2 msg: 1730863476.16 epoch sec; seqNo:4916 2024-11-06T03:24:36Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.03 psi 2024-11-06T03:24:36Z- handleDepth()- Depth_Keller msg: 1730863476.17 epoch sec; seqNo:5238; depth 49.90 2024-11-06T03:24:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863476.20 epoch sec; seqNo:5238; 4061714.61 north; 594713.45 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:24:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863472.97 2024-11-06T03:24:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:36Z- waiting for 1730863476.97, time is 1730863476.20 2024-11-06T03:24:36Z- AHRS_M2 msg: 1730863476.57 epoch sec; seqNo:4917 2024-11-06T03:24:36Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.03 psi 2024-11-06T03:24:36Z- handleDepth()- Depth_Keller msg: 1730863476.58 epoch sec; seqNo:5239; depth 49.92 2024-11-06T03:24:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863476.59 epoch sec; seqNo:5239; 4061714.99 north; 594713.45 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:24:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863472.97 2024-11-06T03:24:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:36Z- waiting for 1730863476.97, time is 1730863476.59 2024-11-06T03:24:36Z- AHRS_M2 msg: 1730863476.97 epoch sec; seqNo:4918 2024-11-06T03:24:36Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.03 psi 2024-11-06T03:24:36Z- handleDepth()- Depth_Keller msg: 1730863476.98 epoch sec; seqNo:5240; depth 49.95 2024-11-06T03:24:37Z- handleDVL() - DVL msg epochMillisec: 1730863477.01 2024-11-06T03:24:37Z- handleDvl() - RDI_Pathfinder msg: 1730863477.01 sec; seqNo:1823; #beams:4 2024-11-06T03:24:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.86 , 28.44 , 31.34 , 31.96 2024-11-06T03:24:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.09 , -0.01 2024-11-06T03:24:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863477.02 epoch sec; seqNo:5240; 4061715.37 north; 594713.44 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:24:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863472.97 2024-11-06T03:24:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:37Z- waiting for 1730863476.97, time is 1730863477.02 2024-11-06T03:24:37Z- LcmTrn::updateTrn() >>>> pose time 1730863477.02, meas time 1730863477.01 <<<< 2024-11-06T03:24:37Z- LcmTrn::updateTrn() equating pose 1730863477.01 and meas times 1730863477.01 2024-11-06T03:24:37Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863477.01 2024-11-06T03:24:37Z- TerrainNav::motionUpdate() - currEstimate = (4061715, 594713.44, 49.95) 2024-11-06T03:24:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:24:37Z- TerrainNav:: SubcloudNIS is 0.63, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:24:37Z- TNavPF::Projecting Measurements 2024-11-06T03:24:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:24:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:24:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:24:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:24:37Z- TNavFilter::4 good beams. 2024-11-06T03:24:37Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:24:37Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:24:37Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:24:37Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:24:37Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:24:37Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:24:37Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:24:37Z- TerrainMapOctree: (vehN, vehE ) = (4061715.37, 594713.44). 2024-11-06T03:24:37Z- TerrainMapOctree: Min Distance = 50.36. 2024-11-06T03:24:37Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:24:37Z- Forcing Subcloud Comparison 2024-11-06T03:24:37Z- Weighting particles with subcloud comparison 2024-11-06T03:24:37Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:37Z- meanExpectedMeasDiff: 0.910520 delta_rms_squared: 0.885609 alpha: 0.977915 2024-11-06T03:24:37Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:37Z- meanExpectedMeasDiff: -0.150162 delta_rms_squared: 0.625082 alpha: 0.968996 2024-11-06T03:24:37Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:37Z- meanExpectedMeasDiff: 1.086979 delta_rms_squared: 0.701343 alpha: 0.972274 2024-11-06T03:24:37Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:37Z- meanExpectedMeasDiff: 0.091385 delta_rms_squared: 0.778940 alpha: 0.974967 2024-11-06T03:24:37Z- Subcloud NIS: 0.607184 2024-11-06T03:24:37Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:24:37Z- TNavPF:: sumWeights = 0.579397 2024-11-06T03:24:37Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:24:37Z- TNavPF::First Term of Map Covariance Matrix is 0.33 2024-11-06T03:24:37Z- TNavPF::Calculated NIS Value : 0.24 numBeams normalized NIS: 0.06 2024-11-06T03:24:37Z- TerrainNav::Resampling particle filter... 2024-11-06T03:24:37Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863477.01 sec, ping # 0. 2024-11-06T03:24:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:24:37Z- variances([0], [2], and psi_berg) = 0.602930 0.136369 0.000000 2024-11-06T03:24:37Z- publishEstimatesDecoder() - at 1730863477.01 sec 2024-11-06T03:24:37Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:24:37Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061745.70 594696.20 61.59 2024-11-06T03:24:37Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061744.93 594696.38 61.93 lat/lon: 36.70 -121.94 2024-11-06T03:24:37Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.60 0.14 0.30 0.00 2024-11-06T03:24:37Z- AHRS_M2 msg: 1730863477.38 epoch sec; seqNo:4919 2024-11-06T03:24:37Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.03 psi 2024-11-06T03:24:37Z- handleDepth()- Depth_Keller msg: 1730863477.39 epoch sec; seqNo:5241; depth 49.97 2024-11-06T03:24:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863477.42 epoch sec; seqNo:5241; 4061715.75 north; 594713.44 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:24:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863477.01 2024-11-06T03:24:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:37Z- waiting for 1730863481.01, time is 1730863477.42 2024-11-06T03:24:37Z- AHRS_M2 msg: 1730863477.79 epoch sec; seqNo:4920 2024-11-06T03:24:37Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.03 psi 2024-11-06T03:24:37Z- handleDepth()- Depth_Keller msg: 1730863477.80 epoch sec; seqNo:5242; depth 49.98 2024-11-06T03:24:37Z- handleDVL() - DVL msg epochMillisec: 1730863477.82 2024-11-06T03:24:37Z- handleDvl() - RDI_Pathfinder msg: 1730863477.82 sec; seqNo:1824; #beams:4 2024-11-06T03:24:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 28.75 , 31.34 , 31.96 2024-11-06T03:24:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.10 , -0.01 2024-11-06T03:24:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863477.84 epoch sec; seqNo:5242; 4061716.11 north; 594712.21 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:24:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863477.01 2024-11-06T03:24:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:37Z- waiting for 1730863481.01, time is 1730863477.84 2024-11-06T03:24:38Z- AHRS_M2 msg: 1730863478.19 epoch sec; seqNo:4921 2024-11-06T03:24:38Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.03 psi 2024-11-06T03:24:38Z- handleDepth()- Depth_Keller msg: 1730863478.20 epoch sec; seqNo:5243; depth 50.00 2024-11-06T03:24:38Z- handleDVL() - DVL msg epochMillisec: 1730863478.20 2024-11-06T03:24:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:24:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:24:38Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:24:38Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:24:38Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:24:38Z- handleDvl() - RDI_Pathfinder msg: 1730863478.20 sec; seqNo:1825; #beams:0 2024-11-06T03:24:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:24:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.10 , -0.01 2024-11-06T03:24:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863478.23 epoch sec; seqNo:5243; 4061716.49 north; 594712.21 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:24:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863477.01 2024-11-06T03:24:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:38Z- waiting for 1730863481.01, time is 1730863478.23 2024-11-06T03:24:38Z- AHRS_M2 msg: 1730863478.59 epoch sec; seqNo:4922 2024-11-06T03:24:38Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.04 psi 2024-11-06T03:24:38Z- handleDepth()- Depth_Keller msg: 1730863478.60 epoch sec; seqNo:5244; depth 50.02 2024-11-06T03:24:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863478.62 epoch sec; seqNo:5244; 4061716.87 north; 594712.21 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:24:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863477.01 2024-11-06T03:24:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:38Z- waiting for 1730863481.01, time is 1730863478.62 2024-11-06T03:24:38Z- AHRS_M2 msg: 1730863478.99 epoch sec; seqNo:4923 2024-11-06T03:24:38Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.03 psi 2024-11-06T03:24:38Z- handleDepth()- Depth_Keller msg: 1730863479.00 epoch sec; seqNo:5245; depth 50.03 2024-11-06T03:24:39Z- handleDVL() - DVL msg epochMillisec: 1730863479.02 2024-11-06T03:24:39Z- handleDvl() - RDI_Pathfinder msg: 1730863479.02 sec; seqNo:1826; #beams:4 2024-11-06T03:24:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 28.59 , 31.50 , 31.96 2024-11-06T03:24:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.09 , -0.00 2024-11-06T03:24:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863479.04 epoch sec; seqNo:5245; 4061717.25 north; 594712.20 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:24:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863477.01 2024-11-06T03:24:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:39Z- waiting for 1730863481.01, time is 1730863479.04 2024-11-06T03:24:39Z- AHRS_M2 msg: 1730863479.39 epoch sec; seqNo:4924 2024-11-06T03:24:39Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 6.04 psi 2024-11-06T03:24:39Z- handleDepth()- Depth_Keller msg: 1730863479.40 epoch sec; seqNo:5246; depth 50.05 2024-11-06T03:24:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863479.42 epoch sec; seqNo:5246; 4061717.63 north; 594712.20 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:24:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863477.01 2024-11-06T03:24:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:39Z- waiting for 1730863481.01, time is 1730863479.42 2024-11-06T03:24:39Z- AHRS_M2 msg: 1730863479.82 epoch sec; seqNo:4925 2024-11-06T03:24:39Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.04 psi 2024-11-06T03:24:39Z- handleDepth()- Depth_Keller msg: 1730863479.82 epoch sec; seqNo:5247; depth 50.06 2024-11-06T03:24:39Z- handleDVL() - DVL msg epochMillisec: 1730863479.86 2024-11-06T03:24:39Z- handleDvl() - RDI_Pathfinder msg: 1730863479.86 sec; seqNo:1827; #beams:4 2024-11-06T03:24:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.48 , 28.29 , 31.81 , 32.11 2024-11-06T03:24:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.08 , 0.00 2024-11-06T03:24:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863479.87 epoch sec; seqNo:5247; 4061718.01 north; 594712.19 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:24:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863477.01 2024-11-06T03:24:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:39Z- waiting for 1730863481.01, time is 1730863479.87 2024-11-06T03:24:40Z- AHRS_M2 msg: 1730863480.20 epoch sec; seqNo:4926 2024-11-06T03:24:40Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.04 psi 2024-11-06T03:24:40Z- handleDepth()- Depth_Keller msg: 1730863480.21 epoch sec; seqNo:5248; depth 50.08 2024-11-06T03:24:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863480.22 epoch sec; seqNo:5248; 4061718.38 north; 594712.19 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:24:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863477.01 2024-11-06T03:24:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:40Z- waiting for 1730863481.01, time is 1730863480.22 2024-11-06T03:24:40Z- AHRS_M2 msg: 1730863480.61 epoch sec; seqNo:4927 2024-11-06T03:24:40Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.04 psi 2024-11-06T03:24:40Z- handleDepth()- Depth_Keller msg: 1730863480.62 epoch sec; seqNo:5249; depth 50.10 2024-11-06T03:24:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863480.64 epoch sec; seqNo:5249; 4061718.76 north; 594712.18 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:24:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863477.01 2024-11-06T03:24:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:40Z- waiting for 1730863481.01, time is 1730863480.64 2024-11-06T03:24:41Z- AHRS_M2 msg: 1730863481.01 epoch sec; seqNo:4928 2024-11-06T03:24:41Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.04 psi 2024-11-06T03:24:41Z- handleDepth()- Depth_Keller msg: 1730863481.02 epoch sec; seqNo:5250; depth 50.11 2024-11-06T03:24:41Z- handleDVL() - DVL msg epochMillisec: 1730863481.04 2024-11-06T03:24:41Z- handleDvl() - RDI_Pathfinder msg: 1730863481.04 sec; seqNo:1828; #beams:4 2024-11-06T03:24:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.78 , 28.59 , 31.65 , 32.11 2024-11-06T03:24:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , 0.00 2024-11-06T03:24:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863481.06 epoch sec; seqNo:5250; 4061719.14 north; 594712.18 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:24:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863477.01 2024-11-06T03:24:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:41Z- waiting for 1730863481.01, time is 1730863481.06 2024-11-06T03:24:41Z- LcmTrn::updateTrn() >>>> pose time 1730863481.06, meas time 1730863481.04 <<<< 2024-11-06T03:24:41Z- LcmTrn::updateTrn() equating pose 1730863481.04 and meas times 1730863481.04 2024-11-06T03:24:41Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863481.04 2024-11-06T03:24:41Z- TerrainNav::motionUpdate() - currEstimate = (4061719, 594712.18, 50.11) 2024-11-06T03:24:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:24:41Z- TerrainNav:: SubcloudNIS is 0.61, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:24:41Z- TNavPF::Projecting Measurements 2024-11-06T03:24:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:24:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:24:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:24:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:24:41Z- TNavFilter::4 good beams. 2024-11-06T03:24:41Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:24:41Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:24:41Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:24:41Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:24:41Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:24:41Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:24:41Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:24:41Z- TerrainMapOctree: (vehN, vehE ) = (4061719.14, 594712.18). 2024-11-06T03:24:41Z- TerrainMapOctree: Min Distance = 48.81. 2024-11-06T03:24:41Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:24:41Z- Forcing Subcloud Comparison 2024-11-06T03:24:41Z- Weighting particles with subcloud comparison 2024-11-06T03:24:41Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:41Z- meanExpectedMeasDiff: 1.732768 delta_rms_squared: 0.901578 alpha: 0.978298 2024-11-06T03:24:41Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:41Z- meanExpectedMeasDiff: 0.001462 delta_rms_squared: 0.632858 alpha: 0.969365 2024-11-06T03:24:41Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:41Z- meanExpectedMeasDiff: 1.307789 delta_rms_squared: 0.714035 alpha: 0.972753 2024-11-06T03:24:41Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:41Z- meanExpectedMeasDiff: 0.252736 delta_rms_squared: 0.788088 alpha: 0.975250 2024-11-06T03:24:41Z- Subcloud NIS: 0.554265 2024-11-06T03:24:41Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:24:41Z- TNavPF:: sumWeights = 0.456775 2024-11-06T03:24:41Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:24:41Z- TNavPF::First Term of Map Covariance Matrix is 0.45 2024-11-06T03:24:41Z- TNavPF::Calculated NIS Value : 0.24 numBeams normalized NIS: 0.06 2024-11-06T03:24:41Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863481.04 sec, ping # 0. 2024-11-06T03:24:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:24:41Z- variances([0], [2], and psi_berg) = 0.595766 0.138115 0.000000 2024-11-06T03:24:41Z- publishEstimatesDecoder() - at 1730863481.04 sec 2024-11-06T03:24:41Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:24:41Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061748.25 594694.39 61.39 2024-11-06T03:24:41Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061748.70 594695.12 61.76 lat/lon: 36.70 -121.94 2024-11-06T03:24:41Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.60 0.14 0.29 0.00 2024-11-06T03:24:41Z- AHRS_M2 msg: 1730863481.41 epoch sec; seqNo:4929 2024-11-06T03:24:41Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.04 psi 2024-11-06T03:24:41Z- handleDepth()- Depth_Keller msg: 1730863481.42 epoch sec; seqNo:5251; depth 50.13 2024-11-06T03:24:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863481.45 epoch sec; seqNo:5251; 4061719.52 north; 594712.18 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:24:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863481.04 2024-11-06T03:24:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:41Z- waiting for 1730863485.04, time is 1730863481.45 2024-11-06T03:24:41Z- handleDepth()- Depth_Keller msg: 1730863481.87 epoch sec; seqNo:5252; depth 50.16 2024-11-06T03:24:41Z- handleDVL() - DVL msg epochMillisec: 1730863481.89 2024-11-06T03:24:41Z- handleDvl() - RDI_Pathfinder msg: 1730863481.89 sec; seqNo:1829; #beams:4 2024-11-06T03:24:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.17 , 28.75 , 31.50 , 31.96 2024-11-06T03:24:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , 0.00 2024-11-06T03:24:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863481.90 epoch sec; seqNo:5252; 4061719.90 north; 594712.17 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:24:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863481.04 2024-11-06T03:24:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:42Z- waiting for 1730863485.04, time is 1730863481.90 2024-11-06T03:24:42Z- AHRS_M2 msg: 1730863482.22 epoch sec; seqNo:4930 2024-11-06T03:24:42Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 6.04 psi 2024-11-06T03:24:42Z- handleDepth()- Depth_Keller msg: 1730863482.23 epoch sec; seqNo:5253; depth 50.17 2024-11-06T03:24:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863482.25 epoch sec; seqNo:5253; 4061719.89 north; 594710.95 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:24:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863481.04 2024-11-06T03:24:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:42Z- waiting for 1730863485.04, time is 1730863482.25 2024-11-06T03:24:42Z- AHRS_M2 msg: 1730863482.63 epoch sec; seqNo:4931 2024-11-06T03:24:42Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.05 psi 2024-11-06T03:24:42Z- handleDepth()- Depth_Keller msg: 1730863482.64 epoch sec; seqNo:5254; depth 50.17 2024-11-06T03:24:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863482.65 epoch sec; seqNo:5254; 4061720.27 north; 594710.95 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:24:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863481.04 2024-11-06T03:24:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:42Z- waiting for 1730863485.04, time is 1730863482.65 2024-11-06T03:24:43Z- AHRS_M2 msg: 1730863483.03 epoch sec; seqNo:4932 2024-11-06T03:24:43Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.05 psi 2024-11-06T03:24:43Z- handleDepth()- Depth_Keller msg: 1730863483.04 epoch sec; seqNo:5255; depth 50.17 2024-11-06T03:24:43Z- handleDVL() - DVL msg epochMillisec: 1730863483.06 2024-11-06T03:24:43Z- handleDvl() - RDI_Pathfinder msg: 1730863483.06 sec; seqNo:1830; #beams:4 2024-11-06T03:24:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 28.59 , 31.65 , 31.65 2024-11-06T03:24:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.01 2024-11-06T03:24:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863483.08 epoch sec; seqNo:5255; 4061720.64 north; 594710.94 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:24:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863481.04 2024-11-06T03:24:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:43Z- waiting for 1730863485.04, time is 1730863483.08 2024-11-06T03:24:43Z- AHRS_M2 msg: 1730863483.44 epoch sec; seqNo:4933 2024-11-06T03:24:43Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.05 psi 2024-11-06T03:24:43Z- handleDepth()- Depth_Keller msg: 1730863483.44 epoch sec; seqNo:5256; depth 50.18 2024-11-06T03:24:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863483.46 epoch sec; seqNo:5256; 4061721.02 north; 594710.94 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:24:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863481.04 2024-11-06T03:24:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:43Z- waiting for 1730863485.04, time is 1730863483.46 2024-11-06T03:24:43Z- AHRS_M2 msg: 1730863483.88 epoch sec; seqNo:4934 2024-11-06T03:24:43Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.05 psi 2024-11-06T03:24:43Z- handleDepth()- Depth_Keller msg: 1730863483.88 epoch sec; seqNo:5257; depth 50.19 2024-11-06T03:24:43Z- handleDVL() - DVL msg epochMillisec: 1730863483.91 2024-11-06T03:24:43Z- handleDvl() - RDI_Pathfinder msg: 1730863483.91 sec; seqNo:1831; #beams:4 2024-11-06T03:24:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 29.21 , 31.81 , 31.65 2024-11-06T03:24:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , -0.02 2024-11-06T03:24:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863483.93 epoch sec; seqNo:5257; 4061721.40 north; 594710.94 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:24:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863481.04 2024-11-06T03:24:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:44Z- waiting for 1730863485.04, time is 1730863483.93 2024-11-06T03:24:44Z- AHRS_M2 msg: 1730863484.24 epoch sec; seqNo:4935 2024-11-06T03:24:44Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.05 psi 2024-11-06T03:24:44Z- handleDepth()- Depth_Keller msg: 1730863484.25 epoch sec; seqNo:5258; depth 50.20 2024-11-06T03:24:44Z- handleDVL() - DVL msg epochMillisec: 1730863484.26 2024-11-06T03:24:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:24:44Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:24:44Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:24:44Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:24:44Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:24:44Z- handleDvl() - RDI_Pathfinder msg: 1730863484.26 sec; seqNo:1832; #beams:0 2024-11-06T03:24:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:24:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , -0.02 2024-11-06T03:24:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863484.28 epoch sec; seqNo:5258; 4061721.78 north; 594710.93 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:24:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863481.04 2024-11-06T03:24:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:44Z- waiting for 1730863485.04, time is 1730863484.28 2024-11-06T03:24:44Z- AHRS_M2 msg: 1730863484.65 epoch sec; seqNo:4936 2024-11-06T03:24:44Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.06 psi 2024-11-06T03:24:44Z- handleDepth()- Depth_Keller msg: 1730863484.65 epoch sec; seqNo:5259; depth 50.21 2024-11-06T03:24:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863484.67 epoch sec; seqNo:5259; 4061722.16 north; 594710.93 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:24:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863481.04 2024-11-06T03:24:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:44Z- waiting for 1730863485.04, time is 1730863484.67 2024-11-06T03:24:45Z- AHRS_M2 msg: 1730863485.06 epoch sec; seqNo:4937 2024-11-06T03:24:45Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.06 psi 2024-11-06T03:24:45Z- handleDepth()- Depth_Keller msg: 1730863485.06 epoch sec; seqNo:5260; depth 50.21 2024-11-06T03:24:45Z- handleDVL() - DVL msg epochMillisec: 1730863485.09 2024-11-06T03:24:45Z- handleDvl() - RDI_Pathfinder msg: 1730863485.09 sec; seqNo:1833; #beams:4 2024-11-06T03:24:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.47 , 29.36 , 32.57 , 31.80 2024-11-06T03:24:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , -0.04 2024-11-06T03:24:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863485.10 epoch sec; seqNo:5260; 4061722.54 north; 594710.92 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:24:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863481.04 2024-11-06T03:24:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:45Z- waiting for 1730863485.04, time is 1730863485.10 2024-11-06T03:24:45Z- LcmTrn::updateTrn() >>>> pose time 1730863485.10, meas time 1730863485.09 <<<< 2024-11-06T03:24:45Z- LcmTrn::updateTrn() equating pose 1730863485.09 and meas times 1730863485.09 2024-11-06T03:24:45Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863485.09 2024-11-06T03:24:45Z- TerrainNav::motionUpdate() - currEstimate = (4061723, 594710.92, 50.21) 2024-11-06T03:24:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:24:45Z- TerrainNav:: SubcloudNIS is 0.55, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:24:45Z- TNavPF::Projecting Measurements 2024-11-06T03:24:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:24:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:24:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:24:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:24:45Z- TNavFilter::4 good beams. 2024-11-06T03:24:45Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:24:45Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:24:45Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:24:45Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:24:45Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:24:45Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:24:45Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:24:45Z- TerrainMapOctree: (vehN, vehE ) = (4061722.54, 594710.92). 2024-11-06T03:24:45Z- TerrainMapOctree: Min Distance = 47.76. 2024-11-06T03:24:45Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:24:45Z- Forcing Subcloud Comparison 2024-11-06T03:24:45Z- Weighting particles with subcloud comparison 2024-11-06T03:24:45Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:45Z- meanExpectedMeasDiff: 1.071353 delta_rms_squared: 0.877520 alpha: 0.977716 2024-11-06T03:24:45Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:45Z- meanExpectedMeasDiff: 0.585356 delta_rms_squared: 0.610796 alpha: 0.968294 2024-11-06T03:24:45Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:45Z- meanExpectedMeasDiff: 1.609646 delta_rms_squared: 0.757577 alpha: 0.974279 2024-11-06T03:24:45Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:45Z- meanExpectedMeasDiff: 0.051031 delta_rms_squared: 0.746068 alpha: 0.973893 2024-11-06T03:24:45Z- Subcloud NIS: 0.473199 2024-11-06T03:24:45Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:24:45Z- TNavPF:: sumWeights = 0.495144 2024-11-06T03:24:45Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:24:45Z- TNavPF::First Term of Map Covariance Matrix is 0.29 2024-11-06T03:24:45Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T03:24:45Z- TerrainNav::Resampling particle filter... 2024-11-06T03:24:45Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863485.09 sec, ping # 0. 2024-11-06T03:24:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:24:45Z- variances([0], [2], and psi_berg) = 0.645723 0.145024 0.000000 2024-11-06T03:24:45Z- publishEstimatesDecoder() - at 1730863485.09 sec 2024-11-06T03:24:45Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:24:45Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061752.93 594693.67 61.89 2024-11-06T03:24:45Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061752.12 594693.85 61.54 lat/lon: 36.70 -121.94 2024-11-06T03:24:45Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.65 0.15 0.29 0.00 2024-11-06T03:24:45Z- handleDepth()- Depth_Keller msg: 1730863485.48 epoch sec; seqNo:5261; depth 50.21 2024-11-06T03:24:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863485.50 epoch sec; seqNo:5261; 4061722.92 north; 594710.92 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:24:45Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:24:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863485.09 2024-11-06T03:24:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:45Z- waiting for 1730863489.09, time is 1730863485.50 2024-11-06T03:24:45Z- AHRS_M2 msg: 1730863485.91 epoch sec; seqNo:4938 2024-11-06T03:24:45Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.06 psi 2024-11-06T03:24:45Z- handleDepth()- Depth_Keller msg: 1730863485.91 epoch sec; seqNo:5262; depth 50.20 2024-11-06T03:24:45Z- handleDVL() - DVL msg epochMillisec: 1730863485.93 2024-11-06T03:24:45Z- handleDvl() - RDI_Pathfinder msg: 1730863485.93 sec; seqNo:1834; #beams:4 2024-11-06T03:24:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.47 , 29.20 , 34.25 , 31.65 2024-11-06T03:24:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.07 , -0.05 2024-11-06T03:24:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863485.95 epoch sec; seqNo:5262; 4061723.30 north; 594710.91 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:24:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863485.09 2024-11-06T03:24:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:46Z- waiting for 1730863489.09, time is 1730863485.95 2024-11-06T03:24:46Z- AHRS_M2 msg: 1730863486.27 epoch sec; seqNo:4939 2024-11-06T03:24:46Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.06 psi 2024-11-06T03:24:46Z- handleDepth()- Depth_Keller msg: 1730863486.27 epoch sec; seqNo:5263; depth 50.21 2024-11-06T03:24:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863486.29 epoch sec; seqNo:5263; 4061723.67 north; 594710.91 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:24:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863485.09 2024-11-06T03:24:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:46Z- waiting for 1730863489.09, time is 1730863486.29 2024-11-06T03:24:46Z- AHRS_M2 msg: 1730863486.67 epoch sec; seqNo:4940 2024-11-06T03:24:46Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.06 psi 2024-11-06T03:24:46Z- handleDepth()- Depth_Keller msg: 1730863486.68 epoch sec; seqNo:5264; depth 50.23 2024-11-06T03:24:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863486.69 epoch sec; seqNo:5264; 4061724.04 north; 594709.69 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:24:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863485.09 2024-11-06T03:24:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:46Z- waiting for 1730863489.09, time is 1730863486.69 2024-11-06T03:24:47Z- AHRS_M2 msg: 1730863487.07 epoch sec; seqNo:4941 2024-11-06T03:24:47Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.06 psi 2024-11-06T03:24:47Z- handleDepth()- Depth_Keller msg: 1730863487.08 epoch sec; seqNo:5265; depth 50.23 2024-11-06T03:24:47Z- handleDVL() - DVL msg epochMillisec: 1730863487.10 2024-11-06T03:24:47Z- handleDvl() - RDI_Pathfinder msg: 1730863487.10 sec; seqNo:1835; #beams:4 2024-11-06T03:24:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.17 , 29.97 , 34.41 , 31.65 2024-11-06T03:24:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.10 , -0.05 2024-11-06T03:24:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863487.12 epoch sec; seqNo:5265; 4061724.42 north; 594709.68 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:24:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863485.09 2024-11-06T03:24:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:47Z- waiting for 1730863489.09, time is 1730863487.12 2024-11-06T03:24:47Z- AHRS_M2 msg: 1730863487.47 epoch sec; seqNo:4942 2024-11-06T03:24:47Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.06 psi 2024-11-06T03:24:47Z- handleDepth()- Depth_Keller msg: 1730863487.49 epoch sec; seqNo:5266; depth 50.22 2024-11-06T03:24:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863487.50 epoch sec; seqNo:5266; 4061724.80 north; 594709.68 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:24:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863485.09 2024-11-06T03:24:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:47Z- waiting for 1730863489.09, time is 1730863487.50 2024-11-06T03:24:47Z- AHRS_M2 msg: 1730863487.92 epoch sec; seqNo:4943 2024-11-06T03:24:47Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.06 psi 2024-11-06T03:24:47Z- handleDepth()- Depth_Keller msg: 1730863487.94 epoch sec; seqNo:5267; depth 50.22 2024-11-06T03:24:47Z- handleDVL() - DVL msg epochMillisec: 1730863487.96 2024-11-06T03:24:47Z- handleDvl() - RDI_Pathfinder msg: 1730863487.96 sec; seqNo:1836; #beams:4 2024-11-06T03:24:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 29.97 , 34.10 , 31.35 2024-11-06T03:24:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.13 , -0.04 2024-11-06T03:24:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863487.98 epoch sec; seqNo:5267; 4061725.18 north; 594709.67 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:24:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863485.09 2024-11-06T03:24:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:48Z- waiting for 1730863489.09, time is 1730863487.98 2024-11-06T03:24:48Z- AHRS_M2 msg: 1730863488.28 epoch sec; seqNo:4944 2024-11-06T03:24:48Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.06 psi 2024-11-06T03:24:48Z- handleDepth()- Depth_Keller msg: 1730863488.29 epoch sec; seqNo:5268; depth 50.22 2024-11-06T03:24:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863488.31 epoch sec; seqNo:5268; 4061725.18 north; 594709.67 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:24:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863485.09 2024-11-06T03:24:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:48Z- waiting for 1730863489.09, time is 1730863488.31 2024-11-06T03:24:48Z- AHRS_M2 msg: 1730863488.69 epoch sec; seqNo:4945 2024-11-06T03:24:48Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.06 psi 2024-11-06T03:24:48Z- handleDepth()- Depth_Keller msg: 1730863488.70 epoch sec; seqNo:5269; depth 50.22 2024-11-06T03:24:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863488.71 epoch sec; seqNo:5269; 4061725.56 north; 594709.67 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:24:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863485.09 2024-11-06T03:24:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:48Z- waiting for 1730863489.09, time is 1730863488.71 2024-11-06T03:24:49Z- AHRS_M2 msg: 1730863489.11 epoch sec; seqNo:4946 2024-11-06T03:24:49Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.06 psi 2024-11-06T03:24:49Z- handleDepth()- Depth_Keller msg: 1730863489.12 epoch sec; seqNo:5270; depth 50.23 2024-11-06T03:24:49Z- handleDVL() - DVL msg epochMillisec: 1730863489.14 2024-11-06T03:24:49Z- handleDvl() - RDI_Pathfinder msg: 1730863489.14 sec; seqNo:1837; #beams:4 2024-11-06T03:24:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 30.28 , 34.87 , 31.50 2024-11-06T03:24:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.13 , -0.03 2024-11-06T03:24:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863489.16 epoch sec; seqNo:5270; 4061725.93 north; 594709.66 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:24:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863485.09 2024-11-06T03:24:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:49Z- waiting for 1730863489.09, time is 1730863489.16 2024-11-06T03:24:49Z- LcmTrn::updateTrn() >>>> pose time 1730863489.16, meas time 1730863489.14 <<<< 2024-11-06T03:24:49Z- LcmTrn::updateTrn() equating pose 1730863489.14 and meas times 1730863489.14 2024-11-06T03:24:49Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863489.14 2024-11-06T03:24:49Z- TerrainNav::motionUpdate() - currEstimate = (4061726, 594709.66, 50.23) 2024-11-06T03:24:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:24:49Z- TerrainNav:: SubcloudNIS is 0.47, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:24:49Z- TNavPF::Projecting Measurements 2024-11-06T03:24:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:24:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:24:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:24:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:24:49Z- TNavFilter::4 good beams. 2024-11-06T03:24:49Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:24:49Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:24:49Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:24:49Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:24:49Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:24:49Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:24:49Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:24:49Z- TerrainMapOctree: (vehN, vehE ) = (4061725.93, 594709.66). 2024-11-06T03:24:49Z- TerrainMapOctree: Min Distance = 46.97. 2024-11-06T03:24:49Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:24:49Z- Forcing Subcloud Comparison 2024-11-06T03:24:49Z- Weighting particles with subcloud comparison 2024-11-06T03:24:49Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:49Z- meanExpectedMeasDiff: 0.913984 delta_rms_squared: 0.861410 alpha: 0.977309 2024-11-06T03:24:49Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:49Z- meanExpectedMeasDiff: 1.010255 delta_rms_squared: 0.620416 alpha: 0.968770 2024-11-06T03:24:49Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:49Z- meanExpectedMeasDiff: 2.250072 delta_rms_squared: 0.778186 alpha: 0.974943 2024-11-06T03:24:49Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:49Z- meanExpectedMeasDiff: -0.292310 delta_rms_squared: 0.733913 alpha: 0.973472 2024-11-06T03:24:49Z- Subcloud NIS: 0.419039 2024-11-06T03:24:49Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:24:49Z- TNavPF:: sumWeights = 0.359483 2024-11-06T03:24:49Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:24:49Z- TNavPF::First Term of Map Covariance Matrix is 0.20 2024-11-06T03:24:49Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.05 2024-11-06T03:24:49Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863489.14 sec, ping # 0. 2024-11-06T03:24:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:24:49Z- variances([0], [2], and psi_berg) = 0.633911 0.146206 0.000000 2024-11-06T03:24:49Z- publishEstimatesDecoder() - at 1730863489.14 sec 2024-11-06T03:24:49Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:24:49Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061755.69 594693.05 60.76 2024-11-06T03:24:49Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061755.52 594692.59 61.19 lat/lon: 36.70 -121.94 2024-11-06T03:24:49Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.63 0.15 0.30 0.00 2024-11-06T03:24:49Z- AHRS_M2 msg: 1730863489.49 epoch sec; seqNo:4947 2024-11-06T03:24:49Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.06 psi 2024-11-06T03:24:49Z- handleDepth()- Depth_Keller msg: 1730863489.51 epoch sec; seqNo:5271; depth 50.22 2024-11-06T03:24:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863489.52 epoch sec; seqNo:5271; 4061726.31 north; 594709.66 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:24:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863489.14 2024-11-06T03:24:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:49Z- waiting for 1730863493.14, time is 1730863489.52 2024-11-06T03:24:49Z- AHRS_M2 msg: 1730863489.90 epoch sec; seqNo:4948 2024-11-06T03:24:49Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.06 psi 2024-11-06T03:24:49Z- handleDepth()- Depth_Keller msg: 1730863489.90 epoch sec; seqNo:5272; depth 50.23 2024-11-06T03:24:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863489.92 epoch sec; seqNo:5272; 4061726.69 north; 594709.66 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:24:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863489.14 2024-11-06T03:24:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:50Z- waiting for 1730863493.14, time is 1730863489.92 2024-11-06T03:24:50Z- AHRS_M2 msg: 1730863490.30 epoch sec; seqNo:4949 2024-11-06T03:24:50Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.06 psi 2024-11-06T03:24:50Z- handleDepth()- Depth_Keller msg: 1730863490.31 epoch sec; seqNo:5273; depth 50.25 2024-11-06T03:24:50Z- handleDVL() - DVL msg epochMillisec: 1730863490.31 2024-11-06T03:24:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:24:50Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:24:50Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:24:50Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:24:50Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:24:50Z- handleDvl() - RDI_Pathfinder msg: 1730863490.31 sec; seqNo:1838; #beams:0 2024-11-06T03:24:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:24:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.13 , -0.03 2024-11-06T03:24:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863490.32 epoch sec; seqNo:5273; 4061727.07 north; 594709.65 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:24:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863489.14 2024-11-06T03:24:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:50Z- waiting for 1730863493.14, time is 1730863490.32 2024-11-06T03:24:50Z- AHRS_M2 msg: 1730863490.71 epoch sec; seqNo:4950 2024-11-06T03:24:50Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.06 psi 2024-11-06T03:24:50Z- handleDepth()- Depth_Keller msg: 1730863490.71 epoch sec; seqNo:5274; depth 50.25 2024-11-06T03:24:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863490.72 epoch sec; seqNo:5274; 4061727.44 north; 594708.43 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:24:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863489.14 2024-11-06T03:24:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:50Z- waiting for 1730863493.14, time is 1730863490.72 2024-11-06T03:24:51Z- AHRS_M2 msg: 1730863491.12 epoch sec; seqNo:4951 2024-11-06T03:24:51Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.06 psi 2024-11-06T03:24:51Z- handleDepth()- Depth_Keller msg: 1730863491.12 epoch sec; seqNo:5275; depth 50.27 2024-11-06T03:24:51Z- handleDVL() - DVL msg epochMillisec: 1730863491.15 2024-11-06T03:24:51Z- handleDvl() - RDI_Pathfinder msg: 1730863491.15 sec; seqNo:1839; #beams:4 2024-11-06T03:24:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.17 , 30.28 , 35.32 , 31.81 2024-11-06T03:24:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.12 , 0.00 2024-11-06T03:24:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863491.18 epoch sec; seqNo:5275; 4061727.82 north; 594708.42 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:24:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863489.14 2024-11-06T03:24:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:51Z- waiting for 1730863493.14, time is 1730863491.18 2024-11-06T03:24:51Z- AHRS_M2 msg: 1730863491.51 epoch sec; seqNo:4952 2024-11-06T03:24:51Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.06 psi 2024-11-06T03:24:51Z- handleDepth()- Depth_Keller msg: 1730863491.53 epoch sec; seqNo:5276; depth 50.28 2024-11-06T03:24:51Z- handleDVL() - DVL msg epochMillisec: 1730863491.53 2024-11-06T03:24:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:24:51Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:24:51Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:24:51Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:24:51Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:24:51Z- handleDvl() - RDI_Pathfinder msg: 1730863491.53 sec; seqNo:1840; #beams:0 2024-11-06T03:24:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:24:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.12 , 0.00 2024-11-06T03:24:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863491.55 epoch sec; seqNo:5276; 4061728.19 north; 594708.42 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:24:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863489.14 2024-11-06T03:24:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:51Z- waiting for 1730863493.14, time is 1730863491.55 2024-11-06T03:24:51Z- AHRS_M2 msg: 1730863491.92 epoch sec; seqNo:4953 2024-11-06T03:24:51Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 6.06 psi 2024-11-06T03:24:51Z- handleDepth()- Depth_Keller msg: 1730863491.92 epoch sec; seqNo:5277; depth 50.28 2024-11-06T03:24:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863491.94 epoch sec; seqNo:5277; 4061728.57 north; 594708.41 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:24:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863489.14 2024-11-06T03:24:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:52Z- waiting for 1730863493.14, time is 1730863491.94 2024-11-06T03:24:52Z- AHRS_M2 msg: 1730863492.32 epoch sec; seqNo:4954 2024-11-06T03:24:52Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 6.06 psi 2024-11-06T03:24:52Z- handleDepth()- Depth_Keller msg: 1730863492.33 epoch sec; seqNo:5278; depth 50.29 2024-11-06T03:24:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863492.34 epoch sec; seqNo:5278; 4061728.95 north; 594708.41 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:24:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863489.14 2024-11-06T03:24:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:52Z- waiting for 1730863493.14, time is 1730863492.34 2024-11-06T03:24:52Z- AHRS_M2 msg: 1730863492.73 epoch sec; seqNo:4955 2024-11-06T03:24:52Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 6.06 psi 2024-11-06T03:24:52Z- handleDepth()- Depth_Keller msg: 1730863492.74 epoch sec; seqNo:5279; depth 50.30 2024-11-06T03:24:52Z- handleDVL() - DVL msg epochMillisec: 1730863492.74 2024-11-06T03:24:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:24:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:24:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:24:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:24:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:24:52Z- handleDvl() - RDI_Pathfinder msg: 1730863492.74 sec; seqNo:1841; #beams:0 2024-11-06T03:24:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:24:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.12 , 0.00 2024-11-06T03:24:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863492.75 epoch sec; seqNo:5279; 4061729.33 north; 594708.41 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:24:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863489.14 2024-11-06T03:24:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:52Z- waiting for 1730863493.14, time is 1730863492.75 2024-11-06T03:24:53Z- AHRS_M2 msg: 1730863493.13 epoch sec; seqNo:4956 2024-11-06T03:24:53Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.06 psi 2024-11-06T03:24:53Z- handleDepth()- Depth_Keller msg: 1730863493.14 epoch sec; seqNo:5280; depth 50.31 2024-11-06T03:24:53Z- handleDVL() - DVL msg epochMillisec: 1730863493.16 2024-11-06T03:24:53Z- handleDvl() - RDI_Pathfinder msg: 1730863493.16 sec; seqNo:1842; #beams:4 2024-11-06T03:24:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.94 , 30.58 , 36.24 , 31.81 2024-11-06T03:24:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.09 , 0.02 2024-11-06T03:24:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863493.18 epoch sec; seqNo:5280; 4061729.71 north; 594708.40 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:24:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863489.14 2024-11-06T03:24:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:53Z- waiting for 1730863493.14, time is 1730863493.18 2024-11-06T03:24:53Z- LcmTrn::updateTrn() >>>> pose time 1730863493.18, meas time 1730863493.16 <<<< 2024-11-06T03:24:53Z- LcmTrn::updateTrn() equating pose 1730863493.16 and meas times 1730863493.16 2024-11-06T03:24:53Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863493.16 2024-11-06T03:24:53Z- TerrainNav::motionUpdate() - currEstimate = (4061730, 594708.40, 50.31) 2024-11-06T03:24:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:24:53Z- TerrainNav:: SubcloudNIS is 0.42, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:24:53Z- TNavPF::Projecting Measurements 2024-11-06T03:24:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:24:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:24:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:24:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:24:53Z- TNavFilter::4 good beams. 2024-11-06T03:24:53Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:24:53Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:24:53Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:24:53Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:24:53Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:24:53Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:24:53Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:24:53Z- TerrainMapOctree: (vehN, vehE ) = (4061729.71, 594708.40). 2024-11-06T03:24:53Z- TerrainMapOctree: Min Distance = 46.28. 2024-11-06T03:24:53Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:24:53Z- Forcing Subcloud Comparison 2024-11-06T03:24:53Z- Weighting particles with subcloud comparison 2024-11-06T03:24:53Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:53Z- meanExpectedMeasDiff: 1.067025 delta_rms_squared: 0.878027 alpha: 0.977729 2024-11-06T03:24:53Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:53Z- meanExpectedMeasDiff: 1.076673 delta_rms_squared: 0.625921 alpha: 0.969036 2024-11-06T03:24:53Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:53Z- meanExpectedMeasDiff: 3.149487 delta_rms_squared: 0.734506 alpha: 0.973493 2024-11-06T03:24:53Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:53Z- meanExpectedMeasDiff: -0.195597 delta_rms_squared: 0.738025 alpha: 0.973616 2024-11-06T03:24:53Z- Subcloud NIS: 0.367427 2024-11-06T03:24:53Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:24:53Z- TNavPF:: sumWeights = 0.219399 2024-11-06T03:24:53Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:24:53Z- TNavPF::First Term of Map Covariance Matrix is 0.13 2024-11-06T03:24:53Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T03:24:53Z- TerrainNav::Resampling particle filter... 2024-11-06T03:24:53Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863493.16 sec, ping # 0. 2024-11-06T03:24:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:24:53Z- variances([0], [2], and psi_berg) = 0.625105 0.149948 0.000000 2024-11-06T03:24:53Z- publishEstimatesDecoder() - at 1730863493.16 sec 2024-11-06T03:24:53Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:24:53Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061760.11 594691.10 62.26 2024-11-06T03:24:53Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061759.28 594691.33 60.81 lat/lon: 36.70 -121.94 2024-11-06T03:24:53Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.63 0.15 0.29 0.00 2024-11-06T03:24:53Z- AHRS_M2 msg: 1730863493.54 epoch sec; seqNo:4957 2024-11-06T03:24:53Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.06 psi 2024-11-06T03:24:53Z- handleDepth()- Depth_Keller msg: 1730863493.54 epoch sec; seqNo:5281; depth 50.31 2024-11-06T03:24:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863493.55 epoch sec; seqNo:5281; 4061730.09 north; 594708.40 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:24:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863493.16 2024-11-06T03:24:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:53Z- waiting for 1730863497.16, time is 1730863493.55 2024-11-06T03:24:53Z- AHRS_M2 msg: 1730863493.94 epoch sec; seqNo:4958 2024-11-06T03:24:53Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.06 psi 2024-11-06T03:24:53Z- handleDepth()- Depth_Keller msg: 1730863493.94 epoch sec; seqNo:5282; depth 50.32 2024-11-06T03:24:53Z- handleDVL() - DVL msg epochMillisec: 1730863493.94 2024-11-06T03:24:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:24:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:24:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:24:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:24:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:24:53Z- handleDvl() - RDI_Pathfinder msg: 1730863493.94 sec; seqNo:1843; #beams:0 2024-11-06T03:24:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:24:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.09 , 0.02 2024-11-06T03:24:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863494.00 epoch sec; seqNo:5282; 4061730.47 north; 594708.39 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:24:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863493.16 2024-11-06T03:24:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:54Z- waiting for 1730863497.16, time is 1730863494.00 2024-11-06T03:24:54Z- AHRS_M2 msg: 1730863494.34 epoch sec; seqNo:4959 2024-11-06T03:24:54Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.06 psi 2024-11-06T03:24:54Z- handleDepth()- Depth_Keller msg: 1730863494.35 epoch sec; seqNo:5283; depth 50.32 2024-11-06T03:24:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863494.36 epoch sec; seqNo:5283; 4061730.47 north; 594708.39 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:24:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863493.16 2024-11-06T03:24:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:54Z- waiting for 1730863497.16, time is 1730863494.36 2024-11-06T03:24:54Z- AHRS_M2 msg: 1730863494.75 epoch sec; seqNo:4960 2024-11-06T03:24:54Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.07 psi 2024-11-06T03:24:54Z- handleDepth()- Depth_Keller msg: 1730863494.75 epoch sec; seqNo:5284; depth 50.33 2024-11-06T03:24:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863494.77 epoch sec; seqNo:5284; 4061730.83 north; 594707.17 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:24:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863493.16 2024-11-06T03:24:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:54Z- waiting for 1730863497.16, time is 1730863494.77 2024-11-06T03:24:55Z- AHRS_M2 msg: 1730863495.15 epoch sec; seqNo:4961 2024-11-06T03:24:55Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.07 psi 2024-11-06T03:24:55Z- handleDepth()- Depth_Keller msg: 1730863495.16 epoch sec; seqNo:5285; depth 50.34 2024-11-06T03:24:55Z- handleDVL() - DVL msg epochMillisec: 1730863495.19 2024-11-06T03:24:55Z- handleDvl() - RDI_Pathfinder msg: 1730863495.19 sec; seqNo:1844; #beams:4 2024-11-06T03:24:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.63 , 31.04 , 36.54 , 31.65 2024-11-06T03:24:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.01 2024-11-06T03:24:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863495.20 epoch sec; seqNo:5285; 4061731.21 north; 594707.17 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:24:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863493.16 2024-11-06T03:24:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:55Z- waiting for 1730863497.16, time is 1730863495.20 2024-11-06T03:24:55Z- AHRS_M2 msg: 1730863495.56 epoch sec; seqNo:4962 2024-11-06T03:24:55Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.07 psi 2024-11-06T03:24:55Z- handleDepth()- Depth_Keller msg: 1730863495.56 epoch sec; seqNo:5286; depth 50.34 2024-11-06T03:24:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863495.59 epoch sec; seqNo:5286; 4061731.59 north; 594707.16 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:24:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863493.16 2024-11-06T03:24:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:55Z- waiting for 1730863497.16, time is 1730863495.59 2024-11-06T03:24:55Z- AHRS_M2 msg: 1730863495.96 epoch sec; seqNo:4963 2024-11-06T03:24:55Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.07 psi 2024-11-06T03:24:55Z- handleDepth()- Depth_Keller msg: 1730863495.96 epoch sec; seqNo:5287; depth 50.33 2024-11-06T03:24:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863495.97 epoch sec; seqNo:5287; 4061731.97 north; 594707.16 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:24:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863493.16 2024-11-06T03:24:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:56Z- waiting for 1730863497.16, time is 1730863495.97 2024-11-06T03:24:56Z- AHRS_M2 msg: 1730863496.36 epoch sec; seqNo:4964 2024-11-06T03:24:56Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.07 psi 2024-11-06T03:24:56Z- handleDepth()- Depth_Keller msg: 1730863496.38 epoch sec; seqNo:5288; depth 50.32 2024-11-06T03:24:56Z- handleDVL() - DVL msg epochMillisec: 1730863496.38 2024-11-06T03:24:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:24:56Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:24:56Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:24:56Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:24:56Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:24:56Z- handleDvl() - RDI_Pathfinder msg: 1730863496.38 sec; seqNo:1845; #beams:0 2024-11-06T03:24:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:24:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.01 2024-11-06T03:24:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863496.39 epoch sec; seqNo:5288; 4061732.35 north; 594707.15 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:24:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863493.16 2024-11-06T03:24:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:56Z- waiting for 1730863497.16, time is 1730863496.39 2024-11-06T03:24:56Z- AHRS_M2 msg: 1730863496.77 epoch sec; seqNo:4965 2024-11-06T03:24:56Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.07 psi 2024-11-06T03:24:56Z- handleDepth()- Depth_Keller msg: 1730863496.77 epoch sec; seqNo:5289; depth 50.34 2024-11-06T03:24:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863496.79 epoch sec; seqNo:5289; 4061732.73 north; 594707.15 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:24:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863493.16 2024-11-06T03:24:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:56Z- waiting for 1730863497.16, time is 1730863496.79 2024-11-06T03:24:57Z- AHRS_M2 msg: 1730863497.17 epoch sec; seqNo:4966 2024-11-06T03:24:57Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.07 psi 2024-11-06T03:24:57Z- handleDepth()- Depth_Keller msg: 1730863497.18 epoch sec; seqNo:5290; depth 50.32 2024-11-06T03:24:57Z- handleDVL() - DVL msg epochMillisec: 1730863497.21 2024-11-06T03:24:57Z- handleDvl() - RDI_Pathfinder msg: 1730863497.21 sec; seqNo:1846; #beams:4 2024-11-06T03:24:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.63 , 31.50 , 36.85 , 31.35 2024-11-06T03:24:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , 0.00 2024-11-06T03:24:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863497.23 epoch sec; seqNo:5290; 4061733.11 north; 594707.14 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:24:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863493.16 2024-11-06T03:24:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:57Z- waiting for 1730863497.16, time is 1730863497.23 2024-11-06T03:24:57Z- LcmTrn::updateTrn() >>>> pose time 1730863497.23, meas time 1730863497.21 <<<< 2024-11-06T03:24:57Z- LcmTrn::updateTrn() equating pose 1730863497.21 and meas times 1730863497.21 2024-11-06T03:24:57Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863497.21 2024-11-06T03:24:57Z- TerrainNav::motionUpdate() - currEstimate = (4061733, 594707.14, 50.32) 2024-11-06T03:24:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:24:57Z- TerrainNav:: SubcloudNIS is 0.37, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:24:57Z- TNavPF::Projecting Measurements 2024-11-06T03:24:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:24:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:24:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:24:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:24:57Z- TNavFilter::4 good beams. 2024-11-06T03:24:57Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:24:57Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:24:57Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:24:57Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:24:57Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:24:57Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:24:57Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:24:57Z- TerrainMapOctree: (vehN, vehE ) = (4061733.11, 594707.14). 2024-11-06T03:24:57Z- TerrainMapOctree: Min Distance = 46.07. 2024-11-06T03:24:57Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:24:57Z- Forcing Subcloud Comparison 2024-11-06T03:24:57Z- Weighting particles with subcloud comparison 2024-11-06T03:24:57Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:57Z- meanExpectedMeasDiff: 0.190150 delta_rms_squared: 0.863554 alpha: 0.977364 2024-11-06T03:24:57Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:57Z- meanExpectedMeasDiff: 1.363005 delta_rms_squared: 0.585089 alpha: 0.966947 2024-11-06T03:24:57Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:57Z- meanExpectedMeasDiff: 3.105287 delta_rms_squared: 0.726442 alpha: 0.973206 2024-11-06T03:24:57Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:24:57Z- meanExpectedMeasDiff: -1.073422 delta_rms_squared: 0.719365 alpha: 0.972950 2024-11-06T03:24:57Z- Subcloud NIS: 0.230277 2024-11-06T03:24:57Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:24:57Z- TNavPF:: sumWeights = 0.166787 2024-11-06T03:24:57Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:24:57Z- TNavPF::First Term of Map Covariance Matrix is 0.09 2024-11-06T03:24:57Z- TNavPF::Calculated NIS Value : 0.09 numBeams normalized NIS: 0.02 2024-11-06T03:24:57Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863497.21 sec, ping # 0. 2024-11-06T03:24:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:24:57Z- variances([0], [2], and psi_berg) = 0.659630 0.153434 0.000000 2024-11-06T03:24:57Z- publishEstimatesDecoder() - at 1730863497.21 sec 2024-11-06T03:24:57Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:24:57Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061763.08 594689.48 60.28 2024-11-06T03:24:57Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061762.71 594690.05 60.52 lat/lon: 36.70 -121.94 2024-11-06T03:24:57Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.66 0.15 0.30 0.00 2024-11-06T03:24:57Z- AHRS_M2 msg: 1730863497.58 epoch sec; seqNo:4967 2024-11-06T03:24:57Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.07 psi 2024-11-06T03:24:57Z- handleDepth()- Depth_Keller msg: 1730863497.59 epoch sec; seqNo:5291; depth 50.31 2024-11-06T03:24:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863497.61 epoch sec; seqNo:5291; 4061733.48 north; 594707.14 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:24:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863497.21 2024-11-06T03:24:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:57Z- waiting for 1730863501.21, time is 1730863497.61 2024-11-06T03:24:57Z- AHRS_M2 msg: 1730863497.98 epoch sec; seqNo:4968 2024-11-06T03:24:57Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.07 psi 2024-11-06T03:24:57Z- handleDepth()- Depth_Keller msg: 1730863497.99 epoch sec; seqNo:5292; depth 50.29 2024-11-06T03:24:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863498.01 epoch sec; seqNo:5292; 4061733.48 north; 594707.14 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:24:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863497.21 2024-11-06T03:24:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:58Z- waiting for 1730863501.21, time is 1730863498.01 2024-11-06T03:24:58Z- AHRS_M2 msg: 1730863498.38 epoch sec; seqNo:4969 2024-11-06T03:24:58Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.07 psi 2024-11-06T03:24:58Z- handleDepth()- Depth_Keller msg: 1730863498.39 epoch sec; seqNo:5293; depth 50.28 2024-11-06T03:24:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863498.40 epoch sec; seqNo:5293; 4061733.86 north; 594707.14 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:24:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863497.21 2024-11-06T03:24:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:58Z- waiting for 1730863501.21, time is 1730863498.40 2024-11-06T03:24:58Z- AHRS_M2 msg: 1730863498.79 epoch sec; seqNo:4970 2024-11-06T03:24:58Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.07 psi 2024-11-06T03:24:58Z- handleDepth()- Depth_Keller msg: 1730863498.80 epoch sec; seqNo:5294; depth 50.27 2024-11-06T03:24:58Z- handleDVL() - DVL msg epochMillisec: 1730863498.80 2024-11-06T03:24:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:24:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:24:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:24:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:24:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:24:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:24:58Z- handleDvl() - RDI_Pathfinder msg: 1730863498.80 sec; seqNo:1847; #beams:0 2024-11-06T03:24:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:24:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , 0.00 2024-11-06T03:24:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863498.81 epoch sec; seqNo:5294; 4061734.24 north; 594707.13 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:24:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863497.21 2024-11-06T03:24:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:58Z- waiting for 1730863501.21, time is 1730863498.81 2024-11-06T03:24:59Z- AHRS_M2 msg: 1730863499.19 epoch sec; seqNo:4971 2024-11-06T03:24:59Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.07 psi 2024-11-06T03:24:59Z- handleDepth()- Depth_Keller msg: 1730863499.21 epoch sec; seqNo:5295; depth 50.25 2024-11-06T03:24:59Z- handleDVL() - DVL msg epochMillisec: 1730863499.23 2024-11-06T03:24:59Z- handleDvl() - RDI_Pathfinder msg: 1730863499.23 sec; seqNo:1848; #beams:4 2024-11-06T03:24:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.48 , 32.88 , 37.16 , 31.04 2024-11-06T03:24:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.04 2024-11-06T03:24:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863499.24 epoch sec; seqNo:5295; 4061734.62 north; 594707.13 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:24:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:24:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863497.21 2024-11-06T03:24:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:59Z- waiting for 1730863501.21, time is 1730863499.24 2024-11-06T03:24:59Z- AHRS_M2 msg: 1730863499.60 epoch sec; seqNo:4972 2024-11-06T03:24:59Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.07 psi 2024-11-06T03:24:59Z- handleDepth()- Depth_Keller msg: 1730863499.62 epoch sec; seqNo:5296; depth 50.25 2024-11-06T03:24:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863499.64 epoch sec; seqNo:5296; 4061735.00 north; 594707.12 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:24:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:24:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863497.21 2024-11-06T03:24:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:24:59Z- waiting for 1730863501.21, time is 1730863499.64 2024-11-06T03:25:00Z- AHRS_M2 msg: 1730863500.00 epoch sec; seqNo:4973 2024-11-06T03:25:00Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.07 psi 2024-11-06T03:25:00Z- handleDepth()- Depth_Keller msg: 1730863500.01 epoch sec; seqNo:5297; depth 50.23 2024-11-06T03:25:00Z- handleDVL() - DVL msg epochMillisec: 1730863500.01 2024-11-06T03:25:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:25:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:25:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:25:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:25:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:25:00Z- handleDvl() - RDI_Pathfinder msg: 1730863500.01 sec; seqNo:1849; #beams:0 2024-11-06T03:25:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:25:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.04 2024-11-06T03:25:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863500.02 epoch sec; seqNo:5297; 4061735.38 north; 594707.12 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:25:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863497.21 2024-11-06T03:25:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:00Z- waiting for 1730863501.21, time is 1730863500.02 2024-11-06T03:25:00Z- AHRS_M2 msg: 1730863500.40 epoch sec; seqNo:4974 2024-11-06T03:25:00Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.07 psi 2024-11-06T03:25:00Z- handleDepth()- Depth_Keller msg: 1730863500.41 epoch sec; seqNo:5298; depth 50.23 2024-11-06T03:25:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863500.43 epoch sec; seqNo:5298; 4061735.74 north; 594705.89 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:25:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863497.21 2024-11-06T03:25:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:00Z- waiting for 1730863501.21, time is 1730863500.43 2024-11-06T03:25:00Z- AHRS_M2 msg: 1730863500.81 epoch sec; seqNo:4975 2024-11-06T03:25:00Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.07 psi 2024-11-06T03:25:00Z- handleDepth()- Depth_Keller msg: 1730863500.82 epoch sec; seqNo:5299; depth 50.21 2024-11-06T03:25:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863500.83 epoch sec; seqNo:5299; 4061736.12 north; 594705.89 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:25:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863497.21 2024-11-06T03:25:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:00Z- waiting for 1730863501.21, time is 1730863500.83 2024-11-06T03:25:01Z- AHRS_M2 msg: 1730863501.21 epoch sec; seqNo:4976 2024-11-06T03:25:01Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.07 psi 2024-11-06T03:25:01Z- handleDepth()- Depth_Keller msg: 1730863501.22 epoch sec; seqNo:5300; depth 50.18 2024-11-06T03:25:01Z- handleDVL() - DVL msg epochMillisec: 1730863501.24 2024-11-06T03:25:01Z- handleDvl() - RDI_Pathfinder msg: 1730863501.24 sec; seqNo:1850; #beams:4 2024-11-06T03:25:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.01 , 33.79 , 37.61 , 29.82 2024-11-06T03:25:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.08 , -0.07 2024-11-06T03:25:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863501.26 epoch sec; seqNo:5300; 4061736.50 north; 594705.89 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:25:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863497.21 2024-11-06T03:25:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:01Z- waiting for 1730863501.21, time is 1730863501.26 2024-11-06T03:25:01Z- LcmTrn::updateTrn() >>>> pose time 1730863501.26, meas time 1730863501.24 <<<< 2024-11-06T03:25:01Z- LcmTrn::updateTrn() equating pose 1730863501.24 and meas times 1730863501.24 2024-11-06T03:25:01Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863501.24 2024-11-06T03:25:01Z- TerrainNav::motionUpdate() - currEstimate = (4061737, 594705.89, 50.18) 2024-11-06T03:25:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:25:01Z- TerrainNav:: SubcloudNIS is 0.23, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:25:01Z- TNavPF::Projecting Measurements 2024-11-06T03:25:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:25:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:25:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:25:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:25:01Z- TNavFilter::4 good beams. 2024-11-06T03:25:01Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:25:01Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:25:01Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:25:01Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:25:01Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:25:01Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:25:01Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:25:01Z- TerrainMapOctree: (vehN, vehE ) = (4061736.50, 594705.89). 2024-11-06T03:25:01Z- TerrainMapOctree: Min Distance = 46.13. 2024-11-06T03:25:01Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:25:01Z- Forcing Subcloud Comparison 2024-11-06T03:25:01Z- Weighting particles with subcloud comparison 2024-11-06T03:25:01Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:01Z- meanExpectedMeasDiff: -0.666097 delta_rms_squared: 0.878764 alpha: 0.977747 2024-11-06T03:25:01Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:01Z- meanExpectedMeasDiff: 2.745732 delta_rms_squared: 0.612150 alpha: 0.968362 2024-11-06T03:25:01Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:01Z- meanExpectedMeasDiff: 3.261645 delta_rms_squared: 0.761265 alpha: 0.974400 2024-11-06T03:25:01Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:01Z- meanExpectedMeasDiff: -2.946425 delta_rms_squared: 0.741964 alpha: 0.973752 2024-11-06T03:25:01Z- Subcloud NIS: 0.258463 2024-11-06T03:25:01Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:25:01Z- TNavPF:: sumWeights = 0.018907 2024-11-06T03:25:01Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:25:01Z- TNavPF::First Term of Map Covariance Matrix is 0.02 2024-11-06T03:25:01Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:25:01Z- TerrainNav::Resampling particle filter... 2024-11-06T03:25:01Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863501.24 sec, ping # 0. 2024-11-06T03:25:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:25:01Z- variances([0], [2], and psi_berg) = 0.822717 0.162885 0.000000 2024-11-06T03:25:01Z- publishEstimatesDecoder() - at 1730863501.24 sec 2024-11-06T03:25:01Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:25:01Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061766.88 594689.15 60.83 2024-11-06T03:25:01Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061766.01 594688.74 60.28 lat/lon: 36.70 -121.94 2024-11-06T03:25:01Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.82 0.16 0.31 0.00 2024-11-06T03:25:01Z- AHRS_M2 msg: 1730863501.62 epoch sec; seqNo:4977 2024-11-06T03:25:01Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.07 psi 2024-11-06T03:25:01Z- handleDepth()- Depth_Keller msg: 1730863501.63 epoch sec; seqNo:5301; depth 50.16 2024-11-06T03:25:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863501.67 epoch sec; seqNo:5301; 4061736.88 north; 594705.88 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:25:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863501.24 2024-11-06T03:25:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:01Z- waiting for 1730863505.24, time is 1730863501.67 2024-11-06T03:25:02Z- AHRS_M2 msg: 1730863502.02 epoch sec; seqNo:4978 2024-11-06T03:25:02Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.07 psi 2024-11-06T03:25:02Z- handleDepth()- Depth_Keller msg: 1730863502.03 epoch sec; seqNo:5302; depth 50.16 2024-11-06T03:25:02Z- handleDVL() - DVL msg epochMillisec: 1730863502.04 2024-11-06T03:25:02Z- handleDvl() - RDI_Pathfinder msg: 1730863502.04 sec; seqNo:1851; #beams:4 2024-11-06T03:25:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.63 , 34.10 , 37.46 , 29.82 2024-11-06T03:25:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.12 , -0.06 2024-11-06T03:25:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863502.06 epoch sec; seqNo:5302; 4061737.26 north; 594705.88 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:25:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863501.24 2024-11-06T03:25:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:02Z- waiting for 1730863505.24, time is 1730863502.06 2024-11-06T03:25:02Z- AHRS_M2 msg: 1730863502.42 epoch sec; seqNo:4979 2024-11-06T03:25:02Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.07 psi 2024-11-06T03:25:02Z- handleDepth()- Depth_Keller msg: 1730863502.43 epoch sec; seqNo:5303; depth 50.15 2024-11-06T03:25:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863502.47 epoch sec; seqNo:5303; 4061737.64 north; 594705.87 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:25:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863501.24 2024-11-06T03:25:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:02Z- waiting for 1730863505.24, time is 1730863502.47 2024-11-06T03:25:02Z- AHRS_M2 msg: 1730863502.83 epoch sec; seqNo:4980 2024-11-06T03:25:02Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.07 psi 2024-11-06T03:25:02Z- handleDepth()- Depth_Keller msg: 1730863502.83 epoch sec; seqNo:5304; depth 50.15 2024-11-06T03:25:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863502.85 epoch sec; seqNo:5304; 4061737.64 north; 594705.87 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:25:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863501.24 2024-11-06T03:25:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:02Z- waiting for 1730863505.24, time is 1730863502.85 2024-11-06T03:25:03Z- AHRS_M2 msg: 1730863503.24 epoch sec; seqNo:4981 2024-11-06T03:25:03Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.06 psi 2024-11-06T03:25:03Z- handleDepth()- Depth_Keller msg: 1730863503.24 epoch sec; seqNo:5305; depth 50.14 2024-11-06T03:25:03Z- handleDVL() - DVL msg epochMillisec: 1730863503.26 2024-11-06T03:25:03Z- handleDvl() - RDI_Pathfinder msg: 1730863503.26 sec; seqNo:1852; #beams:4 2024-11-06T03:25:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.17 , 33.95 , 37.31 , 29.82 2024-11-06T03:25:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.15 , -0.03 2024-11-06T03:25:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863503.28 epoch sec; seqNo:5305; 4061738.02 north; 594705.87 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:25:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863501.24 2024-11-06T03:25:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:03Z- waiting for 1730863505.24, time is 1730863503.28 2024-11-06T03:25:03Z- AHRS_M2 msg: 1730863503.65 epoch sec; seqNo:4982 2024-11-06T03:25:03Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.06 psi 2024-11-06T03:25:03Z- handleDepth()- Depth_Keller msg: 1730863503.65 epoch sec; seqNo:5306; depth 50.14 2024-11-06T03:25:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863503.67 epoch sec; seqNo:5306; 4061738.40 north; 594705.87 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:25:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863501.24 2024-11-06T03:25:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:03Z- waiting for 1730863505.24, time is 1730863503.67 2024-11-06T03:25:04Z- AHRS_M2 msg: 1730863504.04 epoch sec; seqNo:4983 2024-11-06T03:25:04Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.06 psi 2024-11-06T03:25:04Z- handleDepth()- Depth_Keller msg: 1730863504.05 epoch sec; seqNo:5307; depth 50.14 2024-11-06T03:25:04Z- handleDVL() - DVL msg epochMillisec: 1730863504.07 2024-11-06T03:25:04Z- handleDvl() - RDI_Pathfinder msg: 1730863504.07 sec; seqNo:1853; #beams:4 2024-11-06T03:25:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 34.10 , 37.62 , 29.67 2024-11-06T03:25:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.14 , 0.00 2024-11-06T03:25:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863504.09 epoch sec; seqNo:5307; 4061738.77 north; 594705.86 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:25:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863501.24 2024-11-06T03:25:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:04Z- waiting for 1730863505.24, time is 1730863504.09 2024-11-06T03:25:04Z- AHRS_M2 msg: 1730863504.45 epoch sec; seqNo:4984 2024-11-06T03:25:04Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.05 psi 2024-11-06T03:25:04Z- handleDepth()- Depth_Keller msg: 1730863504.45 epoch sec; seqNo:5308; depth 50.14 2024-11-06T03:25:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863504.47 epoch sec; seqNo:5308; 4061739.14 north; 594704.64 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:25:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863501.24 2024-11-06T03:25:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:04Z- waiting for 1730863505.24, time is 1730863504.47 2024-11-06T03:25:04Z- AHRS_M2 msg: 1730863504.85 epoch sec; seqNo:4985 2024-11-06T03:25:04Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.05 psi 2024-11-06T03:25:04Z- handleDepth()- Depth_Keller msg: 1730863504.86 epoch sec; seqNo:5309; depth 50.13 2024-11-06T03:25:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863504.87 epoch sec; seqNo:5309; 4061739.52 north; 594704.63 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:25:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863501.24 2024-11-06T03:25:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:05Z- waiting for 1730863505.24, time is 1730863504.87 2024-11-06T03:25:05Z- AHRS_M2 msg: 1730863505.26 epoch sec; seqNo:4986 2024-11-06T03:25:05Z- AHRS_M2 msg: 0.11 phi; 0.01 theta; 6.05 psi 2024-11-06T03:25:05Z- handleDepth()- Depth_Keller msg: 1730863505.27 epoch sec; seqNo:5310; depth 50.16 2024-11-06T03:25:05Z- handleDVL() - DVL msg epochMillisec: 1730863505.29 2024-11-06T03:25:05Z- handleDvl() - RDI_Pathfinder msg: 1730863505.29 sec; seqNo:1854; #beams:4 2024-11-06T03:25:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.17 , 34.41 , 37.92 , 29.97 2024-11-06T03:25:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.12 , 0.04 2024-11-06T03:25:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863505.30 epoch sec; seqNo:5310; 4061739.90 north; 594704.63 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:25:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863501.24 2024-11-06T03:25:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:05Z- waiting for 1730863505.24, time is 1730863505.30 2024-11-06T03:25:05Z- LcmTrn::updateTrn() >>>> pose time 1730863505.30, meas time 1730863505.29 <<<< 2024-11-06T03:25:05Z- LcmTrn::updateTrn() equating pose 1730863505.29 and meas times 1730863505.29 2024-11-06T03:25:05Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863505.29 2024-11-06T03:25:05Z- TerrainNav::motionUpdate() - currEstimate = (4061740, 594704.63, 50.16) 2024-11-06T03:25:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:25:05Z- TerrainNav:: SubcloudNIS is 0.26, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:25:05Z- TNavPF::Projecting Measurements 2024-11-06T03:25:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:25:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:25:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:25:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:25:05Z- TNavFilter::4 good beams. 2024-11-06T03:25:05Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:25:05Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:25:05Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:25:05Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:25:05Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:25:05Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:25:05Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:25:05Z- TerrainMapOctree: (vehN, vehE ) = (4061739.90, 594704.63). 2024-11-06T03:25:05Z- TerrainMapOctree: Min Distance = 46.48. 2024-11-06T03:25:05Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:25:05Z- Forcing Subcloud Comparison 2024-11-06T03:25:05Z- Weighting particles with subcloud comparison 2024-11-06T03:25:05Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:05Z- meanExpectedMeasDiff: -0.263628 delta_rms_squared: 0.785775 alpha: 0.975179 2024-11-06T03:25:05Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:05Z- meanExpectedMeasDiff: 2.331795 delta_rms_squared: 0.658231 alpha: 0.970512 2024-11-06T03:25:05Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:05Z- meanExpectedMeasDiff: 3.445385 delta_rms_squared: 0.762148 alpha: 0.974429 2024-11-06T03:25:05Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:05Z- meanExpectedMeasDiff: -2.820981 delta_rms_squared: 0.738497 alpha: 0.973632 2024-11-06T03:25:05Z- Subcloud NIS: 0.289736 2024-11-06T03:25:05Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:25:05Z- TNavPF:: sumWeights = 0.026215 2024-11-06T03:25:05Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:25:05Z- TNavPF::First Term of Map Covariance Matrix is 0.02 2024-11-06T03:25:05Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:25:05Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863505.29 sec, ping # 0. 2024-11-06T03:25:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:25:05Z- variances([0], [2], and psi_berg) = 0.811157 0.163992 0.000000 2024-11-06T03:25:05Z- publishEstimatesDecoder() - at 1730863505.29 sec 2024-11-06T03:25:05Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:25:05Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061769.43 594687.91 59.86 2024-11-06T03:25:05Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061769.39 594687.49 60.08 lat/lon: 36.70 -121.94 2024-11-06T03:25:05Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.81 0.16 0.31 0.00 2024-11-06T03:25:05Z- AHRS_M2 msg: 1730863505.65 epoch sec; seqNo:4987 2024-11-06T03:25:05Z- AHRS_M2 msg: 0.11 phi; 0.01 theta; 6.05 psi 2024-11-06T03:25:05Z- handleDepth()- Depth_Keller msg: 1730863505.67 epoch sec; seqNo:5311; depth 50.15 2024-11-06T03:25:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863505.68 epoch sec; seqNo:5311; 4061740.28 north; 594704.62 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:25:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863505.29 2024-11-06T03:25:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:05Z- waiting for 1730863509.29, time is 1730863505.68 2024-11-06T03:25:06Z- AHRS_M2 msg: 1730863506.07 epoch sec; seqNo:4988 2024-11-06T03:25:06Z- AHRS_M2 msg: 0.11 phi; 0.01 theta; 6.04 psi 2024-11-06T03:25:06Z- handleDepth()- Depth_Keller msg: 1730863506.08 epoch sec; seqNo:5312; depth 50.14 2024-11-06T03:25:06Z- handleDVL() - DVL msg epochMillisec: 1730863506.09 2024-11-06T03:25:06Z- handleDvl() - RDI_Pathfinder msg: 1730863506.09 sec; seqNo:1855; #beams:4 2024-11-06T03:25:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 34.71 , 37.92 , 29.82 2024-11-06T03:25:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.09 , 0.04 2024-11-06T03:25:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863506.11 epoch sec; seqNo:5312; 4061740.66 north; 594704.62 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:25:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863505.29 2024-11-06T03:25:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:06Z- waiting for 1730863509.29, time is 1730863506.11 2024-11-06T03:25:06Z- AHRS_M2 msg: 1730863506.46 epoch sec; seqNo:4989 2024-11-06T03:25:06Z- AHRS_M2 msg: 0.11 phi; 0.01 theta; 6.04 psi 2024-11-06T03:25:06Z- handleDepth()- Depth_Keller msg: 1730863506.47 epoch sec; seqNo:5313; depth 50.15 2024-11-06T03:25:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863506.50 epoch sec; seqNo:5313; 4061741.03 north; 594704.62 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:25:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863505.29 2024-11-06T03:25:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:06Z- waiting for 1730863509.29, time is 1730863506.50 2024-11-06T03:25:06Z- AHRS_M2 msg: 1730863506.87 epoch sec; seqNo:4990 2024-11-06T03:25:06Z- AHRS_M2 msg: 0.11 phi; 0.01 theta; 6.04 psi 2024-11-06T03:25:06Z- handleDepth()- Depth_Keller msg: 1730863506.87 epoch sec; seqNo:5314; depth 50.16 2024-11-06T03:25:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863506.88 epoch sec; seqNo:5314; 4061741.41 north; 594704.61 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:25:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863505.29 2024-11-06T03:25:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:07Z- waiting for 1730863509.29, time is 1730863506.88 2024-11-06T03:25:07Z- AHRS_M2 msg: 1730863507.27 epoch sec; seqNo:4991 2024-11-06T03:25:07Z- AHRS_M2 msg: 0.11 phi; 0.01 theta; 6.04 psi 2024-11-06T03:25:07Z- handleDepth()- Depth_Keller msg: 1730863507.28 epoch sec; seqNo:5315; depth 50.13 2024-11-06T03:25:07Z- handleDVL() - DVL msg epochMillisec: 1730863507.30 2024-11-06T03:25:07Z- handleDvl() - RDI_Pathfinder msg: 1730863507.30 sec; seqNo:1856; #beams:4 2024-11-06T03:25:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.00 , 35.00 , 37.89 , 29.90 2024-11-06T03:25:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.04 2024-11-06T03:25:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863507.33 epoch sec; seqNo:5315; 4061741.79 north; 594704.61 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:25:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863505.29 2024-11-06T03:25:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:07Z- waiting for 1730863509.29, time is 1730863507.33 2024-11-06T03:25:07Z- AHRS_M2 msg: 1730863507.68 epoch sec; seqNo:4992 2024-11-06T03:25:07Z- AHRS_M2 msg: 0.11 phi; 0.01 theta; 6.04 psi 2024-11-06T03:25:07Z- handleDepth()- Depth_Keller msg: 1730863507.68 epoch sec; seqNo:5316; depth 50.14 2024-11-06T03:25:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863507.70 epoch sec; seqNo:5316; 4061741.79 north; 594704.61 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:25:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863505.29 2024-11-06T03:25:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:07Z- waiting for 1730863509.29, time is 1730863507.70 2024-11-06T03:25:08Z- AHRS_M2 msg: 1730863508.08 epoch sec; seqNo:4993 2024-11-06T03:25:08Z- AHRS_M2 msg: 0.11 phi; 0.01 theta; 6.04 psi 2024-11-06T03:25:08Z- handleDepth()- Depth_Keller msg: 1730863508.09 epoch sec; seqNo:5317; depth 50.15 2024-11-06T03:25:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863508.11 epoch sec; seqNo:5317; 4061742.17 north; 594704.60 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:25:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863505.29 2024-11-06T03:25:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:08Z- waiting for 1730863509.29, time is 1730863508.11 2024-11-06T03:25:08Z- AHRS_M2 msg: 1730863508.48 epoch sec; seqNo:4994 2024-11-06T03:25:08Z- AHRS_M2 msg: 0.11 phi; 0.01 theta; 6.04 psi 2024-11-06T03:25:08Z- handleDepth()- Depth_Keller msg: 1730863508.50 epoch sec; seqNo:5318; depth 50.14 2024-11-06T03:25:08Z- handleDVL() - DVL msg epochMillisec: 1730863508.52 2024-11-06T03:25:08Z- handleDvl() - RDI_Pathfinder msg: 1730863508.52 sec; seqNo:1857; #beams:4 2024-11-06T03:25:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.34 , 35.17 , 37.88 , 30.24 2024-11-06T03:25:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.02 2024-11-06T03:25:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863508.54 epoch sec; seqNo:5318; 4061742.54 north; 594703.38 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:25:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863505.29 2024-11-06T03:25:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:08Z- waiting for 1730863509.29, time is 1730863508.54 2024-11-06T03:25:08Z- AHRS_M2 msg: 1730863508.89 epoch sec; seqNo:4995 2024-11-06T03:25:08Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.04 psi 2024-11-06T03:25:08Z- handleDepth()- Depth_Keller msg: 1730863508.89 epoch sec; seqNo:5319; depth 50.15 2024-11-06T03:25:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863508.91 epoch sec; seqNo:5319; 4061742.91 north; 594703.37 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:25:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863505.29 2024-11-06T03:25:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:09Z- waiting for 1730863509.29, time is 1730863508.91 2024-11-06T03:25:09Z- AHRS_M2 msg: 1730863509.29 epoch sec; seqNo:4996 2024-11-06T03:25:09Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.04 psi 2024-11-06T03:25:09Z- handleDepth()- Depth_Keller msg: 1730863509.30 epoch sec; seqNo:5320; depth 50.14 2024-11-06T03:25:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863509.31 epoch sec; seqNo:5320; 4061743.29 north; 594703.37 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:25:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863505.29 2024-11-06T03:25:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:09Z- waiting for 1730863509.29, time is 1730863509.31 2024-11-06T03:25:09Z- LcmTrn::updateTrn() >>>> pose time 1730863509.31, meas time 1730863508.52 <<<< 2024-11-06T03:25:09Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863509.31 2024-11-06T03:25:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863508.52 sec, ping # 0; waiting for INS data more recent than 1730863505.29... 2024-11-06T03:25:09Z- TerrainNav::motionUpdate() - currEstimate = (4061743, 594703.37, 50.14) 2024-11-06T03:25:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:25:09Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:25:09Z- TNavPF::Projecting Measurements 2024-11-06T03:25:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:25:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:25:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:25:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:25:09Z- TNavFilter::4 good beams. 2024-11-06T03:25:09Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:25:09Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:25:09Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:25:09Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:25:09Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:25:09Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:25:09Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:25:09Z- TerrainMapOctree: (vehN, vehE ) = (4061742.63, 594703.62). 2024-11-06T03:25:09Z- TerrainMapOctree: Min Distance = 46.97. 2024-11-06T03:25:09Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:25:09Z- Forcing Subcloud Comparison 2024-11-06T03:25:09Z- Weighting particles with subcloud comparison 2024-11-06T03:25:09Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:09Z- meanExpectedMeasDiff: -0.445188 delta_rms_squared: 0.824179 alpha: 0.976308 2024-11-06T03:25:09Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:09Z- meanExpectedMeasDiff: 2.946191 delta_rms_squared: 0.664986 alpha: 0.970802 2024-11-06T03:25:09Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:09Z- meanExpectedMeasDiff: 3.164300 delta_rms_squared: 0.773788 alpha: 0.974804 2024-11-06T03:25:09Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:09Z- meanExpectedMeasDiff: -2.776352 delta_rms_squared: 0.749129 alpha: 0.973997 2024-11-06T03:25:09Z- Subcloud NIS: 0.328268 2024-11-06T03:25:09Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:25:09Z- TNavPF:: sumWeights = 0.022651 2024-11-06T03:25:09Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:25:09Z- TNavPF::First Term of Map Covariance Matrix is 0.02 2024-11-06T03:25:09Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:25:09Z- TerrainNav::Resampling particle filter... 2024-11-06T03:25:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863508.52 sec. 2024-11-06T03:25:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:25:09Z- variances([0], [2], and psi_berg) = 0.803654 0.166720 0.000000 2024-11-06T03:25:09Z- publishEstimatesDecoder() - at 1730863509.31 sec 2024-11-06T03:25:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:25:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061773.62 594686.76 60.14 2024-11-06T03:25:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061772.79 594686.23 59.87 lat/lon: 36.70 -121.94 2024-11-06T03:25:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.80 0.17 0.56 0.00 2024-11-06T03:25:09Z- AHRS_M2 msg: 1730863509.70 epoch sec; seqNo:4997 2024-11-06T03:25:09Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 6.04 psi 2024-11-06T03:25:09Z- handleDepth()- Depth_Keller msg: 1730863509.70 epoch sec; seqNo:5321; depth 50.14 2024-11-06T03:25:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863509.71 epoch sec; seqNo:5321; 4061743.67 north; 594703.37 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:25:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863509.31 2024-11-06T03:25:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:09Z- waiting for 1730863513.31, time is 1730863509.71 2024-11-06T03:25:10Z- AHRS_M2 msg: 1730863510.10 epoch sec; seqNo:4998 2024-11-06T03:25:10Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 6.04 psi 2024-11-06T03:25:10Z- handleDepth()- Depth_Keller msg: 1730863510.11 epoch sec; seqNo:5322; depth 50.13 2024-11-06T03:25:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863510.13 epoch sec; seqNo:5322; 4061744.05 north; 594703.36 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:25:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863509.31 2024-11-06T03:25:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:10Z- waiting for 1730863513.31, time is 1730863510.13 2024-11-06T03:25:10Z- AHRS_M2 msg: 1730863510.50 epoch sec; seqNo:4999 2024-11-06T03:25:10Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.04 psi 2024-11-06T03:25:10Z- handleDepth()- Depth_Keller msg: 1730863510.51 epoch sec; seqNo:5323; depth 50.11 2024-11-06T03:25:10Z- handleDVL() - DVL msg epochMillisec: 1730863510.54 2024-11-06T03:25:10Z- handleDvl() - RDI_Pathfinder msg: 1730863510.54 sec; seqNo:1858; #beams:4 2024-11-06T03:25:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.85 , 35.00 , 38.06 , 30.92 2024-11-06T03:25:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , -0.02 2024-11-06T03:25:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863510.55 epoch sec; seqNo:5323; 4061744.43 north; 594703.36 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:25:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863509.31 2024-11-06T03:25:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:10Z- waiting for 1730863513.31, time is 1730863510.55 2024-11-06T03:25:10Z- AHRS_M2 msg: 1730863510.91 epoch sec; seqNo:5000 2024-11-06T03:25:10Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.04 psi 2024-11-06T03:25:10Z- handleDepth()- Depth_Keller msg: 1730863510.91 epoch sec; seqNo:5324; depth 50.11 2024-11-06T03:25:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863510.93 epoch sec; seqNo:5324; 4061744.43 north; 594703.36 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:25:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863509.31 2024-11-06T03:25:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:11Z- waiting for 1730863513.31, time is 1730863510.93 2024-11-06T03:25:11Z- AHRS_M2 msg: 1730863511.31 epoch sec; seqNo:5001 2024-11-06T03:25:11Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.04 psi 2024-11-06T03:25:11Z- handleDepth()- Depth_Keller msg: 1730863511.32 epoch sec; seqNo:5325; depth 50.10 2024-11-06T03:25:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863511.33 epoch sec; seqNo:5325; 4061744.81 north; 594703.35 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:25:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863509.31 2024-11-06T03:25:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:11Z- waiting for 1730863513.31, time is 1730863511.33 2024-11-06T03:25:11Z- AHRS_M2 msg: 1730863511.71 epoch sec; seqNo:5002 2024-11-06T03:25:11Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.04 psi 2024-11-06T03:25:11Z- handleDepth()- Depth_Keller msg: 1730863511.73 epoch sec; seqNo:5326; depth 50.08 2024-11-06T03:25:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863511.74 epoch sec; seqNo:5326; 4061745.19 north; 594703.35 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:25:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863509.31 2024-11-06T03:25:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:11Z- waiting for 1730863513.31, time is 1730863511.74 2024-11-06T03:25:12Z- AHRS_M2 msg: 1730863512.12 epoch sec; seqNo:5003 2024-11-06T03:25:12Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.04 psi 2024-11-06T03:25:12Z- handleDepth()- Depth_Keller msg: 1730863512.12 epoch sec; seqNo:5327; depth 50.08 2024-11-06T03:25:12Z- handleDVL() - DVL msg epochMillisec: 1730863512.13 2024-11-06T03:25:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:25:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:25:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:25:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:25:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:25:12Z- handleDvl() - RDI_Pathfinder msg: 1730863512.13 sec; seqNo:1859; #beams:0 2024-11-06T03:25:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:25:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , -0.02 2024-11-06T03:25:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863512.13 epoch sec; seqNo:5327; 4061745.57 north; 594703.35 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:25:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863509.31 2024-11-06T03:25:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:12Z- waiting for 1730863513.31, time is 1730863512.13 2024-11-06T03:25:12Z- AHRS_M2 msg: 1730863512.53 epoch sec; seqNo:5004 2024-11-06T03:25:12Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.04 psi 2024-11-06T03:25:12Z- handleDepth()- Depth_Keller msg: 1730863512.53 epoch sec; seqNo:5328; depth 50.05 2024-11-06T03:25:12Z- handleDVL() - DVL msg epochMillisec: 1730863512.55 2024-11-06T03:25:12Z- handleDvl() - RDI_Pathfinder msg: 1730863512.55 sec; seqNo:1860; #beams:4 2024-11-06T03:25:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.36 , 35.00 , 38.40 , 31.09 2024-11-06T03:25:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.05 2024-11-06T03:25:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863512.57 epoch sec; seqNo:5328; 4061745.95 north; 594703.34 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:25:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863509.31 2024-11-06T03:25:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:12Z- waiting for 1730863513.31, time is 1730863512.57 2024-11-06T03:25:12Z- AHRS_M2 msg: 1730863512.93 epoch sec; seqNo:5005 2024-11-06T03:25:12Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.04 psi 2024-11-06T03:25:12Z- handleDepth()- Depth_Keller msg: 1730863512.93 epoch sec; seqNo:5329; depth 50.04 2024-11-06T03:25:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863512.95 epoch sec; seqNo:5329; 4061746.32 north; 594703.34 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:25:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863509.31 2024-11-06T03:25:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:13Z- waiting for 1730863513.31, time is 1730863512.95 2024-11-06T03:25:13Z- AHRS_M2 msg: 1730863513.33 epoch sec; seqNo:5006 2024-11-06T03:25:13Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.04 psi 2024-11-06T03:25:13Z- handleDepth()- Depth_Keller msg: 1730863513.34 epoch sec; seqNo:5330; depth 50.03 2024-11-06T03:25:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863513.35 epoch sec; seqNo:5330; 4061746.69 north; 594702.11 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:25:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863509.31 2024-11-06T03:25:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:13Z- waiting for 1730863513.31, time is 1730863513.35 2024-11-06T03:25:13Z- LcmTrn::updateTrn() >>>> pose time 1730863513.35, meas time 1730863512.55 <<<< 2024-11-06T03:25:13Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863513.35 2024-11-06T03:25:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863512.55 sec, ping # 0; waiting for INS data more recent than 1730863509.31... 2024-11-06T03:25:13Z- TerrainNav::motionUpdate() - currEstimate = (4061747, 594702.11, 50.03) 2024-11-06T03:25:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:25:13Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:25:13Z- TNavPF::Projecting Measurements 2024-11-06T03:25:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:25:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:25:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:25:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:25:13Z- TNavFilter::4 good beams. 2024-11-06T03:25:13Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:25:13Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:25:13Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:25:13Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:25:13Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:25:13Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:25:13Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:25:13Z- TerrainMapOctree: (vehN, vehE ) = (4061746.02, 594702.36). 2024-11-06T03:25:13Z- TerrainMapOctree: Min Distance = 47.81. 2024-11-06T03:25:13Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:25:13Z- Forcing Subcloud Comparison 2024-11-06T03:25:13Z- Weighting particles with subcloud comparison 2024-11-06T03:25:13Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:13Z- meanExpectedMeasDiff: -0.147180 delta_rms_squared: 1.141452 alpha: 0.982780 2024-11-06T03:25:13Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:13Z- meanExpectedMeasDiff: 2.561130 delta_rms_squared: 0.925988 alpha: 0.978858 2024-11-06T03:25:13Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:13Z- meanExpectedMeasDiff: 3.187623 delta_rms_squared: 1.092752 alpha: 0.982027 2024-11-06T03:25:13Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:13Z- meanExpectedMeasDiff: -2.163057 delta_rms_squared: 1.042936 alpha: 0.981184 2024-11-06T03:25:13Z- Subcloud NIS: 0.363428 2024-11-06T03:25:13Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:25:13Z- TNavPF:: sumWeights = 0.047847 2024-11-06T03:25:13Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:25:13Z- TNavPF::First Term of Map Covariance Matrix is 0.04 2024-11-06T03:25:13Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T03:25:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863512.55 sec. 2024-11-06T03:25:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:25:13Z- variances([0], [2], and psi_berg) = 0.716061 0.164427 0.000000 2024-11-06T03:25:13Z- publishEstimatesDecoder() - at 1730863513.35 sec 2024-11-06T03:25:13Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:25:13Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061776.01 594685.50 58.74 2024-11-06T03:25:13Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061776.14 594684.99 59.44 lat/lon: 36.70 -121.94 2024-11-06T03:25:13Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.72 0.16 0.64 0.00 2024-11-06T03:25:13Z- AHRS_M2 msg: 1730863513.73 epoch sec; seqNo:5007 2024-11-06T03:25:13Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.04 psi 2024-11-06T03:25:13Z- handleDepth()- Depth_Keller msg: 1730863513.74 epoch sec; seqNo:5331; depth 50.02 2024-11-06T03:25:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863513.76 epoch sec; seqNo:5331; 4061747.07 north; 594702.11 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:25:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863513.35 2024-11-06T03:25:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:13Z- waiting for 1730863517.35, time is 1730863513.76 2024-11-06T03:25:14Z- AHRS_M2 msg: 1730863514.14 epoch sec; seqNo:5008 2024-11-06T03:25:14Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.03 psi 2024-11-06T03:25:14Z- handleDepth()- Depth_Keller msg: 1730863514.15 epoch sec; seqNo:5332; depth 50.00 2024-11-06T03:25:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863514.17 epoch sec; seqNo:5332; 4061747.45 north; 594702.10 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:25:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863513.35 2024-11-06T03:25:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:14Z- waiting for 1730863517.35, time is 1730863514.17 2024-11-06T03:25:14Z- AHRS_M2 msg: 1730863514.54 epoch sec; seqNo:5009 2024-11-06T03:25:14Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.03 psi 2024-11-06T03:25:14Z- handleDepth()- Depth_Keller msg: 1730863514.56 epoch sec; seqNo:5333; depth 50.00 2024-11-06T03:25:14Z- handleDVL() - DVL msg epochMillisec: 1730863514.58 2024-11-06T03:25:14Z- handleDvl() - RDI_Pathfinder msg: 1730863514.58 sec; seqNo:1861; #beams:4 2024-11-06T03:25:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.02 , 35.00 , 38.23 , 30.75 2024-11-06T03:25:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.10 , -0.04 2024-11-06T03:25:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863514.59 epoch sec; seqNo:5333; 4061747.83 north; 594702.10 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:25:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863513.35 2024-11-06T03:25:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:14Z- waiting for 1730863517.35, time is 1730863514.59 2024-11-06T03:25:14Z- AHRS_M2 msg: 1730863514.95 epoch sec; seqNo:5010 2024-11-06T03:25:14Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.03 psi 2024-11-06T03:25:14Z- handleDepth()- Depth_Keller msg: 1730863514.95 epoch sec; seqNo:5334; depth 49.97 2024-11-06T03:25:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863514.97 epoch sec; seqNo:5334; 4061747.83 north; 594702.10 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:25:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863513.35 2024-11-06T03:25:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:15Z- waiting for 1730863517.35, time is 1730863514.97 2024-11-06T03:25:15Z- AHRS_M2 msg: 1730863515.35 epoch sec; seqNo:5011 2024-11-06T03:25:15Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.03 psi 2024-11-06T03:25:15Z- handleDepth()- Depth_Keller msg: 1730863515.36 epoch sec; seqNo:5335; depth 49.96 2024-11-06T03:25:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863515.37 epoch sec; seqNo:5335; 4061748.21 north; 594702.10 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:25:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863513.35 2024-11-06T03:25:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:15Z- waiting for 1730863517.35, time is 1730863515.37 2024-11-06T03:25:15Z- AHRS_M2 msg: 1730863515.76 epoch sec; seqNo:5012 2024-11-06T03:25:15Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.03 psi 2024-11-06T03:25:15Z- handleDepth()- Depth_Keller msg: 1730863515.76 epoch sec; seqNo:5336; depth 49.95 2024-11-06T03:25:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863515.78 epoch sec; seqNo:5336; 4061748.58 north; 594702.09 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:25:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863513.35 2024-11-06T03:25:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:15Z- waiting for 1730863517.35, time is 1730863515.78 2024-11-06T03:25:16Z- AHRS_M2 msg: 1730863516.18 epoch sec; seqNo:5013 2024-11-06T03:25:16Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.03 psi 2024-11-06T03:25:16Z- handleDepth()- Depth_Keller msg: 1730863516.20 epoch sec; seqNo:5337; depth 49.95 2024-11-06T03:25:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863516.24 epoch sec; seqNo:5337; 4061748.96 north; 594702.09 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:25:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863513.35 2024-11-06T03:25:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:16Z- waiting for 1730863517.35, time is 1730863516.24 2024-11-06T03:25:16Z- AHRS_M2 msg: 1730863516.56 epoch sec; seqNo:5014 2024-11-06T03:25:16Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.03 psi 2024-11-06T03:25:16Z- handleDepth()- Depth_Keller msg: 1730863516.57 epoch sec; seqNo:5338; depth 49.93 2024-11-06T03:25:16Z- handleDVL() - DVL msg epochMillisec: 1730863516.59 2024-11-06T03:25:16Z- handleDvl() - RDI_Pathfinder msg: 1730863516.59 sec; seqNo:1862; #beams:4 2024-11-06T03:25:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.36 , 35.17 , 38.57 , 31.43 2024-11-06T03:25:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.10 , -0.01 2024-11-06T03:25:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863516.61 epoch sec; seqNo:5338; 4061749.34 north; 594702.08 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:25:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863513.35 2024-11-06T03:25:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:16Z- waiting for 1730863517.35, time is 1730863516.61 2024-11-06T03:25:16Z- AHRS_M2 msg: 1730863516.97 epoch sec; seqNo:5015 2024-11-06T03:25:16Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.03 psi 2024-11-06T03:25:16Z- handleDepth()- Depth_Keller msg: 1730863516.97 epoch sec; seqNo:5339; depth 49.93 2024-11-06T03:25:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863516.98 epoch sec; seqNo:5339; 4061749.72 north; 594702.08 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:25:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863513.35 2024-11-06T03:25:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:17Z- waiting for 1730863517.35, time is 1730863516.98 2024-11-06T03:25:17Z- AHRS_M2 msg: 1730863517.37 epoch sec; seqNo:5016 2024-11-06T03:25:17Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.03 psi 2024-11-06T03:25:17Z- handleDepth()- Depth_Keller msg: 1730863517.38 epoch sec; seqNo:5340; depth 49.92 2024-11-06T03:25:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863517.39 epoch sec; seqNo:5340; 4061750.09 north; 594700.85 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:25:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863513.35 2024-11-06T03:25:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:17Z- waiting for 1730863517.35, time is 1730863517.39 2024-11-06T03:25:17Z- LcmTrn::updateTrn() >>>> pose time 1730863517.39, meas time 1730863516.59 <<<< 2024-11-06T03:25:17Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863517.39 2024-11-06T03:25:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863516.59 sec, ping # 0; waiting for INS data more recent than 1730863513.35... 2024-11-06T03:25:17Z- TerrainNav::motionUpdate() - currEstimate = (4061750, 594700.85, 49.92) 2024-11-06T03:25:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:25:17Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:25:17Z- TNavPF::Projecting Measurements 2024-11-06T03:25:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:25:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:25:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:25:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:25:17Z- TNavFilter::4 good beams. 2024-11-06T03:25:17Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:25:17Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:25:17Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:25:17Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:25:17Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:25:17Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:25:17Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:25:17Z- TerrainMapOctree: (vehN, vehE ) = (4061749.42, 594701.10). 2024-11-06T03:25:17Z- TerrainMapOctree: Min Distance = 48.90. 2024-11-06T03:25:17Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:25:17Z- Forcing Subcloud Comparison 2024-11-06T03:25:17Z- Weighting particles with subcloud comparison 2024-11-06T03:25:17Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:17Z- meanExpectedMeasDiff: -1.754621 delta_rms_squared: 1.386495 alpha: 0.985780 2024-11-06T03:25:17Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:17Z- meanExpectedMeasDiff: 2.291717 delta_rms_squared: 1.036747 alpha: 0.981074 2024-11-06T03:25:17Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:17Z- meanExpectedMeasDiff: 2.762428 delta_rms_squared: 1.231506 alpha: 0.984019 2024-11-06T03:25:17Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:17Z- meanExpectedMeasDiff: -2.260001 delta_rms_squared: 1.160689 alpha: 0.983061 2024-11-06T03:25:17Z- Subcloud NIS: 0.395340 2024-11-06T03:25:17Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:25:17Z- TNavPF:: sumWeights = 0.042154 2024-11-06T03:25:17Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:25:17Z- TNavPF::First Term of Map Covariance Matrix is 0.15 2024-11-06T03:25:17Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:25:17Z- TerrainNav::Resampling particle filter... 2024-11-06T03:25:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863516.59 sec. 2024-11-06T03:25:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:25:17Z- variances([0], [2], and psi_berg) = 0.662999 0.159821 0.000000 2024-11-06T03:25:17Z- publishEstimatesDecoder() - at 1730863517.39 sec 2024-11-06T03:25:17Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:25:17Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061780.41 594684.21 59.07 2024-11-06T03:25:17Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061779.50 594683.74 59.31 lat/lon: 36.70 -121.94 2024-11-06T03:25:17Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.66 0.16 0.64 0.00 2024-11-06T03:25:17Z- AHRS_M2 msg: 1730863517.78 epoch sec; seqNo:5017 2024-11-06T03:25:17Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.02 psi 2024-11-06T03:25:17Z- handleDepth()- Depth_Keller msg: 1730863517.80 epoch sec; seqNo:5341; depth 49.91 2024-11-06T03:25:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863517.81 epoch sec; seqNo:5341; 4061750.46 north; 594700.85 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:25:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863517.39 2024-11-06T03:25:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:17Z- waiting for 1730863521.39, time is 1730863517.81 2024-11-06T03:25:18Z- handleDepth()- Depth_Keller msg: 1730863518.24 epoch sec; seqNo:5342; depth 49.90 2024-11-06T03:25:18Z- handleDVL() - DVL msg epochMillisec: 1730863518.27 2024-11-06T03:25:18Z- handleDvl() - RDI_Pathfinder msg: 1730863518.27 sec; seqNo:1863; #beams:4 2024-11-06T03:25:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.70 , 35.00 , 38.56 , 31.43 2024-11-06T03:25:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.08 , 0.01 2024-11-06T03:25:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863518.28 epoch sec; seqNo:5342; 4061750.84 north; 594700.85 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:25:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863517.39 2024-11-06T03:25:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:18Z- waiting for 1730863521.39, time is 1730863518.28 2024-11-06T03:25:18Z- AHRS_M2 msg: 1730863518.59 epoch sec; seqNo:5018 2024-11-06T03:25:18Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.02 psi 2024-11-06T03:25:18Z- handleDepth()- Depth_Keller msg: 1730863518.62 epoch sec; seqNo:5343; depth 49.89 2024-11-06T03:25:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863518.65 epoch sec; seqNo:5343; 4061751.22 north; 594700.84 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:25:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863517.39 2024-11-06T03:25:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:18Z- waiting for 1730863521.39, time is 1730863518.65 2024-11-06T03:25:18Z- AHRS_M2 msg: 1730863518.99 epoch sec; seqNo:5019 2024-11-06T03:25:18Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.02 psi 2024-11-06T03:25:19Z- handleDepth()- Depth_Keller msg: 1730863519.00 epoch sec; seqNo:5344; depth 49.89 2024-11-06T03:25:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863519.01 epoch sec; seqNo:5344; 4061751.60 north; 594700.84 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:25:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863517.39 2024-11-06T03:25:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:19Z- waiting for 1730863521.39, time is 1730863519.01 2024-11-06T03:25:19Z- AHRS_M2 msg: 1730863519.39 epoch sec; seqNo:5020 2024-11-06T03:25:19Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.02 psi 2024-11-06T03:25:19Z- handleDepth()- Depth_Keller msg: 1730863519.39 epoch sec; seqNo:5345; depth 49.87 2024-11-06T03:25:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863519.40 epoch sec; seqNo:5345; 4061751.60 north; 594700.84 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:25:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863517.39 2024-11-06T03:25:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:19Z- waiting for 1730863521.39, time is 1730863519.40 2024-11-06T03:25:19Z- AHRS_M2 msg: 1730863519.79 epoch sec; seqNo:5021 2024-11-06T03:25:19Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.02 psi 2024-11-06T03:25:19Z- handleDepth()- Depth_Keller msg: 1730863519.80 epoch sec; seqNo:5346; depth 49.88 2024-11-06T03:25:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863519.82 epoch sec; seqNo:5346; 4061751.98 north; 594700.83 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:25:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863517.39 2024-11-06T03:25:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:19Z- waiting for 1730863521.39, time is 1730863519.82 2024-11-06T03:25:20Z- handleDepth()- Depth_Keller msg: 1730863520.25 epoch sec; seqNo:5347; depth 49.87 2024-11-06T03:25:20Z- handleDVL() - DVL msg epochMillisec: 1730863520.27 2024-11-06T03:25:20Z- handleDvl() - RDI_Pathfinder msg: 1730863520.27 sec; seqNo:1864; #beams:4 2024-11-06T03:25:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.21 , 35.17 , 38.74 , 31.60 2024-11-06T03:25:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , -0.01 2024-11-06T03:25:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863520.31 epoch sec; seqNo:5347; 4061752.36 north; 594700.83 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:25:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863517.39 2024-11-06T03:25:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:20Z- waiting for 1730863521.39, time is 1730863520.31 2024-11-06T03:25:20Z- AHRS_M2 msg: 1730863520.62 epoch sec; seqNo:5022 2024-11-06T03:25:20Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.02 psi 2024-11-06T03:25:20Z- handleDepth()- Depth_Keller msg: 1730863520.62 epoch sec; seqNo:5348; depth 49.85 2024-11-06T03:25:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863520.64 epoch sec; seqNo:5348; 4061752.74 north; 594700.82 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:25:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863517.39 2024-11-06T03:25:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:20Z- waiting for 1730863521.39, time is 1730863520.64 2024-11-06T03:25:21Z- AHRS_M2 msg: 1730863521.02 epoch sec; seqNo:5023 2024-11-06T03:25:21Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.02 psi 2024-11-06T03:25:21Z- handleDepth()- Depth_Keller msg: 1730863521.03 epoch sec; seqNo:5349; depth 49.84 2024-11-06T03:25:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863521.04 epoch sec; seqNo:5349; 4061753.12 north; 594700.82 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:25:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863517.39 2024-11-06T03:25:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:21Z- waiting for 1730863521.39, time is 1730863521.04 2024-11-06T03:25:21Z- AHRS_M2 msg: 1730863521.41 epoch sec; seqNo:5024 2024-11-06T03:25:21Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.02 psi 2024-11-06T03:25:21Z- handleDepth()- Depth_Keller msg: 1730863521.43 epoch sec; seqNo:5350; depth 49.84 2024-11-06T03:25:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863521.44 epoch sec; seqNo:5350; 4061753.48 north; 594699.60 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:25:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863517.39 2024-11-06T03:25:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:21Z- waiting for 1730863521.39, time is 1730863521.44 2024-11-06T03:25:21Z- LcmTrn::updateTrn() >>>> pose time 1730863521.44, meas time 1730863520.27 <<<< 2024-11-06T03:25:21Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863521.44 2024-11-06T03:25:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863520.27 sec, ping # 0; waiting for INS data more recent than 1730863517.39... 2024-11-06T03:25:21Z- TerrainNav::motionUpdate() - currEstimate = (4061753, 594699.60, 49.84) 2024-11-06T03:25:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:25:21Z- TerrainNav:: SubcloudNIS is 0.40, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:25:21Z- TNavPF::Projecting Measurements 2024-11-06T03:25:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:25:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:25:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:25:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:25:21Z- TNavFilter::4 good beams. 2024-11-06T03:25:21Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:25:21Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:25:21Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:25:21Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:25:21Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:25:21Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:25:21Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:25:21Z- TerrainMapOctree: (vehN, vehE ) = (4061752.50, 594699.96). 2024-11-06T03:25:21Z- TerrainMapOctree: Min Distance = 50.10. 2024-11-06T03:25:21Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:25:21Z- Forcing Subcloud Comparison 2024-11-06T03:25:21Z- Weighting particles with subcloud comparison 2024-11-06T03:25:21Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:21Z- meanExpectedMeasDiff: -1.190488 delta_rms_squared: 1.393986 alpha: 0.985856 2024-11-06T03:25:21Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:21Z- meanExpectedMeasDiff: 2.247118 delta_rms_squared: 1.022040 alpha: 0.980807 2024-11-06T03:25:21Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:21Z- meanExpectedMeasDiff: 2.853854 delta_rms_squared: 1.216062 alpha: 0.983820 2024-11-06T03:25:21Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:21Z- meanExpectedMeasDiff: -3.050374 delta_rms_squared: 0.939173 alpha: 0.979149 2024-11-06T03:25:21Z- Subcloud NIS: 0.422637 2024-11-06T03:25:21Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:25:21Z- TNavPF:: sumWeights = 0.030860 2024-11-06T03:25:21Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:25:21Z- TNavPF::First Term of Map Covariance Matrix is 0.07 2024-11-06T03:25:21Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:25:21Z- TerrainNav::Resampling particle filter... 2024-11-06T03:25:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863520.27 sec. 2024-11-06T03:25:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:25:21Z- variances([0], [2], and psi_berg) = 0.573935 0.157532 0.000000 2024-11-06T03:25:21Z- publishEstimatesDecoder() - at 1730863521.44 sec 2024-11-06T03:25:21Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:25:21Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061783.93 594682.88 58.36 2024-11-06T03:25:21Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061782.67 594682.55 59.10 lat/lon: 36.70 -121.94 2024-11-06T03:25:21Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.16 0.67 0.00 2024-11-06T03:25:21Z- AHRS_M2 msg: 1730863521.82 epoch sec; seqNo:5025 2024-11-06T03:25:21Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 6.02 psi 2024-11-06T03:25:21Z- handleDepth()- Depth_Keller msg: 1730863521.83 epoch sec; seqNo:5351; depth 49.82 2024-11-06T03:25:21Z- handleDVL() - DVL msg epochMillisec: 1730863521.83 2024-11-06T03:25:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:25:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:25:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:25:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:25:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:25:21Z- handleDvl() - RDI_Pathfinder msg: 1730863521.83 sec; seqNo:1865; #beams:0 2024-11-06T03:25:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:25:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , -0.01 2024-11-06T03:25:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863521.84 epoch sec; seqNo:5351; 4061753.86 north; 594699.59 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:25:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863521.44 2024-11-06T03:25:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:21Z- waiting for 1730863525.44, time is 1730863521.84 2024-11-06T03:25:22Z- AHRS_M2 msg: 1730863522.24 epoch sec; seqNo:5026 2024-11-06T03:25:22Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 6.02 psi 2024-11-06T03:25:22Z- handleDepth()- Depth_Keller msg: 1730863522.24 epoch sec; seqNo:5352; depth 49.83 2024-11-06T03:25:22Z- handleDVL() - DVL msg epochMillisec: 1730863522.26 2024-11-06T03:25:22Z- handleDvl() - RDI_Pathfinder msg: 1730863522.26 sec; seqNo:1866; #beams:4 2024-11-06T03:25:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.55 , 35.17 , 38.57 , 31.77 2024-11-06T03:25:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.07 , -0.03 2024-11-06T03:25:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863522.29 epoch sec; seqNo:5352; 4061754.24 north; 594699.59 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:25:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863521.44 2024-11-06T03:25:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:22Z- waiting for 1730863525.44, time is 1730863522.29 2024-11-06T03:25:22Z- AHRS_M2 msg: 1730863522.63 epoch sec; seqNo:5027 2024-11-06T03:25:22Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 6.02 psi 2024-11-06T03:25:22Z- handleDepth()- Depth_Keller msg: 1730863522.64 epoch sec; seqNo:5353; depth 49.83 2024-11-06T03:25:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863522.66 epoch sec; seqNo:5353; 4061754.62 north; 594699.58 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:25:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863521.44 2024-11-06T03:25:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:22Z- waiting for 1730863525.44, time is 1730863522.66 2024-11-06T03:25:23Z- AHRS_M2 msg: 1730863523.03 epoch sec; seqNo:5028 2024-11-06T03:25:23Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 6.02 psi 2024-11-06T03:25:23Z- handleDepth()- Depth_Keller msg: 1730863523.04 epoch sec; seqNo:5354; depth 49.81 2024-11-06T03:25:23Z- handleDVL() - DVL msg epochMillisec: 1730863523.04 2024-11-06T03:25:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:25:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:25:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:25:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:25:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:25:23Z- handleDvl() - RDI_Pathfinder msg: 1730863523.04 sec; seqNo:1867; #beams:0 2024-11-06T03:25:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:25:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.07 , -0.03 2024-11-06T03:25:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863523.05 epoch sec; seqNo:5354; 4061754.62 north; 594699.58 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:25:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863521.44 2024-11-06T03:25:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:23Z- waiting for 1730863525.44, time is 1730863523.05 2024-11-06T03:25:23Z- AHRS_M2 msg: 1730863523.43 epoch sec; seqNo:5029 2024-11-06T03:25:23Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 6.02 psi 2024-11-06T03:25:23Z- handleDepth()- Depth_Keller msg: 1730863523.44 epoch sec; seqNo:5355; depth 49.80 2024-11-06T03:25:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863523.45 epoch sec; seqNo:5355; 4061755.00 north; 594699.58 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:25:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863521.44 2024-11-06T03:25:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:23Z- waiting for 1730863525.44, time is 1730863523.45 2024-11-06T03:25:23Z- AHRS_M2 msg: 1730863523.84 epoch sec; seqNo:5030 2024-11-06T03:25:23Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 6.02 psi 2024-11-06T03:25:23Z- handleDepth()- Depth_Keller msg: 1730863523.85 epoch sec; seqNo:5356; depth 49.78 2024-11-06T03:25:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863523.86 epoch sec; seqNo:5356; 4061755.38 north; 594699.58 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:25:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863521.44 2024-11-06T03:25:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:23Z- waiting for 1730863525.44, time is 1730863523.86 2024-11-06T03:25:24Z- AHRS_M2 msg: 1730863524.25 epoch sec; seqNo:5031 2024-11-06T03:25:24Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.02 psi 2024-11-06T03:25:24Z- handleDepth()- Depth_Keller msg: 1730863524.26 epoch sec; seqNo:5357; depth 49.79 2024-11-06T03:25:24Z- handleDVL() - DVL msg epochMillisec: 1730863524.28 2024-11-06T03:25:24Z- handleDvl() - RDI_Pathfinder msg: 1730863524.28 sec; seqNo:1868; #beams:4 2024-11-06T03:25:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.88 , 35.17 , 38.74 , 32.28 2024-11-06T03:25:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.09 , -0.02 2024-11-06T03:25:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863524.29 epoch sec; seqNo:5357; 4061755.76 north; 594699.57 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:25:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863521.44 2024-11-06T03:25:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:24Z- waiting for 1730863525.44, time is 1730863524.29 2024-11-06T03:25:24Z- AHRS_M2 msg: 1730863524.65 epoch sec; seqNo:5032 2024-11-06T03:25:24Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.02 psi 2024-11-06T03:25:24Z- handleDepth()- Depth_Keller msg: 1730863524.65 epoch sec; seqNo:5358; depth 49.79 2024-11-06T03:25:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863524.68 epoch sec; seqNo:5358; 4061756.13 north; 594699.57 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:25:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863521.44 2024-11-06T03:25:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:24Z- waiting for 1730863525.44, time is 1730863524.68 2024-11-06T03:25:25Z- AHRS_M2 msg: 1730863525.05 epoch sec; seqNo:5033 2024-11-06T03:25:25Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.02 psi 2024-11-06T03:25:25Z- handleDepth()- Depth_Keller msg: 1730863525.06 epoch sec; seqNo:5359; depth 49.79 2024-11-06T03:25:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863525.07 epoch sec; seqNo:5359; 4061756.51 north; 594699.56 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:25:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863521.44 2024-11-06T03:25:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:25Z- waiting for 1730863525.44, time is 1730863525.07 2024-11-06T03:25:25Z- AHRS_M2 msg: 1730863525.46 epoch sec; seqNo:5034 2024-11-06T03:25:25Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.02 psi 2024-11-06T03:25:25Z- handleDepth()- Depth_Keller msg: 1730863525.46 epoch sec; seqNo:5360; depth 49.78 2024-11-06T03:25:25Z- handleDVL() - DVL msg epochMillisec: 1730863525.46 2024-11-06T03:25:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:25:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:25:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:25:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:25:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:25:25Z- handleDvl() - RDI_Pathfinder msg: 1730863525.46 sec; seqNo:1869; #beams:0 2024-11-06T03:25:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:25:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.09 , -0.02 2024-11-06T03:25:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863525.47 epoch sec; seqNo:5360; 4061756.88 north; 594698.34 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:25:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863521.44 2024-11-06T03:25:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:25Z- waiting for 1730863525.44, time is 1730863525.47 2024-11-06T03:25:25Z- LcmTrn::updateTrn() >>>> pose time 1730863525.47, meas time 1730863525.46 <<<< 2024-11-06T03:25:25Z- LcmTrn::updateTrn() equating pose 1730863525.46 and meas times 1730863525.46 2024-11-06T03:25:25Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863525.46 2024-11-06T03:25:25Z- TerrainNav::motionUpdate() - currEstimate = (4061757, 594698.34, 49.78) 2024-11-06T03:25:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:25:25Z- TerrainNav:: SubcloudNIS is 0.42, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:25:25Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:25:25Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:25:25Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:25:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:25:25Z- variances([0], [2], and psi_berg) = 0.574430 0.159954 0.000000 2024-11-06T03:25:25Z- publishEstimatesDecoder() - at 1730863525.46 sec 2024-11-06T03:25:25Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:25:25Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061787.33 594681.62 58.27 2024-11-06T03:25:25Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061786.07 594681.29 59.03 lat/lon: 36.70 -121.94 2024-11-06T03:25:25Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.16 0.92 0.00 2024-11-06T03:25:25Z- AHRS_M2 msg: 1730863525.86 epoch sec; seqNo:5035 2024-11-06T03:25:25Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.02 psi 2024-11-06T03:25:25Z- handleDepth()- Depth_Keller msg: 1730863525.88 epoch sec; seqNo:5361; depth 49.75 2024-11-06T03:25:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863525.89 epoch sec; seqNo:5361; 4061757.26 north; 594698.33 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:25:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863525.46 2024-11-06T03:25:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:26Z- waiting for 1730863529.46, time is 1730863525.89 2024-11-06T03:25:26Z- AHRS_M2 msg: 1730863526.26 epoch sec; seqNo:5036 2024-11-06T03:25:26Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.01 psi 2024-11-06T03:25:26Z- handleDepth()- Depth_Keller msg: 1730863526.27 epoch sec; seqNo:5362; depth 49.77 2024-11-06T03:25:26Z- handleDVL() - DVL msg epochMillisec: 1730863526.30 2024-11-06T03:25:26Z- handleDvl() - RDI_Pathfinder msg: 1730863526.30 sec; seqNo:1870; #beams:4 2024-11-06T03:25:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.06 , 35.17 , 38.74 , 33.13 2024-11-06T03:25:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.08 , 0.00 2024-11-06T03:25:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863526.31 epoch sec; seqNo:5362; 4061757.64 north; 594698.33 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:25:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863525.46 2024-11-06T03:25:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:26Z- waiting for 1730863529.46, time is 1730863526.31 2024-11-06T03:25:26Z- AHRS_M2 msg: 1730863526.68 epoch sec; seqNo:5037 2024-11-06T03:25:26Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.01 psi 2024-11-06T03:25:26Z- handleDepth()- Depth_Keller msg: 1730863526.68 epoch sec; seqNo:5363; depth 49.76 2024-11-06T03:25:26Z- handleDVL() - DVL msg epochMillisec: 1730863526.68 2024-11-06T03:25:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:25:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:25:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:25:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:25:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:25:26Z- handleDvl() - RDI_Pathfinder msg: 1730863526.68 sec; seqNo:1871; #beams:0 2024-11-06T03:25:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:25:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.08 , 0.00 2024-11-06T03:25:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863526.70 epoch sec; seqNo:5363; 4061758.01 north; 594698.33 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:25:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863525.46 2024-11-06T03:25:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:26Z- waiting for 1730863529.46, time is 1730863526.70 2024-11-06T03:25:27Z- AHRS_M2 msg: 1730863527.07 epoch sec; seqNo:5038 2024-11-06T03:25:27Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.01 psi 2024-11-06T03:25:27Z- handleDepth()- Depth_Keller msg: 1730863527.08 epoch sec; seqNo:5364; depth 49.78 2024-11-06T03:25:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863527.09 epoch sec; seqNo:5364; 4061758.39 north; 594698.32 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:25:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863525.46 2024-11-06T03:25:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:27Z- waiting for 1730863529.46, time is 1730863527.09 2024-11-06T03:25:27Z- AHRS_M2 msg: 1730863527.47 epoch sec; seqNo:5039 2024-11-06T03:25:27Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.01 psi 2024-11-06T03:25:27Z- handleDepth()- Depth_Keller msg: 1730863527.49 epoch sec; seqNo:5365; depth 49.75 2024-11-06T03:25:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863527.50 epoch sec; seqNo:5365; 4061758.77 north; 594698.32 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:25:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863525.46 2024-11-06T03:25:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:27Z- waiting for 1730863529.46, time is 1730863527.50 2024-11-06T03:25:27Z- AHRS_M2 msg: 1730863527.88 epoch sec; seqNo:5040 2024-11-06T03:25:27Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.01 psi 2024-11-06T03:25:27Z- handleDepth()- Depth_Keller msg: 1730863527.89 epoch sec; seqNo:5366; depth 49.76 2024-11-06T03:25:27Z- handleDVL() - DVL msg epochMillisec: 1730863527.89 2024-11-06T03:25:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:25:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:25:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:25:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:25:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:25:27Z- handleDvl() - RDI_Pathfinder msg: 1730863527.89 sec; seqNo:1872; #beams:0 2024-11-06T03:25:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:25:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.08 , 0.00 2024-11-06T03:25:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863527.90 epoch sec; seqNo:5366; 4061758.77 north; 594698.32 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:25:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863525.46 2024-11-06T03:25:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:28Z- waiting for 1730863529.46, time is 1730863527.90 2024-11-06T03:25:28Z- AHRS_M2 msg: 1730863528.28 epoch sec; seqNo:5041 2024-11-06T03:25:28Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.01 psi 2024-11-06T03:25:28Z- handleDepth()- Depth_Keller msg: 1730863528.29 epoch sec; seqNo:5367; depth 49.74 2024-11-06T03:25:28Z- handleDVL() - DVL msg epochMillisec: 1730863528.32 2024-11-06T03:25:28Z- handleDvl() - RDI_Pathfinder msg: 1730863528.32 sec; seqNo:1873; #beams:4 2024-11-06T03:25:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.23 , 35.17 , 38.74 , 33.98 2024-11-06T03:25:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , 0.01 2024-11-06T03:25:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863528.33 epoch sec; seqNo:5367; 4061759.15 north; 594698.31 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:25:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863525.46 2024-11-06T03:25:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:28Z- waiting for 1730863529.46, time is 1730863528.33 2024-11-06T03:25:28Z- AHRS_M2 msg: 1730863528.69 epoch sec; seqNo:5042 2024-11-06T03:25:28Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.01 psi 2024-11-06T03:25:28Z- handleDepth()- Depth_Keller msg: 1730863528.69 epoch sec; seqNo:5368; depth 49.76 2024-11-06T03:25:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863528.71 epoch sec; seqNo:5368; 4061759.53 north; 594698.31 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:25:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863525.46 2024-11-06T03:25:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:28Z- waiting for 1730863529.46, time is 1730863528.71 2024-11-06T03:25:29Z- AHRS_M2 msg: 1730863529.09 epoch sec; seqNo:5043 2024-11-06T03:25:29Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.02 psi 2024-11-06T03:25:29Z- handleDepth()- Depth_Keller msg: 1730863529.10 epoch sec; seqNo:5369; depth 49.74 2024-11-06T03:25:29Z- handleDVL() - DVL msg epochMillisec: 1730863529.10 2024-11-06T03:25:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:25:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:25:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:25:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:25:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:25:29Z- handleDvl() - RDI_Pathfinder msg: 1730863529.10 sec; seqNo:1874; #beams:0 2024-11-06T03:25:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:25:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , 0.01 2024-11-06T03:25:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863529.11 epoch sec; seqNo:5369; 4061759.90 north; 594697.08 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:25:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863525.46 2024-11-06T03:25:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:29Z- waiting for 1730863529.46, time is 1730863529.11 2024-11-06T03:25:29Z- AHRS_M2 msg: 1730863529.50 epoch sec; seqNo:5044 2024-11-06T03:25:29Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.02 psi 2024-11-06T03:25:29Z- handleDepth()- Depth_Keller msg: 1730863529.51 epoch sec; seqNo:5370; depth 49.72 2024-11-06T03:25:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863529.52 epoch sec; seqNo:5370; 4061760.27 north; 594697.08 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:25:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863525.46 2024-11-06T03:25:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:29Z- waiting for 1730863529.46, time is 1730863529.52 2024-11-06T03:25:29Z- LcmTrn::updateTrn() >>>> pose time 1730863529.52, meas time 1730863529.10 <<<< 2024-11-06T03:25:29Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863529.52 2024-11-06T03:25:29Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:25:29Z- TerrainNav::motionUpdate() - currEstimate = (4061760, 594697.08, 49.72) 2024-11-06T03:25:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:25:29Z- TerrainNav:: SubcloudNIS is 0.42, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:25:29Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:25:29Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:25:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:25:29Z- variances([0], [2], and psi_berg) = 0.575467 0.161653 0.000000 2024-11-06T03:25:29Z- publishEstimatesDecoder() - at 1730863529.52 sec 2024-11-06T03:25:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:25:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061790.70 594680.28 58.24 2024-11-06T03:25:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061789.46 594680.03 58.97 lat/lon: 36.70 -121.94 2024-11-06T03:25:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.58 0.16 1.16 0.00 2024-11-06T03:25:29Z- AHRS_M2 msg: 1730863529.90 epoch sec; seqNo:5045 2024-11-06T03:25:29Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.02 psi 2024-11-06T03:25:29Z- handleDepth()- Depth_Keller msg: 1730863529.91 epoch sec; seqNo:5371; depth 49.74 2024-11-06T03:25:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863529.92 epoch sec; seqNo:5371; 4061760.65 north; 594697.08 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:25:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863529.52 2024-11-06T03:25:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:30Z- waiting for 1730863533.52, time is 1730863529.92 2024-11-06T03:25:30Z- handleDepth()- Depth_Keller msg: 1730863530.32 epoch sec; seqNo:5372; depth 49.74 2024-11-06T03:25:30Z- handleDVL() - DVL msg epochMillisec: 1730863530.35 2024-11-06T03:25:30Z- handleDvl() - RDI_Pathfinder msg: 1730863530.35 sec; seqNo:1875; #beams:4 2024-11-06T03:25:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.06 , 35.17 , 38.57 , 34.49 2024-11-06T03:25:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , -0.01 2024-11-06T03:25:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863530.36 epoch sec; seqNo:5372; 4061761.03 north; 594697.07 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:25:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863529.52 2024-11-06T03:25:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:30Z- waiting for 1730863533.52, time is 1730863530.36 2024-11-06T03:25:30Z- AHRS_M2 msg: 1730863530.71 epoch sec; seqNo:5046 2024-11-06T03:25:30Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.02 psi 2024-11-06T03:25:30Z- handleDepth()- Depth_Keller msg: 1730863530.71 epoch sec; seqNo:5373; depth 49.71 2024-11-06T03:25:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863530.74 epoch sec; seqNo:5373; 4061761.41 north; 594697.07 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:25:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863529.52 2024-11-06T03:25:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:30Z- waiting for 1730863533.52, time is 1730863530.74 2024-11-06T03:25:31Z- AHRS_M2 msg: 1730863531.11 epoch sec; seqNo:5047 2024-11-06T03:25:31Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.02 psi 2024-11-06T03:25:31Z- handleDepth()- Depth_Keller msg: 1730863531.12 epoch sec; seqNo:5374; depth 49.71 2024-11-06T03:25:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863531.13 epoch sec; seqNo:5374; 4061761.79 north; 594697.06 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:25:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863529.52 2024-11-06T03:25:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:31Z- waiting for 1730863533.52, time is 1730863531.13 2024-11-06T03:25:31Z- AHRS_M2 msg: 1730863531.52 epoch sec; seqNo:5048 2024-11-06T03:25:31Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.02 psi 2024-11-06T03:25:31Z- handleDepth()- Depth_Keller msg: 1730863531.52 epoch sec; seqNo:5375; depth 49.72 2024-11-06T03:25:31Z- handleDVL() - DVL msg epochMillisec: 1730863531.52 2024-11-06T03:25:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:25:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:25:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:25:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:25:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:25:31Z- handleDvl() - RDI_Pathfinder msg: 1730863531.52 sec; seqNo:1876; #beams:0 2024-11-06T03:25:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:25:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , -0.01 2024-11-06T03:25:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863531.53 epoch sec; seqNo:5375; 4061762.17 north; 594697.06 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:25:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863529.52 2024-11-06T03:25:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:31Z- waiting for 1730863533.52, time is 1730863531.53 2024-11-06T03:25:31Z- AHRS_M2 msg: 1730863531.92 epoch sec; seqNo:5049 2024-11-06T03:25:31Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.02 psi 2024-11-06T03:25:31Z- handleDepth()- Depth_Keller msg: 1730863531.92 epoch sec; seqNo:5376; depth 49.70 2024-11-06T03:25:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863531.94 epoch sec; seqNo:5376; 4061762.55 north; 594697.05 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:25:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863529.52 2024-11-06T03:25:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:32Z- waiting for 1730863533.52, time is 1730863531.94 2024-11-06T03:25:32Z- AHRS_M2 msg: 1730863532.32 epoch sec; seqNo:5050 2024-11-06T03:25:32Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.02 psi 2024-11-06T03:25:32Z- handleDepth()- Depth_Keller msg: 1730863532.33 epoch sec; seqNo:5377; depth 49.68 2024-11-06T03:25:32Z- handleDVL() - DVL msg epochMillisec: 1730863532.36 2024-11-06T03:25:32Z- handleDvl() - RDI_Pathfinder msg: 1730863532.36 sec; seqNo:1877; #beams:4 2024-11-06T03:25:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.06 , 35.17 , 38.56 , 35.00 2024-11-06T03:25:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.04 2024-11-06T03:25:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863532.37 epoch sec; seqNo:5377; 4061762.93 north; 594697.05 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:25:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863529.52 2024-11-06T03:25:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:32Z- waiting for 1730863533.52, time is 1730863532.37 2024-11-06T03:25:32Z- AHRS_M2 msg: 1730863532.73 epoch sec; seqNo:5051 2024-11-06T03:25:32Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.02 psi 2024-11-06T03:25:32Z- handleDepth()- Depth_Keller msg: 1730863532.73 epoch sec; seqNo:5378; depth 49.68 2024-11-06T03:25:32Z- handleDVL() - DVL msg epochMillisec: 1730863532.73 2024-11-06T03:25:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:25:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:25:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:25:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:25:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:25:32Z- handleDvl() - RDI_Pathfinder msg: 1730863532.73 sec; seqNo:1878; #beams:0 2024-11-06T03:25:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:25:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.04 2024-11-06T03:25:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863532.75 epoch sec; seqNo:5378; 4061763.31 north; 594697.05 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:25:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863529.52 2024-11-06T03:25:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:32Z- waiting for 1730863533.52, time is 1730863532.75 2024-11-06T03:25:33Z- AHRS_M2 msg: 1730863533.13 epoch sec; seqNo:5052 2024-11-06T03:25:33Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.02 psi 2024-11-06T03:25:33Z- handleDepth()- Depth_Keller msg: 1730863533.14 epoch sec; seqNo:5379; depth 49.66 2024-11-06T03:25:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863533.15 epoch sec; seqNo:5379; 4061763.31 north; 594697.05 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:25:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863529.52 2024-11-06T03:25:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:33Z- waiting for 1730863533.52, time is 1730863533.15 2024-11-06T03:25:33Z- AHRS_M2 msg: 1730863533.54 epoch sec; seqNo:5053 2024-11-06T03:25:33Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.02 psi 2024-11-06T03:25:33Z- handleDepth()- Depth_Keller msg: 1730863533.54 epoch sec; seqNo:5380; depth 49.66 2024-11-06T03:25:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863533.56 epoch sec; seqNo:5380; 4061763.67 north; 594695.82 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:25:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863529.52 2024-11-06T03:25:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:33Z- waiting for 1730863533.52, time is 1730863533.56 2024-11-06T03:25:33Z- LcmTrn::updateTrn() >>>> pose time 1730863533.56, meas time 1730863532.73 <<<< 2024-11-06T03:25:33Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863533.56 2024-11-06T03:25:33Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:25:33Z- TerrainNav::motionUpdate() - currEstimate = (4061764, 594695.82, 49.66) 2024-11-06T03:25:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:25:33Z- TerrainNav:: SubcloudNIS is 0.42, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:25:33Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:25:33Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:25:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:25:33Z- variances([0], [2], and psi_berg) = 0.577596 0.163361 0.000000 2024-11-06T03:25:33Z- publishEstimatesDecoder() - at 1730863533.56 sec 2024-11-06T03:25:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:25:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061794.14 594679.02 57.83 2024-11-06T03:25:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061792.86 594678.77 58.91 lat/lon: 36.70 -121.94 2024-11-06T03:25:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.58 0.16 1.42 0.00 2024-11-06T03:25:33Z- AHRS_M2 msg: 1730863533.94 epoch sec; seqNo:5054 2024-11-06T03:25:33Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.02 psi 2024-11-06T03:25:33Z- handleDepth()- Depth_Keller msg: 1730863533.95 epoch sec; seqNo:5381; depth 49.65 2024-11-06T03:25:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863533.96 epoch sec; seqNo:5381; 4061764.05 north; 594695.82 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:25:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863533.56 2024-11-06T03:25:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:34Z- waiting for 1730863537.56, time is 1730863533.96 2024-11-06T03:25:34Z- AHRS_M2 msg: 1730863534.35 epoch sec; seqNo:5055 2024-11-06T03:25:34Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.02 psi 2024-11-06T03:25:34Z- handleDepth()- Depth_Keller msg: 1730863534.35 epoch sec; seqNo:5382; depth 49.65 2024-11-06T03:25:34Z- handleDVL() - DVL msg epochMillisec: 1730863534.37 2024-11-06T03:25:34Z- handleDvl() - RDI_Pathfinder msg: 1730863534.37 sec; seqNo:1879; #beams:4 2024-11-06T03:25:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.40 , 35.17 , 38.40 , 35.00 2024-11-06T03:25:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.10 , -0.04 2024-11-06T03:25:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863534.40 epoch sec; seqNo:5382; 4061764.43 north; 594695.81 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:25:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863533.56 2024-11-06T03:25:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:34Z- waiting for 1730863537.56, time is 1730863534.40 2024-11-06T03:25:34Z- AHRS_M2 msg: 1730863534.75 epoch sec; seqNo:5056 2024-11-06T03:25:34Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.01 psi 2024-11-06T03:25:34Z- handleDepth()- Depth_Keller msg: 1730863534.76 epoch sec; seqNo:5383; depth 49.63 2024-11-06T03:25:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863534.77 epoch sec; seqNo:5383; 4061764.81 north; 594695.81 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:25:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863533.56 2024-11-06T03:25:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:34Z- waiting for 1730863537.56, time is 1730863534.77 2024-11-06T03:25:35Z- AHRS_M2 msg: 1730863535.15 epoch sec; seqNo:5057 2024-11-06T03:25:35Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 6.01 psi 2024-11-06T03:25:35Z- handleDepth()- Depth_Keller msg: 1730863535.16 epoch sec; seqNo:5384; depth 49.62 2024-11-06T03:25:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863535.18 epoch sec; seqNo:5384; 4061765.19 north; 594695.81 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:25:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863533.56 2024-11-06T03:25:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:35Z- waiting for 1730863537.56, time is 1730863535.18 2024-11-06T03:25:35Z- AHRS_M2 msg: 1730863535.56 epoch sec; seqNo:5058 2024-11-06T03:25:35Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.01 psi 2024-11-06T03:25:35Z- handleDepth()- Depth_Keller msg: 1730863535.56 epoch sec; seqNo:5385; depth 49.62 2024-11-06T03:25:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863535.58 epoch sec; seqNo:5385; 4061765.56 north; 594695.80 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:25:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863533.56 2024-11-06T03:25:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:35Z- waiting for 1730863537.56, time is 1730863535.58 2024-11-06T03:25:35Z- AHRS_M2 msg: 1730863535.96 epoch sec; seqNo:5059 2024-11-06T03:25:35Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.01 psi 2024-11-06T03:25:35Z- handleDepth()- Depth_Keller msg: 1730863535.97 epoch sec; seqNo:5386; depth 49.61 2024-11-06T03:25:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863535.98 epoch sec; seqNo:5386; 4061765.94 north; 594695.80 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:25:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863533.56 2024-11-06T03:25:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:36Z- waiting for 1730863537.56, time is 1730863535.98 2024-11-06T03:25:36Z- AHRS_M2 msg: 1730863536.36 epoch sec; seqNo:5060 2024-11-06T03:25:36Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.01 psi 2024-11-06T03:25:36Z- handleDepth()- Depth_Keller msg: 1730863536.37 epoch sec; seqNo:5387; depth 49.62 2024-11-06T03:25:36Z- handleDVL() - DVL msg epochMillisec: 1730863536.40 2024-11-06T03:25:36Z- handleDvl() - RDI_Pathfinder msg: 1730863536.40 sec; seqNo:1880; #beams:4 2024-11-06T03:25:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.74 , 35.17 , 38.56 , 35.34 2024-11-06T03:25:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.12 , -0.04 2024-11-06T03:25:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863536.41 epoch sec; seqNo:5387; 4061766.32 north; 594695.79 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:25:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863533.56 2024-11-06T03:25:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:36Z- waiting for 1730863537.56, time is 1730863536.41 2024-11-06T03:25:36Z- AHRS_M2 msg: 1730863536.78 epoch sec; seqNo:5061 2024-11-06T03:25:36Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.01 psi 2024-11-06T03:25:36Z- handleDepth()- Depth_Keller msg: 1730863536.82 epoch sec; seqNo:5388; depth 49.63 2024-11-06T03:25:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863536.90 epoch sec; seqNo:5388; 4061766.70 north; 594695.79 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:25:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863533.56 2024-11-06T03:25:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:36Z- waiting for 1730863537.56, time is 1730863536.90 2024-11-06T03:25:37Z- handleDepth()- Depth_Keller msg: 1730863537.19 epoch sec; seqNo:5389; depth 49.62 2024-11-06T03:25:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863537.20 epoch sec; seqNo:5389; 4061766.69 north; 594694.57 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:25:37Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:25:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863533.56 2024-11-06T03:25:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:37Z- waiting for 1730863537.56, time is 1730863537.20 2024-11-06T03:25:37Z- AHRS_M2 msg: 1730863537.60 epoch sec; seqNo:5062 2024-11-06T03:25:37Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.01 psi 2024-11-06T03:25:37Z- handleDepth()- Depth_Keller msg: 1730863537.61 epoch sec; seqNo:5390; depth 49.63 2024-11-06T03:25:37Z- handleDVL() - DVL msg epochMillisec: 1730863537.61 2024-11-06T03:25:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:25:37Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:25:37Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:25:37Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:25:37Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:25:37Z- handleDvl() - RDI_Pathfinder msg: 1730863537.61 sec; seqNo:1881; #beams:0 2024-11-06T03:25:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:25:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.12 , -0.04 2024-11-06T03:25:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863537.63 epoch sec; seqNo:5390; 4061767.07 north; 594694.56 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:25:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863533.56 2024-11-06T03:25:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:37Z- waiting for 1730863537.56, time is 1730863537.63 2024-11-06T03:25:37Z- LcmTrn::updateTrn() >>>> pose time 1730863537.63, meas time 1730863537.61 <<<< 2024-11-06T03:25:37Z- LcmTrn::updateTrn() equating pose 1730863537.61 and meas times 1730863537.61 2024-11-06T03:25:37Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863537.61 2024-11-06T03:25:37Z- TerrainNav::motionUpdate() - currEstimate = (4061767, 594694.56, 49.63) 2024-11-06T03:25:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:25:37Z- TerrainNav:: SubcloudNIS is 0.42, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:25:37Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:25:37Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:25:37Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:25:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:25:37Z- variances([0], [2], and psi_berg) = 0.579744 0.165638 0.000000 2024-11-06T03:25:37Z- publishEstimatesDecoder() - at 1730863537.61 sec 2024-11-06T03:25:37Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:25:37Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061797.54 594677.81 57.14 2024-11-06T03:25:37Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061796.25 594677.51 58.89 lat/lon: 36.70 -121.94 2024-11-06T03:25:37Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.58 0.17 1.66 0.00 2024-11-06T03:25:37Z- AHRS_M2 msg: 1730863537.99 epoch sec; seqNo:5063 2024-11-06T03:25:37Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.01 psi 2024-11-06T03:25:37Z- handleDepth()- Depth_Keller msg: 1730863537.99 epoch sec; seqNo:5391; depth 49.63 2024-11-06T03:25:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863538.01 epoch sec; seqNo:5391; 4061767.45 north; 594694.56 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:25:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863537.61 2024-11-06T03:25:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:38Z- waiting for 1730863541.61, time is 1730863538.01 2024-11-06T03:25:38Z- AHRS_M2 msg: 1730863538.38 epoch sec; seqNo:5064 2024-11-06T03:25:38Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.01 psi 2024-11-06T03:25:38Z- handleDepth()- Depth_Keller msg: 1730863538.39 epoch sec; seqNo:5392; depth 49.62 2024-11-06T03:25:38Z- handleDVL() - DVL msg epochMillisec: 1730863538.41 2024-11-06T03:25:38Z- handleDvl() - RDI_Pathfinder msg: 1730863538.41 sec; seqNo:1882; #beams:4 2024-11-06T03:25:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.91 , 35.34 , 38.57 , 35.51 2024-11-06T03:25:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.11 , -0.01 2024-11-06T03:25:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863538.43 epoch sec; seqNo:5392; 4061767.82 north; 594694.56 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:25:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863537.61 2024-11-06T03:25:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:38Z- waiting for 1730863541.61, time is 1730863538.43 2024-11-06T03:25:38Z- AHRS_M2 msg: 1730863538.79 epoch sec; seqNo:5065 2024-11-06T03:25:38Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.01 psi 2024-11-06T03:25:38Z- handleDepth()- Depth_Keller msg: 1730863538.80 epoch sec; seqNo:5393; depth 49.64 2024-11-06T03:25:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863538.82 epoch sec; seqNo:5393; 4061768.20 north; 594694.55 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:25:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863537.61 2024-11-06T03:25:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:38Z- waiting for 1730863541.61, time is 1730863538.82 2024-11-06T03:25:39Z- AHRS_M2 msg: 1730863539.19 epoch sec; seqNo:5066 2024-11-06T03:25:39Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 6.01 psi 2024-11-06T03:25:39Z- handleDepth()- Depth_Keller msg: 1730863539.19 epoch sec; seqNo:5394; depth 49.65 2024-11-06T03:25:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863539.20 epoch sec; seqNo:5394; 4061768.58 north; 594694.55 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:25:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863537.61 2024-11-06T03:25:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:39Z- waiting for 1730863541.61, time is 1730863539.20 2024-11-06T03:25:39Z- AHRS_M2 msg: 1730863539.60 epoch sec; seqNo:5067 2024-11-06T03:25:39Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 6.01 psi 2024-11-06T03:25:39Z- handleDepth()- Depth_Keller msg: 1730863539.61 epoch sec; seqNo:5395; depth 49.64 2024-11-06T03:25:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863539.62 epoch sec; seqNo:5395; 4061768.96 north; 594694.54 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:25:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863537.61 2024-11-06T03:25:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:39Z- waiting for 1730863541.61, time is 1730863539.62 2024-11-06T03:25:40Z- AHRS_M2 msg: 1730863540.00 epoch sec; seqNo:5068 2024-11-06T03:25:40Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 6.01 psi 2024-11-06T03:25:40Z- handleDepth()- Depth_Keller msg: 1730863540.01 epoch sec; seqNo:5396; depth 49.67 2024-11-06T03:25:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863540.02 epoch sec; seqNo:5396; 4061769.34 north; 594694.54 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:25:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863537.61 2024-11-06T03:25:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:40Z- waiting for 1730863541.61, time is 1730863540.02 2024-11-06T03:25:40Z- AHRS_M2 msg: 1730863540.40 epoch sec; seqNo:5069 2024-11-06T03:25:40Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 6.01 psi 2024-11-06T03:25:40Z- handleDepth()- Depth_Keller msg: 1730863540.42 epoch sec; seqNo:5397; depth 49.67 2024-11-06T03:25:40Z- handleDVL() - DVL msg epochMillisec: 1730863540.44 2024-11-06T03:25:40Z- handleDvl() - RDI_Pathfinder msg: 1730863540.44 sec; seqNo:1883; #beams:4 2024-11-06T03:25:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.41 , 35.17 , 38.74 , 36.02 2024-11-06T03:25:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.09 , 0.02 2024-11-06T03:25:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863540.45 epoch sec; seqNo:5397; 4061769.71 north; 594693.31 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:25:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863537.61 2024-11-06T03:25:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:40Z- waiting for 1730863541.61, time is 1730863540.45 2024-11-06T03:25:40Z- AHRS_M2 msg: 1730863540.81 epoch sec; seqNo:5070 2024-11-06T03:25:40Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 6.01 psi 2024-11-06T03:25:40Z- handleDepth()- Depth_Keller msg: 1730863540.81 epoch sec; seqNo:5398; depth 49.69 2024-11-06T03:25:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863540.83 epoch sec; seqNo:5398; 4061770.08 north; 594693.31 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:25:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863537.61 2024-11-06T03:25:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:40Z- waiting for 1730863541.61, time is 1730863540.83 2024-11-06T03:25:41Z- AHRS_M2 msg: 1730863541.21 epoch sec; seqNo:5071 2024-11-06T03:25:41Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 6.02 psi 2024-11-06T03:25:41Z- handleDepth()- Depth_Keller msg: 1730863541.22 epoch sec; seqNo:5399; depth 49.70 2024-11-06T03:25:41Z- handleDVL() - DVL msg epochMillisec: 1730863541.22 2024-11-06T03:25:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:25:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:25:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:25:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:25:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:25:41Z- handleDvl() - RDI_Pathfinder msg: 1730863541.22 sec; seqNo:1884; #beams:0 2024-11-06T03:25:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:25:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.09 , 0.02 2024-11-06T03:25:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863541.23 epoch sec; seqNo:5399; 4061770.46 north; 594693.31 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:25:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863537.61 2024-11-06T03:25:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:41Z- waiting for 1730863541.61, time is 1730863541.23 2024-11-06T03:25:41Z- AHRS_M2 msg: 1730863541.62 epoch sec; seqNo:5072 2024-11-06T03:25:41Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 6.02 psi 2024-11-06T03:25:41Z- handleDepth()- Depth_Keller msg: 1730863541.64 epoch sec; seqNo:5400; depth 49.71 2024-11-06T03:25:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863541.66 epoch sec; seqNo:5400; 4061770.84 north; 594693.30 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:25:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863537.61 2024-11-06T03:25:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:41Z- waiting for 1730863541.61, time is 1730863541.66 2024-11-06T03:25:41Z- LcmTrn::updateTrn() >>>> pose time 1730863541.66, meas time 1730863541.22 <<<< 2024-11-06T03:25:41Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863541.66 2024-11-06T03:25:41Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:25:41Z- TerrainNav::motionUpdate() - currEstimate = (4061771, 594693.30, 49.71) 2024-11-06T03:25:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:25:41Z- TerrainNav:: SubcloudNIS is 0.42, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:25:41Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:25:41Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:25:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:25:41Z- variances([0], [2], and psi_berg) = 0.582316 0.168599 0.000000 2024-11-06T03:25:41Z- publishEstimatesDecoder() - at 1730863541.66 sec 2024-11-06T03:25:41Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:25:41Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061801.32 594676.55 57.53 2024-11-06T03:25:41Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061800.03 594676.25 58.96 lat/lon: 36.70 -121.94 2024-11-06T03:25:41Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.58 0.17 1.92 0.00 2024-11-06T03:25:42Z- AHRS_M2 msg: 1730863542.02 epoch sec; seqNo:5073 2024-11-06T03:25:42Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 6.02 psi 2024-11-06T03:25:42Z- handleDepth()- Depth_Keller msg: 1730863542.03 epoch sec; seqNo:5401; depth 49.70 2024-11-06T03:25:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863542.04 epoch sec; seqNo:5401; 4061771.22 north; 594693.30 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:25:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863541.66 2024-11-06T03:25:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:42Z- waiting for 1730863545.66, time is 1730863542.04 2024-11-06T03:25:42Z- AHRS_M2 msg: 1730863542.42 epoch sec; seqNo:5074 2024-11-06T03:25:42Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 6.02 psi 2024-11-06T03:25:42Z- handleDepth()- Depth_Keller msg: 1730863542.43 epoch sec; seqNo:5402; depth 49.72 2024-11-06T03:25:42Z- handleDVL() - DVL msg epochMillisec: 1730863542.47 2024-11-06T03:25:42Z- handleDvl() - RDI_Pathfinder msg: 1730863542.47 sec; seqNo:1885; #beams:4 2024-11-06T03:25:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.09 , 35.17 , 38.90 , 36.53 2024-11-06T03:25:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , 0.04 2024-11-06T03:25:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863542.48 epoch sec; seqNo:5402; 4061771.60 north; 594693.29 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:25:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863541.66 2024-11-06T03:25:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:42Z- waiting for 1730863545.66, time is 1730863542.48 2024-11-06T03:25:42Z- AHRS_M2 msg: 1730863542.83 epoch sec; seqNo:5075 2024-11-06T03:25:42Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 6.02 psi 2024-11-06T03:25:42Z- handleDepth()- Depth_Keller msg: 1730863542.83 epoch sec; seqNo:5403; depth 49.74 2024-11-06T03:25:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863542.85 epoch sec; seqNo:5403; 4061771.98 north; 594693.29 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:25:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863541.66 2024-11-06T03:25:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:42Z- waiting for 1730863545.66, time is 1730863542.85 2024-11-06T03:25:43Z- AHRS_M2 msg: 1730863543.23 epoch sec; seqNo:5076 2024-11-06T03:25:43Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.02 psi 2024-11-06T03:25:43Z- handleDepth()- Depth_Keller msg: 1730863543.24 epoch sec; seqNo:5404; depth 49.72 2024-11-06T03:25:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863543.25 epoch sec; seqNo:5404; 4061772.36 north; 594693.29 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:25:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863541.66 2024-11-06T03:25:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:43Z- waiting for 1730863545.66, time is 1730863543.25 2024-11-06T03:25:43Z- AHRS_M2 msg: 1730863543.66 epoch sec; seqNo:5077 2024-11-06T03:25:43Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.02 psi 2024-11-06T03:25:43Z- handleDepth()- Depth_Keller msg: 1730863543.67 epoch sec; seqNo:5405; depth 49.72 2024-11-06T03:25:43Z- handleDVL() - DVL msg epochMillisec: 1730863543.67 2024-11-06T03:25:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:25:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:25:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:25:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:25:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:25:43Z- handleDvl() - RDI_Pathfinder msg: 1730863543.67 sec; seqNo:1886; #beams:0 2024-11-06T03:25:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:25:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.05 , 0.04 2024-11-06T03:25:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863543.68 epoch sec; seqNo:5405; 4061772.36 north; 594693.29 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:25:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863541.66 2024-11-06T03:25:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:43Z- waiting for 1730863545.66, time is 1730863543.68 2024-11-06T03:25:44Z- AHRS_M2 msg: 1730863544.05 epoch sec; seqNo:5078 2024-11-06T03:25:44Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.02 psi 2024-11-06T03:25:44Z- handleDepth()- Depth_Keller msg: 1730863544.05 epoch sec; seqNo:5406; depth 49.72 2024-11-06T03:25:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863544.06 epoch sec; seqNo:5406; 4061772.74 north; 594693.28 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:25:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863541.66 2024-11-06T03:25:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:44Z- waiting for 1730863545.66, time is 1730863544.06 2024-11-06T03:25:44Z- AHRS_M2 msg: 1730863544.44 epoch sec; seqNo:5079 2024-11-06T03:25:44Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:44Z- handleDepth()- Depth_Keller msg: 1730863544.45 epoch sec; seqNo:5407; depth 49.72 2024-11-06T03:25:44Z- handleDVL() - DVL msg epochMillisec: 1730863544.47 2024-11-06T03:25:44Z- handleDvl() - RDI_Pathfinder msg: 1730863544.47 sec; seqNo:1887; #beams:4 2024-11-06T03:25:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.77 , 35.34 , 39.07 , 36.87 2024-11-06T03:25:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.01 2024-11-06T03:25:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863544.48 epoch sec; seqNo:5407; 4061773.10 north; 594692.06 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:25:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863541.66 2024-11-06T03:25:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:44Z- waiting for 1730863545.66, time is 1730863544.48 2024-11-06T03:25:44Z- AHRS_M2 msg: 1730863544.85 epoch sec; seqNo:5080 2024-11-06T03:25:44Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:44Z- handleDepth()- Depth_Keller msg: 1730863544.86 epoch sec; seqNo:5408; depth 49.73 2024-11-06T03:25:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863544.88 epoch sec; seqNo:5408; 4061773.48 north; 594692.05 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:25:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863541.66 2024-11-06T03:25:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:45Z- waiting for 1730863545.66, time is 1730863544.88 2024-11-06T03:25:45Z- AHRS_M2 msg: 1730863545.25 epoch sec; seqNo:5081 2024-11-06T03:25:45Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:45Z- handleDepth()- Depth_Keller msg: 1730863545.26 epoch sec; seqNo:5409; depth 49.72 2024-11-06T03:25:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863545.27 epoch sec; seqNo:5409; 4061773.86 north; 594692.05 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:25:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863541.66 2024-11-06T03:25:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:45Z- waiting for 1730863545.66, time is 1730863545.27 2024-11-06T03:25:45Z- AHRS_M2 msg: 1730863545.66 epoch sec; seqNo:5082 2024-11-06T03:25:45Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:45Z- handleDepth()- Depth_Keller msg: 1730863545.67 epoch sec; seqNo:5410; depth 49.72 2024-11-06T03:25:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863545.67 epoch sec; seqNo:5410; 4061774.24 north; 594692.04 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:25:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863541.66 2024-11-06T03:25:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:45Z- waiting for 1730863545.66, time is 1730863545.67 2024-11-06T03:25:45Z- LcmTrn::updateTrn() >>>> pose time 1730863545.67, meas time 1730863544.47 <<<< 2024-11-06T03:25:45Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863545.67 2024-11-06T03:25:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863544.47 sec, ping # 0; waiting for INS data more recent than 1730863541.66... 2024-11-06T03:25:45Z- TerrainNav::motionUpdate() - currEstimate = (4061774, 594692.04, 49.72) 2024-11-06T03:25:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:25:45Z- TerrainNav:: SubcloudNIS is 0.42, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:25:45Z- TNavPF::Projecting Measurements 2024-11-06T03:25:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:25:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:25:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:25:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:25:45Z- TNavFilter::4 good beams. 2024-11-06T03:25:45Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:25:45Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:25:45Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:25:45Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:25:45Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:25:45Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:25:45Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:25:45Z- TerrainMapOctree: (vehN, vehE ) = (4061773.22, 594692.42). 2024-11-06T03:25:45Z- TerrainMapOctree: Min Distance = 62.08. 2024-11-06T03:25:45Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:25:45Z- Forcing Subcloud Comparison 2024-11-06T03:25:45Z- Weighting particles with subcloud comparison 2024-11-06T03:25:45Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:45Z- meanExpectedMeasDiff: 1.315738 delta_rms_squared: 3.575880 alpha: 0.994438 2024-11-06T03:25:45Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:45Z- meanExpectedMeasDiff: 2.017154 delta_rms_squared: 1.839998 alpha: 0.989247 2024-11-06T03:25:45Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:45Z- meanExpectedMeasDiff: 2.162115 delta_rms_squared: 3.148824 alpha: 0.993689 2024-11-06T03:25:45Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:45Z- meanExpectedMeasDiff: 0.837574 delta_rms_squared: 2.979734 alpha: 0.993333 2024-11-06T03:25:45Z- Subcloud NIS: 0.427325 2024-11-06T03:25:45Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:25:45Z- TNavPF:: sumWeights = 0.300173 2024-11-06T03:25:45Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:25:45Z- TNavPF::First Term of Map Covariance Matrix is 0.30 2024-11-06T03:25:45Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T03:25:45Z- TerrainNav::Resampling particle filter... 2024-11-06T03:25:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863544.47 sec. 2024-11-06T03:25:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:25:45Z- variances([0], [2], and psi_berg) = 0.575102 0.172415 0.000000 2024-11-06T03:25:45Z- publishEstimatesDecoder() - at 1730863545.67 sec 2024-11-06T03:25:45Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:25:45Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061804.72 594675.30 58.60 2024-11-06T03:25:45Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061803.40 594674.98 57.97 lat/lon: 36.70 -121.94 2024-11-06T03:25:45Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.58 0.17 0.85 0.00 2024-11-06T03:25:46Z- AHRS_M2 msg: 1730863546.07 epoch sec; seqNo:5083 2024-11-06T03:25:46Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:46Z- handleDepth()- Depth_Keller msg: 1730863546.08 epoch sec; seqNo:5411; depth 49.70 2024-11-06T03:25:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863546.09 epoch sec; seqNo:5411; 4061774.62 north; 594692.04 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:25:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863545.67 2024-11-06T03:25:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:46Z- waiting for 1730863549.67, time is 1730863546.09 2024-11-06T03:25:46Z- AHRS_M2 msg: 1730863546.47 epoch sec; seqNo:5084 2024-11-06T03:25:46Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:46Z- handleDepth()- Depth_Keller msg: 1730863546.47 epoch sec; seqNo:5412; depth 49.70 2024-11-06T03:25:46Z- handleDVL() - DVL msg epochMillisec: 1730863546.49 2024-11-06T03:25:46Z- handleDvl() - RDI_Pathfinder msg: 1730863546.49 sec; seqNo:1888; #beams:4 2024-11-06T03:25:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.12 , 35.51 , 38.74 , 37.21 2024-11-06T03:25:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , -0.06 2024-11-06T03:25:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863546.52 epoch sec; seqNo:5412; 4061775.00 north; 594692.04 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:25:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863545.67 2024-11-06T03:25:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:46Z- waiting for 1730863549.67, time is 1730863546.52 2024-11-06T03:25:46Z- AHRS_M2 msg: 1730863546.87 epoch sec; seqNo:5085 2024-11-06T03:25:46Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:46Z- handleDepth()- Depth_Keller msg: 1730863546.88 epoch sec; seqNo:5413; depth 49.69 2024-11-06T03:25:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863546.89 epoch sec; seqNo:5413; 4061775.37 north; 594692.03 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:25:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863545.67 2024-11-06T03:25:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:47Z- waiting for 1730863549.67, time is 1730863546.89 2024-11-06T03:25:47Z- AHRS_M2 msg: 1730863547.27 epoch sec; seqNo:5086 2024-11-06T03:25:47Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:47Z- handleDepth()- Depth_Keller msg: 1730863547.28 epoch sec; seqNo:5414; depth 49.67 2024-11-06T03:25:47Z- handleDVL() - DVL msg epochMillisec: 1730863547.28 2024-11-06T03:25:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:25:47Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:25:47Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:25:47Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:25:47Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:25:47Z- handleDvl() - RDI_Pathfinder msg: 1730863547.28 sec; seqNo:1889; #beams:0 2024-11-06T03:25:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:25:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , -0.06 2024-11-06T03:25:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863547.30 epoch sec; seqNo:5414; 4061775.75 north; 594692.03 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:25:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863545.67 2024-11-06T03:25:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:47Z- waiting for 1730863549.67, time is 1730863547.30 2024-11-06T03:25:47Z- AHRS_M2 msg: 1730863547.68 epoch sec; seqNo:5087 2024-11-06T03:25:47Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:47Z- handleDepth()- Depth_Keller msg: 1730863547.68 epoch sec; seqNo:5415; depth 49.68 2024-11-06T03:25:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863547.71 epoch sec; seqNo:5415; 4061775.75 north; 594692.03 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:25:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863545.67 2024-11-06T03:25:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:47Z- waiting for 1730863549.67, time is 1730863547.71 2024-11-06T03:25:48Z- AHRS_M2 msg: 1730863548.09 epoch sec; seqNo:5088 2024-11-06T03:25:48Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:48Z- handleDepth()- Depth_Keller msg: 1730863548.10 epoch sec; seqNo:5416; depth 49.66 2024-11-06T03:25:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863548.13 epoch sec; seqNo:5416; 4061776.13 north; 594692.02 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:25:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863545.67 2024-11-06T03:25:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:48Z- waiting for 1730863549.67, time is 1730863548.13 2024-11-06T03:25:48Z- AHRS_M2 msg: 1730863548.49 epoch sec; seqNo:5089 2024-11-06T03:25:48Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:48Z- handleDepth()- Depth_Keller msg: 1730863548.49 epoch sec; seqNo:5417; depth 49.66 2024-11-06T03:25:48Z- handleDVL() - DVL msg epochMillisec: 1730863548.52 2024-11-06T03:25:48Z- handleDvl() - RDI_Pathfinder msg: 1730863548.52 sec; seqNo:1890; #beams:4 2024-11-06T03:25:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.30 , 35.32 , 38.50 , 37.19 2024-11-06T03:25:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.10 , -0.09 2024-11-06T03:25:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863548.53 epoch sec; seqNo:5417; 4061776.51 north; 594692.02 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:25:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863545.67 2024-11-06T03:25:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:48Z- waiting for 1730863549.67, time is 1730863548.53 2024-11-06T03:25:48Z- AHRS_M2 msg: 1730863548.89 epoch sec; seqNo:5090 2024-11-06T03:25:48Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:48Z- handleDepth()- Depth_Keller msg: 1730863548.90 epoch sec; seqNo:5418; depth 49.66 2024-11-06T03:25:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863548.92 epoch sec; seqNo:5418; 4061776.89 north; 594692.01 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:25:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863545.67 2024-11-06T03:25:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:49Z- waiting for 1730863549.67, time is 1730863548.92 2024-11-06T03:25:49Z- AHRS_M2 msg: 1730863549.30 epoch sec; seqNo:5091 2024-11-06T03:25:49Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:49Z- handleDepth()- Depth_Keller msg: 1730863549.30 epoch sec; seqNo:5419; depth 49.66 2024-11-06T03:25:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863549.31 epoch sec; seqNo:5419; 4061777.26 north; 594690.79 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:25:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863545.67 2024-11-06T03:25:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:49Z- waiting for 1730863549.67, time is 1730863549.31 2024-11-06T03:25:49Z- AHRS_M2 msg: 1730863549.70 epoch sec; seqNo:5092 2024-11-06T03:25:49Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:49Z- handleDepth()- Depth_Keller msg: 1730863549.71 epoch sec; seqNo:5420; depth 49.66 2024-11-06T03:25:49Z- handleDVL() - DVL msg epochMillisec: 1730863549.71 2024-11-06T03:25:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:25:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:25:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:25:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:25:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:25:49Z- handleDvl() - RDI_Pathfinder msg: 1730863549.71 sec; seqNo:1891; #beams:0 2024-11-06T03:25:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:25:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.10 , -0.09 2024-11-06T03:25:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863549.73 epoch sec; seqNo:5420; 4061777.63 north; 594690.79 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:25:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863545.67 2024-11-06T03:25:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:49Z- waiting for 1730863549.67, time is 1730863549.73 2024-11-06T03:25:49Z- LcmTrn::updateTrn() >>>> pose time 1730863549.73, meas time 1730863549.71 <<<< 2024-11-06T03:25:49Z- LcmTrn::updateTrn() equating pose 1730863549.71 and meas times 1730863549.71 2024-11-06T03:25:49Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863549.71 2024-11-06T03:25:49Z- TerrainNav::motionUpdate() - currEstimate = (4061778, 594690.79, 49.66) 2024-11-06T03:25:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:25:49Z- TerrainNav:: SubcloudNIS is 0.43, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:25:49Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:25:49Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:25:49Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:25:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:25:49Z- variances([0], [2], and psi_berg) = 0.578307 0.174524 0.000000 2024-11-06T03:25:49Z- publishEstimatesDecoder() - at 1730863549.71 sec 2024-11-06T03:25:49Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:25:49Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061808.09 594673.99 59.06 2024-11-06T03:25:49Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061806.79 594673.72 57.90 lat/lon: 36.70 -121.94 2024-11-06T03:25:49Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.58 0.17 1.09 0.00 2024-11-06T03:25:50Z- AHRS_M2 msg: 1730863550.10 epoch sec; seqNo:5093 2024-11-06T03:25:50Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:50Z- handleDepth()- Depth_Keller msg: 1730863550.11 epoch sec; seqNo:5421; depth 49.66 2024-11-06T03:25:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863550.12 epoch sec; seqNo:5421; 4061778.01 north; 594690.78 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:25:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863549.71 2024-11-06T03:25:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:50Z- waiting for 1730863553.71, time is 1730863550.12 2024-11-06T03:25:50Z- AHRS_M2 msg: 1730863550.51 epoch sec; seqNo:5094 2024-11-06T03:25:50Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:50Z- handleDepth()- Depth_Keller msg: 1730863550.51 epoch sec; seqNo:5422; depth 49.67 2024-11-06T03:25:50Z- handleDVL() - DVL msg epochMillisec: 1730863550.53 2024-11-06T03:25:50Z- handleDvl() - RDI_Pathfinder msg: 1730863550.53 sec; seqNo:1892; #beams:4 2024-11-06T03:25:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.49 , 35.13 , 38.68 , 37.19 2024-11-06T03:25:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.16 , -0.04 2024-11-06T03:25:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863550.55 epoch sec; seqNo:5422; 4061778.39 north; 594690.78 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:25:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863549.71 2024-11-06T03:25:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:50Z- waiting for 1730863553.71, time is 1730863550.55 2024-11-06T03:25:50Z- handleDepth()- Depth_Keller msg: 1730863550.93 epoch sec; seqNo:5423; depth 49.67 2024-11-06T03:25:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863550.95 epoch sec; seqNo:5423; 4061778.77 north; 594690.77 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:25:51Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:25:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863549.71 2024-11-06T03:25:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:51Z- waiting for 1730863553.71, time is 1730863550.95 2024-11-06T03:25:51Z- AHRS_M2 msg: 1730863551.31 epoch sec; seqNo:5095 2024-11-06T03:25:51Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:51Z- handleDepth()- Depth_Keller msg: 1730863551.32 epoch sec; seqNo:5424; depth 49.68 2024-11-06T03:25:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863551.33 epoch sec; seqNo:5424; 4061779.15 north; 594690.77 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:25:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863549.71 2024-11-06T03:25:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:51Z- waiting for 1730863553.71, time is 1730863551.33 2024-11-06T03:25:51Z- AHRS_M2 msg: 1730863551.72 epoch sec; seqNo:5096 2024-11-06T03:25:51Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:51Z- handleDepth()- Depth_Keller msg: 1730863551.73 epoch sec; seqNo:5425; depth 49.68 2024-11-06T03:25:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863551.74 epoch sec; seqNo:5425; 4061779.53 north; 594690.76 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:25:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863549.71 2024-11-06T03:25:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:51Z- waiting for 1730863553.71, time is 1730863551.74 2024-11-06T03:25:52Z- AHRS_M2 msg: 1730863552.12 epoch sec; seqNo:5097 2024-11-06T03:25:52Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:52Z- handleDepth()- Depth_Keller msg: 1730863552.13 epoch sec; seqNo:5426; depth 49.69 2024-11-06T03:25:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863552.14 epoch sec; seqNo:5426; 4061779.91 north; 594690.76 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:25:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863549.71 2024-11-06T03:25:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:52Z- waiting for 1730863553.71, time is 1730863552.14 2024-11-06T03:25:52Z- AHRS_M2 msg: 1730863552.53 epoch sec; seqNo:5098 2024-11-06T03:25:52Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:52Z- handleDepth()- Depth_Keller msg: 1730863552.53 epoch sec; seqNo:5427; depth 49.69 2024-11-06T03:25:52Z- handleDVL() - DVL msg epochMillisec: 1730863552.56 2024-11-06T03:25:52Z- handleDvl() - RDI_Pathfinder msg: 1730863552.56 sec; seqNo:1893; #beams:4 2024-11-06T03:25:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.67 , 35.32 , 38.87 , 37.56 2024-11-06T03:25:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.14 , 0.03 2024-11-06T03:25:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863552.58 epoch sec; seqNo:5427; 4061780.27 north; 594689.54 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:25:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863549.71 2024-11-06T03:25:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:52Z- waiting for 1730863553.71, time is 1730863552.58 2024-11-06T03:25:52Z- AHRS_M2 msg: 1730863552.93 epoch sec; seqNo:5099 2024-11-06T03:25:52Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.03 psi 2024-11-06T03:25:52Z- handleDepth()- Depth_Keller msg: 1730863552.93 epoch sec; seqNo:5428; depth 49.70 2024-11-06T03:25:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863552.95 epoch sec; seqNo:5428; 4061780.65 north; 594689.53 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:25:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863549.71 2024-11-06T03:25:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:53Z- waiting for 1730863553.71, time is 1730863552.95 2024-11-06T03:25:53Z- AHRS_M2 msg: 1730863553.34 epoch sec; seqNo:5100 2024-11-06T03:25:53Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.04 psi 2024-11-06T03:25:53Z- handleDepth()- Depth_Keller msg: 1730863553.34 epoch sec; seqNo:5429; depth 49.71 2024-11-06T03:25:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863553.35 epoch sec; seqNo:5429; 4061781.03 north; 594689.53 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:25:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863549.71 2024-11-06T03:25:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:53Z- waiting for 1730863553.71, time is 1730863553.35 2024-11-06T03:25:53Z- AHRS_M2 msg: 1730863553.74 epoch sec; seqNo:5101 2024-11-06T03:25:53Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.04 psi 2024-11-06T03:25:53Z- handleDepth()- Depth_Keller msg: 1730863553.76 epoch sec; seqNo:5430; depth 49.74 2024-11-06T03:25:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863553.77 epoch sec; seqNo:5430; 4061781.41 north; 594689.52 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:25:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863549.71 2024-11-06T03:25:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:53Z- waiting for 1730863553.71, time is 1730863553.77 2024-11-06T03:25:53Z- LcmTrn::updateTrn() >>>> pose time 1730863553.77, meas time 1730863552.56 <<<< 2024-11-06T03:25:53Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863553.77 2024-11-06T03:25:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863552.56 sec, ping # 0; waiting for INS data more recent than 1730863549.71... 2024-11-06T03:25:53Z- TerrainNav::motionUpdate() - currEstimate = (4061781, 594689.52, 49.74) 2024-11-06T03:25:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:25:53Z- TerrainNav:: SubcloudNIS is 0.43, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:25:53Z- TNavPF::Projecting Measurements 2024-11-06T03:25:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:25:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:25:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:25:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:25:53Z- TNavFilter::4 good beams. 2024-11-06T03:25:53Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:25:53Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:25:53Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:25:53Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:25:53Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:25:53Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:25:53Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:25:53Z- TerrainMapOctree: (vehN, vehE ) = (4061780.28, 594689.90). 2024-11-06T03:25:53Z- TerrainMapOctree: Min Distance = 67.29. 2024-11-06T03:25:53Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:25:54Z- Forcing Subcloud Comparison 2024-11-06T03:25:54Z- Weighting particles with subcloud comparison 2024-11-06T03:25:54Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:54Z- meanExpectedMeasDiff: 0.982880 delta_rms_squared: 2.161028 alpha: 0.990830 2024-11-06T03:25:54Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:54Z- meanExpectedMeasDiff: 0.229341 delta_rms_squared: 1.507340 alpha: 0.986905 2024-11-06T03:25:54Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:54Z- meanExpectedMeasDiff: 0.879337 delta_rms_squared: 1.768499 alpha: 0.988817 2024-11-06T03:25:54Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:54Z- meanExpectedMeasDiff: 0.275470 delta_rms_squared: 1.812931 alpha: 0.989089 2024-11-06T03:25:54Z- Subcloud NIS: 0.404905 2024-11-06T03:25:54Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:25:54Z- TNavPF:: sumWeights = 0.537953 2024-11-06T03:25:54Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:25:54Z- TNavPF::First Term of Map Covariance Matrix is 0.47 2024-11-06T03:25:54Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.02 2024-11-06T03:25:54Z- TerrainNav::Resampling particle filter... 2024-11-06T03:25:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863552.56 sec. 2024-11-06T03:25:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:25:54Z- variances([0], [2], and psi_berg) = 0.561812 0.175667 0.000000 2024-11-06T03:25:54Z- publishEstimatesDecoder() - at 1730863553.77 sec 2024-11-06T03:25:54Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:25:54Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061811.80 594672.67 58.39 2024-11-06T03:25:54Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061810.55 594672.47 57.68 lat/lon: 36.70 -121.94 2024-11-06T03:25:54Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.56 0.18 0.75 0.00 2024-11-06T03:25:54Z- AHRS_M2 msg: 1730863554.15 epoch sec; seqNo:5102 2024-11-06T03:25:54Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.04 psi 2024-11-06T03:25:54Z- handleDepth()- Depth_Keller msg: 1730863554.19 epoch sec; seqNo:5431; depth 49.74 2024-11-06T03:25:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863554.23 epoch sec; seqNo:5431; 4061781.79 north; 594689.52 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:25:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863553.77 2024-11-06T03:25:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:54Z- waiting for 1730863557.77, time is 1730863554.23 2024-11-06T03:25:54Z- AHRS_M2 msg: 1730863554.55 epoch sec; seqNo:5103 2024-11-06T03:25:54Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.04 psi 2024-11-06T03:25:54Z- handleDepth()- Depth_Keller msg: 1730863554.56 epoch sec; seqNo:5432; depth 49.74 2024-11-06T03:25:54Z- handleDVL() - DVL msg epochMillisec: 1730863554.58 2024-11-06T03:25:54Z- handleDvl() - RDI_Pathfinder msg: 1730863554.58 sec; seqNo:1894; #beams:4 2024-11-06T03:25:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.23 , 35.51 , 38.87 , 37.56 2024-11-06T03:25:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.08 , 0.03 2024-11-06T03:25:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863554.59 epoch sec; seqNo:5432; 4061782.17 north; 594689.52 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:25:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863553.77 2024-11-06T03:25:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:54Z- waiting for 1730863557.77, time is 1730863554.59 2024-11-06T03:25:54Z- AHRS_M2 msg: 1730863554.95 epoch sec; seqNo:5104 2024-11-06T03:25:54Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 6.04 psi 2024-11-06T03:25:54Z- handleDepth()- Depth_Keller msg: 1730863554.96 epoch sec; seqNo:5433; depth 49.75 2024-11-06T03:25:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863554.98 epoch sec; seqNo:5433; 4061782.17 north; 594689.52 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:25:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863553.77 2024-11-06T03:25:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:55Z- waiting for 1730863557.77, time is 1730863554.98 2024-11-06T03:25:55Z- AHRS_M2 msg: 1730863555.36 epoch sec; seqNo:5105 2024-11-06T03:25:55Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.04 psi 2024-11-06T03:25:55Z- handleDepth()- Depth_Keller msg: 1730863555.36 epoch sec; seqNo:5434; depth 49.78 2024-11-06T03:25:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863555.37 epoch sec; seqNo:5434; 4061782.55 north; 594689.51 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:25:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863553.77 2024-11-06T03:25:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:55Z- waiting for 1730863557.77, time is 1730863555.37 2024-11-06T03:25:55Z- AHRS_M2 msg: 1730863555.76 epoch sec; seqNo:5106 2024-11-06T03:25:55Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.05 psi 2024-11-06T03:25:55Z- handleDepth()- Depth_Keller msg: 1730863555.77 epoch sec; seqNo:5435; depth 49.77 2024-11-06T03:25:55Z- handleDVL() - DVL msg epochMillisec: 1730863555.77 2024-11-06T03:25:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:25:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:25:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:25:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:25:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:25:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:25:55Z- handleDvl() - RDI_Pathfinder msg: 1730863555.77 sec; seqNo:1895; #beams:0 2024-11-06T03:25:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:25:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.08 , 0.03 2024-11-06T03:25:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863555.78 epoch sec; seqNo:5435; 4061782.92 north; 594689.51 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:25:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863553.77 2024-11-06T03:25:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:55Z- waiting for 1730863557.77, time is 1730863555.78 2024-11-06T03:25:56Z- AHRS_M2 msg: 1730863556.21 epoch sec; seqNo:5107 2024-11-06T03:25:56Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.05 psi 2024-11-06T03:25:56Z- handleDepth()- Depth_Keller msg: 1730863556.23 epoch sec; seqNo:5436; depth 49.79 2024-11-06T03:25:56Z- handleDVL() - DVL msg epochMillisec: 1730863556.25 2024-11-06T03:25:56Z- handleDvl() - RDI_Pathfinder msg: 1730863556.25 sec; seqNo:1896; #beams:4 2024-11-06T03:25:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.23 , 35.51 , 38.87 , 37.94 2024-11-06T03:25:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , 0.00 2024-11-06T03:25:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863556.28 epoch sec; seqNo:5436; 4061783.29 north; 594688.28 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:25:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863553.77 2024-11-06T03:25:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:56Z- waiting for 1730863557.77, time is 1730863556.28 2024-11-06T03:25:56Z- AHRS_M2 msg: 1730863556.57 epoch sec; seqNo:5108 2024-11-06T03:25:56Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.05 psi 2024-11-06T03:25:56Z- handleDepth()- Depth_Keller msg: 1730863556.58 epoch sec; seqNo:5437; depth 49.79 2024-11-06T03:25:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863556.60 epoch sec; seqNo:5437; 4061783.67 north; 594688.28 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:25:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863553.77 2024-11-06T03:25:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:56Z- waiting for 1730863557.77, time is 1730863556.60 2024-11-06T03:25:56Z- AHRS_M2 msg: 1730863556.97 epoch sec; seqNo:5109 2024-11-06T03:25:56Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.05 psi 2024-11-06T03:25:56Z- handleDepth()- Depth_Keller msg: 1730863556.98 epoch sec; seqNo:5438; depth 49.79 2024-11-06T03:25:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863556.99 epoch sec; seqNo:5438; 4061784.05 north; 594688.27 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:25:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863553.77 2024-11-06T03:25:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:57Z- waiting for 1730863557.77, time is 1730863556.99 2024-11-06T03:25:57Z- AHRS_M2 msg: 1730863557.38 epoch sec; seqNo:5110 2024-11-06T03:25:57Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.05 psi 2024-11-06T03:25:57Z- handleDepth()- Depth_Keller msg: 1730863557.38 epoch sec; seqNo:5439; depth 49.79 2024-11-06T03:25:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863557.39 epoch sec; seqNo:5439; 4061784.43 north; 594688.27 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:25:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863553.77 2024-11-06T03:25:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:57Z- waiting for 1730863557.77, time is 1730863557.39 2024-11-06T03:25:57Z- AHRS_M2 msg: 1730863557.78 epoch sec; seqNo:5111 2024-11-06T03:25:57Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.05 psi 2024-11-06T03:25:57Z- handleDepth()- Depth_Keller msg: 1730863557.79 epoch sec; seqNo:5440; depth 49.81 2024-11-06T03:25:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863557.80 epoch sec; seqNo:5440; 4061784.81 north; 594688.27 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:25:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863553.77 2024-11-06T03:25:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:57Z- waiting for 1730863557.77, time is 1730863557.80 2024-11-06T03:25:57Z- LcmTrn::updateTrn() >>>> pose time 1730863557.80, meas time 1730863556.25 <<<< 2024-11-06T03:25:57Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863557.80 2024-11-06T03:25:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863556.25 sec, ping # 0; waiting for INS data more recent than 1730863553.77... 2024-11-06T03:25:57Z- TerrainNav::motionUpdate() - currEstimate = (4061785, 594688.27, 49.81) 2024-11-06T03:25:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:25:57Z- TerrainNav:: SubcloudNIS is 0.40, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:25:57Z- TNavPF::Projecting Measurements 2024-11-06T03:25:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:25:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:25:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:25:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:25:57Z- TNavFilter::4 good beams. 2024-11-06T03:25:57Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:25:57Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:25:57Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:25:57Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:25:57Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:25:57Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:25:57Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:25:57Z- TerrainMapOctree: (vehN, vehE ) = (4061783.50, 594688.75). 2024-11-06T03:25:57Z- TerrainMapOctree: Min Distance = 69.81. 2024-11-06T03:25:57Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:25:58Z- Forcing Subcloud Comparison 2024-11-06T03:25:58Z- Weighting particles with subcloud comparison 2024-11-06T03:25:58Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:58Z- meanExpectedMeasDiff: 1.214231 delta_rms_squared: 1.609118 alpha: 0.987723 2024-11-06T03:25:58Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:58Z- meanExpectedMeasDiff: 0.153552 delta_rms_squared: 1.120826 alpha: 0.982469 2024-11-06T03:25:58Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:58Z- meanExpectedMeasDiff: 0.615245 delta_rms_squared: 1.314046 alpha: 0.985008 2024-11-06T03:25:58Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:25:58Z- meanExpectedMeasDiff: 0.329081 delta_rms_squared: 1.350905 alpha: 0.985411 2024-11-06T03:25:58Z- Subcloud NIS: 0.396428 2024-11-06T03:25:58Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:25:58Z- TNavPF:: sumWeights = 0.579700 2024-11-06T03:25:58Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:25:58Z- TNavPF::First Term of Map Covariance Matrix is 0.49 2024-11-06T03:25:58Z- TNavPF::Calculated NIS Value : 0.09 numBeams normalized NIS: 0.02 2024-11-06T03:25:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863556.25 sec. 2024-11-06T03:25:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:25:58Z- variances([0], [2], and psi_berg) = 0.560193 0.179610 0.000000 2024-11-06T03:25:58Z- publishEstimatesDecoder() - at 1730863557.80 sec 2024-11-06T03:25:58Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:25:58Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061813.47 594670.71 56.65 2024-11-06T03:25:58Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061813.95 594671.21 57.47 lat/lon: 36.70 -121.94 2024-11-06T03:25:58Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.56 0.18 0.69 0.00 2024-11-06T03:25:58Z- AHRS_M2 msg: 1730863558.25 epoch sec; seqNo:5112 2024-11-06T03:25:58Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.05 psi 2024-11-06T03:25:58Z- handleDepth()- Depth_Keller msg: 1730863558.26 epoch sec; seqNo:5441; depth 49.81 2024-11-06T03:25:58Z- handleDVL() - DVL msg epochMillisec: 1730863558.29 2024-11-06T03:25:58Z- handleDvl() - RDI_Pathfinder msg: 1730863558.29 sec; seqNo:1897; #beams:4 2024-11-06T03:25:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.23 , 35.51 , 38.68 , 38.12 2024-11-06T03:25:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , -0.02 2024-11-06T03:25:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863558.30 epoch sec; seqNo:5441; 4061785.18 north; 594688.26 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:25:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:25:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863557.80 2024-11-06T03:25:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:58Z- waiting for 1730863561.80, time is 1730863558.30 2024-11-06T03:25:58Z- AHRS_M2 msg: 1730863558.59 epoch sec; seqNo:5113 2024-11-06T03:25:58Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.05 psi 2024-11-06T03:25:58Z- handleDepth()- Depth_Keller msg: 1730863558.59 epoch sec; seqNo:5442; depth 49.82 2024-11-06T03:25:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863558.61 epoch sec; seqNo:5442; 4061785.56 north; 594688.26 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:25:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863557.80 2024-11-06T03:25:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:58Z- waiting for 1730863561.80, time is 1730863558.61 2024-11-06T03:25:58Z- AHRS_M2 msg: 1730863558.99 epoch sec; seqNo:5114 2024-11-06T03:25:58Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.05 psi 2024-11-06T03:25:58Z- handleDepth()- Depth_Keller msg: 1730863558.99 epoch sec; seqNo:5443; depth 49.79 2024-11-06T03:25:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863559.01 epoch sec; seqNo:5443; 4061785.94 north; 594688.25 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:25:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863557.80 2024-11-06T03:25:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:59Z- waiting for 1730863561.80, time is 1730863559.01 2024-11-06T03:25:59Z- handleDepth()- Depth_Keller msg: 1730863559.41 epoch sec; seqNo:5444; depth 49.81 2024-11-06T03:25:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863559.43 epoch sec; seqNo:5444; 4061786.32 north; 594688.25 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:25:59Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:25:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863557.80 2024-11-06T03:25:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:59Z- waiting for 1730863561.80, time is 1730863559.43 2024-11-06T03:25:59Z- AHRS_M2 msg: 1730863559.80 epoch sec; seqNo:5115 2024-11-06T03:25:59Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.06 psi 2024-11-06T03:25:59Z- handleDepth()- Depth_Keller msg: 1730863559.81 epoch sec; seqNo:5445; depth 49.82 2024-11-06T03:25:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863559.83 epoch sec; seqNo:5445; 4061786.70 north; 594688.24 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:25:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:25:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863557.80 2024-11-06T03:25:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:25:59Z- waiting for 1730863561.80, time is 1730863559.83 2024-11-06T03:26:00Z- AHRS_M2 msg: 1730863560.25 epoch sec; seqNo:5116 2024-11-06T03:26:00Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.06 psi 2024-11-06T03:26:00Z- handleDepth()- Depth_Keller msg: 1730863560.26 epoch sec; seqNo:5446; depth 49.81 2024-11-06T03:26:00Z- handleDVL() - DVL msg epochMillisec: 1730863560.30 2024-11-06T03:26:00Z- handleDvl() - RDI_Pathfinder msg: 1730863560.30 sec; seqNo:1898; #beams:4 2024-11-06T03:26:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.23 , 35.51 , 38.68 , 38.12 2024-11-06T03:26:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.02 2024-11-06T03:26:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863560.31 epoch sec; seqNo:5446; 4061787.08 north; 594688.24 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:26:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863557.80 2024-11-06T03:26:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:00Z- waiting for 1730863561.80, time is 1730863560.31 2024-11-06T03:26:00Z- AHRS_M2 msg: 1730863560.61 epoch sec; seqNo:5117 2024-11-06T03:26:00Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.06 psi 2024-11-06T03:26:00Z- handleDepth()- Depth_Keller msg: 1730863560.61 epoch sec; seqNo:5447; depth 49.79 2024-11-06T03:26:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863560.64 epoch sec; seqNo:5447; 4061787.46 north; 594688.24 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:26:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863557.80 2024-11-06T03:26:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:00Z- waiting for 1730863561.80, time is 1730863560.64 2024-11-06T03:26:01Z- AHRS_M2 msg: 1730863561.01 epoch sec; seqNo:5118 2024-11-06T03:26:01Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.06 psi 2024-11-06T03:26:01Z- handleDepth()- Depth_Keller msg: 1730863561.02 epoch sec; seqNo:5448; depth 49.80 2024-11-06T03:26:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863561.04 epoch sec; seqNo:5448; 4061787.44 north; 594687.02 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:26:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863557.80 2024-11-06T03:26:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:01Z- waiting for 1730863561.80, time is 1730863561.04 2024-11-06T03:26:01Z- AHRS_M2 msg: 1730863561.42 epoch sec; seqNo:5119 2024-11-06T03:26:01Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.06 psi 2024-11-06T03:26:01Z- handleDepth()- Depth_Keller msg: 1730863561.42 epoch sec; seqNo:5449; depth 49.81 2024-11-06T03:26:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863561.43 epoch sec; seqNo:5449; 4061787.82 north; 594687.01 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:26:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863557.80 2024-11-06T03:26:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:01Z- waiting for 1730863561.80, time is 1730863561.43 2024-11-06T03:26:01Z- AHRS_M2 msg: 1730863561.82 epoch sec; seqNo:5120 2024-11-06T03:26:01Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.06 psi 2024-11-06T03:26:01Z- handleDepth()- Depth_Keller msg: 1730863561.83 epoch sec; seqNo:5450; depth 49.79 2024-11-06T03:26:01Z- handleDVL() - DVL msg epochMillisec: 1730863561.83 2024-11-06T03:26:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:26:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:26:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:26:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:26:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:26:01Z- handleDvl() - RDI_Pathfinder msg: 1730863561.83 sec; seqNo:1899; #beams:0 2024-11-06T03:26:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:26:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.02 2024-11-06T03:26:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863561.84 epoch sec; seqNo:5450; 4061788.20 north; 594687.01 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:26:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863557.80 2024-11-06T03:26:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:01Z- waiting for 1730863561.80, time is 1730863561.84 2024-11-06T03:26:01Z- LcmTrn::updateTrn() >>>> pose time 1730863561.84, meas time 1730863561.83 <<<< 2024-11-06T03:26:01Z- LcmTrn::updateTrn() equating pose 1730863561.83 and meas times 1730863561.83 2024-11-06T03:26:01Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863561.83 2024-11-06T03:26:01Z- TerrainNav::motionUpdate() - currEstimate = (4061788, 594687.01, 49.79) 2024-11-06T03:26:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:26:01Z- TerrainNav:: SubcloudNIS is 0.40, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:26:01Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:26:01Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:26:02Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:26:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:26:02Z- variances([0], [2], and psi_berg) = 0.560520 0.182367 0.000000 2024-11-06T03:26:02Z- publishEstimatesDecoder() - at 1730863561.83 sec 2024-11-06T03:26:02Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:26:02Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061816.83 594669.45 57.41 2024-11-06T03:26:02Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061817.34 594669.95 57.45 lat/lon: 36.70 -121.94 2024-11-06T03:26:02Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.56 0.18 0.92 0.00 2024-11-06T03:26:02Z- AHRS_M2 msg: 1730863562.24 epoch sec; seqNo:5121 2024-11-06T03:26:02Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.06 psi 2024-11-06T03:26:02Z- handleDepth()- Depth_Keller msg: 1730863562.25 epoch sec; seqNo:5451; depth 49.81 2024-11-06T03:26:02Z- handleDVL() - DVL msg epochMillisec: 1730863562.27 2024-11-06T03:26:02Z- handleDvl() - RDI_Pathfinder msg: 1730863562.27 sec; seqNo:1900; #beams:4 2024-11-06T03:26:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.61 , 35.51 , 38.87 , 38.31 2024-11-06T03:26:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.08 , -0.02 2024-11-06T03:26:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863562.28 epoch sec; seqNo:5451; 4061788.58 north; 594687.00 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:26:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863561.83 2024-11-06T03:26:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:02Z- waiting for 1730863565.83, time is 1730863562.28 2024-11-06T03:26:02Z- AHRS_M2 msg: 1730863562.63 epoch sec; seqNo:5122 2024-11-06T03:26:02Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.07 psi 2024-11-06T03:26:02Z- handleDepth()- Depth_Keller msg: 1730863562.64 epoch sec; seqNo:5452; depth 49.79 2024-11-06T03:26:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863562.65 epoch sec; seqNo:5452; 4061788.96 north; 594687.00 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:26:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863561.83 2024-11-06T03:26:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:02Z- waiting for 1730863565.83, time is 1730863562.65 2024-11-06T03:26:03Z- handleDepth()- Depth_Keller msg: 1730863563.05 epoch sec; seqNo:5453; depth 49.78 2024-11-06T03:26:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863563.06 epoch sec; seqNo:5453; 4061789.34 north; 594687.00 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:26:03Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:26:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863561.83 2024-11-06T03:26:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:03Z- waiting for 1730863565.83, time is 1730863563.06 2024-11-06T03:26:03Z- AHRS_M2 msg: 1730863563.43 epoch sec; seqNo:5123 2024-11-06T03:26:03Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.07 psi 2024-11-06T03:26:03Z- handleDepth()- Depth_Keller msg: 1730863563.44 epoch sec; seqNo:5454; depth 49.79 2024-11-06T03:26:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863563.46 epoch sec; seqNo:5454; 4061789.72 north; 594686.99 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:26:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863561.83 2024-11-06T03:26:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:03Z- waiting for 1730863565.83, time is 1730863563.46 2024-11-06T03:26:03Z- handleDepth()- Depth_Keller msg: 1730863563.87 epoch sec; seqNo:5455; depth 49.79 2024-11-06T03:26:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863563.88 epoch sec; seqNo:5455; 4061790.10 north; 594686.99 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:26:04Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:26:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863561.83 2024-11-06T03:26:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:04Z- waiting for 1730863565.83, time is 1730863563.88 2024-11-06T03:26:04Z- AHRS_M2 msg: 1730863564.25 epoch sec; seqNo:5124 2024-11-06T03:26:04Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.07 psi 2024-11-06T03:26:04Z- handleDepth()- Depth_Keller msg: 1730863564.26 epoch sec; seqNo:5456; depth 49.79 2024-11-06T03:26:04Z- handleDVL() - DVL msg epochMillisec: 1730863564.28 2024-11-06T03:26:04Z- handleDvl() - RDI_Pathfinder msg: 1730863564.28 sec; seqNo:1901; #beams:4 2024-11-06T03:26:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.05 , 35.32 , 38.69 , 38.31 2024-11-06T03:26:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.08 , -0.03 2024-11-06T03:26:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863564.30 epoch sec; seqNo:5456; 4061790.47 north; 594686.98 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:26:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863561.83 2024-11-06T03:26:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:04Z- waiting for 1730863565.83, time is 1730863564.30 2024-11-06T03:26:04Z- AHRS_M2 msg: 1730863564.65 epoch sec; seqNo:5125 2024-11-06T03:26:04Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.07 psi 2024-11-06T03:26:04Z- handleDepth()- Depth_Keller msg: 1730863564.65 epoch sec; seqNo:5457; depth 49.79 2024-11-06T03:26:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863564.67 epoch sec; seqNo:5457; 4061790.85 north; 594686.98 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:26:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863561.83 2024-11-06T03:26:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:04Z- waiting for 1730863565.83, time is 1730863564.67 2024-11-06T03:26:05Z- AHRS_M2 msg: 1730863565.05 epoch sec; seqNo:5126 2024-11-06T03:26:05Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.07 psi 2024-11-06T03:26:05Z- handleDepth()- Depth_Keller msg: 1730863565.06 epoch sec; seqNo:5458; depth 49.79 2024-11-06T03:26:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863565.07 epoch sec; seqNo:5458; 4061791.23 north; 594686.97 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:26:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863561.83 2024-11-06T03:26:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:05Z- waiting for 1730863565.83, time is 1730863565.07 2024-11-06T03:26:05Z- AHRS_M2 msg: 1730863565.45 epoch sec; seqNo:5127 2024-11-06T03:26:05Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.07 psi 2024-11-06T03:26:05Z- handleDepth()- Depth_Keller msg: 1730863565.47 epoch sec; seqNo:5459; depth 49.80 2024-11-06T03:26:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863565.48 epoch sec; seqNo:5459; 4061791.60 north; 594685.75 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:26:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863561.83 2024-11-06T03:26:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:05Z- waiting for 1730863565.83, time is 1730863565.48 2024-11-06T03:26:05Z- AHRS_M2 msg: 1730863565.86 epoch sec; seqNo:5128 2024-11-06T03:26:05Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.07 psi 2024-11-06T03:26:05Z- handleDepth()- Depth_Keller msg: 1730863565.87 epoch sec; seqNo:5460; depth 49.79 2024-11-06T03:26:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863565.88 epoch sec; seqNo:5460; 4061791.98 north; 594685.75 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:26:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863561.83 2024-11-06T03:26:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:06Z- waiting for 1730863565.83, time is 1730863565.88 2024-11-06T03:26:06Z- LcmTrn::updateTrn() >>>> pose time 1730863565.88, meas time 1730863564.28 <<<< 2024-11-06T03:26:06Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863565.88 2024-11-06T03:26:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863564.28 sec, ping # 0; waiting for INS data more recent than 1730863561.83... 2024-11-06T03:26:06Z- TerrainNav::motionUpdate() - currEstimate = (4061792, 594685.75, 49.79) 2024-11-06T03:26:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:26:06Z- TerrainNav:: SubcloudNIS is 0.40, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:26:06Z- TNavPF::Projecting Measurements 2024-11-06T03:26:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:26:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:26:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:26:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:26:06Z- TNavFilter::4 good beams. 2024-11-06T03:26:06Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:26:06Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:26:06Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:26:06Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:26:06Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:26:06Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:26:06Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:26:06Z- TerrainMapOctree: (vehN, vehE ) = (4061790.49, 594686.24). 2024-11-06T03:26:06Z- TerrainMapOctree: Min Distance = 75.52. 2024-11-06T03:26:06Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:26:06Z- Forcing Subcloud Comparison 2024-11-06T03:26:06Z- Weighting particles with subcloud comparison 2024-11-06T03:26:06Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:06Z- meanExpectedMeasDiff: 0.839165 delta_rms_squared: 1.889188 alpha: 0.989524 2024-11-06T03:26:06Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:06Z- meanExpectedMeasDiff: -0.384547 delta_rms_squared: 1.331165 alpha: 0.985198 2024-11-06T03:26:06Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:06Z- meanExpectedMeasDiff: 0.247861 delta_rms_squared: 1.544209 alpha: 0.987214 2024-11-06T03:26:06Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:06Z- meanExpectedMeasDiff: 0.314547 delta_rms_squared: 1.604573 alpha: 0.987689 2024-11-06T03:26:06Z- Subcloud NIS: 0.394866 2024-11-06T03:26:06Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:26:06Z- TNavPF:: sumWeights = 0.569385 2024-11-06T03:26:06Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:26:06Z- TNavPF::First Term of Map Covariance Matrix is 0.48 2024-11-06T03:26:06Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.02 2024-11-06T03:26:06Z- TerrainNav::Resampling particle filter... 2024-11-06T03:26:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863564.28 sec. 2024-11-06T03:26:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:26:06Z- variances([0], [2], and psi_berg) = 0.569184 0.186856 0.000000 2024-11-06T03:26:06Z- publishEstimatesDecoder() - at 1730863565.88 sec 2024-11-06T03:26:06Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:26:06Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061822.59 594669.10 59.00 2024-11-06T03:26:06Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061821.12 594668.69 57.32 lat/lon: 36.70 -121.94 2024-11-06T03:26:06Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.19 0.70 0.00 2024-11-06T03:26:06Z- AHRS_M2 msg: 1730863566.27 epoch sec; seqNo:5129 2024-11-06T03:26:06Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.07 psi 2024-11-06T03:26:06Z- handleDepth()- Depth_Keller msg: 1730863566.29 epoch sec; seqNo:5461; depth 49.78 2024-11-06T03:26:06Z- handleDVL() - DVL msg epochMillisec: 1730863566.33 2024-11-06T03:26:06Z- handleDvl() - RDI_Pathfinder msg: 1730863566.33 sec; seqNo:1902; #beams:4 2024-11-06T03:26:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.23 , 35.51 , 38.68 , 38.12 2024-11-06T03:26:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.10 , -0.04 2024-11-06T03:26:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863566.34 epoch sec; seqNo:5461; 4061792.35 north; 594685.74 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:26:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863565.88 2024-11-06T03:26:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:06Z- waiting for 1730863569.88, time is 1730863566.34 2024-11-06T03:26:06Z- handleDepth()- Depth_Keller msg: 1730863566.69 epoch sec; seqNo:5462; depth 49.78 2024-11-06T03:26:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863566.70 epoch sec; seqNo:5462; 4061792.73 north; 594685.74 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:26:06Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:26:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863565.88 2024-11-06T03:26:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:06Z- waiting for 1730863569.88, time is 1730863566.70 2024-11-06T03:26:07Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:26:07Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:26:07Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:26:07Z- AHRS_M2 msg: 1730863567.06 epoch sec; seqNo:5130 2024-11-06T03:26:07Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.07 psi 2024-11-06T03:26:07Z- handleDepth()- Depth_Keller msg: 1730863567.09 epoch sec; seqNo:5463; depth 49.80 2024-11-06T03:26:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863567.10 epoch sec; seqNo:5463; 4061792.73 north; 594685.74 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:26:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863565.88 2024-11-06T03:26:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:07Z- waiting for 1730863569.88, time is 1730863567.10 2024-11-06T03:26:07Z- AHRS_M2 msg: 1730863567.47 epoch sec; seqNo:5131 2024-11-06T03:26:07Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.07 psi 2024-11-06T03:26:07Z- handleDepth()- Depth_Keller msg: 1730863567.48 epoch sec; seqNo:5464; depth 49.79 2024-11-06T03:26:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863567.49 epoch sec; seqNo:5464; 4061793.11 north; 594685.73 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:26:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863565.88 2024-11-06T03:26:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:07Z- waiting for 1730863569.88, time is 1730863567.49 2024-11-06T03:26:07Z- AHRS_M2 msg: 1730863567.88 epoch sec; seqNo:5132 2024-11-06T03:26:07Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.07 psi 2024-11-06T03:26:07Z- handleDepth()- Depth_Keller msg: 1730863567.88 epoch sec; seqNo:5465; depth 49.80 2024-11-06T03:26:07Z- handleDVL() - DVL msg epochMillisec: 1730863567.89 2024-11-06T03:26:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:26:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:26:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:26:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:26:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:26:07Z- handleDvl() - RDI_Pathfinder msg: 1730863567.89 sec; seqNo:1903; #beams:0 2024-11-06T03:26:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:26:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.10 , -0.04 2024-11-06T03:26:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863567.90 epoch sec; seqNo:5465; 4061793.49 north; 594685.73 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:26:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863565.88 2024-11-06T03:26:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:08Z- waiting for 1730863569.88, time is 1730863567.90 2024-11-06T03:26:08Z- AHRS_M2 msg: 1730863568.28 epoch sec; seqNo:5133 2024-11-06T03:26:08Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.08 psi 2024-11-06T03:26:08Z- handleDepth()- Depth_Keller msg: 1730863568.29 epoch sec; seqNo:5466; depth 49.80 2024-11-06T03:26:08Z- handleDVL() - DVL msg epochMillisec: 1730863568.32 2024-11-06T03:26:08Z- handleDvl() - RDI_Pathfinder msg: 1730863568.32 sec; seqNo:1904; #beams:4 2024-11-06T03:26:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.23 , 35.51 , 38.50 , 38.31 2024-11-06T03:26:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.11 , -0.04 2024-11-06T03:26:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863568.33 epoch sec; seqNo:5466; 4061793.87 north; 594685.72 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:26:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863565.88 2024-11-06T03:26:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:08Z- waiting for 1730863569.88, time is 1730863568.33 2024-11-06T03:26:08Z- AHRS_M2 msg: 1730863568.69 epoch sec; seqNo:5134 2024-11-06T03:26:08Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.08 psi 2024-11-06T03:26:08Z- handleDepth()- Depth_Keller msg: 1730863568.69 epoch sec; seqNo:5467; depth 49.82 2024-11-06T03:26:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863568.73 epoch sec; seqNo:5467; 4061794.25 north; 594685.72 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:26:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863565.88 2024-11-06T03:26:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:08Z- waiting for 1730863569.88, time is 1730863568.73 2024-11-06T03:26:09Z- AHRS_M2 msg: 1730863569.10 epoch sec; seqNo:5135 2024-11-06T03:26:09Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.08 psi 2024-11-06T03:26:09Z- handleDepth()- Depth_Keller msg: 1730863569.11 epoch sec; seqNo:5468; depth 49.81 2024-11-06T03:26:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863569.11 epoch sec; seqNo:5468; 4061794.63 north; 594685.72 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:26:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863565.88 2024-11-06T03:26:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:09Z- waiting for 1730863569.88, time is 1730863569.11 2024-11-06T03:26:09Z- AHRS_M2 msg: 1730863569.51 epoch sec; seqNo:5136 2024-11-06T03:26:09Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.07 psi 2024-11-06T03:26:09Z- handleDepth()- Depth_Keller msg: 1730863569.52 epoch sec; seqNo:5469; depth 49.83 2024-11-06T03:26:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863569.53 epoch sec; seqNo:5469; 4061795.01 north; 594685.71 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:26:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863565.88 2024-11-06T03:26:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:09Z- waiting for 1730863569.88, time is 1730863569.53 2024-11-06T03:26:09Z- AHRS_M2 msg: 1730863569.90 epoch sec; seqNo:5137 2024-11-06T03:26:09Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.07 psi 2024-11-06T03:26:09Z- handleDepth()- Depth_Keller msg: 1730863569.92 epoch sec; seqNo:5470; depth 49.84 2024-11-06T03:26:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863569.93 epoch sec; seqNo:5470; 4061795.37 north; 594684.49 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:26:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863565.88 2024-11-06T03:26:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:10Z- waiting for 1730863569.88, time is 1730863569.93 2024-11-06T03:26:10Z- LcmTrn::updateTrn() >>>> pose time 1730863569.93, meas time 1730863568.32 <<<< 2024-11-06T03:26:10Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863569.93 2024-11-06T03:26:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863568.32 sec, ping # 0; waiting for INS data more recent than 1730863565.88... 2024-11-06T03:26:10Z- TerrainNav::motionUpdate() - currEstimate = (4061795, 594684.49, 49.84) 2024-11-06T03:26:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:26:10Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:26:10Z- TNavPF::Projecting Measurements 2024-11-06T03:26:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:26:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:26:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:26:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:26:10Z- TNavFilter::4 good beams. 2024-11-06T03:26:10Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:26:10Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:26:10Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:26:10Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:26:10Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:26:10Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:26:10Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:26:10Z- TerrainMapOctree: (vehN, vehE ) = (4061794.02, 594684.99). 2024-11-06T03:26:10Z- TerrainMapOctree: Min Distance = 78.51. 2024-11-06T03:26:10Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:26:10Z- Forcing Subcloud Comparison 2024-11-06T03:26:10Z- Weighting particles with subcloud comparison 2024-11-06T03:26:10Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:10Z- meanExpectedMeasDiff: 1.011509 delta_rms_squared: 1.521042 alpha: 0.987022 2024-11-06T03:26:10Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:10Z- meanExpectedMeasDiff: -0.370754 delta_rms_squared: 1.081227 alpha: 0.981838 2024-11-06T03:26:10Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:10Z- meanExpectedMeasDiff: -0.057725 delta_rms_squared: 1.250143 alpha: 0.984254 2024-11-06T03:26:10Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:10Z- meanExpectedMeasDiff: 0.220156 delta_rms_squared: 1.297185 alpha: 0.984816 2024-11-06T03:26:10Z- Subcloud NIS: 0.386105 2024-11-06T03:26:10Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:26:10Z- TNavPF:: sumWeights = 0.588910 2024-11-06T03:26:10Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:26:10Z- TNavPF::First Term of Map Covariance Matrix is 0.49 2024-11-06T03:26:10Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T03:26:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863568.32 sec. 2024-11-06T03:26:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:26:10Z- variances([0], [2], and psi_berg) = 0.573176 0.187222 0.000000 2024-11-06T03:26:10Z- publishEstimatesDecoder() - at 1730863569.93 sec 2024-11-06T03:26:10Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:26:10Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061824.38 594667.09 56.40 2024-11-06T03:26:10Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061824.52 594667.44 57.27 lat/lon: 36.70 -121.94 2024-11-06T03:26:10Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.19 0.68 0.00 2024-11-06T03:26:10Z- AHRS_M2 msg: 1730863570.30 epoch sec; seqNo:5138 2024-11-06T03:26:10Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.07 psi 2024-11-06T03:26:10Z- handleDepth()- Depth_Keller msg: 1730863570.31 epoch sec; seqNo:5471; depth 49.85 2024-11-06T03:26:10Z- handleDVL() - DVL msg epochMillisec: 1730863570.35 2024-11-06T03:26:10Z- handleDvl() - RDI_Pathfinder msg: 1730863570.35 sec; seqNo:1905; #beams:4 2024-11-06T03:26:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.86 , 35.51 , 38.50 , 38.12 2024-11-06T03:26:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.12 , 0.00 2024-11-06T03:26:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863570.37 epoch sec; seqNo:5471; 4061795.75 north; 594684.48 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:26:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863569.93 2024-11-06T03:26:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:10Z- waiting for 1730863573.93, time is 1730863570.37 2024-11-06T03:26:10Z- AHRS_M2 msg: 1730863570.72 epoch sec; seqNo:5139 2024-11-06T03:26:10Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.07 psi 2024-11-06T03:26:10Z- handleDepth()- Depth_Keller msg: 1730863570.73 epoch sec; seqNo:5472; depth 49.85 2024-11-06T03:26:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863570.74 epoch sec; seqNo:5472; 4061796.13 north; 594684.48 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:26:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863569.93 2024-11-06T03:26:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:10Z- waiting for 1730863573.93, time is 1730863570.74 2024-11-06T03:26:11Z- AHRS_M2 msg: 1730863571.11 epoch sec; seqNo:5140 2024-11-06T03:26:11Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.07 psi 2024-11-06T03:26:11Z- handleDepth()- Depth_Keller msg: 1730863571.12 epoch sec; seqNo:5473; depth 49.88 2024-11-06T03:26:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863571.14 epoch sec; seqNo:5473; 4061796.51 north; 594684.47 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:26:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863569.93 2024-11-06T03:26:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:11Z- waiting for 1730863573.93, time is 1730863571.14 2024-11-06T03:26:11Z- handleDepth()- Depth_Keller msg: 1730863571.54 epoch sec; seqNo:5474; depth 49.87 2024-11-06T03:26:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863571.55 epoch sec; seqNo:5474; 4061796.89 north; 594684.47 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:26:11Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:26:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863569.93 2024-11-06T03:26:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:11Z- waiting for 1730863573.93, time is 1730863571.55 2024-11-06T03:26:11Z- AHRS_M2 msg: 1730863571.92 epoch sec; seqNo:5141 2024-11-06T03:26:11Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.08 psi 2024-11-06T03:26:11Z- handleDepth()- Depth_Keller msg: 1730863571.93 epoch sec; seqNo:5475; depth 49.88 2024-11-06T03:26:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863571.94 epoch sec; seqNo:5475; 4061797.27 north; 594684.47 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:26:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863569.93 2024-11-06T03:26:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:12Z- waiting for 1730863573.93, time is 1730863571.94 2024-11-06T03:26:12Z- AHRS_M2 msg: 1730863572.32 epoch sec; seqNo:5142 2024-11-06T03:26:12Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.08 psi 2024-11-06T03:26:12Z- handleDepth()- Depth_Keller msg: 1730863572.33 epoch sec; seqNo:5476; depth 49.90 2024-11-06T03:26:12Z- handleDVL() - DVL msg epochMillisec: 1730863572.36 2024-11-06T03:26:12Z- handleDvl() - RDI_Pathfinder msg: 1730863572.36 sec; seqNo:1906; #beams:4 2024-11-06T03:26:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.67 , 35.51 , 38.68 , 38.12 2024-11-06T03:26:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.06 , 0.04 2024-11-06T03:26:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863572.37 epoch sec; seqNo:5476; 4061797.65 north; 594684.46 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:26:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863569.93 2024-11-06T03:26:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:12Z- waiting for 1730863573.93, time is 1730863572.37 2024-11-06T03:26:12Z- AHRS_M2 msg: 1730863572.73 epoch sec; seqNo:5143 2024-11-06T03:26:12Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.08 psi 2024-11-06T03:26:12Z- handleDepth()- Depth_Keller msg: 1730863572.73 epoch sec; seqNo:5477; depth 49.91 2024-11-06T03:26:12Z- handleDVL() - DVL msg epochMillisec: 1730863572.73 2024-11-06T03:26:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:26:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:26:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:26:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:26:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:26:12Z- handleDvl() - RDI_Pathfinder msg: 1730863572.73 sec; seqNo:1907; #beams:0 2024-11-06T03:26:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:26:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.06 , 0.04 2024-11-06T03:26:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863572.75 epoch sec; seqNo:5477; 4061798.02 north; 594684.46 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:26:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863569.93 2024-11-06T03:26:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:12Z- waiting for 1730863573.93, time is 1730863572.75 2024-11-06T03:26:13Z- handleDepth()- Depth_Keller msg: 1730863573.15 epoch sec; seqNo:5478; depth 49.92 2024-11-06T03:26:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863573.18 epoch sec; seqNo:5478; 4061798.40 north; 594684.45 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:26:13Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:26:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863569.93 2024-11-06T03:26:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:13Z- waiting for 1730863573.93, time is 1730863573.18 2024-11-06T03:26:13Z- AHRS_M2 msg: 1730863573.54 epoch sec; seqNo:5144 2024-11-06T03:26:13Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.08 psi 2024-11-06T03:26:13Z- handleDepth()- Depth_Keller msg: 1730863573.54 epoch sec; seqNo:5479; depth 49.91 2024-11-06T03:26:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863573.55 epoch sec; seqNo:5479; 4061798.78 north; 594684.45 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:26:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863569.93 2024-11-06T03:26:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:13Z- waiting for 1730863573.93, time is 1730863573.55 2024-11-06T03:26:13Z- AHRS_M2 msg: 1730863573.94 epoch sec; seqNo:5145 2024-11-06T03:26:13Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.08 psi 2024-11-06T03:26:13Z- handleDepth()- Depth_Keller msg: 1730863573.94 epoch sec; seqNo:5480; depth 49.92 2024-11-06T03:26:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863573.95 epoch sec; seqNo:5480; 4061799.16 north; 594684.45 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:26:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863569.93 2024-11-06T03:26:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:14Z- waiting for 1730863573.93, time is 1730863573.95 2024-11-06T03:26:14Z- LcmTrn::updateTrn() >>>> pose time 1730863573.95, meas time 1730863572.73 <<<< 2024-11-06T03:26:14Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863573.95 2024-11-06T03:26:14Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:26:14Z- TerrainNav::motionUpdate() - currEstimate = (4061799, 594684.45, 49.92) 2024-11-06T03:26:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:26:14Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:26:14Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:26:14Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:26:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:26:14Z- variances([0], [2], and psi_berg) = 0.573907 0.189995 0.000000 2024-11-06T03:26:14Z- publishEstimatesDecoder() - at 1730863573.95 sec 2024-11-06T03:26:14Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:26:14Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061828.15 594667.06 56.04 2024-11-06T03:26:14Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061828.31 594667.40 57.36 lat/lon: 36.70 -121.94 2024-11-06T03:26:14Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.19 0.92 0.00 2024-11-06T03:26:14Z- AHRS_M2 msg: 1730863574.35 epoch sec; seqNo:5146 2024-11-06T03:26:14Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.09 psi 2024-11-06T03:26:14Z- handleDepth()- Depth_Keller msg: 1730863574.35 epoch sec; seqNo:5481; depth 49.93 2024-11-06T03:26:14Z- handleDVL() - DVL msg epochMillisec: 1730863574.38 2024-11-06T03:26:14Z- handleDvl() - RDI_Pathfinder msg: 1730863574.38 sec; seqNo:1908; #beams:4 2024-11-06T03:26:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.23 , 35.51 , 38.87 , 38.50 2024-11-06T03:26:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.02 2024-11-06T03:26:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863574.40 epoch sec; seqNo:5481; 4061799.53 north; 594683.22 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:26:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863573.95 2024-11-06T03:26:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:14Z- waiting for 1730863577.95, time is 1730863574.40 2024-11-06T03:26:14Z- AHRS_M2 msg: 1730863574.75 epoch sec; seqNo:5147 2024-11-06T03:26:14Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.09 psi 2024-11-06T03:26:14Z- handleDepth()- Depth_Keller msg: 1730863574.76 epoch sec; seqNo:5482; depth 49.93 2024-11-06T03:26:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863574.78 epoch sec; seqNo:5482; 4061799.90 north; 594683.22 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:26:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863573.95 2024-11-06T03:26:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:14Z- waiting for 1730863577.95, time is 1730863574.78 2024-11-06T03:26:15Z- handleDepth()- Depth_Keller msg: 1730863575.17 epoch sec; seqNo:5483; depth 49.93 2024-11-06T03:26:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863575.19 epoch sec; seqNo:5483; 4061800.28 north; 594683.21 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:26:15Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:26:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863573.95 2024-11-06T03:26:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:15Z- waiting for 1730863577.95, time is 1730863575.19 2024-11-06T03:26:15Z- AHRS_M2 msg: 1730863575.56 epoch sec; seqNo:5148 2024-11-06T03:26:15Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.09 psi 2024-11-06T03:26:15Z- handleDepth()- Depth_Keller msg: 1730863575.56 epoch sec; seqNo:5484; depth 49.92 2024-11-06T03:26:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863575.57 epoch sec; seqNo:5484; 4061800.66 north; 594683.21 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:26:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863573.95 2024-11-06T03:26:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:15Z- waiting for 1730863577.95, time is 1730863575.57 2024-11-06T03:26:15Z- AHRS_M2 msg: 1730863575.96 epoch sec; seqNo:5149 2024-11-06T03:26:15Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.09 psi 2024-11-06T03:26:15Z- handleDepth()- Depth_Keller msg: 1730863575.96 epoch sec; seqNo:5485; depth 49.93 2024-11-06T03:26:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863575.97 epoch sec; seqNo:5485; 4061800.66 north; 594683.21 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:26:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863573.95 2024-11-06T03:26:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:16Z- waiting for 1730863577.95, time is 1730863575.97 2024-11-06T03:26:16Z- AHRS_M2 msg: 1730863576.37 epoch sec; seqNo:5150 2024-11-06T03:26:16Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.09 psi 2024-11-06T03:26:16Z- handleDepth()- Depth_Keller msg: 1730863576.37 epoch sec; seqNo:5486; depth 49.92 2024-11-06T03:26:16Z- handleDVL() - DVL msg epochMillisec: 1730863576.39 2024-11-06T03:26:16Z- handleDvl() - RDI_Pathfinder msg: 1730863576.39 sec; seqNo:1909; #beams:4 2024-11-06T03:26:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.05 , 35.69 , 38.68 , 38.31 2024-11-06T03:26:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , -0.03 2024-11-06T03:26:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863576.41 epoch sec; seqNo:5486; 4061801.04 north; 594683.20 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:26:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863573.95 2024-11-06T03:26:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:16Z- waiting for 1730863577.95, time is 1730863576.41 2024-11-06T03:26:16Z- AHRS_M2 msg: 1730863576.77 epoch sec; seqNo:5151 2024-11-06T03:26:16Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.09 psi 2024-11-06T03:26:16Z- handleDepth()- Depth_Keller msg: 1730863576.78 epoch sec; seqNo:5487; depth 49.92 2024-11-06T03:26:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863576.80 epoch sec; seqNo:5487; 4061801.42 north; 594683.20 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:26:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863573.95 2024-11-06T03:26:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:16Z- waiting for 1730863577.95, time is 1730863576.80 2024-11-06T03:26:17Z- AHRS_M2 msg: 1730863577.17 epoch sec; seqNo:5152 2024-11-06T03:26:17Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.08 psi 2024-11-06T03:26:17Z- handleDepth()- Depth_Keller msg: 1730863577.18 epoch sec; seqNo:5488; depth 49.92 2024-11-06T03:26:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863577.20 epoch sec; seqNo:5488; 4061801.80 north; 594683.20 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:26:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863573.95 2024-11-06T03:26:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:17Z- waiting for 1730863577.95, time is 1730863577.20 2024-11-06T03:26:17Z- AHRS_M2 msg: 1730863577.58 epoch sec; seqNo:5153 2024-11-06T03:26:17Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.08 psi 2024-11-06T03:26:17Z- handleDepth()- Depth_Keller msg: 1730863577.58 epoch sec; seqNo:5489; depth 49.91 2024-11-06T03:26:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863577.59 epoch sec; seqNo:5489; 4061802.18 north; 594683.19 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:26:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863573.95 2024-11-06T03:26:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:17Z- waiting for 1730863577.95, time is 1730863577.59 2024-11-06T03:26:17Z- AHRS_M2 msg: 1730863577.98 epoch sec; seqNo:5154 2024-11-06T03:26:17Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.08 psi 2024-11-06T03:26:17Z- handleDepth()- Depth_Keller msg: 1730863577.98 epoch sec; seqNo:5490; depth 49.90 2024-11-06T03:26:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863577.99 epoch sec; seqNo:5490; 4061802.56 north; 594683.19 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:26:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863573.95 2024-11-06T03:26:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:18Z- waiting for 1730863577.95, time is 1730863577.99 2024-11-06T03:26:18Z- LcmTrn::updateTrn() >>>> pose time 1730863577.99, meas time 1730863576.39 <<<< 2024-11-06T03:26:18Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863577.99 2024-11-06T03:26:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863576.39 sec, ping # 0; waiting for INS data more recent than 1730863573.95... 2024-11-06T03:26:18Z- TerrainNav::motionUpdate() - currEstimate = (4061803, 594683.19, 49.90) 2024-11-06T03:26:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:26:18Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:26:18Z- TNavPF::Projecting Measurements 2024-11-06T03:26:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:26:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:26:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:26:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:26:18Z- TNavFilter::4 good beams. 2024-11-06T03:26:18Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:26:18Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:26:18Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:26:18Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:26:18Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:26:18Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:26:18Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:26:18Z- TerrainMapOctree: (vehN, vehE ) = (4061801.21, 594683.69). 2024-11-06T03:26:18Z- TerrainMapOctree: Min Distance = 83.79. 2024-11-06T03:26:18Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:26:18Z- Forcing Subcloud Comparison 2024-11-06T03:26:18Z- Weighting particles with subcloud comparison 2024-11-06T03:26:18Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:18Z- meanExpectedMeasDiff: 1.161107 delta_rms_squared: 1.889711 alpha: 0.989527 2024-11-06T03:26:18Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:18Z- meanExpectedMeasDiff: -0.343426 delta_rms_squared: 1.379290 alpha: 0.985707 2024-11-06T03:26:18Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:18Z- meanExpectedMeasDiff: 0.115957 delta_rms_squared: 1.581257 alpha: 0.987510 2024-11-06T03:26:18Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:18Z- meanExpectedMeasDiff: 0.337976 delta_rms_squared: 1.629314 alpha: 0.987874 2024-11-06T03:26:18Z- Subcloud NIS: 0.388234 2024-11-06T03:26:18Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:26:18Z- TNavPF:: sumWeights = 0.539736 2024-11-06T03:26:18Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:26:18Z- TNavPF::First Term of Map Covariance Matrix is 0.51 2024-11-06T03:26:18Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T03:26:18Z- TerrainNav::Resampling particle filter... 2024-11-06T03:26:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863576.39 sec. 2024-11-06T03:26:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:26:18Z- variances([0], [2], and psi_berg) = 0.581923 0.193723 0.000000 2024-11-06T03:26:18Z- publishEstimatesDecoder() - at 1730863577.99 sec 2024-11-06T03:26:18Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:26:18Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061833.08 594666.40 57.34 2024-11-06T03:26:18Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061831.71 594666.14 57.17 lat/lon: 36.70 -121.94 2024-11-06T03:26:18Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.58 0.19 0.72 0.00 2024-11-06T03:26:18Z- handleDepth()- Depth_Keller msg: 1730863578.40 epoch sec; seqNo:5491; depth 49.90 2024-11-06T03:26:18Z- handleDVL() - DVL msg epochMillisec: 1730863578.42 2024-11-06T03:26:18Z- handleDvl() - RDI_Pathfinder msg: 1730863578.42 sec; seqNo:1910; #beams:4 2024-11-06T03:26:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.49 , 35.88 , 38.50 , 38.12 2024-11-06T03:26:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.09 , -0.05 2024-11-06T03:26:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863578.43 epoch sec; seqNo:5491; 4061802.94 north; 594683.18 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:26:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863577.99 2024-11-06T03:26:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:18Z- waiting for 1730863581.99, time is 1730863578.43 2024-11-06T03:26:18Z- AHRS_M2 msg: 1730863578.79 epoch sec; seqNo:5155 2024-11-06T03:26:18Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 6.08 psi 2024-11-06T03:26:18Z- handleDepth()- Depth_Keller msg: 1730863578.79 epoch sec; seqNo:5492; depth 49.89 2024-11-06T03:26:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863578.81 epoch sec; seqNo:5492; 4061803.31 north; 594683.18 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:26:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863577.99 2024-11-06T03:26:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:18Z- waiting for 1730863581.99, time is 1730863578.81 2024-11-06T03:26:19Z- AHRS_M2 msg: 1730863579.19 epoch sec; seqNo:5156 2024-11-06T03:26:19Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 6.08 psi 2024-11-06T03:26:19Z- handleDepth()- Depth_Keller msg: 1730863579.20 epoch sec; seqNo:5493; depth 49.89 2024-11-06T03:26:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863579.21 epoch sec; seqNo:5493; 4061803.69 north; 594683.17 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:26:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863577.99 2024-11-06T03:26:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:19Z- waiting for 1730863581.99, time is 1730863579.21 2024-11-06T03:26:19Z- AHRS_M2 msg: 1730863579.60 epoch sec; seqNo:5157 2024-11-06T03:26:19Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 6.07 psi 2024-11-06T03:26:19Z- handleDepth()- Depth_Keller msg: 1730863579.60 epoch sec; seqNo:5494; depth 49.89 2024-11-06T03:26:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863579.61 epoch sec; seqNo:5494; 4061804.07 north; 594683.17 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:26:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863577.99 2024-11-06T03:26:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:19Z- waiting for 1730863581.99, time is 1730863579.61 2024-11-06T03:26:20Z- AHRS_M2 msg: 1730863580.00 epoch sec; seqNo:5158 2024-11-06T03:26:20Z- AHRS_M2 msg: 0.11 phi; -0.01 theta; 6.07 psi 2024-11-06T03:26:20Z- handleDepth()- Depth_Keller msg: 1730863580.02 epoch sec; seqNo:5495; depth 49.88 2024-11-06T03:26:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863580.02 epoch sec; seqNo:5495; 4061804.06 north; 594681.95 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:26:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863577.99 2024-11-06T03:26:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:20Z- waiting for 1730863581.99, time is 1730863580.02 2024-11-06T03:26:20Z- handleDepth()- Depth_Keller msg: 1730863580.42 epoch sec; seqNo:5496; depth 49.87 2024-11-06T03:26:20Z- handleDVL() - DVL msg epochMillisec: 1730863580.44 2024-11-06T03:26:20Z- handleDvl() - RDI_Pathfinder msg: 1730863580.44 sec; seqNo:1911; #beams:4 2024-11-06T03:26:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.93 , 36.07 , 38.50 , 38.12 2024-11-06T03:26:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.13 , -0.03 2024-11-06T03:26:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863580.46 epoch sec; seqNo:5496; 4061804.44 north; 594681.95 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:26:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863577.99 2024-11-06T03:26:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:20Z- waiting for 1730863581.99, time is 1730863580.46 2024-11-06T03:26:20Z- AHRS_M2 msg: 1730863580.81 epoch sec; seqNo:5159 2024-11-06T03:26:20Z- AHRS_M2 msg: 0.11 phi; -0.01 theta; 6.06 psi 2024-11-06T03:26:20Z- handleDepth()- Depth_Keller msg: 1730863580.82 epoch sec; seqNo:5497; depth 49.87 2024-11-06T03:26:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863580.83 epoch sec; seqNo:5497; 4061804.82 north; 594681.94 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:26:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863577.99 2024-11-06T03:26:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:20Z- waiting for 1730863581.99, time is 1730863580.83 2024-11-06T03:26:21Z- AHRS_M2 msg: 1730863581.21 epoch sec; seqNo:5160 2024-11-06T03:26:21Z- AHRS_M2 msg: 0.11 phi; -0.01 theta; 6.06 psi 2024-11-06T03:26:21Z- handleDepth()- Depth_Keller msg: 1730863581.22 epoch sec; seqNo:5498; depth 49.85 2024-11-06T03:26:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863581.23 epoch sec; seqNo:5498; 4061805.20 north; 594681.94 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:26:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863577.99 2024-11-06T03:26:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:21Z- waiting for 1730863581.99, time is 1730863581.23 2024-11-06T03:26:21Z- AHRS_M2 msg: 1730863581.62 epoch sec; seqNo:5161 2024-11-06T03:26:21Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 6.06 psi 2024-11-06T03:26:21Z- handleDepth()- Depth_Keller msg: 1730863581.64 epoch sec; seqNo:5499; depth 49.87 2024-11-06T03:26:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863581.65 epoch sec; seqNo:5499; 4061805.57 north; 594681.93 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:26:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863577.99 2024-11-06T03:26:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:21Z- waiting for 1730863581.99, time is 1730863581.65 2024-11-06T03:26:22Z- AHRS_M2 msg: 1730863582.02 epoch sec; seqNo:5162 2024-11-06T03:26:22Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 6.06 psi 2024-11-06T03:26:22Z- handleDepth()- Depth_Keller msg: 1730863582.03 epoch sec; seqNo:5500; depth 49.87 2024-11-06T03:26:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863582.04 epoch sec; seqNo:5500; 4061805.95 north; 594681.93 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:26:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863577.99 2024-11-06T03:26:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:22Z- waiting for 1730863581.99, time is 1730863582.04 2024-11-06T03:26:22Z- LcmTrn::updateTrn() >>>> pose time 1730863582.04, meas time 1730863580.44 <<<< 2024-11-06T03:26:22Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863582.04 2024-11-06T03:26:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863580.44 sec, ping # 0; waiting for INS data more recent than 1730863577.99... 2024-11-06T03:26:22Z- TerrainNav::motionUpdate() - currEstimate = (4061806, 594681.93, 49.87) 2024-11-06T03:26:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:26:22Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:26:22Z- TNavPF::Projecting Measurements 2024-11-06T03:26:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:26:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:26:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:26:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:26:22Z- TNavFilter::4 good beams. 2024-11-06T03:26:22Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:26:22Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:26:22Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:26:22Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:26:22Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:26:22Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:26:22Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:26:22Z- TerrainMapOctree: (vehN, vehE ) = (4061804.61, 594682.43). 2024-11-06T03:26:22Z- TerrainMapOctree: Min Distance = 86.88. 2024-11-06T03:26:22Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:26:22Z- Forcing Subcloud Comparison 2024-11-06T03:26:22Z- Weighting particles with subcloud comparison 2024-11-06T03:26:22Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:22Z- meanExpectedMeasDiff: 0.351205 delta_rms_squared: 1.427166 alpha: 0.986180 2024-11-06T03:26:22Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:22Z- meanExpectedMeasDiff: -0.377564 delta_rms_squared: 1.137675 alpha: 0.982724 2024-11-06T03:26:22Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:22Z- meanExpectedMeasDiff: -0.250918 delta_rms_squared: 1.287329 alpha: 0.984702 2024-11-06T03:26:22Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:22Z- meanExpectedMeasDiff: 0.051030 delta_rms_squared: 1.313252 alpha: 0.984999 2024-11-06T03:26:22Z- Subcloud NIS: 0.384860 2024-11-06T03:26:22Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:26:22Z- TNavPF:: sumWeights = 0.637263 2024-11-06T03:26:22Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:26:22Z- TNavPF::First Term of Map Covariance Matrix is 0.44 2024-11-06T03:26:22Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T03:26:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863580.44 sec. 2024-11-06T03:26:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:26:22Z- variances([0], [2], and psi_berg) = 0.555899 0.193251 0.000000 2024-11-06T03:26:22Z- publishEstimatesDecoder() - at 1730863582.04 sec 2024-11-06T03:26:22Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:26:22Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061835.69 594664.99 57.10 2024-11-06T03:26:22Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061835.10 594664.89 57.18 lat/lon: 36.70 -121.94 2024-11-06T03:26:22Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.56 0.19 0.69 0.00 2024-11-06T03:26:22Z- AHRS_M2 msg: 1730863582.42 epoch sec; seqNo:5163 2024-11-06T03:26:22Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 6.06 psi 2024-11-06T03:26:22Z- handleDepth()- Depth_Keller msg: 1730863582.43 epoch sec; seqNo:5501; depth 49.87 2024-11-06T03:26:22Z- handleDVL() - DVL msg epochMillisec: 1730863582.45 2024-11-06T03:26:22Z- handleDvl() - RDI_Pathfinder msg: 1730863582.45 sec; seqNo:1912; #beams:4 2024-11-06T03:26:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.93 , 36.07 , 38.68 , 38.12 2024-11-06T03:26:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.11 , 0.01 2024-11-06T03:26:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863582.48 epoch sec; seqNo:5501; 4061806.33 north; 594681.93 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:26:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863582.04 2024-11-06T03:26:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:22Z- waiting for 1730863586.04, time is 1730863582.48 2024-11-06T03:26:22Z- AHRS_M2 msg: 1730863582.83 epoch sec; seqNo:5164 2024-11-06T03:26:22Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 6.05 psi 2024-11-06T03:26:22Z- handleDepth()- Depth_Keller msg: 1730863582.84 epoch sec; seqNo:5502; depth 49.86 2024-11-06T03:26:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863582.85 epoch sec; seqNo:5502; 4061806.71 north; 594681.92 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:26:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863582.04 2024-11-06T03:26:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:22Z- waiting for 1730863586.04, time is 1730863582.85 2024-11-06T03:26:23Z- AHRS_M2 msg: 1730863583.23 epoch sec; seqNo:5165 2024-11-06T03:26:23Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 6.05 psi 2024-11-06T03:26:23Z- handleDepth()- Depth_Keller msg: 1730863583.24 epoch sec; seqNo:5503; depth 49.87 2024-11-06T03:26:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863583.25 epoch sec; seqNo:5503; 4061807.09 north; 594681.92 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:26:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863582.04 2024-11-06T03:26:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:23Z- waiting for 1730863586.04, time is 1730863583.25 2024-11-06T03:26:23Z- AHRS_M2 msg: 1730863583.67 epoch sec; seqNo:5166 2024-11-06T03:26:23Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 6.05 psi 2024-11-06T03:26:23Z- handleDepth()- Depth_Keller msg: 1730863583.67 epoch sec; seqNo:5504; depth 49.86 2024-11-06T03:26:23Z- handleDVL() - DVL msg epochMillisec: 1730863583.68 2024-11-06T03:26:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:26:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:26:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:26:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:26:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:26:23Z- handleDvl() - RDI_Pathfinder msg: 1730863583.68 sec; seqNo:1913; #beams:0 2024-11-06T03:26:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:26:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.11 , 0.01 2024-11-06T03:26:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863583.68 epoch sec; seqNo:5504; 4061807.45 north; 594680.69 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:26:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863582.04 2024-11-06T03:26:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:23Z- waiting for 1730863586.04, time is 1730863583.68 2024-11-06T03:26:24Z- AHRS_M2 msg: 1730863584.05 epoch sec; seqNo:5167 2024-11-06T03:26:24Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 6.05 psi 2024-11-06T03:26:24Z- handleDepth()- Depth_Keller msg: 1730863584.05 epoch sec; seqNo:5505; depth 49.87 2024-11-06T03:26:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863584.06 epoch sec; seqNo:5505; 4061807.83 north; 594680.69 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:26:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863582.04 2024-11-06T03:26:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:24Z- waiting for 1730863586.04, time is 1730863584.06 2024-11-06T03:26:24Z- handleDepth()- Depth_Keller msg: 1730863584.47 epoch sec; seqNo:5506; depth 49.88 2024-11-06T03:26:24Z- handleDVL() - DVL msg epochMillisec: 1730863584.49 2024-11-06T03:26:24Z- handleDvl() - RDI_Pathfinder msg: 1730863584.49 sec; seqNo:1914; #beams:4 2024-11-06T03:26:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.49 , 36.25 , 38.68 , 38.31 2024-11-06T03:26:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.07 , 0.02 2024-11-06T03:26:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863584.51 epoch sec; seqNo:5506; 4061808.21 north; 594680.68 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:26:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863582.04 2024-11-06T03:26:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:24Z- waiting for 1730863586.04, time is 1730863584.51 2024-11-06T03:26:24Z- AHRS_M2 msg: 1730863584.86 epoch sec; seqNo:5168 2024-11-06T03:26:24Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 6.05 psi 2024-11-06T03:26:24Z- handleDepth()- Depth_Keller msg: 1730863584.87 epoch sec; seqNo:5507; depth 49.88 2024-11-06T03:26:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863584.88 epoch sec; seqNo:5507; 4061808.59 north; 594680.68 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:26:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863582.04 2024-11-06T03:26:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:25Z- waiting for 1730863586.04, time is 1730863584.88 2024-11-06T03:26:25Z- AHRS_M2 msg: 1730863585.26 epoch sec; seqNo:5169 2024-11-06T03:26:25Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 6.05 psi 2024-11-06T03:26:25Z- handleDepth()- Depth_Keller msg: 1730863585.27 epoch sec; seqNo:5508; depth 49.89 2024-11-06T03:26:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863585.30 epoch sec; seqNo:5508; 4061808.97 north; 594680.68 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:26:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863582.04 2024-11-06T03:26:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:25Z- waiting for 1730863586.04, time is 1730863585.30 2024-11-06T03:26:25Z- AHRS_M2 msg: 1730863585.66 epoch sec; seqNo:5170 2024-11-06T03:26:25Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.05 psi 2024-11-06T03:26:25Z- handleDepth()- Depth_Keller msg: 1730863585.67 epoch sec; seqNo:5509; depth 49.88 2024-11-06T03:26:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863585.68 epoch sec; seqNo:5509; 4061809.35 north; 594680.67 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:26:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863582.04 2024-11-06T03:26:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:25Z- waiting for 1730863586.04, time is 1730863585.68 2024-11-06T03:26:26Z- AHRS_M2 msg: 1730863586.07 epoch sec; seqNo:5171 2024-11-06T03:26:26Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.05 psi 2024-11-06T03:26:26Z- handleDepth()- Depth_Keller msg: 1730863586.07 epoch sec; seqNo:5510; depth 49.89 2024-11-06T03:26:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863586.08 epoch sec; seqNo:5510; 4061809.73 north; 594680.67 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:26:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863582.04 2024-11-06T03:26:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:26Z- waiting for 1730863586.04, time is 1730863586.08 2024-11-06T03:26:26Z- LcmTrn::updateTrn() >>>> pose time 1730863586.08, meas time 1730863584.49 <<<< 2024-11-06T03:26:26Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863586.08 2024-11-06T03:26:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863584.49 sec, ping # 0; waiting for INS data more recent than 1730863582.04... 2024-11-06T03:26:26Z- TerrainNav::motionUpdate() - currEstimate = (4061810, 594680.67, 49.89) 2024-11-06T03:26:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:26:26Z- TerrainNav:: SubcloudNIS is 0.38, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:26:26Z- TNavPF::Projecting Measurements 2024-11-06T03:26:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:26:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:26:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:26:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:26:26Z- TNavFilter::4 good beams. 2024-11-06T03:26:26Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:26:26Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:26:26Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:26:26Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:26:26Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:26:26Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:26:26Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:26:26Z- TerrainMapOctree: (vehN, vehE ) = (4061808.25, 594681.16). 2024-11-06T03:26:26Z- TerrainMapOctree: Min Distance = 90.17. 2024-11-06T03:26:26Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:26:26Z- Forcing Subcloud Comparison 2024-11-06T03:26:26Z- Weighting particles with subcloud comparison 2024-11-06T03:26:26Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:26Z- meanExpectedMeasDiff: 1.975146 delta_rms_squared: 1.447773 alpha: 0.986374 2024-11-06T03:26:26Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:26Z- meanExpectedMeasDiff: -0.107442 delta_rms_squared: 1.097123 alpha: 0.982097 2024-11-06T03:26:26Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:26Z- meanExpectedMeasDiff: -0.084141 delta_rms_squared: 1.248530 alpha: 0.984234 2024-11-06T03:26:26Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:26Z- meanExpectedMeasDiff: -0.244634 delta_rms_squared: 0.911170 alpha: 0.978522 2024-11-06T03:26:26Z- Subcloud NIS: 0.389213 2024-11-06T03:26:26Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:26:26Z- TNavPF:: sumWeights = 0.456516 2024-11-06T03:26:26Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:26:26Z- TNavPF::First Term of Map Covariance Matrix is 0.73 2024-11-06T03:26:26Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.05 2024-11-06T03:26:26Z- TerrainNav::Resampling particle filter... 2024-11-06T03:26:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863584.49 sec. 2024-11-06T03:26:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:26:26Z- variances([0], [2], and psi_berg) = 0.528399 0.197727 0.000000 2024-11-06T03:26:26Z- publishEstimatesDecoder() - at 1730863586.08 sec 2024-11-06T03:26:26Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:26:26Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061840.25 594663.72 57.81 2024-11-06T03:26:26Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061838.85 594663.62 57.00 lat/lon: 36.70 -121.94 2024-11-06T03:26:26Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.53 0.20 0.70 0.00 2024-11-06T03:26:26Z- AHRS_M2 msg: 1730863586.47 epoch sec; seqNo:5172 2024-11-06T03:26:26Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.04 psi 2024-11-06T03:26:26Z- handleDepth()- Depth_Keller msg: 1730863586.48 epoch sec; seqNo:5511; depth 49.89 2024-11-06T03:26:26Z- handleDVL() - DVL msg epochMillisec: 1730863586.50 2024-11-06T03:26:26Z- handleDvl() - RDI_Pathfinder msg: 1730863586.50 sec; seqNo:1915; #beams:4 2024-11-06T03:26:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.49 , 36.07 , 38.68 , 38.12 2024-11-06T03:26:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.07 , -0.00 2024-11-06T03:26:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863586.53 epoch sec; seqNo:5511; 4061810.11 north; 594680.66 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:26:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863586.08 2024-11-06T03:26:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:26Z- waiting for 1730863590.08, time is 1730863586.53 2024-11-06T03:26:26Z- AHRS_M2 msg: 1730863586.88 epoch sec; seqNo:5173 2024-11-06T03:26:26Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.04 psi 2024-11-06T03:26:26Z- handleDepth()- Depth_Keller msg: 1730863586.88 epoch sec; seqNo:5512; depth 49.89 2024-11-06T03:26:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863586.90 epoch sec; seqNo:5512; 4061810.49 north; 594680.66 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:26:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863586.08 2024-11-06T03:26:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:27Z- waiting for 1730863590.08, time is 1730863586.90 2024-11-06T03:26:27Z- AHRS_M2 msg: 1730863587.28 epoch sec; seqNo:5174 2024-11-06T03:26:27Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.04 psi 2024-11-06T03:26:27Z- handleDepth()- Depth_Keller msg: 1730863587.28 epoch sec; seqNo:5513; depth 49.90 2024-11-06T03:26:27Z- handleDVL() - DVL msg epochMillisec: 1730863587.30 2024-11-06T03:26:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:26:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:26:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:26:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:26:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:26:27Z- handleDvl() - RDI_Pathfinder msg: 1730863587.30 sec; seqNo:1916; #beams:0 2024-11-06T03:26:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:26:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.07 , -0.00 2024-11-06T03:26:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863587.31 epoch sec; seqNo:5513; 4061810.86 north; 594680.65 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:26:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863586.08 2024-11-06T03:26:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:27Z- waiting for 1730863590.08, time is 1730863587.31 2024-11-06T03:26:27Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:26:27Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:26:27Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:26:27Z- AHRS_M2 msg: 1730863587.67 epoch sec; seqNo:5175 2024-11-06T03:26:27Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.04 psi 2024-11-06T03:26:27Z- handleDepth()- Depth_Keller msg: 1730863587.71 epoch sec; seqNo:5514; depth 49.90 2024-11-06T03:26:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863587.72 epoch sec; seqNo:5514; 4061811.23 north; 594679.43 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:26:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863586.08 2024-11-06T03:26:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:27Z- waiting for 1730863590.08, time is 1730863587.72 2024-11-06T03:26:28Z- AHRS_M2 msg: 1730863588.09 epoch sec; seqNo:5176 2024-11-06T03:26:28Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.04 psi 2024-11-06T03:26:28Z- handleDepth()- Depth_Keller msg: 1730863588.11 epoch sec; seqNo:5515; depth 49.91 2024-11-06T03:26:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863588.12 epoch sec; seqNo:5515; 4061811.61 north; 594679.43 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:26:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863586.08 2024-11-06T03:26:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:28Z- waiting for 1730863590.08, time is 1730863588.12 2024-11-06T03:26:28Z- AHRS_M2 msg: 1730863588.50 epoch sec; seqNo:5177 2024-11-06T03:26:28Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.04 psi 2024-11-06T03:26:28Z- handleDepth()- Depth_Keller msg: 1730863588.51 epoch sec; seqNo:5516; depth 49.90 2024-11-06T03:26:28Z- handleDVL() - DVL msg epochMillisec: 1730863588.53 2024-11-06T03:26:28Z- handleDvl() - RDI_Pathfinder msg: 1730863588.53 sec; seqNo:1917; #beams:4 2024-11-06T03:26:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.67 , 35.88 , 38.68 , 38.50 2024-11-06T03:26:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.07 , -0.01 2024-11-06T03:26:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863588.55 epoch sec; seqNo:5516; 4061811.99 north; 594679.42 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:26:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863586.08 2024-11-06T03:26:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:28Z- waiting for 1730863590.08, time is 1730863588.55 2024-11-06T03:26:28Z- AHRS_M2 msg: 1730863588.90 epoch sec; seqNo:5178 2024-11-06T03:26:28Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.04 psi 2024-11-06T03:26:28Z- handleDepth()- Depth_Keller msg: 1730863588.90 epoch sec; seqNo:5517; depth 49.90 2024-11-06T03:26:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863588.94 epoch sec; seqNo:5517; 4061812.37 north; 594679.42 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:26:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863586.08 2024-11-06T03:26:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:29Z- waiting for 1730863590.08, time is 1730863588.94 2024-11-06T03:26:29Z- AHRS_M2 msg: 1730863589.30 epoch sec; seqNo:5179 2024-11-06T03:26:29Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 6.03 psi 2024-11-06T03:26:29Z- handleDepth()- Depth_Keller msg: 1730863589.30 epoch sec; seqNo:5518; depth 49.91 2024-11-06T03:26:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863589.31 epoch sec; seqNo:5518; 4061812.37 north; 594679.42 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:26:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863586.08 2024-11-06T03:26:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:29Z- waiting for 1730863590.08, time is 1730863589.31 2024-11-06T03:26:29Z- AHRS_M2 msg: 1730863589.70 epoch sec; seqNo:5180 2024-11-06T03:26:29Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 6.03 psi 2024-11-06T03:26:29Z- handleDepth()- Depth_Keller msg: 1730863589.71 epoch sec; seqNo:5519; depth 49.91 2024-11-06T03:26:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863589.72 epoch sec; seqNo:5519; 4061812.75 north; 594679.41 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:26:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863586.08 2024-11-06T03:26:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:29Z- waiting for 1730863590.08, time is 1730863589.72 2024-11-06T03:26:30Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:26:30Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:26:30Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:26:30Z- AHRS_M2 msg: 1730863590.10 epoch sec; seqNo:5181 2024-11-06T03:26:30Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 6.03 psi 2024-11-06T03:26:30Z- handleDepth()- Depth_Keller msg: 1730863590.13 epoch sec; seqNo:5520; depth 49.91 2024-11-06T03:26:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863590.14 epoch sec; seqNo:5520; 4061813.12 north; 594679.41 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:26:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863586.08 2024-11-06T03:26:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:30Z- waiting for 1730863590.08, time is 1730863590.14 2024-11-06T03:26:30Z- LcmTrn::updateTrn() >>>> pose time 1730863590.14, meas time 1730863588.53 <<<< 2024-11-06T03:26:30Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863590.14 2024-11-06T03:26:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863588.53 sec, ping # 0; waiting for INS data more recent than 1730863586.08... 2024-11-06T03:26:30Z- TerrainNav::motionUpdate() - currEstimate = (4061813, 594679.41, 49.91) 2024-11-06T03:26:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:26:30Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:26:30Z- TNavPF::Projecting Measurements 2024-11-06T03:26:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:26:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:26:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:26:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:26:30Z- TNavFilter::4 good beams. 2024-11-06T03:26:30Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:26:30Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:26:30Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:26:30Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:26:30Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:26:30Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:26:30Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:26:30Z- TerrainMapOctree: (vehN, vehE ) = (4061811.78, 594679.91). 2024-11-06T03:26:30Z- TerrainMapOctree: Min Distance = 93.43. 2024-11-06T03:26:30Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:26:30Z- Forcing Subcloud Comparison 2024-11-06T03:26:30Z- Weighting particles with subcloud comparison 2024-11-06T03:26:30Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:30Z- meanExpectedMeasDiff: 1.986502 delta_rms_squared: 1.402136 alpha: 0.985937 2024-11-06T03:26:30Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:30Z- meanExpectedMeasDiff: -0.595472 delta_rms_squared: 1.104627 alpha: 0.982216 2024-11-06T03:26:30Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:30Z- meanExpectedMeasDiff: -0.228208 delta_rms_squared: 1.244076 alpha: 0.984178 2024-11-06T03:26:30Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:30Z- meanExpectedMeasDiff: -1.008958 delta_rms_squared: 1.262826 alpha: 0.984409 2024-11-06T03:26:30Z- Subcloud NIS: 0.394850 2024-11-06T03:26:30Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:26:30Z- TNavPF:: sumWeights = 0.373591 2024-11-06T03:26:30Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:26:30Z- TNavPF::First Term of Map Covariance Matrix is 0.79 2024-11-06T03:26:30Z- TNavPF::Calculated NIS Value : 0.21 numBeams normalized NIS: 0.05 2024-11-06T03:26:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863588.53 sec. 2024-11-06T03:26:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:26:30Z- variances([0], [2], and psi_berg) = 0.388706 0.186279 0.000000 2024-11-06T03:26:30Z- publishEstimatesDecoder() - at 1730863590.14 sec 2024-11-06T03:26:30Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:26:30Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061840.37 594662.66 56.46 2024-11-06T03:26:30Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061842.16 594662.39 57.03 lat/lon: 36.70 -121.94 2024-11-06T03:26:30Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.39 0.19 0.68 0.00 2024-11-06T03:26:30Z- AHRS_M2 msg: 1730863590.51 epoch sec; seqNo:5182 2024-11-06T03:26:30Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 6.03 psi 2024-11-06T03:26:30Z- handleDepth()- Depth_Keller msg: 1730863590.52 epoch sec; seqNo:5521; depth 49.92 2024-11-06T03:26:30Z- handleDVL() - DVL msg epochMillisec: 1730863590.54 2024-11-06T03:26:30Z- handleDvl() - RDI_Pathfinder msg: 1730863590.54 sec; seqNo:1918; #beams:4 2024-11-06T03:26:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.67 , 35.88 , 38.50 , 38.50 2024-11-06T03:26:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , -0.01 2024-11-06T03:26:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863590.56 epoch sec; seqNo:5521; 4061813.50 north; 594679.41 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:26:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863590.14 2024-11-06T03:26:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:30Z- waiting for 1730863594.14, time is 1730863590.56 2024-11-06T03:26:30Z- AHRS_M2 msg: 1730863590.93 epoch sec; seqNo:5183 2024-11-06T03:26:30Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 6.03 psi 2024-11-06T03:26:30Z- handleDepth()- Depth_Keller msg: 1730863590.94 epoch sec; seqNo:5522; depth 49.92 2024-11-06T03:26:30Z- handleDVL() - DVL msg epochMillisec: 1730863590.94 2024-11-06T03:26:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:26:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:26:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:26:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:26:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:26:30Z- handleDvl() - RDI_Pathfinder msg: 1730863590.94 sec; seqNo:1919; #beams:0 2024-11-06T03:26:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:26:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , -0.01 2024-11-06T03:26:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863590.96 epoch sec; seqNo:5522; 4061813.88 north; 594679.40 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:26:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863590.14 2024-11-06T03:26:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:31Z- waiting for 1730863594.14, time is 1730863590.96 2024-11-06T03:26:31Z- AHRS_M2 msg: 1730863591.32 epoch sec; seqNo:5184 2024-11-06T03:26:31Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 6.03 psi 2024-11-06T03:26:31Z- handleDepth()- Depth_Keller msg: 1730863591.33 epoch sec; seqNo:5523; depth 49.91 2024-11-06T03:26:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863591.33 epoch sec; seqNo:5523; 4061814.26 north; 594679.40 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:26:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863590.14 2024-11-06T03:26:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:31Z- waiting for 1730863594.14, time is 1730863591.33 2024-11-06T03:26:31Z- AHRS_M2 msg: 1730863591.72 epoch sec; seqNo:5185 2024-11-06T03:26:31Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 6.03 psi 2024-11-06T03:26:31Z- handleDepth()- Depth_Keller msg: 1730863591.73 epoch sec; seqNo:5524; depth 49.91 2024-11-06T03:26:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863591.74 epoch sec; seqNo:5524; 4061814.63 north; 594678.17 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:26:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863590.14 2024-11-06T03:26:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:31Z- waiting for 1730863594.14, time is 1730863591.74 2024-11-06T03:26:32Z- AHRS_M2 msg: 1730863592.13 epoch sec; seqNo:5186 2024-11-06T03:26:32Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 6.03 psi 2024-11-06T03:26:32Z- handleDepth()- Depth_Keller msg: 1730863592.13 epoch sec; seqNo:5525; depth 49.90 2024-11-06T03:26:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863592.15 epoch sec; seqNo:5525; 4061815.00 north; 594678.17 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:26:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863590.14 2024-11-06T03:26:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:32Z- waiting for 1730863594.14, time is 1730863592.15 2024-11-06T03:26:32Z- AHRS_M2 msg: 1730863592.53 epoch sec; seqNo:5187 2024-11-06T03:26:32Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.03 psi 2024-11-06T03:26:32Z- handleDepth()- Depth_Keller msg: 1730863592.54 epoch sec; seqNo:5526; depth 49.92 2024-11-06T03:26:32Z- handleDVL() - DVL msg epochMillisec: 1730863592.57 2024-11-06T03:26:32Z- handleDvl() - RDI_Pathfinder msg: 1730863592.57 sec; seqNo:1920; #beams:4 2024-11-06T03:26:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.86 , 35.88 , 38.50 , 38.50 2024-11-06T03:26:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , 0.01 2024-11-06T03:26:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863592.58 epoch sec; seqNo:5526; 4061815.38 north; 594678.16 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:26:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863590.14 2024-11-06T03:26:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:32Z- waiting for 1730863594.14, time is 1730863592.58 2024-11-06T03:26:32Z- AHRS_M2 msg: 1730863592.93 epoch sec; seqNo:5188 2024-11-06T03:26:32Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.03 psi 2024-11-06T03:26:32Z- handleDepth()- Depth_Keller msg: 1730863592.95 epoch sec; seqNo:5527; depth 49.91 2024-11-06T03:26:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863592.96 epoch sec; seqNo:5527; 4061815.76 north; 594678.16 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:26:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863590.14 2024-11-06T03:26:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:33Z- waiting for 1730863594.14, time is 1730863592.96 2024-11-06T03:26:33Z- AHRS_M2 msg: 1730863593.34 epoch sec; seqNo:5189 2024-11-06T03:26:33Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.03 psi 2024-11-06T03:26:33Z- handleDepth()- Depth_Keller msg: 1730863593.35 epoch sec; seqNo:5528; depth 49.92 2024-11-06T03:26:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863593.36 epoch sec; seqNo:5528; 4061816.14 north; 594678.16 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:26:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863590.14 2024-11-06T03:26:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:33Z- waiting for 1730863594.14, time is 1730863593.36 2024-11-06T03:26:33Z- AHRS_M2 msg: 1730863593.75 epoch sec; seqNo:5190 2024-11-06T03:26:33Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.03 psi 2024-11-06T03:26:33Z- handleDepth()- Depth_Keller msg: 1730863593.76 epoch sec; seqNo:5529; depth 49.91 2024-11-06T03:26:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863593.77 epoch sec; seqNo:5529; 4061816.52 north; 594678.15 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:26:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863590.14 2024-11-06T03:26:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:33Z- waiting for 1730863594.14, time is 1730863593.77 2024-11-06T03:26:34Z- AHRS_M2 msg: 1730863594.15 epoch sec; seqNo:5191 2024-11-06T03:26:34Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.03 psi 2024-11-06T03:26:34Z- handleDepth()- Depth_Keller msg: 1730863594.16 epoch sec; seqNo:5530; depth 49.92 2024-11-06T03:26:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863594.23 epoch sec; seqNo:5530; 4061816.90 north; 594678.15 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:26:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863590.14 2024-11-06T03:26:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:34Z- waiting for 1730863594.14, time is 1730863594.23 2024-11-06T03:26:34Z- LcmTrn::updateTrn() >>>> pose time 1730863594.23, meas time 1730863592.57 <<<< 2024-11-06T03:26:34Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863594.23 2024-11-06T03:26:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863592.57 sec, ping # 0; waiting for INS data more recent than 1730863590.14... 2024-11-06T03:26:34Z- TerrainNav::motionUpdate() - currEstimate = (4061817, 594678.15, 49.92) 2024-11-06T03:26:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:26:34Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:26:34Z- TNavPF::Projecting Measurements 2024-11-06T03:26:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:26:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:26:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:26:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:26:34Z- TNavFilter::4 good beams. 2024-11-06T03:26:34Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:26:34Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:26:34Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:26:34Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:26:34Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:26:34Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:26:34Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:26:34Z- TerrainMapOctree: (vehN, vehE ) = (4061815.36, 594678.66). 2024-11-06T03:26:34Z- TerrainMapOctree: Min Distance = 96.76. 2024-11-06T03:26:34Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:26:34Z- Forcing Subcloud Comparison 2024-11-06T03:26:34Z- Weighting particles with subcloud comparison 2024-11-06T03:26:34Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:34Z- meanExpectedMeasDiff: 4.067046 delta_rms_squared: 1.576757 alpha: 0.987475 2024-11-06T03:26:34Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:34Z- meanExpectedMeasDiff: -1.513006 delta_rms_squared: 1.094370 alpha: 0.982053 2024-11-06T03:26:34Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:34Z- meanExpectedMeasDiff: -0.254193 delta_rms_squared: 1.208353 alpha: 0.983718 2024-11-06T03:26:34Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:34Z- meanExpectedMeasDiff: -1.186692 delta_rms_squared: 1.267695 alpha: 0.984468 2024-11-06T03:26:34Z- Subcloud NIS: 0.413028 2024-11-06T03:26:34Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:26:34Z- TNavPF:: sumWeights = 0.071539 2024-11-06T03:26:34Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:26:34Z- TNavPF::First Term of Map Covariance Matrix is 0.96 2024-11-06T03:26:34Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.01 2024-11-06T03:26:34Z- TerrainNav::Resampling particle filter... 2024-11-06T03:26:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863592.57 sec. 2024-11-06T03:26:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:26:34Z- variances([0], [2], and psi_berg) = 0.389398 0.194877 0.000000 2024-11-06T03:26:34Z- publishEstimatesDecoder() - at 1730863594.23 sec 2024-11-06T03:26:34Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:26:34Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061847.42 594661.17 58.63 2024-11-06T03:26:34Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061845.77 594661.18 56.97 lat/lon: 36.70 -121.94 2024-11-06T03:26:34Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.39 0.19 0.73 0.00 2024-11-06T03:26:34Z- handleDepth()- Depth_Keller msg: 1730863594.57 epoch sec; seqNo:5531; depth 49.91 2024-11-06T03:26:34Z- handleDVL() - DVL msg epochMillisec: 1730863594.59 2024-11-06T03:26:34Z- handleDvl() - RDI_Pathfinder msg: 1730863594.59 sec; seqNo:1921; #beams:4 2024-11-06T03:26:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.67 , 35.88 , 38.68 , 38.87 2024-11-06T03:26:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , 0.01 2024-11-06T03:26:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863594.61 epoch sec; seqNo:5531; 4061816.90 north; 594678.15 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:26:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863594.23 2024-11-06T03:26:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:34Z- waiting for 1730863598.23, time is 1730863594.61 2024-11-06T03:26:34Z- AHRS_M2 msg: 1730863594.96 epoch sec; seqNo:5192 2024-11-06T03:26:34Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:26:34Z- handleDepth()- Depth_Keller msg: 1730863594.97 epoch sec; seqNo:5532; depth 49.92 2024-11-06T03:26:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863594.98 epoch sec; seqNo:5532; 4061817.28 north; 594678.14 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:26:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863594.23 2024-11-06T03:26:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:35Z- waiting for 1730863598.23, time is 1730863594.98 2024-11-06T03:26:35Z- AHRS_M2 msg: 1730863595.36 epoch sec; seqNo:5193 2024-11-06T03:26:35Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:26:35Z- handleDepth()- Depth_Keller msg: 1730863595.37 epoch sec; seqNo:5533; depth 49.92 2024-11-06T03:26:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863595.38 epoch sec; seqNo:5533; 4061817.66 north; 594678.14 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:26:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863594.23 2024-11-06T03:26:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:35Z- waiting for 1730863598.23, time is 1730863595.38 2024-11-06T03:26:35Z- AHRS_M2 msg: 1730863595.77 epoch sec; seqNo:5194 2024-11-06T03:26:35Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:26:35Z- handleDepth()- Depth_Keller msg: 1730863595.78 epoch sec; seqNo:5534; depth 49.91 2024-11-06T03:26:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863595.80 epoch sec; seqNo:5534; 4061818.02 north; 594676.91 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:26:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863594.23 2024-11-06T03:26:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:35Z- waiting for 1730863598.23, time is 1730863595.80 2024-11-06T03:26:36Z- AHRS_M2 msg: 1730863596.19 epoch sec; seqNo:5195 2024-11-06T03:26:36Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:26:36Z- handleDepth()- Depth_Keller msg: 1730863596.23 epoch sec; seqNo:5535; depth 49.92 2024-11-06T03:26:36Z- handleDVL() - DVL msg epochMillisec: 1730863596.25 2024-11-06T03:26:36Z- handleDvl() - RDI_Pathfinder msg: 1730863596.25 sec; seqNo:1922; #beams:4 2024-11-06T03:26:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.05 , 35.88 , 38.50 , 38.87 2024-11-06T03:26:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , -0.00 2024-11-06T03:26:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863596.27 epoch sec; seqNo:5535; 4061818.40 north; 594676.91 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:26:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863594.23 2024-11-06T03:26:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:36Z- waiting for 1730863598.23, time is 1730863596.27 2024-11-06T03:26:36Z- AHRS_M2 msg: 1730863596.58 epoch sec; seqNo:5196 2024-11-06T03:26:36Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:26:36Z- handleDepth()- Depth_Keller msg: 1730863596.58 epoch sec; seqNo:5536; depth 49.91 2024-11-06T03:26:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863596.60 epoch sec; seqNo:5536; 4061818.78 north; 594676.91 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:26:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863594.23 2024-11-06T03:26:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:36Z- waiting for 1730863598.23, time is 1730863596.60 2024-11-06T03:26:36Z- AHRS_M2 msg: 1730863596.98 epoch sec; seqNo:5197 2024-11-06T03:26:36Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:26:36Z- handleDepth()- Depth_Keller msg: 1730863596.99 epoch sec; seqNo:5537; depth 49.90 2024-11-06T03:26:36Z- handleDVL() - DVL msg epochMillisec: 1730863596.99 2024-11-06T03:26:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:26:36Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:26:36Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:26:36Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:26:36Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:26:36Z- handleDvl() - RDI_Pathfinder msg: 1730863596.99 sec; seqNo:1923; #beams:0 2024-11-06T03:26:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:26:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , -0.00 2024-11-06T03:26:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863597.00 epoch sec; seqNo:5537; 4061819.16 north; 594676.90 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:26:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863594.23 2024-11-06T03:26:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:37Z- waiting for 1730863598.23, time is 1730863597.00 2024-11-06T03:26:37Z- AHRS_M2 msg: 1730863597.38 epoch sec; seqNo:5198 2024-11-06T03:26:37Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:26:37Z- handleDepth()- Depth_Keller msg: 1730863597.39 epoch sec; seqNo:5538; depth 49.90 2024-11-06T03:26:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863597.40 epoch sec; seqNo:5538; 4061819.54 north; 594676.90 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:26:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863594.23 2024-11-06T03:26:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:37Z- waiting for 1730863598.23, time is 1730863597.40 2024-11-06T03:26:37Z- AHRS_M2 msg: 1730863597.78 epoch sec; seqNo:5199 2024-11-06T03:26:37Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:26:37Z- handleDepth()- Depth_Keller msg: 1730863597.79 epoch sec; seqNo:5539; depth 49.90 2024-11-06T03:26:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863597.81 epoch sec; seqNo:5539; 4061819.92 north; 594676.89 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:26:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863594.23 2024-11-06T03:26:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:37Z- waiting for 1730863598.23, time is 1730863597.81 2024-11-06T03:26:38Z- AHRS_M2 msg: 1730863598.22 epoch sec; seqNo:5200 2024-11-06T03:26:38Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.04 psi 2024-11-06T03:26:38Z- handleDepth()- Depth_Keller msg: 1730863598.23 epoch sec; seqNo:5540; depth 49.90 2024-11-06T03:26:38Z- handleDVL() - DVL msg epochMillisec: 1730863598.25 2024-11-06T03:26:38Z- handleDvl() - RDI_Pathfinder msg: 1730863598.25 sec; seqNo:1924; #beams:4 2024-11-06T03:26:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.86 , 35.88 , 38.12 , 38.87 2024-11-06T03:26:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.06 , -0.04 2024-11-06T03:26:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863598.28 epoch sec; seqNo:5540; 4061820.30 north; 594676.89 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:26:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863594.23 2024-11-06T03:26:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:38Z- waiting for 1730863598.23, time is 1730863598.28 2024-11-06T03:26:38Z- LcmTrn::updateTrn() >>>> pose time 1730863598.28, meas time 1730863598.25 <<<< 2024-11-06T03:26:38Z- LcmTrn::updateTrn() equating pose 1730863598.25 and meas times 1730863598.25 2024-11-06T03:26:38Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863598.25 2024-11-06T03:26:38Z- TerrainNav::motionUpdate() - currEstimate = (4061820, 594676.89, 49.90) 2024-11-06T03:26:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:26:38Z- TerrainNav:: SubcloudNIS is 0.41, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:26:38Z- TNavPF::Projecting Measurements 2024-11-06T03:26:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:26:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:26:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:26:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:26:38Z- TNavFilter::4 good beams. 2024-11-06T03:26:38Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:26:38Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:26:38Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:26:38Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:26:38Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:26:38Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:26:38Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:26:38Z- TerrainMapOctree: (vehN, vehE ) = (4061820.30, 594676.89). 2024-11-06T03:26:38Z- TerrainMapOctree: Min Distance = 101.42. 2024-11-06T03:26:38Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:26:38Z- Forcing Subcloud Comparison 2024-11-06T03:26:38Z- Weighting particles with subcloud comparison 2024-11-06T03:26:38Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:38Z- meanExpectedMeasDiff: 4.540045 delta_rms_squared: 2.733043 alpha: 0.992735 2024-11-06T03:26:38Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:38Z- meanExpectedMeasDiff: -1.637942 delta_rms_squared: 1.142432 alpha: 0.982795 2024-11-06T03:26:38Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:38Z- meanExpectedMeasDiff: -0.503748 delta_rms_squared: 1.285066 alpha: 0.984675 2024-11-06T03:26:38Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:38Z- meanExpectedMeasDiff: -1.193213 delta_rms_squared: 1.383180 alpha: 0.985747 2024-11-06T03:26:38Z- Subcloud NIS: 0.431705 2024-11-06T03:26:38Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:26:38Z- TNavPF:: sumWeights = 0.047659 2024-11-06T03:26:38Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:26:38Z- TNavPF::First Term of Map Covariance Matrix is 0.72 2024-11-06T03:26:38Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:26:38Z- TerrainNav::Resampling particle filter... 2024-11-06T03:26:38Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863598.25 sec, ping # 0. 2024-11-06T03:26:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:26:38Z- variances([0], [2], and psi_berg) = 0.435335 0.173821 0.000000 2024-11-06T03:26:38Z- publishEstimatesDecoder() - at 1730863598.25 sec 2024-11-06T03:26:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:26:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061850.96 594659.95 57.23 2024-11-06T03:26:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061849.19 594659.84 56.73 lat/lon: 36.70 -121.94 2024-11-06T03:26:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.44 0.17 0.37 0.00 2024-11-06T03:26:38Z- AHRS_M2 msg: 1730863598.62 epoch sec; seqNo:5201 2024-11-06T03:26:38Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.04 psi 2024-11-06T03:26:38Z- handleDepth()- Depth_Keller msg: 1730863598.63 epoch sec; seqNo:5541; depth 49.90 2024-11-06T03:26:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863598.69 epoch sec; seqNo:5541; 4061820.67 north; 594676.88 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:26:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863598.25 2024-11-06T03:26:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:38Z- waiting for 1730863602.25, time is 1730863598.69 2024-11-06T03:26:39Z- handleDepth()- Depth_Keller msg: 1730863599.01 epoch sec; seqNo:5542; depth 49.92 2024-11-06T03:26:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863599.02 epoch sec; seqNo:5542; 4061821.05 north; 594676.88 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:26:39Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:26:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863598.25 2024-11-06T03:26:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:39Z- waiting for 1730863602.25, time is 1730863599.02 2024-11-06T03:26:39Z- AHRS_M2 msg: 1730863599.40 epoch sec; seqNo:5202 2024-11-06T03:26:39Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:26:39Z- handleDepth()- Depth_Keller msg: 1730863599.40 epoch sec; seqNo:5543; depth 49.90 2024-11-06T03:26:39Z- handleDVL() - DVL msg epochMillisec: 1730863599.41 2024-11-06T03:26:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:26:39Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:26:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:26:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:26:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:26:39Z- handleDvl() - RDI_Pathfinder msg: 1730863599.41 sec; seqNo:1925; #beams:0 2024-11-06T03:26:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:26:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.06 , -0.04 2024-11-06T03:26:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863599.41 epoch sec; seqNo:5543; 4061821.43 north; 594676.88 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:26:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863598.25 2024-11-06T03:26:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:39Z- waiting for 1730863602.25, time is 1730863599.41 2024-11-06T03:26:39Z- AHRS_M2 msg: 1730863599.80 epoch sec; seqNo:5203 2024-11-06T03:26:39Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:26:39Z- handleDepth()- Depth_Keller msg: 1730863599.81 epoch sec; seqNo:5544; depth 49.92 2024-11-06T03:26:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863599.82 epoch sec; seqNo:5544; 4061821.81 north; 594676.87 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:26:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863598.25 2024-11-06T03:26:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:39Z- waiting for 1730863602.25, time is 1730863599.82 2024-11-06T03:26:40Z- AHRS_M2 msg: 1730863600.23 epoch sec; seqNo:5204 2024-11-06T03:26:40Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.03 psi 2024-11-06T03:26:40Z- handleDepth()- Depth_Keller msg: 1730863600.24 epoch sec; seqNo:5545; depth 49.93 2024-11-06T03:26:40Z- handleDVL() - DVL msg epochMillisec: 1730863600.26 2024-11-06T03:26:40Z- handleDvl() - RDI_Pathfinder msg: 1730863600.26 sec; seqNo:1926; #beams:4 2024-11-06T03:26:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.68 , 35.70 , 37.94 , 38.87 2024-11-06T03:26:40Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.11 , -0.04 2024-11-06T03:26:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863600.28 epoch sec; seqNo:5545; 4061822.18 north; 594675.65 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:26:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863598.25 2024-11-06T03:26:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:40Z- waiting for 1730863602.25, time is 1730863600.28 2024-11-06T03:26:40Z- AHRS_M2 msg: 1730863600.61 epoch sec; seqNo:5205 2024-11-06T03:26:40Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.03 psi 2024-11-06T03:26:40Z- handleDepth()- Depth_Keller msg: 1730863600.63 epoch sec; seqNo:5546; depth 49.95 2024-11-06T03:26:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863600.65 epoch sec; seqNo:5546; 4061822.55 north; 594675.64 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:26:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863598.25 2024-11-06T03:26:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:40Z- waiting for 1730863602.25, time is 1730863600.65 2024-11-06T03:26:41Z- AHRS_M2 msg: 1730863601.01 epoch sec; seqNo:5206 2024-11-06T03:26:41Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.03 psi 2024-11-06T03:26:41Z- handleDepth()- Depth_Keller msg: 1730863601.02 epoch sec; seqNo:5547; depth 49.94 2024-11-06T03:26:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863601.03 epoch sec; seqNo:5547; 4061822.93 north; 594675.64 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:26:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863598.25 2024-11-06T03:26:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:41Z- waiting for 1730863602.25, time is 1730863601.03 2024-11-06T03:26:41Z- AHRS_M2 msg: 1730863601.42 epoch sec; seqNo:5207 2024-11-06T03:26:41Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.03 psi 2024-11-06T03:26:41Z- handleDepth()- Depth_Keller msg: 1730863601.43 epoch sec; seqNo:5548; depth 49.96 2024-11-06T03:26:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863601.43 epoch sec; seqNo:5548; 4061823.31 north; 594675.64 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:26:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863598.25 2024-11-06T03:26:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:41Z- waiting for 1730863602.25, time is 1730863601.43 2024-11-06T03:26:41Z- AHRS_M2 msg: 1730863601.82 epoch sec; seqNo:5208 2024-11-06T03:26:41Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.03 psi 2024-11-06T03:26:41Z- handleDepth()- Depth_Keller msg: 1730863601.83 epoch sec; seqNo:5549; depth 49.98 2024-11-06T03:26:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863601.84 epoch sec; seqNo:5549; 4061823.69 north; 594675.63 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:26:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863598.25 2024-11-06T03:26:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:41Z- waiting for 1730863602.25, time is 1730863601.84 2024-11-06T03:26:42Z- AHRS_M2 msg: 1730863602.23 epoch sec; seqNo:5209 2024-11-06T03:26:42Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.03 psi 2024-11-06T03:26:42Z- handleDepth()- Depth_Keller msg: 1730863602.24 epoch sec; seqNo:5550; depth 50.00 2024-11-06T03:26:42Z- handleDVL() - DVL msg epochMillisec: 1730863602.26 2024-11-06T03:26:42Z- handleDvl() - RDI_Pathfinder msg: 1730863602.26 sec; seqNo:1927; #beams:4 2024-11-06T03:26:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 36.07 , 37.94 , 39.06 2024-11-06T03:26:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.14 , 0.02 2024-11-06T03:26:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863602.28 epoch sec; seqNo:5550; 4061824.07 north; 594675.63 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:26:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863598.25 2024-11-06T03:26:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:42Z- waiting for 1730863602.25, time is 1730863602.28 2024-11-06T03:26:42Z- LcmTrn::updateTrn() >>>> pose time 1730863602.28, meas time 1730863602.26 <<<< 2024-11-06T03:26:42Z- LcmTrn::updateTrn() equating pose 1730863602.26 and meas times 1730863602.26 2024-11-06T03:26:42Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863602.26 2024-11-06T03:26:42Z- TerrainNav::motionUpdate() - currEstimate = (4061824, 594675.63, 50.00) 2024-11-06T03:26:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:26:42Z- TerrainNav:: SubcloudNIS is 0.43, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:26:42Z- TNavPF::Projecting Measurements 2024-11-06T03:26:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:26:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:26:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:26:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:26:42Z- TNavFilter::4 good beams. 2024-11-06T03:26:42Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:26:42Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:26:42Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:26:42Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:26:42Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:26:42Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:26:42Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:26:42Z- TerrainMapOctree: (vehN, vehE ) = (4061824.07, 594675.63). 2024-11-06T03:26:42Z- TerrainMapOctree: Min Distance = 104.97. 2024-11-06T03:26:42Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:26:42Z- Forcing Subcloud Comparison 2024-11-06T03:26:42Z- Weighting particles with subcloud comparison 2024-11-06T03:26:42Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:42Z- meanExpectedMeasDiff: 3.874033 delta_rms_squared: 1.153731 alpha: 0.982960 2024-11-06T03:26:42Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:42Z- meanExpectedMeasDiff: -1.560576 delta_rms_squared: 0.701186 alpha: 0.972268 2024-11-06T03:26:42Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:42Z- meanExpectedMeasDiff: -0.655512 delta_rms_squared: 0.782939 alpha: 0.975091 2024-11-06T03:26:42Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:42Z- meanExpectedMeasDiff: -1.383359 delta_rms_squared: 0.860727 alpha: 0.977291 2024-11-06T03:26:42Z- Subcloud NIS: 0.451198 2024-11-06T03:26:42Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:26:42Z- TNavPF:: sumWeights = 0.082749 2024-11-06T03:26:42Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:26:42Z- TNavPF::First Term of Map Covariance Matrix is 1.05 2024-11-06T03:26:42Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T03:26:42Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863602.26 sec, ping # 0. 2024-11-06T03:26:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:26:42Z- variances([0], [2], and psi_berg) = 0.563346 0.188387 0.000000 2024-11-06T03:26:42Z- publishEstimatesDecoder() - at 1730863602.26 sec 2024-11-06T03:26:42Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:26:42Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061854.50 594658.63 57.03 2024-11-06T03:26:42Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061853.09 594658.56 56.81 lat/lon: 36.70 -121.94 2024-11-06T03:26:42Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.56 0.19 0.35 0.00 2024-11-06T03:26:42Z- AHRS_M2 msg: 1730863602.63 epoch sec; seqNo:5210 2024-11-06T03:26:42Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.03 psi 2024-11-06T03:26:42Z- handleDepth()- Depth_Keller msg: 1730863602.64 epoch sec; seqNo:5551; depth 50.00 2024-11-06T03:26:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863602.66 epoch sec; seqNo:5551; 4061824.45 north; 594675.62 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:26:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863602.26 2024-11-06T03:26:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:42Z- waiting for 1730863606.26, time is 1730863602.66 2024-11-06T03:26:43Z- AHRS_M2 msg: 1730863603.04 epoch sec; seqNo:5211 2024-11-06T03:26:43Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.03 psi 2024-11-06T03:26:43Z- handleDepth()- Depth_Keller msg: 1730863603.04 epoch sec; seqNo:5552; depth 50.02 2024-11-06T03:26:43Z- handleDVL() - DVL msg epochMillisec: 1730863603.04 2024-11-06T03:26:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:26:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:26:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:26:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:26:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:26:43Z- handleDvl() - RDI_Pathfinder msg: 1730863603.04 sec; seqNo:1928; #beams:0 2024-11-06T03:26:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:26:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.14 , 0.02 2024-11-06T03:26:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863603.05 epoch sec; seqNo:5552; 4061824.83 north; 594675.62 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:26:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863602.26 2024-11-06T03:26:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:43Z- waiting for 1730863606.26, time is 1730863603.05 2024-11-06T03:26:43Z- AHRS_M2 msg: 1730863603.44 epoch sec; seqNo:5212 2024-11-06T03:26:43Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.03 psi 2024-11-06T03:26:43Z- handleDepth()- Depth_Keller msg: 1730863603.45 epoch sec; seqNo:5553; depth 50.05 2024-11-06T03:26:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863603.46 epoch sec; seqNo:5553; 4061825.19 north; 594674.39 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:26:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863602.26 2024-11-06T03:26:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:43Z- waiting for 1730863606.26, time is 1730863603.46 2024-11-06T03:26:43Z- AHRS_M2 msg: 1730863603.85 epoch sec; seqNo:5213 2024-11-06T03:26:43Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.03 psi 2024-11-06T03:26:43Z- handleDepth()- Depth_Keller msg: 1730863603.85 epoch sec; seqNo:5554; depth 50.06 2024-11-06T03:26:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863603.87 epoch sec; seqNo:5554; 4061825.57 north; 594674.39 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:26:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863602.26 2024-11-06T03:26:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:44Z- waiting for 1730863606.26, time is 1730863603.87 2024-11-06T03:26:44Z- AHRS_M2 msg: 1730863604.25 epoch sec; seqNo:5214 2024-11-06T03:26:44Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.03 psi 2024-11-06T03:26:44Z- handleDepth()- Depth_Keller msg: 1730863604.26 epoch sec; seqNo:5555; depth 50.08 2024-11-06T03:26:44Z- handleDVL() - DVL msg epochMillisec: 1730863604.29 2024-11-06T03:26:44Z- handleDvl() - RDI_Pathfinder msg: 1730863604.29 sec; seqNo:1929; #beams:4 2024-11-06T03:26:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.05 , 35.88 , 37.94 , 39.06 2024-11-06T03:26:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.11 , 0.05 2024-11-06T03:26:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863604.30 epoch sec; seqNo:5555; 4061825.95 north; 594674.39 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:26:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863602.26 2024-11-06T03:26:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:44Z- waiting for 1730863606.26, time is 1730863604.30 2024-11-06T03:26:44Z- handleDepth()- Depth_Keller msg: 1730863604.69 epoch sec; seqNo:5556; depth 50.10 2024-11-06T03:26:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863604.71 epoch sec; seqNo:5556; 4061825.95 north; 594674.39 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:26:44Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:26:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863602.26 2024-11-06T03:26:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:44Z- waiting for 1730863606.26, time is 1730863604.71 2024-11-06T03:26:45Z- AHRS_M2 msg: 1730863605.06 epoch sec; seqNo:5215 2024-11-06T03:26:45Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.03 psi 2024-11-06T03:26:45Z- handleDepth()- Depth_Keller msg: 1730863605.06 epoch sec; seqNo:5557; depth 50.12 2024-11-06T03:26:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863605.08 epoch sec; seqNo:5557; 4061826.33 north; 594674.38 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:26:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863602.26 2024-11-06T03:26:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:45Z- waiting for 1730863606.26, time is 1730863605.08 2024-11-06T03:26:45Z- AHRS_M2 msg: 1730863605.47 epoch sec; seqNo:5216 2024-11-06T03:26:45Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.03 psi 2024-11-06T03:26:45Z- handleDepth()- Depth_Keller msg: 1730863605.47 epoch sec; seqNo:5558; depth 50.15 2024-11-06T03:26:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863605.48 epoch sec; seqNo:5558; 4061826.71 north; 594674.38 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:26:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863602.26 2024-11-06T03:26:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:45Z- waiting for 1730863606.26, time is 1730863605.48 2024-11-06T03:26:45Z- AHRS_M2 msg: 1730863605.87 epoch sec; seqNo:5217 2024-11-06T03:26:45Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:45Z- handleDepth()- Depth_Keller msg: 1730863605.88 epoch sec; seqNo:5559; depth 50.14 2024-11-06T03:26:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863605.89 epoch sec; seqNo:5559; 4061827.09 north; 594674.37 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:26:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863602.26 2024-11-06T03:26:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:46Z- waiting for 1730863606.26, time is 1730863605.89 2024-11-06T03:26:46Z- AHRS_M2 msg: 1730863606.27 epoch sec; seqNo:5218 2024-11-06T03:26:46Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:46Z- handleDepth()- Depth_Keller msg: 1730863606.27 epoch sec; seqNo:5560; depth 50.17 2024-11-06T03:26:46Z- handleDVL() - DVL msg epochMillisec: 1730863606.29 2024-11-06T03:26:46Z- handleDvl() - RDI_Pathfinder msg: 1730863606.29 sec; seqNo:1930; #beams:4 2024-11-06T03:26:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 35.88 , 37.94 , 39.25 2024-11-06T03:26:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.05 2024-11-06T03:26:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863606.31 epoch sec; seqNo:5560; 4061827.47 north; 594674.37 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:26:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863602.26 2024-11-06T03:26:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:46Z- waiting for 1730863606.26, time is 1730863606.31 2024-11-06T03:26:46Z- LcmTrn::updateTrn() >>>> pose time 1730863606.31, meas time 1730863606.29 <<<< 2024-11-06T03:26:46Z- LcmTrn::updateTrn() equating pose 1730863606.29 and meas times 1730863606.29 2024-11-06T03:26:46Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863606.29 2024-11-06T03:26:46Z- TerrainNav::motionUpdate() - currEstimate = (4061827, 594674.37, 50.17) 2024-11-06T03:26:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:26:46Z- TerrainNav:: SubcloudNIS is 0.45, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:26:46Z- TNavPF::Projecting Measurements 2024-11-06T03:26:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:26:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:26:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:26:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:26:46Z- TNavFilter::4 good beams. 2024-11-06T03:26:46Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:26:46Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:26:46Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:26:46Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:26:46Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:26:46Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:26:46Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:26:46Z- TerrainMapOctree: (vehN, vehE ) = (4061827.47, 594674.37). 2024-11-06T03:26:46Z- TerrainMapOctree: Min Distance = 108.26. 2024-11-06T03:26:46Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:26:46Z- Forcing Subcloud Comparison 2024-11-06T03:26:46Z- Weighting particles with subcloud comparison 2024-11-06T03:26:46Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:46Z- meanExpectedMeasDiff: 2.841343 delta_rms_squared: 0.951043 alpha: 0.979404 2024-11-06T03:26:46Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:46Z- meanExpectedMeasDiff: -1.570462 delta_rms_squared: 0.682988 alpha: 0.971550 2024-11-06T03:26:46Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:46Z- meanExpectedMeasDiff: -0.424782 delta_rms_squared: 0.760868 alpha: 0.974387 2024-11-06T03:26:46Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:46Z- meanExpectedMeasDiff: -1.123022 delta_rms_squared: 0.843599 alpha: 0.976841 2024-11-06T03:26:46Z- Subcloud NIS: 0.468390 2024-11-06T03:26:46Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:26:46Z- TNavPF:: sumWeights = 0.194137 2024-11-06T03:26:46Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:26:46Z- TNavPF::First Term of Map Covariance Matrix is 1.15 2024-11-06T03:26:46Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T03:26:46Z- TerrainNav::Resampling particle filter... 2024-11-06T03:26:46Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863606.29 sec, ping # 0. 2024-11-06T03:26:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:26:46Z- variances([0], [2], and psi_berg) = 0.565812 0.190925 0.000000 2024-11-06T03:26:46Z- publishEstimatesDecoder() - at 1730863606.29 sec 2024-11-06T03:26:46Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:26:46Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061857.80 594656.88 57.80 2024-11-06T03:26:46Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061856.48 594657.30 57.01 lat/lon: 36.70 -121.94 2024-11-06T03:26:46Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.19 0.33 0.00 2024-11-06T03:26:46Z- AHRS_M2 msg: 1730863606.67 epoch sec; seqNo:5219 2024-11-06T03:26:46Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:46Z- handleDepth()- Depth_Keller msg: 1730863606.69 epoch sec; seqNo:5561; depth 50.19 2024-11-06T03:26:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863606.73 epoch sec; seqNo:5561; 4061827.85 north; 594674.36 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:26:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863606.29 2024-11-06T03:26:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:46Z- waiting for 1730863610.29, time is 1730863606.73 2024-11-06T03:26:47Z- AHRS_M2 msg: 1730863607.08 epoch sec; seqNo:5220 2024-11-06T03:26:47Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:47Z- handleDepth()- Depth_Keller msg: 1730863607.08 epoch sec; seqNo:5562; depth 50.20 2024-11-06T03:26:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863607.10 epoch sec; seqNo:5562; 4061828.22 north; 594674.36 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:26:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863606.29 2024-11-06T03:26:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:47Z- waiting for 1730863610.29, time is 1730863607.10 2024-11-06T03:26:47Z- AHRS_M2 msg: 1730863607.48 epoch sec; seqNo:5221 2024-11-06T03:26:47Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:47Z- handleDepth()- Depth_Keller msg: 1730863607.48 epoch sec; seqNo:5563; depth 50.21 2024-11-06T03:26:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863607.49 epoch sec; seqNo:5563; 4061828.21 north; 594673.14 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:26:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863606.29 2024-11-06T03:26:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:47Z- waiting for 1730863610.29, time is 1730863607.49 2024-11-06T03:26:47Z- AHRS_M2 msg: 1730863607.89 epoch sec; seqNo:5222 2024-11-06T03:26:47Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:47Z- handleDepth()- Depth_Keller msg: 1730863607.90 epoch sec; seqNo:5564; depth 50.22 2024-11-06T03:26:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863607.91 epoch sec; seqNo:5564; 4061828.59 north; 594673.14 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:26:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863606.29 2024-11-06T03:26:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:48Z- waiting for 1730863610.29, time is 1730863607.91 2024-11-06T03:26:48Z- AHRS_M2 msg: 1730863608.29 epoch sec; seqNo:5223 2024-11-06T03:26:48Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:48Z- handleDepth()- Depth_Keller msg: 1730863608.30 epoch sec; seqNo:5565; depth 50.23 2024-11-06T03:26:48Z- handleDVL() - DVL msg epochMillisec: 1730863608.32 2024-11-06T03:26:48Z- handleDvl() - RDI_Pathfinder msg: 1730863608.32 sec; seqNo:1931; #beams:4 2024-11-06T03:26:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 35.88 , 37.94 , 39.44 2024-11-06T03:26:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.00 , 0.01 2024-11-06T03:26:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863608.34 epoch sec; seqNo:5565; 4061828.97 north; 594673.13 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:26:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863606.29 2024-11-06T03:26:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:48Z- waiting for 1730863610.29, time is 1730863608.34 2024-11-06T03:26:48Z- AHRS_M2 msg: 1730863608.69 epoch sec; seqNo:5224 2024-11-06T03:26:48Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 6.04 psi 2024-11-06T03:26:48Z- handleDepth()- Depth_Keller msg: 1730863608.70 epoch sec; seqNo:5566; depth 50.22 2024-11-06T03:26:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863608.73 epoch sec; seqNo:5566; 4061829.35 north; 594673.13 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:26:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863606.29 2024-11-06T03:26:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:48Z- waiting for 1730863610.29, time is 1730863608.73 2024-11-06T03:26:49Z- AHRS_M2 msg: 1730863609.10 epoch sec; seqNo:5225 2024-11-06T03:26:49Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 6.04 psi 2024-11-06T03:26:49Z- handleDepth()- Depth_Keller msg: 1730863609.10 epoch sec; seqNo:5567; depth 50.22 2024-11-06T03:26:49Z- handleDVL() - DVL msg epochMillisec: 1730863609.10 2024-11-06T03:26:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:26:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:26:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:26:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:26:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:26:49Z- handleDvl() - RDI_Pathfinder msg: 1730863609.10 sec; seqNo:1932; #beams:0 2024-11-06T03:26:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:26:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.00 , 0.01 2024-11-06T03:26:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863609.12 epoch sec; seqNo:5567; 4061829.73 north; 594673.12 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:26:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863606.29 2024-11-06T03:26:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:49Z- waiting for 1730863610.29, time is 1730863609.12 2024-11-06T03:26:49Z- AHRS_M2 msg: 1730863609.50 epoch sec; seqNo:5226 2024-11-06T03:26:49Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 6.04 psi 2024-11-06T03:26:49Z- handleDepth()- Depth_Keller msg: 1730863609.50 epoch sec; seqNo:5568; depth 50.23 2024-11-06T03:26:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863609.51 epoch sec; seqNo:5568; 4061830.10 north; 594673.12 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:26:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863606.29 2024-11-06T03:26:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:49Z- waiting for 1730863610.29, time is 1730863609.51 2024-11-06T03:26:49Z- AHRS_M2 msg: 1730863609.90 epoch sec; seqNo:5227 2024-11-06T03:26:49Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 6.05 psi 2024-11-06T03:26:49Z- handleDepth()- Depth_Keller msg: 1730863609.91 epoch sec; seqNo:5569; depth 50.22 2024-11-06T03:26:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863609.92 epoch sec; seqNo:5569; 4061830.48 north; 594673.12 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:26:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863606.29 2024-11-06T03:26:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:50Z- waiting for 1730863610.29, time is 1730863609.92 2024-11-06T03:26:50Z- handleDepth()- Depth_Keller msg: 1730863610.33 epoch sec; seqNo:5570; depth 50.22 2024-11-06T03:26:50Z- handleDVL() - DVL msg epochMillisec: 1730863610.35 2024-11-06T03:26:50Z- handleDvl() - RDI_Pathfinder msg: 1730863610.35 sec; seqNo:1933; #beams:4 2024-11-06T03:26:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.68 , 36.07 , 37.75 , 39.25 2024-11-06T03:26:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , -0.04 2024-11-06T03:26:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863610.37 epoch sec; seqNo:5570; 4061830.86 north; 594673.11 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:26:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863606.29 2024-11-06T03:26:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:50Z- waiting for 1730863610.29, time is 1730863610.37 2024-11-06T03:26:50Z- LcmTrn::updateTrn() >>>> pose time 1730863610.37, meas time 1730863610.35 <<<< 2024-11-06T03:26:50Z- LcmTrn::updateTrn() equating pose 1730863610.35 and meas times 1730863610.35 2024-11-06T03:26:50Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863610.35 2024-11-06T03:26:50Z- TerrainNav::motionUpdate() - currEstimate = (4061831, 594673.11, 50.22) 2024-11-06T03:26:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:26:50Z- TerrainNav:: SubcloudNIS is 0.47, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:26:50Z- TNavPF::Projecting Measurements 2024-11-06T03:26:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:26:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:26:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:26:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:26:50Z- TNavFilter::4 good beams. 2024-11-06T03:26:50Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:26:50Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:26:50Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:26:50Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:26:50Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:26:50Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:26:50Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:26:50Z- TerrainMapOctree: (vehN, vehE ) = (4061830.86, 594673.11). 2024-11-06T03:26:50Z- TerrainMapOctree: Min Distance = 111.58. 2024-11-06T03:26:50Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:26:50Z- Forcing Subcloud Comparison 2024-11-06T03:26:50Z- Weighting particles with subcloud comparison 2024-11-06T03:26:50Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:50Z- meanExpectedMeasDiff: 2.783664 delta_rms_squared: 0.940644 alpha: 0.979181 2024-11-06T03:26:50Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:50Z- meanExpectedMeasDiff: -1.303317 delta_rms_squared: 0.676657 alpha: 0.971291 2024-11-06T03:26:50Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:50Z- meanExpectedMeasDiff: -0.477118 delta_rms_squared: 0.751842 alpha: 0.974088 2024-11-06T03:26:50Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:50Z- meanExpectedMeasDiff: -1.056473 delta_rms_squared: 0.836760 alpha: 0.976656 2024-11-06T03:26:50Z- Subcloud NIS: 0.467013 2024-11-06T03:26:50Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:26:50Z- TNavPF:: sumWeights = 0.223430 2024-11-06T03:26:50Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:26:50Z- TNavPF::First Term of Map Covariance Matrix is 1.17 2024-11-06T03:26:50Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T03:26:50Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863610.35 sec, ping # 0. 2024-11-06T03:26:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:26:50Z- variances([0], [2], and psi_berg) = 0.567362 0.194553 0.000000 2024-11-06T03:26:50Z- publishEstimatesDecoder() - at 1730863610.35 sec 2024-11-06T03:26:50Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:26:50Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061859.50 594656.12 57.08 2024-11-06T03:26:50Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061859.88 594656.05 57.07 lat/lon: 36.70 -121.94 2024-11-06T03:26:50Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.19 0.33 0.00 2024-11-06T03:26:50Z- AHRS_M2 msg: 1730863610.71 epoch sec; seqNo:5228 2024-11-06T03:26:50Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.05 psi 2024-11-06T03:26:50Z- handleDepth()- Depth_Keller msg: 1730863610.72 epoch sec; seqNo:5571; depth 50.21 2024-11-06T03:26:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863610.76 epoch sec; seqNo:5571; 4061831.24 north; 594673.11 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:26:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863610.35 2024-11-06T03:26:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:50Z- waiting for 1730863614.35, time is 1730863610.76 2024-11-06T03:26:51Z- AHRS_M2 msg: 1730863611.12 epoch sec; seqNo:5229 2024-11-06T03:26:51Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:51Z- handleDepth()- Depth_Keller msg: 1730863611.12 epoch sec; seqNo:5572; depth 50.22 2024-11-06T03:26:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863611.13 epoch sec; seqNo:5572; 4061831.24 north; 594673.11 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:26:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863610.35 2024-11-06T03:26:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:51Z- waiting for 1730863614.35, time is 1730863611.13 2024-11-06T03:26:51Z- AHRS_M2 msg: 1730863611.52 epoch sec; seqNo:5230 2024-11-06T03:26:51Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:51Z- handleDepth()- Depth_Keller msg: 1730863611.52 epoch sec; seqNo:5573; depth 50.23 2024-11-06T03:26:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863611.53 epoch sec; seqNo:5573; 4061831.62 north; 594673.10 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:26:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863610.35 2024-11-06T03:26:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:51Z- waiting for 1730863614.35, time is 1730863611.53 2024-11-06T03:26:51Z- AHRS_M2 msg: 1730863611.92 epoch sec; seqNo:5231 2024-11-06T03:26:51Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:51Z- handleDepth()- Depth_Keller msg: 1730863611.93 epoch sec; seqNo:5574; depth 50.23 2024-11-06T03:26:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863611.95 epoch sec; seqNo:5574; 4061832.00 north; 594673.10 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:26:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863610.35 2024-11-06T03:26:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:52Z- waiting for 1730863614.35, time is 1730863611.95 2024-11-06T03:26:52Z- AHRS_M2 msg: 1730863612.33 epoch sec; seqNo:5232 2024-11-06T03:26:52Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:52Z- handleDepth()- Depth_Keller msg: 1730863612.34 epoch sec; seqNo:5575; depth 50.22 2024-11-06T03:26:52Z- handleDVL() - DVL msg epochMillisec: 1730863612.37 2024-11-06T03:26:52Z- handleDvl() - RDI_Pathfinder msg: 1730863612.37 sec; seqNo:1934; #beams:4 2024-11-06T03:26:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.49 , 35.88 , 37.57 , 39.06 2024-11-06T03:26:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.06 , -0.06 2024-11-06T03:26:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863612.38 epoch sec; seqNo:5575; 4061832.38 north; 594673.09 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:26:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863610.35 2024-11-06T03:26:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:52Z- waiting for 1730863614.35, time is 1730863612.38 2024-11-06T03:26:52Z- AHRS_M2 msg: 1730863612.73 epoch sec; seqNo:5233 2024-11-06T03:26:52Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:52Z- handleDepth()- Depth_Keller msg: 1730863612.74 epoch sec; seqNo:5576; depth 50.23 2024-11-06T03:26:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863612.75 epoch sec; seqNo:5576; 4061832.74 north; 594671.87 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:26:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863610.35 2024-11-06T03:26:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:52Z- waiting for 1730863614.35, time is 1730863612.75 2024-11-06T03:26:53Z- AHRS_M2 msg: 1730863613.14 epoch sec; seqNo:5234 2024-11-06T03:26:53Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:53Z- handleDepth()- Depth_Keller msg: 1730863613.14 epoch sec; seqNo:5577; depth 50.22 2024-11-06T03:26:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863613.16 epoch sec; seqNo:5577; 4061833.12 north; 594671.87 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:26:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863610.35 2024-11-06T03:26:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:53Z- waiting for 1730863614.35, time is 1730863613.16 2024-11-06T03:26:53Z- AHRS_M2 msg: 1730863613.54 epoch sec; seqNo:5235 2024-11-06T03:26:53Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:53Z- handleDepth()- Depth_Keller msg: 1730863613.54 epoch sec; seqNo:5578; depth 50.22 2024-11-06T03:26:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863613.55 epoch sec; seqNo:5578; 4061833.50 north; 594671.86 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:26:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863610.35 2024-11-06T03:26:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:53Z- waiting for 1730863614.35, time is 1730863613.55 2024-11-06T03:26:53Z- AHRS_M2 msg: 1730863613.94 epoch sec; seqNo:5236 2024-11-06T03:26:53Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:53Z- handleDepth()- Depth_Keller msg: 1730863613.96 epoch sec; seqNo:5579; depth 50.22 2024-11-06T03:26:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863613.97 epoch sec; seqNo:5579; 4061833.88 north; 594671.86 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:26:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863610.35 2024-11-06T03:26:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:54Z- waiting for 1730863614.35, time is 1730863613.97 2024-11-06T03:26:54Z- AHRS_M2 msg: 1730863614.35 epoch sec; seqNo:5237 2024-11-06T03:26:54Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:54Z- handleDepth()- Depth_Keller msg: 1730863614.35 epoch sec; seqNo:5580; depth 50.23 2024-11-06T03:26:54Z- handleDVL() - DVL msg epochMillisec: 1730863614.38 2024-11-06T03:26:54Z- handleDvl() - RDI_Pathfinder msg: 1730863614.38 sec; seqNo:1935; #beams:4 2024-11-06T03:26:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.93 , 35.88 , 37.57 , 38.87 2024-11-06T03:26:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.13 , -0.04 2024-11-06T03:26:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863614.39 epoch sec; seqNo:5580; 4061834.26 north; 594671.85 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:26:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863610.35 2024-11-06T03:26:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:54Z- waiting for 1730863614.35, time is 1730863614.39 2024-11-06T03:26:54Z- LcmTrn::updateTrn() >>>> pose time 1730863614.39, meas time 1730863614.38 <<<< 2024-11-06T03:26:54Z- LcmTrn::updateTrn() equating pose 1730863614.38 and meas times 1730863614.38 2024-11-06T03:26:54Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863614.38 2024-11-06T03:26:54Z- TerrainNav::motionUpdate() - currEstimate = (4061834, 594671.85, 50.23) 2024-11-06T03:26:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:26:54Z- TerrainNav:: SubcloudNIS is 0.47, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:26:54Z- TNavPF::Projecting Measurements 2024-11-06T03:26:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:26:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:26:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:26:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:26:54Z- TNavFilter::4 good beams. 2024-11-06T03:26:54Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:26:54Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:26:54Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:26:54Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:26:54Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:26:54Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:26:54Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:26:54Z- TerrainMapOctree: (vehN, vehE ) = (4061834.26, 594671.85). 2024-11-06T03:26:54Z- TerrainMapOctree: Min Distance = 114.92. 2024-11-06T03:26:54Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:26:54Z- Forcing Subcloud Comparison 2024-11-06T03:26:54Z- Weighting particles with subcloud comparison 2024-11-06T03:26:54Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:54Z- meanExpectedMeasDiff: 2.334285 delta_rms_squared: 0.931050 alpha: 0.978971 2024-11-06T03:26:54Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:54Z- meanExpectedMeasDiff: -1.608475 delta_rms_squared: 0.682482 alpha: 0.971530 2024-11-06T03:26:54Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:54Z- meanExpectedMeasDiff: -0.627897 delta_rms_squared: 0.752050 alpha: 0.974095 2024-11-06T03:26:54Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:54Z- meanExpectedMeasDiff: -1.313475 delta_rms_squared: 0.833678 alpha: 0.976572 2024-11-06T03:26:54Z- Subcloud NIS: 0.446635 2024-11-06T03:26:54Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:26:54Z- TNavPF:: sumWeights = 0.234246 2024-11-06T03:26:54Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:26:54Z- TNavPF::First Term of Map Covariance Matrix is 0.96 2024-11-06T03:26:54Z- TNavPF::Calculated NIS Value : 0.14 numBeams normalized NIS: 0.04 2024-11-06T03:26:54Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863614.38 sec, ping # 0. 2024-11-06T03:26:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:26:54Z- variances([0], [2], and psi_berg) = 0.607108 0.197955 0.000000 2024-11-06T03:26:54Z- publishEstimatesDecoder() - at 1730863614.38 sec 2024-11-06T03:26:54Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:26:54Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061865.71 594654.18 57.45 2024-11-06T03:26:54Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061863.28 594654.79 57.20 lat/lon: 36.70 -121.94 2024-11-06T03:26:54Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.61 0.20 0.33 0.00 2024-11-06T03:26:54Z- AHRS_M2 msg: 1730863614.76 epoch sec; seqNo:5238 2024-11-06T03:26:54Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:54Z- handleDepth()- Depth_Keller msg: 1730863614.77 epoch sec; seqNo:5581; depth 50.26 2024-11-06T03:26:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863614.78 epoch sec; seqNo:5581; 4061834.64 north; 594671.85 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:26:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863614.38 2024-11-06T03:26:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:54Z- waiting for 1730863618.38, time is 1730863614.78 2024-11-06T03:26:55Z- AHRS_M2 msg: 1730863615.16 epoch sec; seqNo:5239 2024-11-06T03:26:55Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:55Z- handleDepth()- Depth_Keller msg: 1730863615.16 epoch sec; seqNo:5582; depth 50.28 2024-11-06T03:26:55Z- handleDVL() - DVL msg epochMillisec: 1730863615.16 2024-11-06T03:26:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:26:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:26:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:26:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:26:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:26:55Z- handleDvl() - RDI_Pathfinder msg: 1730863615.16 sec; seqNo:1936; #beams:0 2024-11-06T03:26:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:26:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.13 , -0.04 2024-11-06T03:26:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863615.17 epoch sec; seqNo:5582; 4061835.02 north; 594671.84 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:26:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863614.38 2024-11-06T03:26:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:55Z- waiting for 1730863618.38, time is 1730863615.17 2024-11-06T03:26:55Z- AHRS_M2 msg: 1730863615.56 epoch sec; seqNo:5240 2024-11-06T03:26:55Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:55Z- handleDepth()- Depth_Keller msg: 1730863615.57 epoch sec; seqNo:5583; depth 50.29 2024-11-06T03:26:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863615.58 epoch sec; seqNo:5583; 4061835.40 north; 594671.84 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:26:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863614.38 2024-11-06T03:26:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:55Z- waiting for 1730863618.38, time is 1730863615.58 2024-11-06T03:26:55Z- AHRS_M2 msg: 1730863615.96 epoch sec; seqNo:5241 2024-11-06T03:26:55Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:55Z- handleDepth()- Depth_Keller msg: 1730863615.97 epoch sec; seqNo:5584; depth 50.31 2024-11-06T03:26:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863616.00 epoch sec; seqNo:5584; 4061835.77 north; 594671.84 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:26:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863614.38 2024-11-06T03:26:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:56Z- waiting for 1730863618.38, time is 1730863616.00 2024-11-06T03:26:56Z- AHRS_M2 msg: 1730863616.37 epoch sec; seqNo:5242 2024-11-06T03:26:56Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:56Z- handleDepth()- Depth_Keller msg: 1730863616.38 epoch sec; seqNo:5585; depth 50.32 2024-11-06T03:26:56Z- handleDVL() - DVL msg epochMillisec: 1730863616.40 2024-11-06T03:26:56Z- handleDvl() - RDI_Pathfinder msg: 1730863616.40 sec; seqNo:1937; #beams:4 2024-11-06T03:26:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.12 , 35.70 , 37.57 , 38.87 2024-11-06T03:26:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.16 , -0.01 2024-11-06T03:26:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863616.42 epoch sec; seqNo:5585; 4061836.14 north; 594670.61 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:26:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863614.38 2024-11-06T03:26:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:56Z- waiting for 1730863618.38, time is 1730863616.42 2024-11-06T03:26:56Z- AHRS_M2 msg: 1730863616.78 epoch sec; seqNo:5243 2024-11-06T03:26:56Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 6.03 psi 2024-11-06T03:26:56Z- handleDepth()- Depth_Keller msg: 1730863616.78 epoch sec; seqNo:5586; depth 50.34 2024-11-06T03:26:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863616.80 epoch sec; seqNo:5586; 4061836.52 north; 594670.61 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:26:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863614.38 2024-11-06T03:26:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:56Z- waiting for 1730863618.38, time is 1730863616.80 2024-11-06T03:26:57Z- AHRS_M2 msg: 1730863617.18 epoch sec; seqNo:5244 2024-11-06T03:26:57Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 6.04 psi 2024-11-06T03:26:57Z- handleDepth()- Depth_Keller msg: 1730863617.19 epoch sec; seqNo:5587; depth 50.36 2024-11-06T03:26:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863617.21 epoch sec; seqNo:5587; 4061836.90 north; 594670.60 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:26:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863614.38 2024-11-06T03:26:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:57Z- waiting for 1730863618.38, time is 1730863617.21 2024-11-06T03:26:57Z- AHRS_M2 msg: 1730863617.58 epoch sec; seqNo:5245 2024-11-06T03:26:57Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 6.03 psi 2024-11-06T03:26:57Z- handleDepth()- Depth_Keller msg: 1730863617.59 epoch sec; seqNo:5588; depth 50.40 2024-11-06T03:26:57Z- handleDVL() - DVL msg epochMillisec: 1730863617.59 2024-11-06T03:26:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:26:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:26:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:26:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:26:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:26:57Z- handleDvl() - RDI_Pathfinder msg: 1730863617.59 sec; seqNo:1938; #beams:0 2024-11-06T03:26:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:26:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.16 , -0.01 2024-11-06T03:26:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863617.60 epoch sec; seqNo:5588; 4061837.28 north; 594670.60 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:26:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863614.38 2024-11-06T03:26:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:57Z- waiting for 1730863618.38, time is 1730863617.60 2024-11-06T03:26:57Z- AHRS_M2 msg: 1730863617.99 epoch sec; seqNo:5246 2024-11-06T03:26:57Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 6.03 psi 2024-11-06T03:26:58Z- handleDepth()- Depth_Keller msg: 1730863618.00 epoch sec; seqNo:5589; depth 50.42 2024-11-06T03:26:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863618.01 epoch sec; seqNo:5589; 4061837.65 north; 594670.59 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:26:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863614.38 2024-11-06T03:26:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:58Z- waiting for 1730863618.38, time is 1730863618.01 2024-11-06T03:26:58Z- AHRS_M2 msg: 1730863618.39 epoch sec; seqNo:5247 2024-11-06T03:26:58Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 6.03 psi 2024-11-06T03:26:58Z- handleDepth()- Depth_Keller msg: 1730863618.39 epoch sec; seqNo:5590; depth 50.45 2024-11-06T03:26:58Z- handleDVL() - DVL msg epochMillisec: 1730863618.41 2024-11-06T03:26:58Z- handleDvl() - RDI_Pathfinder msg: 1730863618.41 sec; seqNo:1939; #beams:4 2024-11-06T03:26:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.30 , 35.51 , 37.57 , 38.87 2024-11-06T03:26:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.15 , 0.04 2024-11-06T03:26:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863618.43 epoch sec; seqNo:5590; 4061838.03 north; 594670.59 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:26:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863614.38 2024-11-06T03:26:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:58Z- waiting for 1730863618.38, time is 1730863618.43 2024-11-06T03:26:58Z- LcmTrn::updateTrn() >>>> pose time 1730863618.43, meas time 1730863618.41 <<<< 2024-11-06T03:26:58Z- LcmTrn::updateTrn() equating pose 1730863618.41 and meas times 1730863618.41 2024-11-06T03:26:58Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863618.41 2024-11-06T03:26:58Z- TerrainNav::motionUpdate() - currEstimate = (4061838, 594670.59, 50.45) 2024-11-06T03:26:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:26:58Z- TerrainNav:: SubcloudNIS is 0.45, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:26:58Z- TNavPF::Projecting Measurements 2024-11-06T03:26:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:26:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:26:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:26:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:26:58Z- TNavFilter::4 good beams. 2024-11-06T03:26:58Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:26:58Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:26:58Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:26:58Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:26:58Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:26:58Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:26:58Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:26:58Z- TerrainMapOctree: (vehN, vehE ) = (4061838.03, 594670.59). 2024-11-06T03:26:58Z- TerrainMapOctree: Min Distance = 118.56. 2024-11-06T03:26:58Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:26:58Z- Forcing Subcloud Comparison 2024-11-06T03:26:58Z- Weighting particles with subcloud comparison 2024-11-06T03:26:58Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:58Z- meanExpectedMeasDiff: 1.724245 delta_rms_squared: 0.897008 alpha: 0.978190 2024-11-06T03:26:58Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:58Z- meanExpectedMeasDiff: -1.562854 delta_rms_squared: 0.656988 alpha: 0.970457 2024-11-06T03:26:58Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:58Z- meanExpectedMeasDiff: -0.380092 delta_rms_squared: 0.731420 alpha: 0.973384 2024-11-06T03:26:58Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:26:58Z- meanExpectedMeasDiff: -0.795547 delta_rms_squared: 0.800298 alpha: 0.975619 2024-11-06T03:26:58Z- Subcloud NIS: 0.419460 2024-11-06T03:26:58Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:26:58Z- TNavPF:: sumWeights = 0.369632 2024-11-06T03:26:58Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:26:58Z- TNavPF::First Term of Map Covariance Matrix is 0.69 2024-11-06T03:26:58Z- TNavPF::Calculated NIS Value : 0.25 numBeams normalized NIS: 0.06 2024-11-06T03:26:58Z- TerrainNav::Resampling particle filter... 2024-11-06T03:26:58Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863618.41 sec, ping # 0. 2024-11-06T03:26:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:26:58Z- variances([0], [2], and psi_berg) = 0.691142 0.202855 0.000000 2024-11-06T03:26:58Z- publishEstimatesDecoder() - at 1730863618.41 sec 2024-11-06T03:26:58Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:26:58Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061868.37 594653.13 57.65 2024-11-06T03:26:58Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061867.11 594653.53 57.51 lat/lon: 36.70 -121.94 2024-11-06T03:26:58Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.69 0.20 0.33 0.00 2024-11-06T03:26:58Z- AHRS_M2 msg: 1730863618.79 epoch sec; seqNo:5248 2024-11-06T03:26:58Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 6.03 psi 2024-11-06T03:26:58Z- handleDepth()- Depth_Keller msg: 1730863618.80 epoch sec; seqNo:5591; depth 50.46 2024-11-06T03:26:58Z- handleDVL() - DVL msg epochMillisec: 1730863618.81 2024-11-06T03:26:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:26:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:26:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:26:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:26:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:26:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:26:58Z- handleDvl() - RDI_Pathfinder msg: 1730863618.81 sec; seqNo:1940; #beams:0 2024-11-06T03:26:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:26:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.15 , 0.04 2024-11-06T03:26:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863618.84 epoch sec; seqNo:5591; 4061838.41 north; 594670.59 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:26:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:26:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863618.41 2024-11-06T03:26:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:58Z- waiting for 1730863622.41, time is 1730863618.84 2024-11-06T03:26:59Z- handleDepth()- Depth_Keller msg: 1730863619.22 epoch sec; seqNo:5592; depth 50.50 2024-11-06T03:26:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863619.24 epoch sec; seqNo:5592; 4061838.79 north; 594670.58 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:26:59Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:26:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863618.41 2024-11-06T03:26:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:59Z- waiting for 1730863622.41, time is 1730863619.24 2024-11-06T03:26:59Z- AHRS_M2 msg: 1730863619.60 epoch sec; seqNo:5249 2024-11-06T03:26:59Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.04 psi 2024-11-06T03:26:59Z- handleDepth()- Depth_Keller msg: 1730863619.61 epoch sec; seqNo:5593; depth 50.51 2024-11-06T03:26:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863619.62 epoch sec; seqNo:5593; 4061839.16 north; 594669.36 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:26:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:26:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863618.41 2024-11-06T03:26:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:26:59Z- waiting for 1730863622.41, time is 1730863619.62 2024-11-06T03:27:00Z- AHRS_M2 msg: 1730863620.00 epoch sec; seqNo:5250 2024-11-06T03:27:00Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.04 psi 2024-11-06T03:27:00Z- handleDepth()- Depth_Keller msg: 1730863620.02 epoch sec; seqNo:5594; depth 50.54 2024-11-06T03:27:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863620.03 epoch sec; seqNo:5594; 4061839.16 north; 594669.36 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:27:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863618.41 2024-11-06T03:27:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:00Z- waiting for 1730863622.41, time is 1730863620.03 2024-11-06T03:27:00Z- AHRS_M2 msg: 1730863620.41 epoch sec; seqNo:5251 2024-11-06T03:27:00Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.04 psi 2024-11-06T03:27:00Z- handleDepth()- Depth_Keller msg: 1730863620.41 epoch sec; seqNo:5595; depth 50.55 2024-11-06T03:27:00Z- handleDVL() - DVL msg epochMillisec: 1730863620.47 2024-11-06T03:27:00Z- handleDvl() - RDI_Pathfinder msg: 1730863620.47 sec; seqNo:1941; #beams:4 2024-11-06T03:27:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.49 , 35.70 , 37.94 , 38.69 2024-11-06T03:27:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , 0.08 2024-11-06T03:27:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863620.49 epoch sec; seqNo:5595; 4061839.54 north; 594669.35 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:27:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863618.41 2024-11-06T03:27:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:00Z- waiting for 1730863622.41, time is 1730863620.49 2024-11-06T03:27:00Z- AHRS_M2 msg: 1730863620.81 epoch sec; seqNo:5252 2024-11-06T03:27:00Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.04 psi 2024-11-06T03:27:00Z- handleDepth()- Depth_Keller msg: 1730863620.82 epoch sec; seqNo:5596; depth 50.57 2024-11-06T03:27:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863620.83 epoch sec; seqNo:5596; 4061839.91 north; 594669.35 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T03:27:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863618.41 2024-11-06T03:27:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:00Z- waiting for 1730863622.41, time is 1730863620.83 2024-11-06T03:27:01Z- AHRS_M2 msg: 1730863621.22 epoch sec; seqNo:5253 2024-11-06T03:27:01Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.04 psi 2024-11-06T03:27:01Z- handleDepth()- Depth_Keller msg: 1730863621.22 epoch sec; seqNo:5597; depth 50.59 2024-11-06T03:27:01Z- handleDVL() - DVL msg epochMillisec: 1730863621.23 2024-11-06T03:27:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:27:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:27:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:27:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:27:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:27:01Z- handleDvl() - RDI_Pathfinder msg: 1730863621.23 sec; seqNo:1942; #beams:0 2024-11-06T03:27:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:27:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , 0.08 2024-11-06T03:27:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863621.24 epoch sec; seqNo:5597; 4061840.29 north; 594669.35 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:27:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863618.41 2024-11-06T03:27:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:01Z- waiting for 1730863622.41, time is 1730863621.24 2024-11-06T03:27:01Z- AHRS_M2 msg: 1730863621.63 epoch sec; seqNo:5254 2024-11-06T03:27:01Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.04 psi 2024-11-06T03:27:01Z- handleDepth()- Depth_Keller msg: 1730863621.65 epoch sec; seqNo:5598; depth 50.60 2024-11-06T03:27:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863621.66 epoch sec; seqNo:5598; 4061840.67 north; 594669.34 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:27:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863618.41 2024-11-06T03:27:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:01Z- waiting for 1730863622.41, time is 1730863621.66 2024-11-06T03:27:02Z- AHRS_M2 msg: 1730863622.02 epoch sec; seqNo:5255 2024-11-06T03:27:02Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.04 psi 2024-11-06T03:27:02Z- handleDepth()- Depth_Keller msg: 1730863622.03 epoch sec; seqNo:5599; depth 50.59 2024-11-06T03:27:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863622.04 epoch sec; seqNo:5599; 4061841.05 north; 594669.34 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:27:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863618.41 2024-11-06T03:27:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:02Z- waiting for 1730863622.41, time is 1730863622.04 2024-11-06T03:27:02Z- AHRS_M2 msg: 1730863622.43 epoch sec; seqNo:5256 2024-11-06T03:27:02Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.04 psi 2024-11-06T03:27:02Z- handleDepth()- Depth_Keller msg: 1730863622.44 epoch sec; seqNo:5600; depth 50.59 2024-11-06T03:27:02Z- handleDVL() - DVL msg epochMillisec: 1730863622.46 2024-11-06T03:27:02Z- handleDvl() - RDI_Pathfinder msg: 1730863622.46 sec; seqNo:1943; #beams:4 2024-11-06T03:27:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.05 , 35.70 , 38.13 , 38.87 2024-11-06T03:27:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.02 , 0.05 2024-11-06T03:27:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863622.49 epoch sec; seqNo:5600; 4061841.43 north; 594669.33 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:27:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863618.41 2024-11-06T03:27:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:02Z- waiting for 1730863622.41, time is 1730863622.49 2024-11-06T03:27:02Z- LcmTrn::updateTrn() >>>> pose time 1730863622.49, meas time 1730863622.46 <<<< 2024-11-06T03:27:02Z- LcmTrn::updateTrn() equating pose 1730863622.46 and meas times 1730863622.46 2024-11-06T03:27:02Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863622.46 2024-11-06T03:27:02Z- TerrainNav::motionUpdate() - currEstimate = (4061841, 594669.33, 50.59) 2024-11-06T03:27:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:27:02Z- TerrainNav:: SubcloudNIS is 0.42, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:27:02Z- TNavPF::Projecting Measurements 2024-11-06T03:27:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:27:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:27:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:27:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:27:02Z- TNavFilter::4 good beams. 2024-11-06T03:27:02Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:27:02Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:27:02Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:27:02Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:27:02Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:27:02Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:27:02Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:27:02Z- TerrainMapOctree: (vehN, vehE ) = (4061841.43, 594669.33). 2024-11-06T03:27:02Z- TerrainMapOctree: Min Distance = 121.93. 2024-11-06T03:27:02Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:27:02Z- Forcing Subcloud Comparison 2024-11-06T03:27:02Z- Weighting particles with subcloud comparison 2024-11-06T03:27:02Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:02Z- meanExpectedMeasDiff: 1.256354 delta_rms_squared: 0.929383 alpha: 0.978934 2024-11-06T03:27:02Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:02Z- meanExpectedMeasDiff: -0.950893 delta_rms_squared: 0.662169 alpha: 0.970682 2024-11-06T03:27:02Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:02Z- meanExpectedMeasDiff: 0.094421 delta_rms_squared: 0.750595 alpha: 0.974046 2024-11-06T03:27:02Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:02Z- meanExpectedMeasDiff: -0.103444 delta_rms_squared: 0.834612 alpha: 0.976598 2024-11-06T03:27:02Z- Subcloud NIS: 0.386052 2024-11-06T03:27:02Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:27:02Z- TNavPF:: sumWeights = 0.562926 2024-11-06T03:27:02Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:27:02Z- TNavPF::First Term of Map Covariance Matrix is 0.47 2024-11-06T03:27:02Z- TNavPF::Calculated NIS Value : 0.29 numBeams normalized NIS: 0.07 2024-11-06T03:27:02Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863622.46 sec, ping # 0. 2024-11-06T03:27:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:27:02Z- variances([0], [2], and psi_berg) = 0.684909 0.205159 0.000000 2024-11-06T03:27:02Z- publishEstimatesDecoder() - at 1730863622.46 sec 2024-11-06T03:27:02Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:27:02Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061871.09 594651.30 57.54 2024-11-06T03:27:02Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061870.50 594652.27 57.63 lat/lon: 36.70 -121.94 2024-11-06T03:27:02Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.68 0.21 0.34 0.00 2024-11-06T03:27:02Z- AHRS_M2 msg: 1730863622.84 epoch sec; seqNo:5257 2024-11-06T03:27:02Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.04 psi 2024-11-06T03:27:02Z- handleDepth()- Depth_Keller msg: 1730863622.85 epoch sec; seqNo:5601; depth 50.59 2024-11-06T03:27:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863622.87 epoch sec; seqNo:5601; 4061841.43 north; 594669.33 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:27:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863622.46 2024-11-06T03:27:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:02Z- waiting for 1730863626.46, time is 1730863622.87 2024-11-06T03:27:03Z- AHRS_M2 msg: 1730863623.24 epoch sec; seqNo:5258 2024-11-06T03:27:03Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.05 psi 2024-11-06T03:27:03Z- handleDepth()- Depth_Keller msg: 1730863623.25 epoch sec; seqNo:5602; depth 50.59 2024-11-06T03:27:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863623.29 epoch sec; seqNo:5602; 4061841.81 north; 594669.33 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:27:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863622.46 2024-11-06T03:27:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:03Z- waiting for 1730863626.46, time is 1730863623.29 2024-11-06T03:27:03Z- AHRS_M2 msg: 1730863623.64 epoch sec; seqNo:5259 2024-11-06T03:27:03Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.05 psi 2024-11-06T03:27:03Z- handleDepth()- Depth_Keller msg: 1730863623.65 epoch sec; seqNo:5603; depth 50.59 2024-11-06T03:27:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863623.66 epoch sec; seqNo:5603; 4061842.19 north; 594669.32 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:27:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863622.46 2024-11-06T03:27:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:03Z- waiting for 1730863626.46, time is 1730863623.66 2024-11-06T03:27:04Z- AHRS_M2 msg: 1730863624.04 epoch sec; seqNo:5260 2024-11-06T03:27:04Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.05 psi 2024-11-06T03:27:04Z- handleDepth()- Depth_Keller msg: 1730863624.05 epoch sec; seqNo:5604; depth 50.57 2024-11-06T03:27:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863624.06 epoch sec; seqNo:5604; 4061842.57 north; 594669.32 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T03:27:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863622.46 2024-11-06T03:27:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:04Z- waiting for 1730863626.46, time is 1730863624.06 2024-11-06T03:27:04Z- handleDepth()- Depth_Keller msg: 1730863624.47 epoch sec; seqNo:5605; depth 50.56 2024-11-06T03:27:04Z- handleDVL() - DVL msg epochMillisec: 1730863624.49 2024-11-06T03:27:04Z- handleDvl() - RDI_Pathfinder msg: 1730863624.49 sec; seqNo:1944; #beams:4 2024-11-06T03:27:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 35.70 , 38.31 , 38.50 2024-11-06T03:27:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.04 , -0.04 2024-11-06T03:27:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863624.51 epoch sec; seqNo:5605; 4061842.93 north; 594668.10 east; 36.70 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T03:27:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863622.46 2024-11-06T03:27:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:04Z- waiting for 1730863626.46, time is 1730863624.51 2024-11-06T03:27:04Z- AHRS_M2 msg: 1730863624.85 epoch sec; seqNo:5261 2024-11-06T03:27:04Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.05 psi 2024-11-06T03:27:04Z- handleDepth()- Depth_Keller msg: 1730863624.86 epoch sec; seqNo:5606; depth 50.55 2024-11-06T03:27:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863624.87 epoch sec; seqNo:5606; 4061843.31 north; 594668.09 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:27:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863622.46 2024-11-06T03:27:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:05Z- waiting for 1730863626.46, time is 1730863624.87 2024-11-06T03:27:05Z- AHRS_M2 msg: 1730863625.27 epoch sec; seqNo:5262 2024-11-06T03:27:05Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.05 psi 2024-11-06T03:27:05Z- handleDepth()- Depth_Keller msg: 1730863625.27 epoch sec; seqNo:5607; depth 50.51 2024-11-06T03:27:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863625.28 epoch sec; seqNo:5607; 4061843.69 north; 594668.09 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:27:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863622.46 2024-11-06T03:27:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:05Z- waiting for 1730863626.46, time is 1730863625.28 2024-11-06T03:27:05Z- AHRS_M2 msg: 1730863625.66 epoch sec; seqNo:5263 2024-11-06T03:27:05Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.05 psi 2024-11-06T03:27:05Z- handleDepth()- Depth_Keller msg: 1730863625.67 epoch sec; seqNo:5608; depth 50.50 2024-11-06T03:27:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863625.68 epoch sec; seqNo:5608; 4061844.07 north; 594668.08 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:27:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863622.46 2024-11-06T03:27:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:05Z- waiting for 1730863626.46, time is 1730863625.68 2024-11-06T03:27:06Z- AHRS_M2 msg: 1730863626.06 epoch sec; seqNo:5264 2024-11-06T03:27:06Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.05 psi 2024-11-06T03:27:06Z- handleDepth()- Depth_Keller msg: 1730863626.07 epoch sec; seqNo:5609; depth 50.48 2024-11-06T03:27:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863626.09 epoch sec; seqNo:5609; 4061844.45 north; 594668.08 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:27:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863622.46 2024-11-06T03:27:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:06Z- waiting for 1730863626.46, time is 1730863626.09 2024-11-06T03:27:06Z- AHRS_M2 msg: 1730863626.47 epoch sec; seqNo:5265 2024-11-06T03:27:06Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.05 psi 2024-11-06T03:27:06Z- handleDepth()- Depth_Keller msg: 1730863626.48 epoch sec; seqNo:5610; depth 50.47 2024-11-06T03:27:06Z- handleDVL() - DVL msg epochMillisec: 1730863626.51 2024-11-06T03:27:06Z- handleDvl() - RDI_Pathfinder msg: 1730863626.51 sec; seqNo:1945; #beams:4 2024-11-06T03:27:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.30 , 35.88 , 38.31 , 38.31 2024-11-06T03:27:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.05 , -0.12 2024-11-06T03:27:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863626.52 epoch sec; seqNo:5610; 4061844.83 north; 594668.07 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:27:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863622.46 2024-11-06T03:27:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:06Z- waiting for 1730863626.46, time is 1730863626.52 2024-11-06T03:27:06Z- LcmTrn::updateTrn() >>>> pose time 1730863626.52, meas time 1730863626.51 <<<< 2024-11-06T03:27:06Z- LcmTrn::updateTrn() equating pose 1730863626.51 and meas times 1730863626.51 2024-11-06T03:27:06Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863626.51 2024-11-06T03:27:06Z- TerrainNav::motionUpdate() - currEstimate = (4061845, 594668.07, 50.47) 2024-11-06T03:27:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:27:06Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:27:06Z- TNavPF::Projecting Measurements 2024-11-06T03:27:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:27:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:27:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:27:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:27:06Z- TNavFilter::4 good beams. 2024-11-06T03:27:06Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:27:06Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:27:06Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:27:06Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:27:06Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:27:06Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:27:06Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:27:06Z- TerrainMapOctree: (vehN, vehE ) = (4061844.83, 594668.07). 2024-11-06T03:27:06Z- TerrainMapOctree: Min Distance = 125.31. 2024-11-06T03:27:06Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:27:06Z- Forcing Subcloud Comparison 2024-11-06T03:27:06Z- Weighting particles with subcloud comparison 2024-11-06T03:27:06Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:06Z- meanExpectedMeasDiff: 0.571910 delta_rms_squared: 0.950309 alpha: 0.979388 2024-11-06T03:27:06Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:06Z- meanExpectedMeasDiff: -1.019105 delta_rms_squared: 0.688950 alpha: 0.971789 2024-11-06T03:27:06Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:06Z- meanExpectedMeasDiff: 0.838562 delta_rms_squared: 0.751471 alpha: 0.974075 2024-11-06T03:27:06Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:06Z- meanExpectedMeasDiff: -0.403104 delta_rms_squared: 0.796734 alpha: 0.975512 2024-11-06T03:27:06Z- Subcloud NIS: 0.359225 2024-11-06T03:27:06Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:27:06Z- TNavPF:: sumWeights = 0.572057 2024-11-06T03:27:06Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:27:06Z- TNavPF::First Term of Map Covariance Matrix is 0.35 2024-11-06T03:27:06Z- TNavPF::Calculated NIS Value : 0.29 numBeams normalized NIS: 0.07 2024-11-06T03:27:06Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863626.51 sec, ping # 0. 2024-11-06T03:27:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:27:06Z- variances([0], [2], and psi_berg) = 0.699210 0.207250 0.000000 2024-11-06T03:27:06Z- publishEstimatesDecoder() - at 1730863626.51 sec 2024-11-06T03:27:06Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:27:06Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061872.81 594652.03 57.75 2024-11-06T03:27:06Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061873.89 594651.02 57.51 lat/lon: 36.70 -121.94 2024-11-06T03:27:06Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.21 0.34 0.00 2024-11-06T03:27:06Z- AHRS_M2 msg: 1730863626.87 epoch sec; seqNo:5266 2024-11-06T03:27:06Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.05 psi 2024-11-06T03:27:06Z- handleDepth()- Depth_Keller msg: 1730863626.89 epoch sec; seqNo:5611; depth 50.45 2024-11-06T03:27:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863626.90 epoch sec; seqNo:5611; 4061845.20 north; 594668.07 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:27:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863626.51 2024-11-06T03:27:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:07Z- waiting for 1730863630.51, time is 1730863626.90 2024-11-06T03:27:07Z- AHRS_M2 msg: 1730863627.28 epoch sec; seqNo:5267 2024-11-06T03:27:07Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.05 psi 2024-11-06T03:27:07Z- handleDepth()- Depth_Keller msg: 1730863627.29 epoch sec; seqNo:5612; depth 50.43 2024-11-06T03:27:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863627.29 epoch sec; seqNo:5612; 4061845.20 north; 594668.07 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:27:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863626.51 2024-11-06T03:27:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:07Z- waiting for 1730863630.51, time is 1730863627.29 2024-11-06T03:27:07Z- AHRS_M2 msg: 1730863627.68 epoch sec; seqNo:5268 2024-11-06T03:27:07Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.05 psi 2024-11-06T03:27:07Z- handleDepth()- Depth_Keller msg: 1730863627.69 epoch sec; seqNo:5613; depth 50.43 2024-11-06T03:27:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863627.70 epoch sec; seqNo:5613; 4061845.58 north; 594668.07 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:27:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863626.51 2024-11-06T03:27:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:07Z- waiting for 1730863630.51, time is 1730863627.70 2024-11-06T03:27:08Z- AHRS_M2 msg: 1730863628.09 epoch sec; seqNo:5269 2024-11-06T03:27:08Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.05 psi 2024-11-06T03:27:08Z- handleDepth()- Depth_Keller msg: 1730863628.10 epoch sec; seqNo:5614; depth 50.42 2024-11-06T03:27:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863628.11 epoch sec; seqNo:5614; 4061845.96 north; 594668.06 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:27:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863626.51 2024-11-06T03:27:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:08Z- waiting for 1730863630.51, time is 1730863628.11 2024-11-06T03:27:08Z- AHRS_M2 msg: 1730863628.49 epoch sec; seqNo:5270 2024-11-06T03:27:08Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.04 psi 2024-11-06T03:27:08Z- handleDepth()- Depth_Keller msg: 1730863628.50 epoch sec; seqNo:5615; depth 50.39 2024-11-06T03:27:08Z- handleDVL() - DVL msg epochMillisec: 1730863628.53 2024-11-06T03:27:08Z- handleDvl() - RDI_Pathfinder msg: 1730863628.53 sec; seqNo:1946; #beams:4 2024-11-06T03:27:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.93 , 35.51 , 38.13 , 37.75 2024-11-06T03:27:08Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.18 , -0.09 2024-11-06T03:27:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863628.55 epoch sec; seqNo:5615; 4061846.72 north; 594668.05 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:27:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863626.51 2024-11-06T03:27:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:08Z- waiting for 1730863630.51, time is 1730863628.55 2024-11-06T03:27:08Z- AHRS_M2 msg: 1730863628.89 epoch sec; seqNo:5271 2024-11-06T03:27:08Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.04 psi 2024-11-06T03:27:08Z- handleDepth()- Depth_Keller msg: 1730863628.90 epoch sec; seqNo:5616; depth 50.42 2024-11-06T03:27:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863628.92 epoch sec; seqNo:5616; 4061846.72 north; 594668.05 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:27:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863626.51 2024-11-06T03:27:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:09Z- waiting for 1730863630.51, time is 1730863628.92 2024-11-06T03:27:09Z- AHRS_M2 msg: 1730863629.30 epoch sec; seqNo:5272 2024-11-06T03:27:09Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.04 psi 2024-11-06T03:27:09Z- handleDepth()- Depth_Keller msg: 1730863629.30 epoch sec; seqNo:5617; depth 50.40 2024-11-06T03:27:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863629.32 epoch sec; seqNo:5617; 4061847.09 north; 594666.83 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:27:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863626.51 2024-11-06T03:27:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:09Z- waiting for 1730863630.51, time is 1730863629.32 2024-11-06T03:27:09Z- AHRS_M2 msg: 1730863629.70 epoch sec; seqNo:5273 2024-11-06T03:27:09Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.04 psi 2024-11-06T03:27:09Z- handleDepth()- Depth_Keller msg: 1730863629.70 epoch sec; seqNo:5618; depth 50.40 2024-11-06T03:27:09Z- handleDVL() - DVL msg epochMillisec: 1730863629.71 2024-11-06T03:27:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:27:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:27:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:27:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:27:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:27:09Z- handleDvl() - RDI_Pathfinder msg: 1730863629.71 sec; seqNo:1947; #beams:0 2024-11-06T03:27:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:27:09Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.18 , -0.09 2024-11-06T03:27:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863629.71 epoch sec; seqNo:5618; 4061847.46 north; 594666.83 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:27:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863626.51 2024-11-06T03:27:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:09Z- waiting for 1730863630.51, time is 1730863629.71 2024-11-06T03:27:10Z- AHRS_M2 msg: 1730863630.11 epoch sec; seqNo:5274 2024-11-06T03:27:10Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.04 psi 2024-11-06T03:27:10Z- handleDepth()- Depth_Keller msg: 1730863630.12 epoch sec; seqNo:5619; depth 50.41 2024-11-06T03:27:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863630.13 epoch sec; seqNo:5619; 4061847.84 north; 594666.82 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:27:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863626.51 2024-11-06T03:27:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:10Z- waiting for 1730863630.51, time is 1730863630.13 2024-11-06T03:27:10Z- AHRS_M2 msg: 1730863630.51 epoch sec; seqNo:5275 2024-11-06T03:27:10Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.04 psi 2024-11-06T03:27:10Z- handleDepth()- Depth_Keller msg: 1730863630.51 epoch sec; seqNo:5620; depth 50.42 2024-11-06T03:27:10Z- handleDVL() - DVL msg epochMillisec: 1730863630.54 2024-11-06T03:27:10Z- handleDvl() - RDI_Pathfinder msg: 1730863630.54 sec; seqNo:1948; #beams:4 2024-11-06T03:27:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.30 , 35.51 , 38.31 , 37.57 2024-11-06T03:27:10Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.22 , 0.01 2024-11-06T03:27:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863630.57 epoch sec; seqNo:5620; 4061848.60 north; 594666.81 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:27:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863626.51 2024-11-06T03:27:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:10Z- waiting for 1730863630.51, time is 1730863630.57 2024-11-06T03:27:10Z- LcmTrn::updateTrn() >>>> pose time 1730863630.57, meas time 1730863630.54 <<<< 2024-11-06T03:27:10Z- LcmTrn::updateTrn() equating pose 1730863630.54 and meas times 1730863630.54 2024-11-06T03:27:10Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863630.54 2024-11-06T03:27:10Z- TerrainNav::motionUpdate() - currEstimate = (4061849, 594666.81, 50.42) 2024-11-06T03:27:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:27:10Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:27:10Z- TNavPF::Projecting Measurements 2024-11-06T03:27:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:27:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:27:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:27:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:27:10Z- TNavFilter::4 good beams. 2024-11-06T03:27:10Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:27:10Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:27:10Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:27:10Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:27:10Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:27:10Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:27:10Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:27:10Z- TerrainMapOctree: (vehN, vehE ) = (4061848.60, 594666.81). 2024-11-06T03:27:10Z- TerrainMapOctree: Min Distance = 129.00. 2024-11-06T03:27:10Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:27:10Z- Forcing Subcloud Comparison 2024-11-06T03:27:10Z- Weighting particles with subcloud comparison 2024-11-06T03:27:10Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:10Z- meanExpectedMeasDiff: 0.243993 delta_rms_squared: 0.941055 alpha: 0.979190 2024-11-06T03:27:10Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:10Z- meanExpectedMeasDiff: -1.306540 delta_rms_squared: 0.667849 alpha: 0.970924 2024-11-06T03:27:10Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:10Z- meanExpectedMeasDiff: 1.395768 delta_rms_squared: 0.703957 alpha: 0.972374 2024-11-06T03:27:10Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:10Z- meanExpectedMeasDiff: -1.073758 delta_rms_squared: 0.782286 alpha: 0.975071 2024-11-06T03:27:10Z- Subcloud NIS: 0.331785 2024-11-06T03:27:10Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:27:10Z- TNavPF:: sumWeights = 0.421529 2024-11-06T03:27:10Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:27:10Z- TNavPF::First Term of Map Covariance Matrix is 0.26 2024-11-06T03:27:10Z- TNavPF::Calculated NIS Value : 0.27 numBeams normalized NIS: 0.07 2024-11-06T03:27:10Z- TerrainNav::Resampling particle filter... 2024-11-06T03:27:10Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863630.54 sec, ping # 0. 2024-11-06T03:27:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:27:10Z- variances([0], [2], and psi_berg) = 0.705278 0.208093 0.000000 2024-11-06T03:27:10Z- publishEstimatesDecoder() - at 1730863630.54 sec 2024-11-06T03:27:10Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:27:10Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061878.83 594649.31 56.80 2024-11-06T03:27:10Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061877.67 594649.81 57.58 lat/lon: 36.70 -121.94 2024-11-06T03:27:10Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.71 0.21 0.36 0.00 2024-11-06T03:27:10Z- AHRS_M2 msg: 1730863630.91 epoch sec; seqNo:5276 2024-11-06T03:27:10Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.04 psi 2024-11-06T03:27:10Z- handleDepth()- Depth_Keller msg: 1730863630.92 epoch sec; seqNo:5621; depth 50.43 2024-11-06T03:27:10Z- handleDVL() - DVL msg epochMillisec: 1730863630.93 2024-11-06T03:27:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:27:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:27:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:27:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:27:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:27:10Z- handleDvl() - RDI_Pathfinder msg: 1730863630.93 sec; seqNo:1949; #beams:0 2024-11-06T03:27:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:27:10Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.22 , 0.01 2024-11-06T03:27:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863630.94 epoch sec; seqNo:5621; 4061848.60 north; 594666.81 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:27:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863630.54 2024-11-06T03:27:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:11Z- waiting for 1730863634.54, time is 1730863630.94 2024-11-06T03:27:11Z- AHRS_M2 msg: 1730863631.32 epoch sec; seqNo:5277 2024-11-06T03:27:11Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.04 psi 2024-11-06T03:27:11Z- handleDepth()- Depth_Keller msg: 1730863631.33 epoch sec; seqNo:5622; depth 50.44 2024-11-06T03:27:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863631.34 epoch sec; seqNo:5622; 4061848.98 north; 594666.81 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:27:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863630.54 2024-11-06T03:27:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:11Z- waiting for 1730863634.54, time is 1730863631.34 2024-11-06T03:27:11Z- AHRS_M2 msg: 1730863631.72 epoch sec; seqNo:5278 2024-11-06T03:27:11Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.04 psi 2024-11-06T03:27:11Z- handleDepth()- Depth_Keller msg: 1730863631.72 epoch sec; seqNo:5623; depth 50.44 2024-11-06T03:27:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863631.73 epoch sec; seqNo:5623; 4061849.36 north; 594666.80 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:27:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863630.54 2024-11-06T03:27:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:11Z- waiting for 1730863634.54, time is 1730863631.73 2024-11-06T03:27:12Z- AHRS_M2 msg: 1730863632.12 epoch sec; seqNo:5279 2024-11-06T03:27:12Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.04 psi 2024-11-06T03:27:12Z- handleDepth()- Depth_Keller msg: 1730863632.13 epoch sec; seqNo:5624; depth 50.46 2024-11-06T03:27:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863632.15 epoch sec; seqNo:5624; 4061849.72 north; 594665.58 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:27:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863630.54 2024-11-06T03:27:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:12Z- waiting for 1730863634.54, time is 1730863632.15 2024-11-06T03:27:12Z- AHRS_M2 msg: 1730863632.53 epoch sec; seqNo:5280 2024-11-06T03:27:12Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.04 psi 2024-11-06T03:27:12Z- handleDepth()- Depth_Keller msg: 1730863632.54 epoch sec; seqNo:5625; depth 50.45 2024-11-06T03:27:12Z- handleDVL() - DVL msg epochMillisec: 1730863632.56 2024-11-06T03:27:12Z- handleDvl() - RDI_Pathfinder msg: 1730863632.56 sec; seqNo:1950; #beams:4 2024-11-06T03:27:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 35.51 , 38.69 , 37.75 2024-11-06T03:27:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.14 , 0.09 2024-11-06T03:27:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863632.58 epoch sec; seqNo:5625; 4061850.10 north; 594665.58 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:27:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863630.54 2024-11-06T03:27:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:12Z- waiting for 1730863634.54, time is 1730863632.58 2024-11-06T03:27:12Z- AHRS_M2 msg: 1730863632.93 epoch sec; seqNo:5281 2024-11-06T03:27:12Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.04 psi 2024-11-06T03:27:12Z- handleDepth()- Depth_Keller msg: 1730863632.94 epoch sec; seqNo:5626; depth 50.47 2024-11-06T03:27:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863632.96 epoch sec; seqNo:5626; 4061850.48 north; 594665.57 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:27:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863630.54 2024-11-06T03:27:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:13Z- waiting for 1730863634.54, time is 1730863632.96 2024-11-06T03:27:13Z- AHRS_M2 msg: 1730863633.34 epoch sec; seqNo:5282 2024-11-06T03:27:13Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.04 psi 2024-11-06T03:27:13Z- handleDepth()- Depth_Keller msg: 1730863633.34 epoch sec; seqNo:5627; depth 50.48 2024-11-06T03:27:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863633.36 epoch sec; seqNo:5627; 4061850.86 north; 594665.57 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:27:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863630.54 2024-11-06T03:27:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:13Z- waiting for 1730863634.54, time is 1730863633.36 2024-11-06T03:27:13Z- AHRS_M2 msg: 1730863633.74 epoch sec; seqNo:5283 2024-11-06T03:27:13Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.04 psi 2024-11-06T03:27:13Z- handleDepth()- Depth_Keller msg: 1730863633.75 epoch sec; seqNo:5628; depth 50.49 2024-11-06T03:27:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863633.79 epoch sec; seqNo:5628; 4061851.24 north; 594665.56 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:27:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863630.54 2024-11-06T03:27:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:13Z- waiting for 1730863634.54, time is 1730863633.79 2024-11-06T03:27:14Z- AHRS_M2 msg: 1730863634.15 epoch sec; seqNo:5284 2024-11-06T03:27:14Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.04 psi 2024-11-06T03:27:14Z- handleDepth()- Depth_Keller msg: 1730863634.15 epoch sec; seqNo:5629; depth 50.47 2024-11-06T03:27:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863634.18 epoch sec; seqNo:5629; 4061851.62 north; 594665.56 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:27:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863630.54 2024-11-06T03:27:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:14Z- waiting for 1730863634.54, time is 1730863634.18 2024-11-06T03:27:14Z- AHRS_M2 msg: 1730863634.55 epoch sec; seqNo:5285 2024-11-06T03:27:14Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 6.04 psi 2024-11-06T03:27:14Z- handleDepth()- Depth_Keller msg: 1730863634.56 epoch sec; seqNo:5630; depth 50.48 2024-11-06T03:27:14Z- handleDVL() - DVL msg epochMillisec: 1730863634.58 2024-11-06T03:27:14Z- handleDvl() - RDI_Pathfinder msg: 1730863634.58 sec; seqNo:1951; #beams:4 2024-11-06T03:27:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.24 , 35.70 , 39.06 , 38.13 2024-11-06T03:27:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , 0.08 2024-11-06T03:27:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863634.59 epoch sec; seqNo:5630; 4061852.00 north; 594665.55 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:27:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863630.54 2024-11-06T03:27:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:14Z- waiting for 1730863634.54, time is 1730863634.59 2024-11-06T03:27:14Z- LcmTrn::updateTrn() >>>> pose time 1730863634.59, meas time 1730863634.58 <<<< 2024-11-06T03:27:14Z- LcmTrn::updateTrn() equating pose 1730863634.58 and meas times 1730863634.58 2024-11-06T03:27:14Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863634.58 2024-11-06T03:27:14Z- TerrainNav::motionUpdate() - currEstimate = (4061852, 594665.55, 50.48) 2024-11-06T03:27:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:27:14Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:27:14Z- TNavPF::Projecting Measurements 2024-11-06T03:27:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:27:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:27:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:27:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:27:14Z- TNavFilter::4 good beams. 2024-11-06T03:27:14Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:27:14Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:27:14Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:27:14Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:27:14Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:27:14Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:27:14Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:27:14Z- TerrainMapOctree: (vehN, vehE ) = (4061852.00, 594665.55). 2024-11-06T03:27:14Z- TerrainMapOctree: Min Distance = 132.42. 2024-11-06T03:27:14Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:27:14Z- Forcing Subcloud Comparison 2024-11-06T03:27:14Z- Weighting particles with subcloud comparison 2024-11-06T03:27:14Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:14Z- meanExpectedMeasDiff: 1.231580 delta_rms_squared: 0.980171 alpha: 0.980003 2024-11-06T03:27:14Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:14Z- meanExpectedMeasDiff: -0.854935 delta_rms_squared: 0.686951 alpha: 0.971710 2024-11-06T03:27:14Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:14Z- meanExpectedMeasDiff: 2.493869 delta_rms_squared: 0.828115 alpha: 0.976418 2024-11-06T03:27:14Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:14Z- meanExpectedMeasDiff: -0.126930 delta_rms_squared: 0.800284 alpha: 0.975618 2024-11-06T03:27:14Z- Subcloud NIS: 0.308229 2024-11-06T03:27:14Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:27:14Z- TNavPF:: sumWeights = 0.306901 2024-11-06T03:27:14Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:27:14Z- TNavPF::First Term of Map Covariance Matrix is 0.29 2024-11-06T03:27:14Z- TNavPF::Calculated NIS Value : 0.19 numBeams normalized NIS: 0.05 2024-11-06T03:27:14Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863634.58 sec, ping # 0. 2024-11-06T03:27:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:27:14Z- variances([0], [2], and psi_berg) = 0.697927 0.214691 0.000000 2024-11-06T03:27:14Z- publishEstimatesDecoder() - at 1730863634.58 sec 2024-11-06T03:27:14Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:27:14Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061885.31 594648.44 57.35 2024-11-06T03:27:14Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061881.06 594648.55 57.39 lat/lon: 36.70 -121.94 2024-11-06T03:27:14Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.21 0.34 0.00 2024-11-06T03:27:14Z- AHRS_M2 msg: 1730863634.95 epoch sec; seqNo:5286 2024-11-06T03:27:14Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.04 psi 2024-11-06T03:27:14Z- handleDepth()- Depth_Keller msg: 1730863634.96 epoch sec; seqNo:5631; depth 50.48 2024-11-06T03:27:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863634.99 epoch sec; seqNo:5631; 4061852.38 north; 594665.55 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:27:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863634.58 2024-11-06T03:27:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:15Z- waiting for 1730863638.58, time is 1730863634.99 2024-11-06T03:27:15Z- AHRS_M2 msg: 1730863635.35 epoch sec; seqNo:5287 2024-11-06T03:27:15Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.04 psi 2024-11-06T03:27:15Z- handleDepth()- Depth_Keller msg: 1730863635.36 epoch sec; seqNo:5632; depth 50.46 2024-11-06T03:27:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863635.37 epoch sec; seqNo:5632; 4061852.75 north; 594665.55 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:27:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863634.58 2024-11-06T03:27:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:15Z- waiting for 1730863638.58, time is 1730863635.37 2024-11-06T03:27:15Z- AHRS_M2 msg: 1730863635.76 epoch sec; seqNo:5288 2024-11-06T03:27:15Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.04 psi 2024-11-06T03:27:15Z- handleDepth()- Depth_Keller msg: 1730863635.77 epoch sec; seqNo:5633; depth 50.46 2024-11-06T03:27:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863635.78 epoch sec; seqNo:5633; 4061853.12 north; 594664.32 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:27:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863634.58 2024-11-06T03:27:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:15Z- waiting for 1730863638.58, time is 1730863635.78 2024-11-06T03:27:16Z- AHRS_M2 msg: 1730863636.19 epoch sec; seqNo:5289 2024-11-06T03:27:16Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:16Z- handleDepth()- Depth_Keller msg: 1730863636.20 epoch sec; seqNo:5634; depth 50.44 2024-11-06T03:27:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863636.23 epoch sec; seqNo:5634; 4061853.50 north; 594664.32 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:27:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863634.58 2024-11-06T03:27:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:16Z- waiting for 1730863638.58, time is 1730863636.23 2024-11-06T03:27:16Z- AHRS_M2 msg: 1730863636.57 epoch sec; seqNo:5290 2024-11-06T03:27:16Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:16Z- handleDepth()- Depth_Keller msg: 1730863636.57 epoch sec; seqNo:5635; depth 50.42 2024-11-06T03:27:16Z- handleDVL() - DVL msg epochMillisec: 1730863636.60 2024-11-06T03:27:16Z- handleDvl() - RDI_Pathfinder msg: 1730863636.60 sec; seqNo:1952; #beams:4 2024-11-06T03:27:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.24 , 35.70 , 39.06 , 38.13 2024-11-06T03:27:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , 0.01 2024-11-06T03:27:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863636.62 epoch sec; seqNo:5635; 4061853.50 north; 594664.32 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:27:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863634.58 2024-11-06T03:27:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:16Z- waiting for 1730863638.58, time is 1730863636.62 2024-11-06T03:27:16Z- AHRS_M2 msg: 1730863636.97 epoch sec; seqNo:5291 2024-11-06T03:27:16Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:16Z- handleDepth()- Depth_Keller msg: 1730863636.98 epoch sec; seqNo:5636; depth 50.41 2024-11-06T03:27:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863636.99 epoch sec; seqNo:5636; 4061853.88 north; 594664.31 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:27:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863634.58 2024-11-06T03:27:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:17Z- waiting for 1730863638.58, time is 1730863636.99 2024-11-06T03:27:17Z- AHRS_M2 msg: 1730863637.38 epoch sec; seqNo:5292 2024-11-06T03:27:17Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:17Z- handleDepth()- Depth_Keller msg: 1730863637.39 epoch sec; seqNo:5637; depth 50.37 2024-11-06T03:27:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863637.39 epoch sec; seqNo:5637; 4061854.26 north; 594664.31 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:27:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863634.58 2024-11-06T03:27:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:17Z- waiting for 1730863638.58, time is 1730863637.39 2024-11-06T03:27:17Z- AHRS_M2 msg: 1730863637.78 epoch sec; seqNo:5293 2024-11-06T03:27:17Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:17Z- handleDepth()- Depth_Keller msg: 1730863637.78 epoch sec; seqNo:5638; depth 50.35 2024-11-06T03:27:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863637.80 epoch sec; seqNo:5638; 4061854.64 north; 594664.30 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:27:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863634.58 2024-11-06T03:27:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:17Z- waiting for 1730863638.58, time is 1730863637.80 2024-11-06T03:27:18Z- AHRS_M2 msg: 1730863638.22 epoch sec; seqNo:5294 2024-11-06T03:27:18Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:18Z- handleDepth()- Depth_Keller msg: 1730863638.23 epoch sec; seqNo:5639; depth 50.33 2024-11-06T03:27:18Z- handleDVL() - DVL msg epochMillisec: 1730863638.28 2024-11-06T03:27:18Z- handleDvl() - RDI_Pathfinder msg: 1730863638.28 sec; seqNo:1953; #beams:4 2024-11-06T03:27:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.05 , 35.70 , 39.25 , 38.13 2024-11-06T03:27:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , -0.07 2024-11-06T03:27:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863638.30 epoch sec; seqNo:5639; 4061855.01 north; 594664.30 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:27:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863634.58 2024-11-06T03:27:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:18Z- waiting for 1730863638.58, time is 1730863638.30 2024-11-06T03:27:18Z- handleDepth()- Depth_Keller msg: 1730863638.61 epoch sec; seqNo:5640; depth 50.30 2024-11-06T03:27:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863638.64 epoch sec; seqNo:5640; 4061855.39 north; 594664.30 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:27:18Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:27:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863634.58 2024-11-06T03:27:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:18Z- waiting for 1730863638.58, time is 1730863638.64 2024-11-06T03:27:18Z- LcmTrn::updateTrn() >>>> pose time 1730863638.64, meas time 1730863638.28 <<<< 2024-11-06T03:27:18Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863638.64 2024-11-06T03:27:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863638.28 sec, ping # 0; waiting for INS data more recent than 1730863634.58... 2024-11-06T03:27:18Z- TerrainNav::motionUpdate() - currEstimate = (4061855, 594664.30, 50.30) 2024-11-06T03:27:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:27:18Z- TerrainNav:: SubcloudNIS is 0.31, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:27:18Z- TNavPF::Projecting Measurements 2024-11-06T03:27:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:27:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:27:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:27:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:27:18Z- TNavFilter::4 good beams. 2024-11-06T03:27:18Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:27:18Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:27:18Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:27:18Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:27:18Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:27:18Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:27:18Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:27:18Z- TerrainMapOctree: (vehN, vehE ) = (4061855.09, 594664.41). 2024-11-06T03:27:18Z- TerrainMapOctree: Min Distance = 135.53. 2024-11-06T03:27:18Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:27:18Z- Forcing Subcloud Comparison 2024-11-06T03:27:18Z- Weighting particles with subcloud comparison 2024-11-06T03:27:18Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:18Z- meanExpectedMeasDiff: 0.391486 delta_rms_squared: 0.968382 alpha: 0.979765 2024-11-06T03:27:18Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:18Z- meanExpectedMeasDiff: -1.242697 delta_rms_squared: 0.672475 alpha: 0.971118 2024-11-06T03:27:18Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:18Z- meanExpectedMeasDiff: 2.068309 delta_rms_squared: 0.828602 alpha: 0.976432 2024-11-06T03:27:18Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:18Z- meanExpectedMeasDiff: -0.537283 delta_rms_squared: 0.783069 alpha: 0.975096 2024-11-06T03:27:18Z- Subcloud NIS: 0.315364 2024-11-06T03:27:18Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:27:18Z- TNavPF:: sumWeights = 0.358515 2024-11-06T03:27:18Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:27:18Z- TNavPF::First Term of Map Covariance Matrix is 0.21 2024-11-06T03:27:18Z- TNavPF::Calculated NIS Value : 0.24 numBeams normalized NIS: 0.06 2024-11-06T03:27:18Z- TerrainNav::Resampling particle filter... 2024-11-06T03:27:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863638.28 sec. 2024-11-06T03:27:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:27:18Z- variances([0], [2], and psi_berg) = 0.737243 0.221615 0.000000 2024-11-06T03:27:18Z- publishEstimatesDecoder() - at 1730863638.64 sec 2024-11-06T03:27:18Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:27:18Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061885.54 594646.78 56.54 2024-11-06T03:27:18Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061884.47 594647.30 57.16 lat/lon: 36.70 -121.94 2024-11-06T03:27:18Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.74 0.22 0.59 0.00 2024-11-06T03:27:19Z- AHRS_M2 msg: 1730863638.99 epoch sec; seqNo:5295 2024-11-06T03:27:19Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.06 psi 2024-11-06T03:27:19Z- handleDepth()- Depth_Keller msg: 1730863639.01 epoch sec; seqNo:5641; depth 50.27 2024-11-06T03:27:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863639.04 epoch sec; seqNo:5641; 4061855.77 north; 594664.29 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:27:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863638.64 2024-11-06T03:27:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:19Z- waiting for 1730863642.64, time is 1730863639.04 2024-11-06T03:27:19Z- AHRS_M2 msg: 1730863639.41 epoch sec; seqNo:5296 2024-11-06T03:27:19Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.06 psi 2024-11-06T03:27:19Z- handleDepth()- Depth_Keller msg: 1730863639.42 epoch sec; seqNo:5642; depth 50.26 2024-11-06T03:27:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863639.43 epoch sec; seqNo:5642; 4061856.15 north; 594664.29 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:27:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863638.64 2024-11-06T03:27:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:19Z- waiting for 1730863642.64, time is 1730863639.43 2024-11-06T03:27:19Z- AHRS_M2 msg: 1730863639.80 epoch sec; seqNo:5297 2024-11-06T03:27:19Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.06 psi 2024-11-06T03:27:19Z- handleDepth()- Depth_Keller msg: 1730863639.81 epoch sec; seqNo:5643; depth 50.22 2024-11-06T03:27:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863639.82 epoch sec; seqNo:5643; 4061856.53 north; 594664.28 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:27:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863638.64 2024-11-06T03:27:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:19Z- waiting for 1730863642.64, time is 1730863639.82 2024-11-06T03:27:20Z- AHRS_M2 msg: 1730863640.23 epoch sec; seqNo:5298 2024-11-06T03:27:20Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:20Z- handleDepth()- Depth_Keller msg: 1730863640.24 epoch sec; seqNo:5644; depth 50.21 2024-11-06T03:27:20Z- handleDVL() - DVL msg epochMillisec: 1730863640.26 2024-11-06T03:27:20Z- handleDvl() - RDI_Pathfinder msg: 1730863640.26 sec; seqNo:1954; #beams:4 2024-11-06T03:27:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.05 , 35.88 , 38.69 , 37.94 2024-11-06T03:27:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.09 , -0.10 2024-11-06T03:27:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863640.29 epoch sec; seqNo:5644; 4061856.91 north; 594664.28 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:27:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863638.64 2024-11-06T03:27:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:20Z- waiting for 1730863642.64, time is 1730863640.29 2024-11-06T03:27:20Z- AHRS_M2 msg: 1730863640.61 epoch sec; seqNo:5299 2024-11-06T03:27:20Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:20Z- handleDepth()- Depth_Keller msg: 1730863640.62 epoch sec; seqNo:5645; depth 50.17 2024-11-06T03:27:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863640.65 epoch sec; seqNo:5645; 4061857.29 north; 594664.28 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:27:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863638.64 2024-11-06T03:27:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:20Z- waiting for 1730863642.64, time is 1730863640.65 2024-11-06T03:27:21Z- AHRS_M2 msg: 1730863641.01 epoch sec; seqNo:5300 2024-11-06T03:27:21Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:21Z- handleDepth()- Depth_Keller msg: 1730863641.02 epoch sec; seqNo:5646; depth 50.15 2024-11-06T03:27:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863641.03 epoch sec; seqNo:5646; 4061857.29 north; 594664.28 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:27:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863638.64 2024-11-06T03:27:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:21Z- waiting for 1730863642.64, time is 1730863641.03 2024-11-06T03:27:21Z- AHRS_M2 msg: 1730863641.41 epoch sec; seqNo:5301 2024-11-06T03:27:21Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:21Z- handleDepth()- Depth_Keller msg: 1730863641.43 epoch sec; seqNo:5647; depth 50.13 2024-11-06T03:27:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863641.44 epoch sec; seqNo:5647; 4061857.67 north; 594664.27 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:27:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863638.64 2024-11-06T03:27:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:21Z- waiting for 1730863642.64, time is 1730863641.44 2024-11-06T03:27:21Z- AHRS_M2 msg: 1730863641.82 epoch sec; seqNo:5302 2024-11-06T03:27:21Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:21Z- handleDepth()- Depth_Keller msg: 1730863641.82 epoch sec; seqNo:5648; depth 50.12 2024-11-06T03:27:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863641.83 epoch sec; seqNo:5648; 4061858.03 north; 594663.05 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:27:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863638.64 2024-11-06T03:27:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:21Z- waiting for 1730863642.64, time is 1730863641.83 2024-11-06T03:27:22Z- AHRS_M2 msg: 1730863642.23 epoch sec; seqNo:5303 2024-11-06T03:27:22Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:22Z- handleDepth()- Depth_Keller msg: 1730863642.24 epoch sec; seqNo:5649; depth 50.11 2024-11-06T03:27:22Z- handleDVL() - DVL msg epochMillisec: 1730863642.27 2024-11-06T03:27:22Z- handleDvl() - RDI_Pathfinder msg: 1730863642.27 sec; seqNo:1955; #beams:4 2024-11-06T03:27:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 35.70 , 38.69 , 37.75 2024-11-06T03:27:22Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.17 , -0.05 2024-11-06T03:27:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863642.28 epoch sec; seqNo:5649; 4061858.79 north; 594663.04 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:27:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863638.64 2024-11-06T03:27:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:22Z- waiting for 1730863642.64, time is 1730863642.28 2024-11-06T03:27:22Z- AHRS_M2 msg: 1730863642.63 epoch sec; seqNo:5304 2024-11-06T03:27:22Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:22Z- handleDepth()- Depth_Keller msg: 1730863642.64 epoch sec; seqNo:5650; depth 50.11 2024-11-06T03:27:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863642.66 epoch sec; seqNo:5650; 4061858.79 north; 594663.04 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:27:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863638.64 2024-11-06T03:27:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:22Z- waiting for 1730863642.64, time is 1730863642.66 2024-11-06T03:27:22Z- LcmTrn::updateTrn() >>>> pose time 1730863642.66, meas time 1730863642.27 <<<< 2024-11-06T03:27:22Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863642.66 2024-11-06T03:27:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863642.27 sec, ping # 0; waiting for INS data more recent than 1730863638.64... 2024-11-06T03:27:22Z- TerrainNav::motionUpdate() - currEstimate = (4061859, 594663.04, 50.11) 2024-11-06T03:27:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:27:22Z- TerrainNav:: SubcloudNIS is 0.32, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:27:22Z- TNavPF::Projecting Measurements 2024-11-06T03:27:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:27:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:27:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:27:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:27:22Z- TNavFilter::4 good beams. 2024-11-06T03:27:22Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:27:22Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:27:22Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:27:22Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:27:22Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:27:22Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:27:22Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:27:22Z- TerrainMapOctree: (vehN, vehE ) = (4061858.46, 594663.16). 2024-11-06T03:27:22Z- TerrainMapOctree: Min Distance = 138.94. 2024-11-06T03:27:22Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:27:22Z- Forcing Subcloud Comparison 2024-11-06T03:27:22Z- Weighting particles with subcloud comparison 2024-11-06T03:27:22Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:22Z- meanExpectedMeasDiff: 0.185068 delta_rms_squared: 1.375564 alpha: 0.985669 2024-11-06T03:27:22Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:22Z- meanExpectedMeasDiff: -1.616305 delta_rms_squared: 1.063725 alpha: 0.981545 2024-11-06T03:27:22Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:22Z- meanExpectedMeasDiff: 1.085085 delta_rms_squared: 1.414732 alpha: 0.986060 2024-11-06T03:27:22Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:22Z- meanExpectedMeasDiff: -1.192178 delta_rms_squared: 1.120357 alpha: 0.982462 2024-11-06T03:27:22Z- Subcloud NIS: 0.327991 2024-11-06T03:27:22Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:27:22Z- TNavPF:: sumWeights = 0.359527 2024-11-06T03:27:22Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:27:22Z- TNavPF::First Term of Map Covariance Matrix is 0.26 2024-11-06T03:27:22Z- TNavPF::Calculated NIS Value : 0.21 numBeams normalized NIS: 0.05 2024-11-06T03:27:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863642.27 sec. 2024-11-06T03:27:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:27:22Z- variances([0], [2], and psi_berg) = 0.769721 0.233088 0.000000 2024-11-06T03:27:22Z- publishEstimatesDecoder() - at 1730863642.66 sec 2024-11-06T03:27:22Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:27:22Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061889.27 594646.12 57.18 2024-11-06T03:27:22Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061887.93 594646.06 57.13 lat/lon: 36.70 -121.94 2024-11-06T03:27:22Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.77 0.23 0.64 0.00 2024-11-06T03:27:23Z- AHRS_M2 msg: 1730863643.03 epoch sec; seqNo:5305 2024-11-06T03:27:23Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:23Z- handleDepth()- Depth_Keller msg: 1730863643.04 epoch sec; seqNo:5651; depth 50.10 2024-11-06T03:27:23Z- handleDVL() - DVL msg epochMillisec: 1730863643.04 2024-11-06T03:27:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:27:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:27:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:27:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:27:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:27:23Z- handleDvl() - RDI_Pathfinder msg: 1730863643.04 sec; seqNo:1956; #beams:0 2024-11-06T03:27:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:27:23Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.17 , -0.05 2024-11-06T03:27:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863643.05 epoch sec; seqNo:5651; 4061859.17 north; 594663.03 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:27:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863642.66 2024-11-06T03:27:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:23Z- waiting for 1730863646.66, time is 1730863643.05 2024-11-06T03:27:23Z- AHRS_M2 msg: 1730863643.43 epoch sec; seqNo:5306 2024-11-06T03:27:23Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.04 psi 2024-11-06T03:27:23Z- handleDepth()- Depth_Keller msg: 1730863643.45 epoch sec; seqNo:5652; depth 50.10 2024-11-06T03:27:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863643.46 epoch sec; seqNo:5652; 4061859.55 north; 594663.03 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:27:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863642.66 2024-11-06T03:27:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:23Z- waiting for 1730863646.66, time is 1730863643.46 2024-11-06T03:27:23Z- AHRS_M2 msg: 1730863643.84 epoch sec; seqNo:5307 2024-11-06T03:27:23Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.04 psi 2024-11-06T03:27:23Z- handleDepth()- Depth_Keller msg: 1730863643.85 epoch sec; seqNo:5653; depth 50.09 2024-11-06T03:27:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863643.86 epoch sec; seqNo:5653; 4061859.93 north; 594663.03 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:27:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863642.66 2024-11-06T03:27:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:24Z- waiting for 1730863646.66, time is 1730863643.86 2024-11-06T03:27:24Z- AHRS_M2 msg: 1730863644.24 epoch sec; seqNo:5308 2024-11-06T03:27:24Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.04 psi 2024-11-06T03:27:24Z- handleDepth()- Depth_Keller msg: 1730863644.25 epoch sec; seqNo:5654; depth 50.08 2024-11-06T03:27:24Z- handleDVL() - DVL msg epochMillisec: 1730863644.27 2024-11-06T03:27:24Z- handleDvl() - RDI_Pathfinder msg: 1730863644.27 sec; seqNo:1957; #beams:4 2024-11-06T03:27:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.24 , 35.88 , 39.06 , 37.94 2024-11-06T03:27:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.18 , 0.05 2024-11-06T03:27:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863644.28 epoch sec; seqNo:5654; 4061860.30 north; 594663.02 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:27:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863642.66 2024-11-06T03:27:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:24Z- waiting for 1730863646.66, time is 1730863644.28 2024-11-06T03:27:24Z- AHRS_M2 msg: 1730863644.65 epoch sec; seqNo:5309 2024-11-06T03:27:24Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.04 psi 2024-11-06T03:27:24Z- handleDepth()- Depth_Keller msg: 1730863644.66 epoch sec; seqNo:5655; depth 50.08 2024-11-06T03:27:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863644.68 epoch sec; seqNo:5655; 4061860.68 north; 594663.02 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:27:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863642.66 2024-11-06T03:27:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:24Z- waiting for 1730863646.66, time is 1730863644.68 2024-11-06T03:27:25Z- AHRS_M2 msg: 1730863645.06 epoch sec; seqNo:5310 2024-11-06T03:27:25Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.04 psi 2024-11-06T03:27:25Z- handleDepth()- Depth_Keller msg: 1730863645.06 epoch sec; seqNo:5656; depth 50.09 2024-11-06T03:27:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863645.07 epoch sec; seqNo:5656; 4061861.05 north; 594661.79 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:27:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863642.66 2024-11-06T03:27:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:25Z- waiting for 1730863646.66, time is 1730863645.07 2024-11-06T03:27:25Z- AHRS_M2 msg: 1730863645.46 epoch sec; seqNo:5311 2024-11-06T03:27:25Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:25Z- handleDepth()- Depth_Keller msg: 1730863645.46 epoch sec; seqNo:5657; depth 50.09 2024-11-06T03:27:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863645.47 epoch sec; seqNo:5657; 4061861.43 north; 594661.79 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:27:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863642.66 2024-11-06T03:27:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:25Z- waiting for 1730863646.66, time is 1730863645.47 2024-11-06T03:27:25Z- AHRS_M2 msg: 1730863645.86 epoch sec; seqNo:5312 2024-11-06T03:27:25Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:25Z- handleDepth()- Depth_Keller msg: 1730863645.87 epoch sec; seqNo:5658; depth 50.08 2024-11-06T03:27:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863645.88 epoch sec; seqNo:5658; 4061861.81 north; 594661.78 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:27:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863642.66 2024-11-06T03:27:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:26Z- waiting for 1730863646.66, time is 1730863645.88 2024-11-06T03:27:26Z- AHRS_M2 msg: 1730863646.27 epoch sec; seqNo:5313 2024-11-06T03:27:26Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:26Z- handleDepth()- Depth_Keller msg: 1730863646.28 epoch sec; seqNo:5659; depth 50.10 2024-11-06T03:27:26Z- handleDVL() - DVL msg epochMillisec: 1730863646.30 2024-11-06T03:27:26Z- handleDvl() - RDI_Pathfinder msg: 1730863646.30 sec; seqNo:1958; #beams:4 2024-11-06T03:27:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.42 , 35.88 , 39.44 , 38.13 2024-11-06T03:27:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.07 , 0.08 2024-11-06T03:27:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863646.31 epoch sec; seqNo:5659; 4061862.19 north; 594661.78 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:27:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863642.66 2024-11-06T03:27:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:26Z- waiting for 1730863646.66, time is 1730863646.31 2024-11-06T03:27:26Z- AHRS_M2 msg: 1730863646.67 epoch sec; seqNo:5314 2024-11-06T03:27:26Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:26Z- handleDepth()- Depth_Keller msg: 1730863646.68 epoch sec; seqNo:5660; depth 50.11 2024-11-06T03:27:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863646.69 epoch sec; seqNo:5660; 4061862.56 north; 594661.78 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:27:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863642.66 2024-11-06T03:27:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:26Z- waiting for 1730863646.66, time is 1730863646.69 2024-11-06T03:27:26Z- LcmTrn::updateTrn() >>>> pose time 1730863646.69, meas time 1730863646.30 <<<< 2024-11-06T03:27:26Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863646.69 2024-11-06T03:27:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863646.30 sec, ping # 0; waiting for INS data more recent than 1730863642.66... 2024-11-06T03:27:26Z- TerrainNav::motionUpdate() - currEstimate = (4061863, 594661.78, 50.11) 2024-11-06T03:27:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:27:26Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:27:26Z- TNavPF::Projecting Measurements 2024-11-06T03:27:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:27:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:27:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:27:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:27:26Z- TNavFilter::4 good beams. 2024-11-06T03:27:26Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:27:26Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:27:26Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:27:26Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:27:26Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:27:26Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:27:26Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:27:26Z- TerrainMapOctree: (vehN, vehE ) = (4061862.19, 594661.90). 2024-11-06T03:27:26Z- TerrainMapOctree: Min Distance = 142.65. 2024-11-06T03:27:26Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:27:26Z- Forcing Subcloud Comparison 2024-11-06T03:27:26Z- Weighting particles with subcloud comparison 2024-11-06T03:27:26Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:26Z- meanExpectedMeasDiff: 2.080920 delta_rms_squared: 1.421878 alpha: 0.986129 2024-11-06T03:27:26Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:26Z- meanExpectedMeasDiff: -0.362306 delta_rms_squared: 1.521615 alpha: 0.987027 2024-11-06T03:27:26Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:26Z- meanExpectedMeasDiff: 1.631733 delta_rms_squared: 1.481479 alpha: 0.986680 2024-11-06T03:27:26Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:26Z- meanExpectedMeasDiff: -0.624648 delta_rms_squared: 1.185220 alpha: 0.983406 2024-11-06T03:27:26Z- Subcloud NIS: 0.342476 2024-11-06T03:27:26Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:27:26Z- TNavPF:: sumWeights = 0.305954 2024-11-06T03:27:26Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:27:26Z- TNavPF::First Term of Map Covariance Matrix is 0.65 2024-11-06T03:27:26Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T03:27:26Z- TerrainNav::Resampling particle filter... 2024-11-06T03:27:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863646.30 sec. 2024-11-06T03:27:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:27:26Z- variances([0], [2], and psi_berg) = 0.839422 0.240001 0.000000 2024-11-06T03:27:26Z- publishEstimatesDecoder() - at 1730863646.69 sec 2024-11-06T03:27:26Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:27:26Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061892.66 594644.23 56.00 2024-11-06T03:27:26Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061891.80 594644.81 56.87 lat/lon: 36.70 -121.94 2024-11-06T03:27:26Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.84 0.24 0.66 0.00 2024-11-06T03:27:27Z- AHRS_M2 msg: 1730863647.07 epoch sec; seqNo:5315 2024-11-06T03:27:27Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:27Z- handleDepth()- Depth_Keller msg: 1730863647.08 epoch sec; seqNo:5661; depth 50.10 2024-11-06T03:27:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863647.09 epoch sec; seqNo:5661; 4061862.94 north; 594661.77 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:27:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863646.69 2024-11-06T03:27:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:27Z- waiting for 1730863650.69, time is 1730863647.09 2024-11-06T03:27:27Z- AHRS_M2 msg: 1730863647.47 epoch sec; seqNo:5316 2024-11-06T03:27:27Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:27Z- handleDepth()- Depth_Keller msg: 1730863647.49 epoch sec; seqNo:5662; depth 50.10 2024-11-06T03:27:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863647.50 epoch sec; seqNo:5662; 4061863.32 north; 594661.77 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:27:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863646.69 2024-11-06T03:27:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:27Z- waiting for 1730863650.69, time is 1730863647.50 2024-11-06T03:27:27Z- AHRS_M2 msg: 1730863647.88 epoch sec; seqNo:5317 2024-11-06T03:27:27Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.05 psi 2024-11-06T03:27:27Z- handleDepth()- Depth_Keller msg: 1730863647.88 epoch sec; seqNo:5663; depth 50.09 2024-11-06T03:27:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863647.89 epoch sec; seqNo:5663; 4061863.70 north; 594661.76 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:27:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863646.69 2024-11-06T03:27:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:28Z- waiting for 1730863650.69, time is 1730863647.89 2024-11-06T03:27:28Z- AHRS_M2 msg: 1730863648.29 epoch sec; seqNo:5318 2024-11-06T03:27:28Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.06 psi 2024-11-06T03:27:28Z- handleDepth()- Depth_Keller msg: 1730863648.30 epoch sec; seqNo:5664; depth 50.08 2024-11-06T03:27:28Z- handleDVL() - DVL msg epochMillisec: 1730863648.32 2024-11-06T03:27:28Z- handleDvl() - RDI_Pathfinder msg: 1730863648.32 sec; seqNo:1959; #beams:4 2024-11-06T03:27:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.42 , 36.07 , 39.44 , 38.31 2024-11-06T03:27:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , 0.03 2024-11-06T03:27:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863648.34 epoch sec; seqNo:5664; 4061864.08 north; 594661.76 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:27:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863646.69 2024-11-06T03:27:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:28Z- waiting for 1730863650.69, time is 1730863648.34 2024-11-06T03:27:28Z- AHRS_M2 msg: 1730863648.69 epoch sec; seqNo:5319 2024-11-06T03:27:28Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.06 psi 2024-11-06T03:27:28Z- handleDepth()- Depth_Keller msg: 1730863648.72 epoch sec; seqNo:5665; depth 50.07 2024-11-06T03:27:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863648.74 epoch sec; seqNo:5665; 4061864.44 north; 594660.54 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:27:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863646.69 2024-11-06T03:27:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:28Z- waiting for 1730863650.69, time is 1730863648.74 2024-11-06T03:27:29Z- AHRS_M2 msg: 1730863649.09 epoch sec; seqNo:5320 2024-11-06T03:27:29Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.06 psi 2024-11-06T03:27:29Z- handleDepth()- Depth_Keller msg: 1730863649.10 epoch sec; seqNo:5666; depth 50.06 2024-11-06T03:27:29Z- handleDVL() - DVL msg epochMillisec: 1730863649.10 2024-11-06T03:27:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:27:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:27:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:27:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:27:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:27:29Z- handleDvl() - RDI_Pathfinder msg: 1730863649.10 sec; seqNo:1960; #beams:0 2024-11-06T03:27:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:27:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , 0.03 2024-11-06T03:27:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863649.11 epoch sec; seqNo:5666; 4061864.82 north; 594660.53 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:27:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863646.69 2024-11-06T03:27:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:29Z- waiting for 1730863650.69, time is 1730863649.11 2024-11-06T03:27:29Z- AHRS_M2 msg: 1730863649.49 epoch sec; seqNo:5321 2024-11-06T03:27:29Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.06 psi 2024-11-06T03:27:29Z- handleDepth()- Depth_Keller msg: 1730863649.50 epoch sec; seqNo:5667; depth 50.04 2024-11-06T03:27:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863649.51 epoch sec; seqNo:5667; 4061864.82 north; 594660.53 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:27:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863646.69 2024-11-06T03:27:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:29Z- waiting for 1730863650.69, time is 1730863649.51 2024-11-06T03:27:29Z- AHRS_M2 msg: 1730863649.90 epoch sec; seqNo:5322 2024-11-06T03:27:29Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.06 psi 2024-11-06T03:27:29Z- handleDepth()- Depth_Keller msg: 1730863649.91 epoch sec; seqNo:5668; depth 50.04 2024-11-06T03:27:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863649.91 epoch sec; seqNo:5668; 4061865.20 north; 594660.53 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:27:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863646.69 2024-11-06T03:27:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:30Z- waiting for 1730863650.69, time is 1730863649.91 2024-11-06T03:27:30Z- AHRS_M2 msg: 1730863650.30 epoch sec; seqNo:5323 2024-11-06T03:27:30Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.06 psi 2024-11-06T03:27:30Z- handleDepth()- Depth_Keller msg: 1730863650.31 epoch sec; seqNo:5669; depth 50.02 2024-11-06T03:27:30Z- handleDVL() - DVL msg epochMillisec: 1730863650.34 2024-11-06T03:27:30Z- handleDvl() - RDI_Pathfinder msg: 1730863650.34 sec; seqNo:1961; #beams:4 2024-11-06T03:27:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.24 , 36.26 , 39.62 , 38.31 2024-11-06T03:27:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , -0.03 2024-11-06T03:27:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863650.35 epoch sec; seqNo:5669; 4061865.58 north; 594660.52 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:27:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863646.69 2024-11-06T03:27:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:30Z- waiting for 1730863650.69, time is 1730863650.35 2024-11-06T03:27:30Z- AHRS_M2 msg: 1730863650.71 epoch sec; seqNo:5324 2024-11-06T03:27:30Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.06 psi 2024-11-06T03:27:30Z- handleDepth()- Depth_Keller msg: 1730863650.71 epoch sec; seqNo:5670; depth 50.02 2024-11-06T03:27:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863650.73 epoch sec; seqNo:5670; 4061865.96 north; 594660.52 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:27:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863646.69 2024-11-06T03:27:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:30Z- waiting for 1730863650.69, time is 1730863650.73 2024-11-06T03:27:30Z- LcmTrn::updateTrn() >>>> pose time 1730863650.73, meas time 1730863650.34 <<<< 2024-11-06T03:27:30Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863650.73 2024-11-06T03:27:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863650.34 sec, ping # 0; waiting for INS data more recent than 1730863646.69... 2024-11-06T03:27:30Z- TerrainNav::motionUpdate() - currEstimate = (4061866, 594660.52, 50.02) 2024-11-06T03:27:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:27:30Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:27:30Z- TNavPF::Projecting Measurements 2024-11-06T03:27:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:27:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:27:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:27:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:27:30Z- TNavFilter::4 good beams. 2024-11-06T03:27:30Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:27:30Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:27:30Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:27:30Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:27:30Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:27:30Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:27:30Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:27:30Z- TerrainMapOctree: (vehN, vehE ) = (4061865.63, 594660.64). 2024-11-06T03:27:30Z- TerrainMapOctree: Min Distance = 146.14. 2024-11-06T03:27:30Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:27:30Z- Forcing Subcloud Comparison 2024-11-06T03:27:30Z- Weighting particles with subcloud comparison 2024-11-06T03:27:30Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:30Z- meanExpectedMeasDiff: 1.399666 delta_rms_squared: 1.483471 alpha: 0.986697 2024-11-06T03:27:30Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:30Z- meanExpectedMeasDiff: -0.118432 delta_rms_squared: 1.039123 alpha: 0.981116 2024-11-06T03:27:30Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:30Z- meanExpectedMeasDiff: 0.742598 delta_rms_squared: 1.271635 alpha: 0.984516 2024-11-06T03:27:30Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:30Z- meanExpectedMeasDiff: -0.792780 delta_rms_squared: 1.202523 alpha: 0.983640 2024-11-06T03:27:30Z- Subcloud NIS: 0.347597 2024-11-06T03:27:30Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:27:30Z- TNavPF:: sumWeights = 0.487474 2024-11-06T03:27:30Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:27:30Z- TNavPF::First Term of Map Covariance Matrix is 0.52 2024-11-06T03:27:30Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.06 2024-11-06T03:27:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863650.34 sec. 2024-11-06T03:27:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:27:31Z- variances([0], [2], and psi_berg) = 0.840476 0.243082 0.000000 2024-11-06T03:27:31Z- publishEstimatesDecoder() - at 1730863650.73 sec 2024-11-06T03:27:31Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:27:31Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061895.53 594642.38 56.81 2024-11-06T03:27:31Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061895.20 594643.55 56.65 lat/lon: 36.70 -121.94 2024-11-06T03:27:31Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.84 0.24 0.68 0.00 2024-11-06T03:27:31Z- AHRS_M2 msg: 1730863651.11 epoch sec; seqNo:5325 2024-11-06T03:27:31Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.06 psi 2024-11-06T03:27:31Z- handleDepth()- Depth_Keller msg: 1730863651.12 epoch sec; seqNo:5671; depth 49.98 2024-11-06T03:27:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863651.13 epoch sec; seqNo:5671; 4061866.34 north; 594660.51 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:27:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863650.73 2024-11-06T03:27:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:31Z- waiting for 1730863654.73, time is 1730863651.13 2024-11-06T03:27:31Z- AHRS_M2 msg: 1730863651.51 epoch sec; seqNo:5326 2024-11-06T03:27:31Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.07 psi 2024-11-06T03:27:31Z- handleDepth()- Depth_Keller msg: 1730863651.53 epoch sec; seqNo:5672; depth 49.97 2024-11-06T03:27:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863651.54 epoch sec; seqNo:5672; 4061866.72 north; 594660.51 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:27:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863650.73 2024-11-06T03:27:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:31Z- waiting for 1730863654.73, time is 1730863651.54 2024-11-06T03:27:31Z- AHRS_M2 msg: 1730863651.92 epoch sec; seqNo:5327 2024-11-06T03:27:31Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.07 psi 2024-11-06T03:27:31Z- handleDepth()- Depth_Keller msg: 1730863651.93 epoch sec; seqNo:5673; depth 49.94 2024-11-06T03:27:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863651.94 epoch sec; seqNo:5673; 4061867.10 north; 594660.51 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:27:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863650.73 2024-11-06T03:27:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:32Z- waiting for 1730863654.73, time is 1730863651.94 2024-11-06T03:27:32Z- AHRS_M2 msg: 1730863652.32 epoch sec; seqNo:5328 2024-11-06T03:27:32Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.07 psi 2024-11-06T03:27:32Z- handleDepth()- Depth_Keller msg: 1730863652.33 epoch sec; seqNo:5674; depth 49.92 2024-11-06T03:27:32Z- handleDVL() - DVL msg epochMillisec: 1730863652.34 2024-11-06T03:27:32Z- handleDvl() - RDI_Pathfinder msg: 1730863652.34 sec; seqNo:1962; #beams:4 2024-11-06T03:27:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.24 , 36.07 , 39.44 , 38.31 2024-11-06T03:27:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.06 2024-11-06T03:27:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863652.38 epoch sec; seqNo:5674; 4061867.48 north; 594660.50 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:27:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863650.73 2024-11-06T03:27:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:32Z- waiting for 1730863654.73, time is 1730863652.38 2024-11-06T03:27:32Z- AHRS_M2 msg: 1730863652.73 epoch sec; seqNo:5329 2024-11-06T03:27:32Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.07 psi 2024-11-06T03:27:32Z- handleDepth()- Depth_Keller msg: 1730863652.74 epoch sec; seqNo:5675; depth 49.90 2024-11-06T03:27:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863652.75 epoch sec; seqNo:5675; 4061867.85 north; 594660.50 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:27:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863650.73 2024-11-06T03:27:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:32Z- waiting for 1730863654.73, time is 1730863652.75 2024-11-06T03:27:33Z- AHRS_M2 msg: 1730863653.13 epoch sec; seqNo:5330 2024-11-06T03:27:33Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.07 psi 2024-11-06T03:27:33Z- handleDepth()- Depth_Keller msg: 1730863653.14 epoch sec; seqNo:5676; depth 49.88 2024-11-06T03:27:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863653.16 epoch sec; seqNo:5676; 4061868.23 north; 594660.49 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:27:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863650.73 2024-11-06T03:27:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:33Z- waiting for 1730863654.73, time is 1730863653.16 2024-11-06T03:27:33Z- AHRS_M2 msg: 1730863653.53 epoch sec; seqNo:5331 2024-11-06T03:27:33Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.07 psi 2024-11-06T03:27:33Z- handleDepth()- Depth_Keller msg: 1730863653.54 epoch sec; seqNo:5677; depth 49.85 2024-11-06T03:27:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863653.55 epoch sec; seqNo:5677; 4061868.23 north; 594660.49 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:27:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863650.73 2024-11-06T03:27:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:33Z- waiting for 1730863654.73, time is 1730863653.55 2024-11-06T03:27:33Z- AHRS_M2 msg: 1730863653.94 epoch sec; seqNo:5332 2024-11-06T03:27:33Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 6.07 psi 2024-11-06T03:27:33Z- handleDepth()- Depth_Keller msg: 1730863653.94 epoch sec; seqNo:5678; depth 49.84 2024-11-06T03:27:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863653.95 epoch sec; seqNo:5678; 4061868.61 north; 594660.49 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:27:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863650.73 2024-11-06T03:27:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:34Z- waiting for 1730863654.73, time is 1730863653.95 2024-11-06T03:27:34Z- handleDepth()- Depth_Keller msg: 1730863654.37 epoch sec; seqNo:5679; depth 49.82 2024-11-06T03:27:34Z- handleDVL() - DVL msg epochMillisec: 1730863654.39 2024-11-06T03:27:34Z- handleDvl() - RDI_Pathfinder msg: 1730863654.39 sec; seqNo:1963; #beams:4 2024-11-06T03:27:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.05 , 36.07 , 39.25 , 38.13 2024-11-06T03:27:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.10 , -0.05 2024-11-06T03:27:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863654.42 epoch sec; seqNo:5679; 4061868.99 north; 594660.48 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:27:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863650.73 2024-11-06T03:27:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:34Z- waiting for 1730863654.73, time is 1730863654.42 2024-11-06T03:27:34Z- AHRS_M2 msg: 1730863654.75 epoch sec; seqNo:5333 2024-11-06T03:27:34Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 6.06 psi 2024-11-06T03:27:34Z- handleDepth()- Depth_Keller msg: 1730863654.76 epoch sec; seqNo:5680; depth 49.81 2024-11-06T03:27:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863654.78 epoch sec; seqNo:5680; 4061869.36 north; 594659.26 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:27:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863650.73 2024-11-06T03:27:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:34Z- waiting for 1730863654.73, time is 1730863654.78 2024-11-06T03:27:34Z- LcmTrn::updateTrn() >>>> pose time 1730863654.78, meas time 1730863654.39 <<<< 2024-11-06T03:27:34Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863654.78 2024-11-06T03:27:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863654.39 sec, ping # 0; waiting for INS data more recent than 1730863650.73... 2024-11-06T03:27:34Z- TerrainNav::motionUpdate() - currEstimate = (4061869, 594659.26, 49.81) 2024-11-06T03:27:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:27:34Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:27:34Z- TNavPF::Projecting Measurements 2024-11-06T03:27:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:27:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:27:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:27:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:27:34Z- TNavFilter::4 good beams. 2024-11-06T03:27:34Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:27:34Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:27:34Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:27:34Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:27:34Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:27:34Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:27:34Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:27:34Z- TerrainMapOctree: (vehN, vehE ) = (4061869.03, 594659.38). 2024-11-06T03:27:34Z- TerrainMapOctree: Min Distance = 149.60. 2024-11-06T03:27:34Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:27:35Z- Forcing Subcloud Comparison 2024-11-06T03:27:35Z- Weighting particles with subcloud comparison 2024-11-06T03:27:35Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:35Z- meanExpectedMeasDiff: 1.106579 delta_rms_squared: 1.489149 alpha: 0.986747 2024-11-06T03:27:35Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:35Z- meanExpectedMeasDiff: -0.830045 delta_rms_squared: 1.069477 alpha: 0.981643 2024-11-06T03:27:35Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:35Z- meanExpectedMeasDiff: 0.086828 delta_rms_squared: 1.279516 alpha: 0.984610 2024-11-06T03:27:35Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:35Z- meanExpectedMeasDiff: -1.389919 delta_rms_squared: 1.227948 alpha: 0.983974 2024-11-06T03:27:35Z- Subcloud NIS: 0.353371 2024-11-06T03:27:35Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:27:35Z- TNavPF:: sumWeights = 0.434073 2024-11-06T03:27:35Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:27:35Z- TNavPF::First Term of Map Covariance Matrix is 0.52 2024-11-06T03:27:35Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T03:27:35Z- TerrainNav::Resampling particle filter... 2024-11-06T03:27:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863654.39 sec. 2024-11-06T03:27:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:27:35Z- variances([0], [2], and psi_berg) = 0.834499 0.247648 0.000000 2024-11-06T03:27:35Z- publishEstimatesDecoder() - at 1730863654.78 sec 2024-11-06T03:27:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:27:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061899.96 594641.56 57.71 2024-11-06T03:27:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061898.60 594642.29 56.55 lat/lon: 36.70 -121.94 2024-11-06T03:27:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.83 0.25 0.67 0.00 2024-11-06T03:27:35Z- AHRS_M2 msg: 1730863655.15 epoch sec; seqNo:5334 2024-11-06T03:27:35Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 6.06 psi 2024-11-06T03:27:35Z- handleDepth()- Depth_Keller msg: 1730863655.16 epoch sec; seqNo:5681; depth 49.80 2024-11-06T03:27:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863655.17 epoch sec; seqNo:5681; 4061869.74 north; 594659.26 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:27:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863654.78 2024-11-06T03:27:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:35Z- waiting for 1730863658.78, time is 1730863655.17 2024-11-06T03:27:35Z- AHRS_M2 msg: 1730863655.55 epoch sec; seqNo:5335 2024-11-06T03:27:35Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 6.06 psi 2024-11-06T03:27:35Z- handleDepth()- Depth_Keller msg: 1730863655.57 epoch sec; seqNo:5682; depth 49.77 2024-11-06T03:27:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863655.58 epoch sec; seqNo:5682; 4061870.11 north; 594659.25 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:27:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863654.78 2024-11-06T03:27:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:35Z- waiting for 1730863658.78, time is 1730863655.58 2024-11-06T03:27:35Z- AHRS_M2 msg: 1730863655.96 epoch sec; seqNo:5336 2024-11-06T03:27:35Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 6.06 psi 2024-11-06T03:27:35Z- handleDepth()- Depth_Keller msg: 1730863655.97 epoch sec; seqNo:5683; depth 49.77 2024-11-06T03:27:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863655.99 epoch sec; seqNo:5683; 4061870.49 north; 594659.25 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:27:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863654.78 2024-11-06T03:27:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:36Z- waiting for 1730863658.78, time is 1730863655.99 2024-11-06T03:27:36Z- handleDepth()- Depth_Keller msg: 1730863656.39 epoch sec; seqNo:5684; depth 49.77 2024-11-06T03:27:36Z- handleDVL() - DVL msg epochMillisec: 1730863656.42 2024-11-06T03:27:36Z- handleDvl() - RDI_Pathfinder msg: 1730863656.42 sec; seqNo:1964; #beams:4 2024-11-06T03:27:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.68 , 36.26 , 39.06 , 38.31 2024-11-06T03:27:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.11 , -0.02 2024-11-06T03:27:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863656.44 epoch sec; seqNo:5684; 4061870.87 north; 594659.24 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:27:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863654.78 2024-11-06T03:27:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:36Z- waiting for 1730863658.78, time is 1730863656.44 2024-11-06T03:27:36Z- AHRS_M2 msg: 1730863656.78 epoch sec; seqNo:5337 2024-11-06T03:27:36Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.06 psi 2024-11-06T03:27:36Z- handleDepth()- Depth_Keller msg: 1730863656.79 epoch sec; seqNo:5685; depth 49.76 2024-11-06T03:27:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863656.80 epoch sec; seqNo:5685; 4061871.25 north; 594659.24 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:27:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863654.78 2024-11-06T03:27:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:36Z- waiting for 1730863658.78, time is 1730863656.80 2024-11-06T03:27:37Z- AHRS_M2 msg: 1730863657.17 epoch sec; seqNo:5338 2024-11-06T03:27:37Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.06 psi 2024-11-06T03:27:37Z- handleDepth()- Depth_Keller msg: 1730863657.18 epoch sec; seqNo:5686; depth 49.76 2024-11-06T03:27:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863657.19 epoch sec; seqNo:5686; 4061871.63 north; 594659.24 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:27:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863654.78 2024-11-06T03:27:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:37Z- waiting for 1730863658.78, time is 1730863657.19 2024-11-06T03:27:37Z- AHRS_M2 msg: 1730863657.58 epoch sec; seqNo:5339 2024-11-06T03:27:37Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:37Z- handleDepth()- Depth_Keller msg: 1730863657.59 epoch sec; seqNo:5687; depth 49.76 2024-11-06T03:27:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863657.60 epoch sec; seqNo:5687; 4061872.01 north; 594659.23 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:27:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863654.78 2024-11-06T03:27:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:37Z- waiting for 1730863658.78, time is 1730863657.60 2024-11-06T03:27:37Z- AHRS_M2 msg: 1730863657.98 epoch sec; seqNo:5340 2024-11-06T03:27:37Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:37Z- handleDepth()- Depth_Keller msg: 1730863657.99 epoch sec; seqNo:5688; depth 49.75 2024-11-06T03:27:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863658.00 epoch sec; seqNo:5688; 4061872.39 north; 594659.23 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:27:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863654.78 2024-11-06T03:27:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:38Z- waiting for 1730863658.78, time is 1730863658.00 2024-11-06T03:27:38Z- AHRS_M2 msg: 1730863658.38 epoch sec; seqNo:5341 2024-11-06T03:27:38Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:38Z- handleDepth()- Depth_Keller msg: 1730863658.39 epoch sec; seqNo:5689; depth 49.75 2024-11-06T03:27:38Z- handleDVL() - DVL msg epochMillisec: 1730863658.42 2024-11-06T03:27:38Z- handleDvl() - RDI_Pathfinder msg: 1730863658.42 sec; seqNo:1965; #beams:4 2024-11-06T03:27:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 36.26 , 39.25 , 38.31 2024-11-06T03:27:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.11 , 0.01 2024-11-06T03:27:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863658.43 epoch sec; seqNo:5689; 4061872.75 north; 594658.00 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:27:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863654.78 2024-11-06T03:27:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:38Z- waiting for 1730863658.78, time is 1730863658.43 2024-11-06T03:27:38Z- AHRS_M2 msg: 1730863658.79 epoch sec; seqNo:5342 2024-11-06T03:27:38Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:38Z- handleDepth()- Depth_Keller msg: 1730863658.80 epoch sec; seqNo:5690; depth 49.75 2024-11-06T03:27:38Z- handleDVL() - DVL msg epochMillisec: 1730863658.80 2024-11-06T03:27:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:27:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:27:38Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:27:38Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:27:38Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:27:38Z- handleDvl() - RDI_Pathfinder msg: 1730863658.80 sec; seqNo:1966; #beams:0 2024-11-06T03:27:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:27:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.11 , 0.01 2024-11-06T03:27:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863658.83 epoch sec; seqNo:5690; 4061873.13 north; 594658.00 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:27:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863654.78 2024-11-06T03:27:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:38Z- waiting for 1730863658.78, time is 1730863658.83 2024-11-06T03:27:38Z- LcmTrn::updateTrn() >>>> pose time 1730863658.83, meas time 1730863658.80 <<<< 2024-11-06T03:27:38Z- LcmTrn::updateTrn() equating pose 1730863658.80 and meas times 1730863658.80 2024-11-06T03:27:38Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863658.80 2024-11-06T03:27:38Z- TerrainNav::motionUpdate() - currEstimate = (4061873, 594658.00, 49.75) 2024-11-06T03:27:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:27:38Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:27:38Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:27:38Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:27:38Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:27:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:27:38Z- variances([0], [2], and psi_berg) = 0.835690 0.250522 0.000000 2024-11-06T03:27:38Z- publishEstimatesDecoder() - at 1730863658.80 sec 2024-11-06T03:27:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:27:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061903.70 594640.32 58.20 2024-11-06T03:27:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061902.37 594641.03 56.49 lat/lon: 36.70 -121.94 2024-11-06T03:27:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.84 0.25 0.91 0.00 2024-11-06T03:27:39Z- AHRS_M2 msg: 1730863659.19 epoch sec; seqNo:5343 2024-11-06T03:27:39Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:39Z- handleDepth()- Depth_Keller msg: 1730863659.20 epoch sec; seqNo:5691; depth 49.75 2024-11-06T03:27:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863659.21 epoch sec; seqNo:5691; 4061873.13 north; 594658.00 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:27:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863658.80 2024-11-06T03:27:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:39Z- waiting for 1730863662.80, time is 1730863659.21 2024-11-06T03:27:39Z- AHRS_M2 msg: 1730863659.60 epoch sec; seqNo:5344 2024-11-06T03:27:39Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:39Z- handleDepth()- Depth_Keller msg: 1730863659.61 epoch sec; seqNo:5692; depth 49.76 2024-11-06T03:27:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863659.62 epoch sec; seqNo:5692; 4061873.51 north; 594657.99 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:27:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863658.80 2024-11-06T03:27:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:39Z- waiting for 1730863662.80, time is 1730863659.62 2024-11-06T03:27:40Z- AHRS_M2 msg: 1730863660.00 epoch sec; seqNo:5345 2024-11-06T03:27:40Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:40Z- handleDepth()- Depth_Keller msg: 1730863660.01 epoch sec; seqNo:5693; depth 49.75 2024-11-06T03:27:40Z- handleDVL() - DVL msg epochMillisec: 1730863660.01 2024-11-06T03:27:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:27:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:27:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:27:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:27:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:27:40Z- handleDvl() - RDI_Pathfinder msg: 1730863660.01 sec; seqNo:1967; #beams:0 2024-11-06T03:27:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:27:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.11 , 0.01 2024-11-06T03:27:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863660.02 epoch sec; seqNo:5693; 4061873.89 north; 594657.99 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:27:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863658.80 2024-11-06T03:27:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:40Z- waiting for 1730863662.80, time is 1730863660.02 2024-11-06T03:27:40Z- AHRS_M2 msg: 1730863660.40 epoch sec; seqNo:5346 2024-11-06T03:27:40Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:40Z- handleDepth()- Depth_Keller msg: 1730863660.41 epoch sec; seqNo:5694; depth 49.75 2024-11-06T03:27:40Z- handleDVL() - DVL msg epochMillisec: 1730863660.43 2024-11-06T03:27:40Z- handleDvl() - RDI_Pathfinder msg: 1730863660.43 sec; seqNo:1968; #beams:4 2024-11-06T03:27:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.05 , 36.26 , 39.25 , 38.50 2024-11-06T03:27:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.08 , 0.03 2024-11-06T03:27:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863660.46 epoch sec; seqNo:5694; 4061874.27 north; 594657.99 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:27:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863658.80 2024-11-06T03:27:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:40Z- waiting for 1730863662.80, time is 1730863660.46 2024-11-06T03:27:40Z- AHRS_M2 msg: 1730863660.81 epoch sec; seqNo:5347 2024-11-06T03:27:40Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:40Z- handleDepth()- Depth_Keller msg: 1730863660.82 epoch sec; seqNo:5695; depth 49.77 2024-11-06T03:27:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863660.84 epoch sec; seqNo:5695; 4061874.65 north; 594657.98 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:27:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863658.80 2024-11-06T03:27:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:40Z- waiting for 1730863662.80, time is 1730863660.84 2024-11-06T03:27:41Z- AHRS_M2 msg: 1730863661.21 epoch sec; seqNo:5348 2024-11-06T03:27:41Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:41Z- handleDepth()- Depth_Keller msg: 1730863661.22 epoch sec; seqNo:5696; depth 49.75 2024-11-06T03:27:41Z- handleDVL() - DVL msg epochMillisec: 1730863661.22 2024-11-06T03:27:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:27:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:27:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:27:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:27:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:27:41Z- handleDvl() - RDI_Pathfinder msg: 1730863661.22 sec; seqNo:1969; #beams:0 2024-11-06T03:27:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:27:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.08 , 0.03 2024-11-06T03:27:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863661.24 epoch sec; seqNo:5696; 4061875.03 north; 594657.98 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:27:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863658.80 2024-11-06T03:27:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:41Z- waiting for 1730863662.80, time is 1730863661.24 2024-11-06T03:27:41Z- AHRS_M2 msg: 1730863661.62 epoch sec; seqNo:5349 2024-11-06T03:27:41Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:41Z- handleDepth()- Depth_Keller msg: 1730863661.63 epoch sec; seqNo:5697; depth 49.75 2024-11-06T03:27:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863661.65 epoch sec; seqNo:5697; 4061875.40 north; 594657.97 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:27:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863658.80 2024-11-06T03:27:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:41Z- waiting for 1730863662.80, time is 1730863661.65 2024-11-06T03:27:42Z- AHRS_M2 msg: 1730863662.02 epoch sec; seqNo:5350 2024-11-06T03:27:42Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:42Z- handleDepth()- Depth_Keller msg: 1730863662.03 epoch sec; seqNo:5698; depth 49.76 2024-11-06T03:27:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863662.04 epoch sec; seqNo:5698; 4061875.78 north; 594657.97 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:27:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863658.80 2024-11-06T03:27:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:42Z- waiting for 1730863662.80, time is 1730863662.04 2024-11-06T03:27:42Z- AHRS_M2 msg: 1730863662.42 epoch sec; seqNo:5351 2024-11-06T03:27:42Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:42Z- handleDepth()- Depth_Keller msg: 1730863662.43 epoch sec; seqNo:5699; depth 49.75 2024-11-06T03:27:42Z- handleDVL() - DVL msg epochMillisec: 1730863662.46 2024-11-06T03:27:42Z- handleDvl() - RDI_Pathfinder msg: 1730863662.46 sec; seqNo:1970; #beams:4 2024-11-06T03:27:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.24 , 36.44 , 39.25 , 38.50 2024-11-06T03:27:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , 0.03 2024-11-06T03:27:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863662.47 epoch sec; seqNo:5699; 4061876.15 north; 594656.74 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:27:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863658.80 2024-11-06T03:27:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:42Z- waiting for 1730863662.80, time is 1730863662.47 2024-11-06T03:27:42Z- AHRS_M2 msg: 1730863662.83 epoch sec; seqNo:5352 2024-11-06T03:27:42Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:42Z- handleDepth()- Depth_Keller msg: 1730863662.83 epoch sec; seqNo:5700; depth 49.75 2024-11-06T03:27:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863662.85 epoch sec; seqNo:5700; 4061876.53 north; 594656.74 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:27:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863658.80 2024-11-06T03:27:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:42Z- waiting for 1730863662.80, time is 1730863662.85 2024-11-06T03:27:42Z- LcmTrn::updateTrn() >>>> pose time 1730863662.85, meas time 1730863662.46 <<<< 2024-11-06T03:27:42Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863662.85 2024-11-06T03:27:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863662.46 sec, ping # 0; waiting for INS data more recent than 1730863658.80... 2024-11-06T03:27:42Z- TerrainNav::motionUpdate() - currEstimate = (4061877, 594656.74, 49.75) 2024-11-06T03:27:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:27:42Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:27:43Z- TNavPF::Projecting Measurements 2024-11-06T03:27:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:27:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:27:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:27:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:27:43Z- TNavFilter::4 good beams. 2024-11-06T03:27:43Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:27:43Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:27:43Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:27:43Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:27:43Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:27:43Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:27:43Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:27:43Z- TerrainMapOctree: (vehN, vehE ) = (4061876.20, 594656.86). 2024-11-06T03:27:43Z- TerrainMapOctree: Min Distance = 156.85. 2024-11-06T03:27:43Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:27:43Z- Forcing Subcloud Comparison 2024-11-06T03:27:43Z- Weighting particles with subcloud comparison 2024-11-06T03:27:43Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:43Z- meanExpectedMeasDiff: 1.417782 delta_rms_squared: 1.859945 alpha: 0.989361 2024-11-06T03:27:43Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:43Z- meanExpectedMeasDiff: -0.450312 delta_rms_squared: 1.344089 alpha: 0.985338 2024-11-06T03:27:43Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:43Z- meanExpectedMeasDiff: 0.335500 delta_rms_squared: 1.588324 alpha: 0.987565 2024-11-06T03:27:43Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:43Z- meanExpectedMeasDiff: -0.019393 delta_rms_squared: 1.661985 alpha: 0.988109 2024-11-06T03:27:43Z- Subcloud NIS: 0.358180 2024-11-06T03:27:43Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:27:43Z- TNavPF:: sumWeights = 0.501279 2024-11-06T03:27:43Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:27:43Z- TNavPF::First Term of Map Covariance Matrix is 0.56 2024-11-06T03:27:43Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T03:27:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863662.46 sec. 2024-11-06T03:27:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:27:43Z- variances([0], [2], and psi_berg) = 0.834182 0.251201 0.000000 2024-11-06T03:27:43Z- publishEstimatesDecoder() - at 1730863662.85 sec 2024-11-06T03:27:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:27:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061909.41 594639.72 55.64 2024-11-06T03:27:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061905.78 594639.77 56.32 lat/lon: 36.70 -121.94 2024-11-06T03:27:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.83 0.25 0.71 0.00 2024-11-06T03:27:43Z- AHRS_M2 msg: 1730863663.23 epoch sec; seqNo:5353 2024-11-06T03:27:43Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:43Z- handleDepth()- Depth_Keller msg: 1730863663.24 epoch sec; seqNo:5701; depth 49.75 2024-11-06T03:27:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863663.24 epoch sec; seqNo:5701; 4061876.91 north; 594656.74 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:27:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863662.85 2024-11-06T03:27:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:43Z- waiting for 1730863666.85, time is 1730863663.24 2024-11-06T03:27:43Z- AHRS_M2 msg: 1730863663.64 epoch sec; seqNo:5354 2024-11-06T03:27:43Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:43Z- handleDepth()- Depth_Keller msg: 1730863663.65 epoch sec; seqNo:5702; depth 49.74 2024-11-06T03:27:43Z- handleDVL() - DVL msg epochMillisec: 1730863663.66 2024-11-06T03:27:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:27:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:27:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:27:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:27:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:27:43Z- handleDvl() - RDI_Pathfinder msg: 1730863663.66 sec; seqNo:1971; #beams:0 2024-11-06T03:27:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:27:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , 0.03 2024-11-06T03:27:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863663.67 epoch sec; seqNo:5702; 4061877.29 north; 594656.73 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:27:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863662.85 2024-11-06T03:27:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:43Z- waiting for 1730863666.85, time is 1730863663.67 2024-11-06T03:27:44Z- AHRS_M2 msg: 1730863664.04 epoch sec; seqNo:5355 2024-11-06T03:27:44Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:44Z- handleDepth()- Depth_Keller msg: 1730863664.05 epoch sec; seqNo:5703; depth 49.73 2024-11-06T03:27:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863664.07 epoch sec; seqNo:5703; 4061877.66 north; 594656.73 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:27:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863662.85 2024-11-06T03:27:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:44Z- waiting for 1730863666.85, time is 1730863664.07 2024-11-06T03:27:44Z- AHRS_M2 msg: 1730863664.44 epoch sec; seqNo:5356 2024-11-06T03:27:44Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:44Z- handleDepth()- Depth_Keller msg: 1730863664.45 epoch sec; seqNo:5704; depth 49.72 2024-11-06T03:27:44Z- handleDVL() - DVL msg epochMillisec: 1730863664.47 2024-11-06T03:27:44Z- handleDvl() - RDI_Pathfinder msg: 1730863664.47 sec; seqNo:1972; #beams:4 2024-11-06T03:27:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.42 , 36.44 , 39.25 , 38.69 2024-11-06T03:27:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.00 2024-11-06T03:27:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863664.49 epoch sec; seqNo:5704; 4061877.66 north; 594656.73 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:27:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863662.85 2024-11-06T03:27:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:44Z- waiting for 1730863666.85, time is 1730863664.49 2024-11-06T03:27:44Z- AHRS_M2 msg: 1730863664.85 epoch sec; seqNo:5357 2024-11-06T03:27:44Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:44Z- handleDepth()- Depth_Keller msg: 1730863664.86 epoch sec; seqNo:5705; depth 49.71 2024-11-06T03:27:44Z- handleDVL() - DVL msg epochMillisec: 1730863664.86 2024-11-06T03:27:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:27:44Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:27:44Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:27:44Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:27:44Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:27:44Z- handleDvl() - RDI_Pathfinder msg: 1730863664.86 sec; seqNo:1973; #beams:0 2024-11-06T03:27:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:27:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.00 2024-11-06T03:27:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863664.87 epoch sec; seqNo:5705; 4061878.04 north; 594656.72 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:27:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863662.85 2024-11-06T03:27:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:45Z- waiting for 1730863666.85, time is 1730863664.87 2024-11-06T03:27:45Z- AHRS_M2 msg: 1730863665.25 epoch sec; seqNo:5358 2024-11-06T03:27:45Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:45Z- handleDepth()- Depth_Keller msg: 1730863665.26 epoch sec; seqNo:5706; depth 49.70 2024-11-06T03:27:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863665.27 epoch sec; seqNo:5706; 4061878.42 north; 594656.72 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:27:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863662.85 2024-11-06T03:27:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:45Z- waiting for 1730863666.85, time is 1730863665.27 2024-11-06T03:27:45Z- AHRS_M2 msg: 1730863665.66 epoch sec; seqNo:5359 2024-11-06T03:27:45Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:45Z- handleDepth()- Depth_Keller msg: 1730863665.68 epoch sec; seqNo:5707; depth 49.68 2024-11-06T03:27:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863665.69 epoch sec; seqNo:5707; 4061878.80 north; 594656.72 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:27:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863662.85 2024-11-06T03:27:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:45Z- waiting for 1730863666.85, time is 1730863665.69 2024-11-06T03:27:46Z- AHRS_M2 msg: 1730863666.06 epoch sec; seqNo:5360 2024-11-06T03:27:46Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:46Z- handleDepth()- Depth_Keller msg: 1730863666.06 epoch sec; seqNo:5708; depth 49.69 2024-11-06T03:27:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863666.07 epoch sec; seqNo:5708; 4061879.18 north; 594656.71 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:27:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863662.85 2024-11-06T03:27:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:46Z- waiting for 1730863666.85, time is 1730863666.07 2024-11-06T03:27:46Z- AHRS_M2 msg: 1730863666.47 epoch sec; seqNo:5361 2024-11-06T03:27:46Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:46Z- handleDepth()- Depth_Keller msg: 1730863666.48 epoch sec; seqNo:5709; depth 49.67 2024-11-06T03:27:46Z- handleDVL() - DVL msg epochMillisec: 1730863666.50 2024-11-06T03:27:46Z- handleDvl() - RDI_Pathfinder msg: 1730863666.50 sec; seqNo:1974; #beams:4 2024-11-06T03:27:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.42 , 36.44 , 39.06 , 38.69 2024-11-06T03:27:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.04 2024-11-06T03:27:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863666.52 epoch sec; seqNo:5709; 4061879.56 north; 594656.71 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:27:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863662.85 2024-11-06T03:27:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:46Z- waiting for 1730863666.85, time is 1730863666.52 2024-11-06T03:27:46Z- AHRS_M2 msg: 1730863666.87 epoch sec; seqNo:5362 2024-11-06T03:27:46Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:46Z- handleDepth()- Depth_Keller msg: 1730863666.87 epoch sec; seqNo:5710; depth 49.65 2024-11-06T03:27:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863666.89 epoch sec; seqNo:5710; 4061879.94 north; 594656.70 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:27:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863662.85 2024-11-06T03:27:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:47Z- waiting for 1730863666.85, time is 1730863666.89 2024-11-06T03:27:47Z- LcmTrn::updateTrn() >>>> pose time 1730863666.89, meas time 1730863666.50 <<<< 2024-11-06T03:27:47Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863666.89 2024-11-06T03:27:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863666.50 sec, ping # 0; waiting for INS data more recent than 1730863662.85... 2024-11-06T03:27:47Z- TerrainNav::motionUpdate() - currEstimate = (4061880, 594656.70, 49.65) 2024-11-06T03:27:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:27:47Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:27:47Z- TNavPF::Projecting Measurements 2024-11-06T03:27:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:27:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:27:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:27:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:27:47Z- TNavFilter::4 good beams. 2024-11-06T03:27:47Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:27:47Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:27:47Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:27:47Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:27:47Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:27:47Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:27:47Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:27:47Z- TerrainMapOctree: (vehN, vehE ) = (4061879.61, 594656.71). 2024-11-06T03:27:47Z- TerrainMapOctree: Min Distance = 159.69. 2024-11-06T03:27:47Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:27:47Z- Forcing Subcloud Comparison 2024-11-06T03:27:47Z- Weighting particles with subcloud comparison 2024-11-06T03:27:47Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:47Z- meanExpectedMeasDiff: 1.312698 delta_rms_squared: 1.519402 alpha: 0.987008 2024-11-06T03:27:47Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:47Z- meanExpectedMeasDiff: -0.724291 delta_rms_squared: 1.099394 alpha: 0.982133 2024-11-06T03:27:47Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:47Z- meanExpectedMeasDiff: -0.064693 delta_rms_squared: 1.293875 alpha: 0.984778 2024-11-06T03:27:47Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:47Z- meanExpectedMeasDiff: -0.106964 delta_rms_squared: 1.438195 alpha: 0.986284 2024-11-06T03:27:47Z- Subcloud NIS: 0.359627 2024-11-06T03:27:47Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:27:47Z- TNavPF:: sumWeights = 0.517866 2024-11-06T03:27:47Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:27:47Z- TNavPF::First Term of Map Covariance Matrix is 0.54 2024-11-06T03:27:47Z- TNavPF::Calculated NIS Value : 0.21 numBeams normalized NIS: 0.05 2024-11-06T03:27:47Z- TerrainNav::Resampling particle filter... 2024-11-06T03:27:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863666.50 sec. 2024-11-06T03:27:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:27:47Z- variances([0], [2], and psi_berg) = 0.816449 0.253194 0.000000 2024-11-06T03:27:47Z- publishEstimatesDecoder() - at 1730863666.89 sec 2024-11-06T03:27:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:27:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061910.58 594639.14 57.24 2024-11-06T03:27:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061909.18 594639.73 56.19 lat/lon: 36.70 -121.94 2024-11-06T03:27:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.82 0.25 0.67 0.00 2024-11-06T03:27:47Z- AHRS_M2 msg: 1730863667.27 epoch sec; seqNo:5363 2024-11-06T03:27:47Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:47Z- handleDepth()- Depth_Keller msg: 1730863667.28 epoch sec; seqNo:5711; depth 49.63 2024-11-06T03:27:47Z- handleDVL() - DVL msg epochMillisec: 1730863667.29 2024-11-06T03:27:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:27:47Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:27:47Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:27:47Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:27:47Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:27:47Z- handleDvl() - RDI_Pathfinder msg: 1730863667.29 sec; seqNo:1975; #beams:0 2024-11-06T03:27:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:27:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.04 2024-11-06T03:27:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863667.30 epoch sec; seqNo:5711; 4061880.32 north; 594656.70 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:27:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863666.89 2024-11-06T03:27:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:47Z- waiting for 1730863670.89, time is 1730863667.30 2024-11-06T03:27:47Z- AHRS_M2 msg: 1730863667.67 epoch sec; seqNo:5364 2024-11-06T03:27:47Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:47Z- handleDepth()- Depth_Keller msg: 1730863667.68 epoch sec; seqNo:5712; depth 49.61 2024-11-06T03:27:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863667.69 epoch sec; seqNo:5712; 4061880.32 north; 594656.70 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:27:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863666.89 2024-11-06T03:27:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:47Z- waiting for 1730863670.89, time is 1730863667.69 2024-11-06T03:27:48Z- AHRS_M2 msg: 1730863668.08 epoch sec; seqNo:5365 2024-11-06T03:27:48Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 6.05 psi 2024-11-06T03:27:48Z- handleDepth()- Depth_Keller msg: 1730863668.09 epoch sec; seqNo:5713; depth 49.61 2024-11-06T03:27:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863668.10 epoch sec; seqNo:5713; 4061880.68 north; 594655.47 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:27:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863666.89 2024-11-06T03:27:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:48Z- waiting for 1730863670.89, time is 1730863668.10 2024-11-06T03:27:48Z- AHRS_M2 msg: 1730863668.48 epoch sec; seqNo:5366 2024-11-06T03:27:48Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.05 psi 2024-11-06T03:27:48Z- handleDepth()- Depth_Keller msg: 1730863668.49 epoch sec; seqNo:5714; depth 49.60 2024-11-06T03:27:48Z- handleDVL() - DVL msg epochMillisec: 1730863668.51 2024-11-06T03:27:48Z- handleDvl() - RDI_Pathfinder msg: 1730863668.51 sec; seqNo:1976; #beams:4 2024-11-06T03:27:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.05 , 36.26 , 39.06 , 38.69 2024-11-06T03:27:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , -0.06 2024-11-06T03:27:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863668.53 epoch sec; seqNo:5714; 4061881.06 north; 594655.47 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:27:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863666.89 2024-11-06T03:27:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:48Z- waiting for 1730863670.89, time is 1730863668.53 2024-11-06T03:27:48Z- AHRS_M2 msg: 1730863668.89 epoch sec; seqNo:5367 2024-11-06T03:27:48Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 6.05 psi 2024-11-06T03:27:48Z- handleDepth()- Depth_Keller msg: 1730863668.90 epoch sec; seqNo:5715; depth 49.59 2024-11-06T03:27:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863668.92 epoch sec; seqNo:5715; 4061881.44 north; 594655.47 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:27:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863666.89 2024-11-06T03:27:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:49Z- waiting for 1730863670.89, time is 1730863668.92 2024-11-06T03:27:49Z- AHRS_M2 msg: 1730863669.29 epoch sec; seqNo:5368 2024-11-06T03:27:49Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 6.04 psi 2024-11-06T03:27:49Z- handleDepth()- Depth_Keller msg: 1730863669.30 epoch sec; seqNo:5716; depth 49.58 2024-11-06T03:27:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863669.32 epoch sec; seqNo:5716; 4061881.82 north; 594655.46 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:27:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863666.89 2024-11-06T03:27:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:49Z- waiting for 1730863670.89, time is 1730863669.32 2024-11-06T03:27:49Z- AHRS_M2 msg: 1730863669.70 epoch sec; seqNo:5369 2024-11-06T03:27:49Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 6.04 psi 2024-11-06T03:27:49Z- handleDepth()- Depth_Keller msg: 1730863669.70 epoch sec; seqNo:5717; depth 49.58 2024-11-06T03:27:49Z- handleDVL() - DVL msg epochMillisec: 1730863669.70 2024-11-06T03:27:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:27:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:27:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:27:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:27:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:27:49Z- handleDvl() - RDI_Pathfinder msg: 1730863669.70 sec; seqNo:1977; #beams:0 2024-11-06T03:27:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:27:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.07 , -0.06 2024-11-06T03:27:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863669.72 epoch sec; seqNo:5717; 4061882.20 north; 594655.46 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:27:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863666.89 2024-11-06T03:27:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:49Z- waiting for 1730863670.89, time is 1730863669.72 2024-11-06T03:27:50Z- AHRS_M2 msg: 1730863670.11 epoch sec; seqNo:5370 2024-11-06T03:27:50Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 6.04 psi 2024-11-06T03:27:50Z- handleDepth()- Depth_Keller msg: 1730863670.11 epoch sec; seqNo:5718; depth 49.58 2024-11-06T03:27:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863670.12 epoch sec; seqNo:5718; 4061882.58 north; 594655.45 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:27:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863666.89 2024-11-06T03:27:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:50Z- waiting for 1730863670.89, time is 1730863670.12 2024-11-06T03:27:50Z- AHRS_M2 msg: 1730863670.50 epoch sec; seqNo:5371 2024-11-06T03:27:50Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 6.04 psi 2024-11-06T03:27:50Z- handleDepth()- Depth_Keller msg: 1730863670.51 epoch sec; seqNo:5719; depth 49.57 2024-11-06T03:27:50Z- handleDVL() - DVL msg epochMillisec: 1730863670.54 2024-11-06T03:27:50Z- handleDvl() - RDI_Pathfinder msg: 1730863670.54 sec; seqNo:1978; #beams:4 2024-11-06T03:27:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 36.26 , 39.06 , 38.87 2024-11-06T03:27:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.12 , -0.03 2024-11-06T03:27:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863670.55 epoch sec; seqNo:5719; 4061882.95 north; 594655.45 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:27:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863666.89 2024-11-06T03:27:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:50Z- waiting for 1730863670.89, time is 1730863670.55 2024-11-06T03:27:50Z- AHRS_M2 msg: 1730863670.91 epoch sec; seqNo:5372 2024-11-06T03:27:50Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 6.04 psi 2024-11-06T03:27:50Z- handleDepth()- Depth_Keller msg: 1730863670.92 epoch sec; seqNo:5720; depth 49.57 2024-11-06T03:27:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863670.94 epoch sec; seqNo:5720; 4061883.33 north; 594655.44 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:27:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863666.89 2024-11-06T03:27:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:51Z- waiting for 1730863670.89, time is 1730863670.94 2024-11-06T03:27:51Z- LcmTrn::updateTrn() >>>> pose time 1730863670.94, meas time 1730863670.54 <<<< 2024-11-06T03:27:51Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863670.94 2024-11-06T03:27:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863670.54 sec, ping # 0; waiting for INS data more recent than 1730863666.89... 2024-11-06T03:27:51Z- TerrainNav::motionUpdate() - currEstimate = (4061883, 594655.44, 49.57) 2024-11-06T03:27:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:27:51Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:27:51Z- TNavPF::Projecting Measurements 2024-11-06T03:27:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:27:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:27:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:27:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:27:51Z- TNavFilter::4 good beams. 2024-11-06T03:27:51Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:27:51Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:27:51Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:27:51Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:27:51Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:27:51Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:27:51Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:27:51Z- TerrainMapOctree: (vehN, vehE ) = (4061882.99, 594655.57). 2024-11-06T03:27:51Z- TerrainMapOctree: Min Distance = 163.11. 2024-11-06T03:27:51Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:27:51Z- Forcing Subcloud Comparison 2024-11-06T03:27:51Z- Weighting particles with subcloud comparison 2024-11-06T03:27:51Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:51Z- meanExpectedMeasDiff: 0.836069 delta_rms_squared: 1.312826 alpha: 0.984994 2024-11-06T03:27:51Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:51Z- meanExpectedMeasDiff: -1.099859 delta_rms_squared: 1.078199 alpha: 0.981788 2024-11-06T03:27:51Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:51Z- meanExpectedMeasDiff: -0.121232 delta_rms_squared: 1.259461 alpha: 0.984368 2024-11-06T03:27:51Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:51Z- meanExpectedMeasDiff: 0.909477 delta_rms_squared: 1.562313 alpha: 0.987360 2024-11-06T03:27:51Z- Subcloud NIS: 0.358686 2024-11-06T03:27:51Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:27:51Z- TNavPF:: sumWeights = 0.480149 2024-11-06T03:27:51Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:27:51Z- TNavPF::First Term of Map Covariance Matrix is 0.37 2024-11-06T03:27:51Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T03:27:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863670.54 sec. 2024-11-06T03:27:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:27:51Z- variances([0], [2], and psi_berg) = 0.797463 0.246678 0.000000 2024-11-06T03:27:51Z- publishEstimatesDecoder() - at 1730863670.94 sec 2024-11-06T03:27:51Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:27:51Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061911.47 594639.85 56.66 2024-11-06T03:27:51Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061912.58 594638.49 56.05 lat/lon: 36.70 -121.94 2024-11-06T03:27:51Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.80 0.25 0.67 0.00 2024-11-06T03:27:51Z- handleDepth()- Depth_Keller msg: 1730863671.34 epoch sec; seqNo:5721; depth 49.57 2024-11-06T03:27:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863671.36 epoch sec; seqNo:5721; 4061883.71 north; 594655.44 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:27:51Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:27:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863670.94 2024-11-06T03:27:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:51Z- waiting for 1730863674.94, time is 1730863671.36 2024-11-06T03:27:51Z- AHRS_M2 msg: 1730863671.71 epoch sec; seqNo:5373 2024-11-06T03:27:51Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 6.04 psi 2024-11-06T03:27:51Z- handleDepth()- Depth_Keller msg: 1730863671.73 epoch sec; seqNo:5722; depth 49.55 2024-11-06T03:27:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863671.74 epoch sec; seqNo:5722; 4061884.09 north; 594655.44 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:27:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863670.94 2024-11-06T03:27:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:51Z- waiting for 1730863674.94, time is 1730863671.74 2024-11-06T03:27:52Z- AHRS_M2 msg: 1730863672.13 epoch sec; seqNo:5374 2024-11-06T03:27:52Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 6.04 psi 2024-11-06T03:27:52Z- handleDepth()- Depth_Keller msg: 1730863672.13 epoch sec; seqNo:5723; depth 49.59 2024-11-06T03:27:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863672.20 epoch sec; seqNo:5723; 4061884.46 north; 594654.21 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:27:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863670.94 2024-11-06T03:27:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:52Z- waiting for 1730863674.94, time is 1730863672.20 2024-11-06T03:27:52Z- AHRS_M2 msg: 1730863672.53 epoch sec; seqNo:5375 2024-11-06T03:27:52Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 6.04 psi 2024-11-06T03:27:52Z- handleDepth()- Depth_Keller msg: 1730863672.53 epoch sec; seqNo:5724; depth 49.59 2024-11-06T03:27:52Z- handleDVL() - DVL msg epochMillisec: 1730863672.55 2024-11-06T03:27:52Z- handleDvl() - RDI_Pathfinder msg: 1730863672.55 sec; seqNo:1979; #beams:4 2024-11-06T03:27:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.05 , 36.26 , 38.87 , 38.87 2024-11-06T03:27:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.14 , 0.01 2024-11-06T03:27:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863672.58 epoch sec; seqNo:5724; 4061884.84 north; 594654.21 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:27:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863670.94 2024-11-06T03:27:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:52Z- waiting for 1730863674.94, time is 1730863672.58 2024-11-06T03:27:52Z- AHRS_M2 msg: 1730863672.93 epoch sec; seqNo:5376 2024-11-06T03:27:52Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 6.03 psi 2024-11-06T03:27:52Z- handleDepth()- Depth_Keller msg: 1730863672.94 epoch sec; seqNo:5725; depth 49.60 2024-11-06T03:27:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863672.96 epoch sec; seqNo:5725; 4061884.84 north; 594654.21 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:27:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863670.94 2024-11-06T03:27:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:53Z- waiting for 1730863674.94, time is 1730863672.96 2024-11-06T03:27:53Z- handleDepth()- Depth_Keller msg: 1730863673.36 epoch sec; seqNo:5726; depth 49.58 2024-11-06T03:27:53Z- handleDVL() - DVL msg epochMillisec: 1730863673.36 2024-11-06T03:27:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:27:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:27:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:27:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:27:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:27:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:27:53Z- handleDvl() - RDI_Pathfinder msg: 1730863673.36 sec; seqNo:1980; #beams:0 2024-11-06T03:27:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:27:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.14 , 0.01 2024-11-06T03:27:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863673.37 epoch sec; seqNo:5726; 4061885.21 north; 594654.20 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:27:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863670.94 2024-11-06T03:27:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:53Z- waiting for 1730863674.94, time is 1730863673.37 2024-11-06T03:27:53Z- AHRS_M2 msg: 1730863673.74 epoch sec; seqNo:5377 2024-11-06T03:27:53Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.03 psi 2024-11-06T03:27:53Z- handleDepth()- Depth_Keller msg: 1730863673.76 epoch sec; seqNo:5727; depth 49.60 2024-11-06T03:27:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863673.79 epoch sec; seqNo:5727; 4061885.59 north; 594654.20 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:27:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863670.94 2024-11-06T03:27:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:53Z- waiting for 1730863674.94, time is 1730863673.79 2024-11-06T03:27:54Z- AHRS_M2 msg: 1730863674.16 epoch sec; seqNo:5378 2024-11-06T03:27:54Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.03 psi 2024-11-06T03:27:54Z- handleDepth()- Depth_Keller msg: 1730863674.20 epoch sec; seqNo:5728; depth 49.61 2024-11-06T03:27:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863674.23 epoch sec; seqNo:5728; 4061885.97 north; 594654.19 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:27:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863670.94 2024-11-06T03:27:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:54Z- waiting for 1730863674.94, time is 1730863674.23 2024-11-06T03:27:54Z- AHRS_M2 msg: 1730863674.55 epoch sec; seqNo:5379 2024-11-06T03:27:54Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:27:54Z- handleDepth()- Depth_Keller msg: 1730863674.56 epoch sec; seqNo:5729; depth 49.61 2024-11-06T03:27:54Z- handleDVL() - DVL msg epochMillisec: 1730863674.58 2024-11-06T03:27:54Z- handleDvl() - RDI_Pathfinder msg: 1730863674.58 sec; seqNo:1981; #beams:4 2024-11-06T03:27:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.42 , 36.07 , 38.87 , 39.06 2024-11-06T03:27:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.08 , 0.03 2024-11-06T03:27:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863674.59 epoch sec; seqNo:5729; 4061886.35 north; 594654.19 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:27:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863670.94 2024-11-06T03:27:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:54Z- waiting for 1730863674.94, time is 1730863674.59 2024-11-06T03:27:54Z- AHRS_M2 msg: 1730863674.95 epoch sec; seqNo:5380 2024-11-06T03:27:54Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:27:54Z- handleDepth()- Depth_Keller msg: 1730863674.95 epoch sec; seqNo:5730; depth 49.62 2024-11-06T03:27:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863674.97 epoch sec; seqNo:5730; 4061886.73 north; 594654.19 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:27:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863670.94 2024-11-06T03:27:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:55Z- waiting for 1730863674.94, time is 1730863674.97 2024-11-06T03:27:55Z- LcmTrn::updateTrn() >>>> pose time 1730863674.97, meas time 1730863674.58 <<<< 2024-11-06T03:27:55Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863674.97 2024-11-06T03:27:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863674.58 sec, ping # 0; waiting for INS data more recent than 1730863670.94... 2024-11-06T03:27:55Z- TerrainNav::motionUpdate() - currEstimate = (4061887, 594654.19, 49.62) 2024-11-06T03:27:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:27:55Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:27:55Z- TNavPF::Projecting Measurements 2024-11-06T03:27:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:27:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:27:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:27:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:27:55Z- TNavFilter::4 good beams. 2024-11-06T03:27:55Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:27:55Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:27:55Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:27:55Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:27:55Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:27:55Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:27:55Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:27:55Z- TerrainMapOctree: (vehN, vehE ) = (4061886.40, 594654.31). 2024-11-06T03:27:55Z- TerrainMapOctree: Min Distance = 166.62. 2024-11-06T03:27:55Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:27:55Z- Forcing Subcloud Comparison 2024-11-06T03:27:55Z- Weighting particles with subcloud comparison 2024-11-06T03:27:55Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:55Z- meanExpectedMeasDiff: 2.447571 delta_rms_squared: 1.387529 alpha: 0.985791 2024-11-06T03:27:55Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:55Z- meanExpectedMeasDiff: -1.307783 delta_rms_squared: 1.064911 alpha: 0.981565 2024-11-06T03:27:55Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:55Z- meanExpectedMeasDiff: -0.153305 delta_rms_squared: 1.232624 alpha: 0.984034 2024-11-06T03:27:55Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:55Z- meanExpectedMeasDiff: 1.534556 delta_rms_squared: 1.792099 alpha: 0.988963 2024-11-06T03:27:55Z- Subcloud NIS: 0.365735 2024-11-06T03:27:55Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:27:55Z- TNavPF:: sumWeights = 0.229908 2024-11-06T03:27:55Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:27:55Z- TNavPF::First Term of Map Covariance Matrix is 0.75 2024-11-06T03:27:55Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T03:27:55Z- TerrainNav::Resampling particle filter... 2024-11-06T03:27:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863674.58 sec. 2024-11-06T03:27:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:27:55Z- variances([0], [2], and psi_berg) = 0.849950 0.253452 0.000000 2024-11-06T03:27:55Z- publishEstimatesDecoder() - at 1730863674.97 sec 2024-11-06T03:27:55Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:27:55Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061917.18 594636.82 55.63 2024-11-06T03:27:55Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061915.97 594637.30 55.82 lat/lon: 36.70 -121.94 2024-11-06T03:27:55Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.85 0.25 0.68 0.00 2024-11-06T03:27:55Z- handleDepth()- Depth_Keller msg: 1730863675.37 epoch sec; seqNo:5731; depth 49.64 2024-11-06T03:27:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863675.38 epoch sec; seqNo:5731; 4061887.11 north; 594654.18 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:27:55Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:27:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863674.97 2024-11-06T03:27:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:55Z- waiting for 1730863678.97, time is 1730863675.38 2024-11-06T03:27:55Z- AHRS_M2 msg: 1730863675.76 epoch sec; seqNo:5381 2024-11-06T03:27:55Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.03 psi 2024-11-06T03:27:55Z- handleDepth()- Depth_Keller msg: 1730863675.77 epoch sec; seqNo:5732; depth 49.64 2024-11-06T03:27:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863675.78 epoch sec; seqNo:5732; 4061887.47 north; 594652.96 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:27:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863674.97 2024-11-06T03:27:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:55Z- waiting for 1730863678.97, time is 1730863675.78 2024-11-06T03:27:56Z- AHRS_M2 msg: 1730863676.22 epoch sec; seqNo:5382 2024-11-06T03:27:56Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 6.03 psi 2024-11-06T03:27:56Z- handleDepth()- Depth_Keller msg: 1730863676.23 epoch sec; seqNo:5733; depth 49.65 2024-11-06T03:27:56Z- handleDVL() - DVL msg epochMillisec: 1730863676.25 2024-11-06T03:27:56Z- handleDvl() - RDI_Pathfinder msg: 1730863676.25 sec; seqNo:1982; #beams:4 2024-11-06T03:27:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.24 , 36.26 , 38.87 , 39.06 2024-11-06T03:27:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.03 2024-11-06T03:27:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863676.27 epoch sec; seqNo:5733; 4061887.85 north; 594652.95 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:27:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863674.97 2024-11-06T03:27:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:56Z- waiting for 1730863678.97, time is 1730863676.27 2024-11-06T03:27:56Z- AHRS_M2 msg: 1730863676.57 epoch sec; seqNo:5383 2024-11-06T03:27:56Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:27:56Z- handleDepth()- Depth_Keller msg: 1730863676.57 epoch sec; seqNo:5734; depth 49.65 2024-11-06T03:27:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863676.59 epoch sec; seqNo:5734; 4061888.23 north; 594652.95 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:27:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863674.97 2024-11-06T03:27:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:56Z- waiting for 1730863678.97, time is 1730863676.59 2024-11-06T03:27:56Z- AHRS_M2 msg: 1730863676.97 epoch sec; seqNo:5384 2024-11-06T03:27:56Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:27:56Z- handleDepth()- Depth_Keller msg: 1730863676.97 epoch sec; seqNo:5735; depth 49.65 2024-11-06T03:27:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863676.98 epoch sec; seqNo:5735; 4061888.61 north; 594652.95 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:27:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863674.97 2024-11-06T03:27:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:57Z- waiting for 1730863678.97, time is 1730863676.98 2024-11-06T03:27:57Z- AHRS_M2 msg: 1730863677.38 epoch sec; seqNo:5385 2024-11-06T03:27:57Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:27:57Z- handleDepth()- Depth_Keller msg: 1730863677.39 epoch sec; seqNo:5736; depth 49.66 2024-11-06T03:27:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863677.40 epoch sec; seqNo:5736; 4061888.99 north; 594652.94 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:27:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863674.97 2024-11-06T03:27:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:57Z- waiting for 1730863678.97, time is 1730863677.40 2024-11-06T03:27:57Z- AHRS_M2 msg: 1730863677.78 epoch sec; seqNo:5386 2024-11-06T03:27:57Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:27:57Z- handleDepth()- Depth_Keller msg: 1730863677.80 epoch sec; seqNo:5737; depth 49.67 2024-11-06T03:27:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863677.80 epoch sec; seqNo:5737; 4061889.37 north; 594652.94 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:27:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863674.97 2024-11-06T03:27:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:57Z- waiting for 1730863678.97, time is 1730863677.80 2024-11-06T03:27:58Z- AHRS_M2 msg: 1730863678.24 epoch sec; seqNo:5387 2024-11-06T03:27:58Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:27:58Z- handleDepth()- Depth_Keller msg: 1730863678.25 epoch sec; seqNo:5738; depth 49.65 2024-11-06T03:27:58Z- handleDVL() - DVL msg epochMillisec: 1730863678.27 2024-11-06T03:27:58Z- handleDvl() - RDI_Pathfinder msg: 1730863678.27 sec; seqNo:1983; #beams:4 2024-11-06T03:27:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.61 , 36.26 , 39.06 , 39.06 2024-11-06T03:27:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , 0.00 2024-11-06T03:27:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863678.28 epoch sec; seqNo:5738; 4061889.75 north; 594652.93 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:27:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:27:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863674.97 2024-11-06T03:27:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:58Z- waiting for 1730863678.97, time is 1730863678.28 2024-11-06T03:27:58Z- AHRS_M2 msg: 1730863678.59 epoch sec; seqNo:5388 2024-11-06T03:27:58Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:27:58Z- handleDepth()- Depth_Keller msg: 1730863678.59 epoch sec; seqNo:5739; depth 49.66 2024-11-06T03:27:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863678.61 epoch sec; seqNo:5739; 4061889.75 north; 594652.93 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:27:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863674.97 2024-11-06T03:27:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:58Z- waiting for 1730863678.97, time is 1730863678.61 2024-11-06T03:27:59Z- AHRS_M2 msg: 1730863678.99 epoch sec; seqNo:5389 2024-11-06T03:27:59Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:27:59Z- handleDepth()- Depth_Keller msg: 1730863679.00 epoch sec; seqNo:5740; depth 49.66 2024-11-06T03:27:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863679.01 epoch sec; seqNo:5740; 4061890.13 north; 594652.93 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:27:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863674.97 2024-11-06T03:27:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:59Z- waiting for 1730863678.97, time is 1730863679.01 2024-11-06T03:27:59Z- LcmTrn::updateTrn() >>>> pose time 1730863679.01, meas time 1730863678.27 <<<< 2024-11-06T03:27:59Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863679.01 2024-11-06T03:27:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863678.27 sec, ping # 0; waiting for INS data more recent than 1730863674.97... 2024-11-06T03:27:59Z- TerrainNav::motionUpdate() - currEstimate = (4061890, 594652.93, 49.66) 2024-11-06T03:27:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:27:59Z- TerrainNav:: SubcloudNIS is 0.37, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:27:59Z- TNavPF::Projecting Measurements 2024-11-06T03:27:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:27:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:27:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:27:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:27:59Z- TNavFilter::4 good beams. 2024-11-06T03:27:59Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:27:59Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:27:59Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:27:59Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:27:59Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:27:59Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:27:59Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:27:59Z- TerrainMapOctree: (vehN, vehE ) = (4061889.50, 594653.16). 2024-11-06T03:27:59Z- TerrainMapOctree: Min Distance = 169.82. 2024-11-06T03:27:59Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:27:59Z- Forcing Subcloud Comparison 2024-11-06T03:27:59Z- Weighting particles with subcloud comparison 2024-11-06T03:27:59Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:59Z- meanExpectedMeasDiff: 2.155290 delta_rms_squared: 1.484914 alpha: 0.986710 2024-11-06T03:27:59Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:59Z- meanExpectedMeasDiff: -1.281658 delta_rms_squared: 1.064939 alpha: 0.981566 2024-11-06T03:27:59Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:59Z- meanExpectedMeasDiff: -0.140558 delta_rms_squared: 1.229063 alpha: 0.983988 2024-11-06T03:27:59Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:27:59Z- meanExpectedMeasDiff: 1.209611 delta_rms_squared: 1.098963 alpha: 0.982126 2024-11-06T03:27:59Z- Subcloud NIS: 0.346480 2024-11-06T03:27:59Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:27:59Z- TNavPF:: sumWeights = 0.296511 2024-11-06T03:27:59Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:27:59Z- TNavPF::First Term of Map Covariance Matrix is 0.74 2024-11-06T03:27:59Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T03:27:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863678.27 sec. 2024-11-06T03:27:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:27:59Z- variances([0], [2], and psi_berg) = 0.867263 0.263567 0.000000 2024-11-06T03:27:59Z- publishEstimatesDecoder() - at 1730863679.01 sec 2024-11-06T03:27:59Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:27:59Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061920.44 594635.66 55.57 2024-11-06T03:27:59Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061919.44 594636.05 55.66 lat/lon: 36.70 -121.94 2024-11-06T03:27:59Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.87 0.26 0.68 0.00 2024-11-06T03:27:59Z- AHRS_M2 msg: 1730863679.39 epoch sec; seqNo:5390 2024-11-06T03:27:59Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:27:59Z- handleDepth()- Depth_Keller msg: 1730863679.40 epoch sec; seqNo:5741; depth 49.66 2024-11-06T03:27:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863679.43 epoch sec; seqNo:5741; 4061890.50 north; 594652.92 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:27:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863679.01 2024-11-06T03:27:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:59Z- waiting for 1730863683.01, time is 1730863679.43 2024-11-06T03:27:59Z- AHRS_M2 msg: 1730863679.80 epoch sec; seqNo:5391 2024-11-06T03:27:59Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:27:59Z- handleDepth()- Depth_Keller msg: 1730863679.81 epoch sec; seqNo:5742; depth 49.63 2024-11-06T03:27:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863679.82 epoch sec; seqNo:5742; 4061890.88 north; 594652.92 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:27:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:27:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863679.01 2024-11-06T03:27:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:27:59Z- waiting for 1730863683.01, time is 1730863679.82 2024-11-06T03:28:00Z- AHRS_M2 msg: 1730863680.24 epoch sec; seqNo:5392 2024-11-06T03:28:00Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 6.03 psi 2024-11-06T03:28:00Z- handleDepth()- Depth_Keller msg: 1730863680.25 epoch sec; seqNo:5743; depth 49.64 2024-11-06T03:28:00Z- handleDVL() - DVL msg epochMillisec: 1730863680.27 2024-11-06T03:28:00Z- handleDvl() - RDI_Pathfinder msg: 1730863680.27 sec; seqNo:1984; #beams:4 2024-11-06T03:28:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.80 , 36.07 , 38.69 , 39.44 2024-11-06T03:28:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.05 2024-11-06T03:28:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863680.28 epoch sec; seqNo:5743; 4061891.26 north; 594652.92 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:28:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863679.01 2024-11-06T03:28:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:00Z- waiting for 1730863683.01, time is 1730863680.28 2024-11-06T03:28:00Z- AHRS_M2 msg: 1730863680.61 epoch sec; seqNo:5393 2024-11-06T03:28:00Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.03 psi 2024-11-06T03:28:00Z- handleDepth()- Depth_Keller msg: 1730863680.62 epoch sec; seqNo:5744; depth 49.64 2024-11-06T03:28:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863680.64 epoch sec; seqNo:5744; 4061891.63 north; 594651.69 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:28:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863679.01 2024-11-06T03:28:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:00Z- waiting for 1730863683.01, time is 1730863680.64 2024-11-06T03:28:01Z- AHRS_M2 msg: 1730863681.02 epoch sec; seqNo:5394 2024-11-06T03:28:01Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.03 psi 2024-11-06T03:28:01Z- handleDepth()- Depth_Keller msg: 1730863681.03 epoch sec; seqNo:5745; depth 49.63 2024-11-06T03:28:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863681.04 epoch sec; seqNo:5745; 4061892.01 north; 594651.69 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:28:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863679.01 2024-11-06T03:28:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:01Z- waiting for 1730863683.01, time is 1730863681.04 2024-11-06T03:28:01Z- AHRS_M2 msg: 1730863681.41 epoch sec; seqNo:5395 2024-11-06T03:28:01Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.03 psi 2024-11-06T03:28:01Z- handleDepth()- Depth_Keller msg: 1730863681.42 epoch sec; seqNo:5746; depth 49.63 2024-11-06T03:28:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863681.43 epoch sec; seqNo:5746; 4061892.01 north; 594651.69 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:28:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863679.01 2024-11-06T03:28:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:01Z- waiting for 1730863683.01, time is 1730863681.43 2024-11-06T03:28:01Z- AHRS_M2 msg: 1730863681.82 epoch sec; seqNo:5396 2024-11-06T03:28:01Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.03 psi 2024-11-06T03:28:01Z- handleDepth()- Depth_Keller msg: 1730863681.82 epoch sec; seqNo:5747; depth 49.62 2024-11-06T03:28:01Z- handleDVL() - DVL msg epochMillisec: 1730863681.82 2024-11-06T03:28:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:28:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:28:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:28:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:28:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:28:01Z- handleDvl() - RDI_Pathfinder msg: 1730863681.82 sec; seqNo:1985; #beams:0 2024-11-06T03:28:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:28:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.05 2024-11-06T03:28:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863681.84 epoch sec; seqNo:5747; 4061892.39 north; 594651.68 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:28:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863679.01 2024-11-06T03:28:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:01Z- waiting for 1730863683.01, time is 1730863681.84 2024-11-06T03:28:02Z- AHRS_M2 msg: 1730863682.24 epoch sec; seqNo:5397 2024-11-06T03:28:02Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.03 psi 2024-11-06T03:28:02Z- handleDepth()- Depth_Keller msg: 1730863682.25 epoch sec; seqNo:5748; depth 49.62 2024-11-06T03:28:02Z- handleDVL() - DVL msg epochMillisec: 1730863682.29 2024-11-06T03:28:02Z- handleDvl() - RDI_Pathfinder msg: 1730863682.29 sec; seqNo:1986; #beams:4 2024-11-06T03:28:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.24 , 35.88 , 38.50 , 39.25 2024-11-06T03:28:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.08 , -0.07 2024-11-06T03:28:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863682.30 epoch sec; seqNo:5748; 4061892.76 north; 594651.68 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:28:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863679.01 2024-11-06T03:28:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:02Z- waiting for 1730863683.01, time is 1730863682.30 2024-11-06T03:28:02Z- AHRS_M2 msg: 1730863682.63 epoch sec; seqNo:5398 2024-11-06T03:28:02Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.03 psi 2024-11-06T03:28:02Z- handleDepth()- Depth_Keller msg: 1730863682.64 epoch sec; seqNo:5749; depth 49.62 2024-11-06T03:28:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863682.66 epoch sec; seqNo:5749; 4061893.14 north; 594651.67 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:28:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863679.01 2024-11-06T03:28:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:02Z- waiting for 1730863683.01, time is 1730863682.66 2024-11-06T03:28:03Z- AHRS_M2 msg: 1730863683.03 epoch sec; seqNo:5399 2024-11-06T03:28:03Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.03 psi 2024-11-06T03:28:03Z- handleDepth()- Depth_Keller msg: 1730863683.04 epoch sec; seqNo:5750; depth 49.62 2024-11-06T03:28:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863683.05 epoch sec; seqNo:5750; 4061893.52 north; 594651.67 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:28:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863679.01 2024-11-06T03:28:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:03Z- waiting for 1730863683.01, time is 1730863683.05 2024-11-06T03:28:03Z- LcmTrn::updateTrn() >>>> pose time 1730863683.05, meas time 1730863682.29 <<<< 2024-11-06T03:28:03Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863683.05 2024-11-06T03:28:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863682.29 sec, ping # 0; waiting for INS data more recent than 1730863679.01... 2024-11-06T03:28:03Z- TerrainNav::motionUpdate() - currEstimate = (4061894, 594651.67, 49.62) 2024-11-06T03:28:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:28:03Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:28:03Z- TNavPF::Projecting Measurements 2024-11-06T03:28:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:28:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:28:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:28:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:28:03Z- TNavFilter::4 good beams. 2024-11-06T03:28:03Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:28:03Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:28:03Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:28:03Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:28:03Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:28:03Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:28:03Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:28:03Z- TerrainMapOctree: (vehN, vehE ) = (4061892.88, 594651.91). 2024-11-06T03:28:03Z- TerrainMapOctree: Min Distance = 173.31. 2024-11-06T03:28:03Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:28:03Z- Forcing Subcloud Comparison 2024-11-06T03:28:03Z- Weighting particles with subcloud comparison 2024-11-06T03:28:03Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:03Z- meanExpectedMeasDiff: 1.490481 delta_rms_squared: 1.465857 alpha: 0.986540 2024-11-06T03:28:03Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:03Z- meanExpectedMeasDiff: -1.897335 delta_rms_squared: 1.050868 alpha: 0.981324 2024-11-06T03:28:03Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:03Z- meanExpectedMeasDiff: -0.461999 delta_rms_squared: 0.972631 alpha: 0.979852 2024-11-06T03:28:03Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:03Z- meanExpectedMeasDiff: 0.255469 delta_rms_squared: 1.230304 alpha: 0.984004 2024-11-06T03:28:03Z- Subcloud NIS: 0.329137 2024-11-06T03:28:03Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:28:03Z- TNavPF:: sumWeights = 0.327559 2024-11-06T03:28:03Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:28:03Z- TNavPF::First Term of Map Covariance Matrix is 0.60 2024-11-06T03:28:03Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T03:28:03Z- TerrainNav::Resampling particle filter... 2024-11-06T03:28:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863682.29 sec. 2024-11-06T03:28:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:28:03Z- variances([0], [2], and psi_berg) = 0.858638 0.269835 0.000000 2024-11-06T03:28:03Z- publishEstimatesDecoder() - at 1730863683.05 sec 2024-11-06T03:28:03Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:28:03Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061924.01 594634.18 54.75 2024-11-06T03:28:03Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061922.86 594634.79 55.69 lat/lon: 36.70 -121.94 2024-11-06T03:28:03Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.86 0.27 0.67 0.00 2024-11-06T03:28:03Z- AHRS_M2 msg: 1730863683.44 epoch sec; seqNo:5400 2024-11-06T03:28:03Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.03 psi 2024-11-06T03:28:03Z- handleDepth()- Depth_Keller msg: 1730863683.44 epoch sec; seqNo:5751; depth 49.61 2024-11-06T03:28:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863683.45 epoch sec; seqNo:5751; 4061893.90 north; 594651.67 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:28:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863683.05 2024-11-06T03:28:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:03Z- waiting for 1730863687.05, time is 1730863683.45 2024-11-06T03:28:03Z- AHRS_M2 msg: 1730863683.84 epoch sec; seqNo:5401 2024-11-06T03:28:03Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.03 psi 2024-11-06T03:28:03Z- handleDepth()- Depth_Keller msg: 1730863683.85 epoch sec; seqNo:5752; depth 49.62 2024-11-06T03:28:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863683.86 epoch sec; seqNo:5752; 4061894.28 north; 594651.66 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:28:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863683.05 2024-11-06T03:28:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:03Z- waiting for 1730863687.05, time is 1730863683.86 2024-11-06T03:28:04Z- AHRS_M2 msg: 1730863684.24 epoch sec; seqNo:5402 2024-11-06T03:28:04Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.03 psi 2024-11-06T03:28:04Z- handleDepth()- Depth_Keller msg: 1730863684.25 epoch sec; seqNo:5753; depth 49.64 2024-11-06T03:28:04Z- handleDVL() - DVL msg epochMillisec: 1730863684.28 2024-11-06T03:28:04Z- handleDvl() - RDI_Pathfinder msg: 1730863684.28 sec; seqNo:1987; #beams:4 2024-11-06T03:28:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.05 , 35.70 , 38.13 , 39.25 2024-11-06T03:28:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.12 , -0.04 2024-11-06T03:28:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863684.29 epoch sec; seqNo:5753; 4061894.66 north; 594651.66 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:28:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863683.05 2024-11-06T03:28:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:04Z- waiting for 1730863687.05, time is 1730863684.29 2024-11-06T03:28:04Z- AHRS_M2 msg: 1730863684.65 epoch sec; seqNo:5403 2024-11-06T03:28:04Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.03 psi 2024-11-06T03:28:04Z- handleDepth()- Depth_Keller msg: 1730863684.65 epoch sec; seqNo:5754; depth 49.64 2024-11-06T03:28:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863684.67 epoch sec; seqNo:5754; 4061895.04 north; 594651.65 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:28:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863683.05 2024-11-06T03:28:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:04Z- waiting for 1730863687.05, time is 1730863684.67 2024-11-06T03:28:05Z- AHRS_M2 msg: 1730863685.06 epoch sec; seqNo:5404 2024-11-06T03:28:05Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.03 psi 2024-11-06T03:28:05Z- handleDepth()- Depth_Keller msg: 1730863685.06 epoch sec; seqNo:5755; depth 49.64 2024-11-06T03:28:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863685.07 epoch sec; seqNo:5755; 4061895.40 north; 594650.43 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:28:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863683.05 2024-11-06T03:28:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:05Z- waiting for 1730863687.05, time is 1730863685.07 2024-11-06T03:28:05Z- AHRS_M2 msg: 1730863685.45 epoch sec; seqNo:5405 2024-11-06T03:28:05Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 6.03 psi 2024-11-06T03:28:05Z- handleDepth()- Depth_Keller msg: 1730863685.46 epoch sec; seqNo:5756; depth 49.66 2024-11-06T03:28:05Z- handleDVL() - DVL msg epochMillisec: 1730863685.46 2024-11-06T03:28:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:28:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:28:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:28:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:28:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:28:05Z- handleDvl() - RDI_Pathfinder msg: 1730863685.46 sec; seqNo:1988; #beams:0 2024-11-06T03:28:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:28:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.12 , -0.04 2024-11-06T03:28:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863685.48 epoch sec; seqNo:5756; 4061895.78 north; 594650.43 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:28:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863683.05 2024-11-06T03:28:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:05Z- waiting for 1730863687.05, time is 1730863685.48 2024-11-06T03:28:05Z- AHRS_M2 msg: 1730863685.86 epoch sec; seqNo:5406 2024-11-06T03:28:05Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:28:05Z- handleDepth()- Depth_Keller msg: 1730863685.86 epoch sec; seqNo:5757; depth 49.68 2024-11-06T03:28:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863685.88 epoch sec; seqNo:5757; 4061896.16 north; 594650.42 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:28:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863683.05 2024-11-06T03:28:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:06Z- waiting for 1730863687.05, time is 1730863685.88 2024-11-06T03:28:06Z- AHRS_M2 msg: 1730863686.26 epoch sec; seqNo:5407 2024-11-06T03:28:06Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:28:06Z- handleDepth()- Depth_Keller msg: 1730863686.27 epoch sec; seqNo:5758; depth 49.68 2024-11-06T03:28:06Z- handleDVL() - DVL msg epochMillisec: 1730863686.29 2024-11-06T03:28:06Z- handleDvl() - RDI_Pathfinder msg: 1730863686.29 sec; seqNo:1989; #beams:4 2024-11-06T03:28:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.24 , 35.88 , 38.13 , 39.25 2024-11-06T03:28:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.12 , 0.01 2024-11-06T03:28:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863686.32 epoch sec; seqNo:5758; 4061896.54 north; 594650.42 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:28:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863683.05 2024-11-06T03:28:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:06Z- waiting for 1730863687.05, time is 1730863686.32 2024-11-06T03:28:06Z- AHRS_M2 msg: 1730863686.67 epoch sec; seqNo:5408 2024-11-06T03:28:06Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:28:06Z- handleDepth()- Depth_Keller msg: 1730863686.68 epoch sec; seqNo:5759; depth 49.73 2024-11-06T03:28:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863686.69 epoch sec; seqNo:5759; 4061896.92 north; 594650.41 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:28:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863683.05 2024-11-06T03:28:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:06Z- waiting for 1730863687.05, time is 1730863686.69 2024-11-06T03:28:07Z- AHRS_M2 msg: 1730863687.07 epoch sec; seqNo:5409 2024-11-06T03:28:07Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.02 psi 2024-11-06T03:28:07Z- handleDepth()- Depth_Keller msg: 1730863687.08 epoch sec; seqNo:5760; depth 49.73 2024-11-06T03:28:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863687.09 epoch sec; seqNo:5760; 4061897.30 north; 594650.41 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:28:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863683.05 2024-11-06T03:28:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:07Z- waiting for 1730863687.05, time is 1730863687.09 2024-11-06T03:28:07Z- LcmTrn::updateTrn() >>>> pose time 1730863687.09, meas time 1730863686.29 <<<< 2024-11-06T03:28:07Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863687.09 2024-11-06T03:28:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863686.29 sec, ping # 0; waiting for INS data more recent than 1730863683.05... 2024-11-06T03:28:07Z- TerrainNav::motionUpdate() - currEstimate = (4061897, 594650.41, 49.73) 2024-11-06T03:28:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:28:07Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:28:07Z- TNavPF::Projecting Measurements 2024-11-06T03:28:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:28:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:28:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:28:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:28:07Z- TNavFilter::4 good beams. 2024-11-06T03:28:07Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:28:07Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:28:07Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:28:07Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:28:07Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:28:07Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:28:07Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:28:07Z- TerrainMapOctree: (vehN, vehE ) = (4061896.55, 594650.66). 2024-11-06T03:28:07Z- TerrainMapOctree: Min Distance = 177.05. 2024-11-06T03:28:07Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:28:07Z- Forcing Subcloud Comparison 2024-11-06T03:28:07Z- Weighting particles with subcloud comparison 2024-11-06T03:28:07Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:07Z- meanExpectedMeasDiff: 1.615307 delta_rms_squared: 1.469292 alpha: 0.986571 2024-11-06T03:28:07Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:07Z- meanExpectedMeasDiff: -1.715096 delta_rms_squared: 1.056640 alpha: 0.981424 2024-11-06T03:28:07Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:07Z- meanExpectedMeasDiff: 0.155562 delta_rms_squared: 1.229431 alpha: 0.983993 2024-11-06T03:28:07Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:07Z- meanExpectedMeasDiff: 0.280981 delta_rms_squared: 1.279164 alpha: 0.984605 2024-11-06T03:28:07Z- Subcloud NIS: 0.316104 2024-11-06T03:28:07Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:28:07Z- TNavPF:: sumWeights = 0.342747 2024-11-06T03:28:07Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:28:07Z- TNavPF::First Term of Map Covariance Matrix is 0.60 2024-11-06T03:28:07Z- TNavPF::Calculated NIS Value : 0.21 numBeams normalized NIS: 0.05 2024-11-06T03:28:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863686.29 sec. 2024-11-06T03:28:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:28:07Z- variances([0], [2], and psi_berg) = 0.865850 0.271766 0.000000 2024-11-06T03:28:07Z- publishEstimatesDecoder() - at 1730863687.09 sec 2024-11-06T03:28:07Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:28:07Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061926.87 594633.88 56.31 2024-11-06T03:28:07Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061926.63 594633.52 55.76 lat/lon: 36.70 -121.94 2024-11-06T03:28:07Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.87 0.27 0.68 0.00 2024-11-06T03:28:07Z- AHRS_M2 msg: 1730863687.48 epoch sec; seqNo:5410 2024-11-06T03:28:07Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.02 psi 2024-11-06T03:28:07Z- handleDepth()- Depth_Keller msg: 1730863687.48 epoch sec; seqNo:5761; depth 49.75 2024-11-06T03:28:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863687.51 epoch sec; seqNo:5761; 4061897.68 north; 594650.40 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:28:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863687.09 2024-11-06T03:28:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:07Z- waiting for 1730863691.09, time is 1730863687.51 2024-11-06T03:28:07Z- AHRS_M2 msg: 1730863687.89 epoch sec; seqNo:5411 2024-11-06T03:28:07Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:28:07Z- handleDepth()- Depth_Keller msg: 1730863687.90 epoch sec; seqNo:5762; depth 49.78 2024-11-06T03:28:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863687.91 epoch sec; seqNo:5762; 4061897.68 north; 594650.40 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:28:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863687.09 2024-11-06T03:28:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:08Z- waiting for 1730863691.09, time is 1730863687.91 2024-11-06T03:28:08Z- AHRS_M2 msg: 1730863688.29 epoch sec; seqNo:5412 2024-11-06T03:28:08Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.03 psi 2024-11-06T03:28:08Z- handleDepth()- Depth_Keller msg: 1730863688.31 epoch sec; seqNo:5763; depth 49.78 2024-11-06T03:28:08Z- handleDVL() - DVL msg epochMillisec: 1730863688.34 2024-11-06T03:28:08Z- handleDvl() - RDI_Pathfinder msg: 1730863688.34 sec; seqNo:1990; #beams:4 2024-11-06T03:28:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.24 , 35.51 , 38.31 , 39.25 2024-11-06T03:28:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , 0.04 2024-11-06T03:28:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863688.37 epoch sec; seqNo:5763; 4061898.42 north; 594649.18 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:28:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863687.09 2024-11-06T03:28:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:08Z- waiting for 1730863691.09, time is 1730863688.37 2024-11-06T03:28:08Z- handleDepth()- Depth_Keller msg: 1730863688.71 epoch sec; seqNo:5764; depth 49.82 2024-11-06T03:28:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863688.72 epoch sec; seqNo:5764; 4061898.42 north; 594649.18 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:28:08Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:28:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863687.09 2024-11-06T03:28:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:08Z- waiting for 1730863691.09, time is 1730863688.72 2024-11-06T03:28:09Z- AHRS_M2 msg: 1730863689.09 epoch sec; seqNo:5413 2024-11-06T03:28:09Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.02 psi 2024-11-06T03:28:09Z- handleDepth()- Depth_Keller msg: 1730863689.10 epoch sec; seqNo:5765; depth 49.83 2024-11-06T03:28:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863689.11 epoch sec; seqNo:5765; 4061898.80 north; 594649.17 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:28:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863687.09 2024-11-06T03:28:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:09Z- waiting for 1730863691.09, time is 1730863689.11 2024-11-06T03:28:09Z- AHRS_M2 msg: 1730863689.49 epoch sec; seqNo:5414 2024-11-06T03:28:09Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.02 psi 2024-11-06T03:28:09Z- handleDepth()- Depth_Keller msg: 1730863689.51 epoch sec; seqNo:5766; depth 49.85 2024-11-06T03:28:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863689.53 epoch sec; seqNo:5766; 4061899.18 north; 594649.17 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:28:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863687.09 2024-11-06T03:28:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:09Z- waiting for 1730863691.09, time is 1730863689.53 2024-11-06T03:28:09Z- AHRS_M2 msg: 1730863689.90 epoch sec; seqNo:5415 2024-11-06T03:28:09Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.02 psi 2024-11-06T03:28:09Z- handleDepth()- Depth_Keller msg: 1730863689.90 epoch sec; seqNo:5767; depth 49.87 2024-11-06T03:28:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863689.92 epoch sec; seqNo:5767; 4061899.56 north; 594649.16 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:28:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863687.09 2024-11-06T03:28:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:10Z- waiting for 1730863691.09, time is 1730863689.92 2024-11-06T03:28:10Z- AHRS_M2 msg: 1730863690.33 epoch sec; seqNo:5416 2024-11-06T03:28:10Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.02 psi 2024-11-06T03:28:10Z- handleDepth()- Depth_Keller msg: 1730863690.33 epoch sec; seqNo:5768; depth 49.89 2024-11-06T03:28:10Z- handleDVL() - DVL msg epochMillisec: 1730863690.35 2024-11-06T03:28:10Z- handleDvl() - RDI_Pathfinder msg: 1730863690.35 sec; seqNo:1991; #beams:4 2024-11-06T03:28:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.42 , 35.51 , 38.31 , 39.44 2024-11-06T03:28:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.03 2024-11-06T03:28:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863690.38 epoch sec; seqNo:5768; 4061899.94 north; 594649.16 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:28:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863687.09 2024-11-06T03:28:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:10Z- waiting for 1730863691.09, time is 1730863690.38 2024-11-06T03:28:10Z- handleDepth()- Depth_Keller msg: 1730863690.72 epoch sec; seqNo:5769; depth 49.91 2024-11-06T03:28:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863690.74 epoch sec; seqNo:5769; 4061900.31 north; 594649.15 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:28:10Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:28:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863687.09 2024-11-06T03:28:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:10Z- waiting for 1730863691.09, time is 1730863690.74 2024-11-06T03:28:11Z- AHRS_M2 msg: 1730863691.12 epoch sec; seqNo:5417 2024-11-06T03:28:11Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.02 psi 2024-11-06T03:28:11Z- handleDepth()- Depth_Keller msg: 1730863691.13 epoch sec; seqNo:5770; depth 49.93 2024-11-06T03:28:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863691.13 epoch sec; seqNo:5770; 4061900.69 north; 594649.15 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:28:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863687.09 2024-11-06T03:28:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:11Z- waiting for 1730863691.09, time is 1730863691.13 2024-11-06T03:28:11Z- LcmTrn::updateTrn() >>>> pose time 1730863691.13, meas time 1730863690.35 <<<< 2024-11-06T03:28:11Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863691.13 2024-11-06T03:28:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863690.35 sec, ping # 0; waiting for INS data more recent than 1730863687.09... 2024-11-06T03:28:11Z- TerrainNav::motionUpdate() - currEstimate = (4061901, 594649.15, 49.93) 2024-11-06T03:28:11Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:28:11Z- TerrainNav:: SubcloudNIS is 0.32, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:28:11Z- TNavPF::Projecting Measurements 2024-11-06T03:28:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:28:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:28:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:28:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:28:11Z- TNavFilter::4 good beams. 2024-11-06T03:28:11Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:28:11Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:28:11Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:28:11Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:28:11Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:28:11Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:28:11Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:28:11Z- TerrainMapOctree: (vehN, vehE ) = (4061900.04, 594649.39). 2024-11-06T03:28:11Z- TerrainMapOctree: Min Distance = 180.65. 2024-11-06T03:28:11Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:28:11Z- Forcing Subcloud Comparison 2024-11-06T03:28:11Z- Weighting particles with subcloud comparison 2024-11-06T03:28:11Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:11Z- meanExpectedMeasDiff: 2.130618 delta_rms_squared: 1.093651 alpha: 0.982041 2024-11-06T03:28:11Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:11Z- meanExpectedMeasDiff: -1.924736 delta_rms_squared: 1.067191 alpha: 0.981604 2024-11-06T03:28:11Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:11Z- meanExpectedMeasDiff: 0.760019 delta_rms_squared: 1.204664 alpha: 0.983669 2024-11-06T03:28:11Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:11Z- meanExpectedMeasDiff: 0.463928 delta_rms_squared: 1.295282 alpha: 0.984794 2024-11-06T03:28:11Z- Subcloud NIS: 0.313048 2024-11-06T03:28:11Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:28:11Z- TNavPF:: sumWeights = 0.247923 2024-11-06T03:28:11Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:28:11Z- TNavPF::First Term of Map Covariance Matrix is 0.65 2024-11-06T03:28:11Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T03:28:11Z- TerrainNav::Resampling particle filter... 2024-11-06T03:28:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863690.35 sec. 2024-11-06T03:28:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:28:11Z- variances([0], [2], and psi_berg) = 0.840833 0.256397 0.000000 2024-11-06T03:28:11Z- publishEstimatesDecoder() - at 1730863691.13 sec 2024-11-06T03:28:11Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:28:11Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061931.21 594631.56 54.83 2024-11-06T03:28:11Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061930.01 594632.30 55.84 lat/lon: 36.70 -121.94 2024-11-06T03:28:11Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.84 0.26 0.68 0.00 2024-11-06T03:28:11Z- AHRS_M2 msg: 1730863691.52 epoch sec; seqNo:5418 2024-11-06T03:28:11Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.02 psi 2024-11-06T03:28:11Z- handleDepth()- Depth_Keller msg: 1730863691.54 epoch sec; seqNo:5771; depth 49.94 2024-11-06T03:28:11Z- handleDVL() - DVL msg epochMillisec: 1730863691.55 2024-11-06T03:28:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:28:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:28:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:28:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:28:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:28:11Z- handleDvl() - RDI_Pathfinder msg: 1730863691.55 sec; seqNo:1992; #beams:0 2024-11-06T03:28:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:28:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.03 2024-11-06T03:28:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863691.56 epoch sec; seqNo:5771; 4061901.07 north; 594649.15 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:28:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863691.13 2024-11-06T03:28:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:11Z- waiting for 1730863695.13, time is 1730863691.56 2024-11-06T03:28:11Z- AHRS_M2 msg: 1730863691.92 epoch sec; seqNo:5419 2024-11-06T03:28:11Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.02 psi 2024-11-06T03:28:11Z- handleDepth()- Depth_Keller msg: 1730863691.93 epoch sec; seqNo:5772; depth 49.96 2024-11-06T03:28:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863691.94 epoch sec; seqNo:5772; 4061901.45 north; 594649.14 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:28:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863691.13 2024-11-06T03:28:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:12Z- waiting for 1730863695.13, time is 1730863691.94 2024-11-06T03:28:12Z- AHRS_M2 msg: 1730863692.32 epoch sec; seqNo:5420 2024-11-06T03:28:12Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:28:12Z- handleDepth()- Depth_Keller msg: 1730863692.33 epoch sec; seqNo:5773; depth 49.96 2024-11-06T03:28:12Z- handleDVL() - DVL msg epochMillisec: 1730863692.36 2024-11-06T03:28:12Z- handleDvl() - RDI_Pathfinder msg: 1730863692.36 sec; seqNo:1993; #beams:4 2024-11-06T03:28:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.24 , 35.70 , 38.13 , 39.25 2024-11-06T03:28:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.01 2024-11-06T03:28:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863692.37 epoch sec; seqNo:5773; 4061901.82 north; 594647.92 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:28:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863691.13 2024-11-06T03:28:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:12Z- waiting for 1730863695.13, time is 1730863692.37 2024-11-06T03:28:12Z- AHRS_M2 msg: 1730863692.73 epoch sec; seqNo:5421 2024-11-06T03:28:12Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:28:12Z- handleDepth()- Depth_Keller msg: 1730863692.73 epoch sec; seqNo:5774; depth 49.97 2024-11-06T03:28:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863692.75 epoch sec; seqNo:5774; 4061901.82 north; 594647.92 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:28:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863691.13 2024-11-06T03:28:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:12Z- waiting for 1730863695.13, time is 1730863692.75 2024-11-06T03:28:13Z- AHRS_M2 msg: 1730863693.14 epoch sec; seqNo:5422 2024-11-06T03:28:13Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.03 psi 2024-11-06T03:28:13Z- handleDepth()- Depth_Keller msg: 1730863693.14 epoch sec; seqNo:5775; depth 49.97 2024-11-06T03:28:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863693.15 epoch sec; seqNo:5775; 4061902.19 north; 594647.91 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:28:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863691.13 2024-11-06T03:28:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:13Z- waiting for 1730863695.13, time is 1730863693.15 2024-11-06T03:28:13Z- AHRS_M2 msg: 1730863693.54 epoch sec; seqNo:5423 2024-11-06T03:28:13Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:28:13Z- handleDepth()- Depth_Keller msg: 1730863693.54 epoch sec; seqNo:5776; depth 49.97 2024-11-06T03:28:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863693.56 epoch sec; seqNo:5776; 4061902.57 north; 594647.91 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:28:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863691.13 2024-11-06T03:28:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:13Z- waiting for 1730863695.13, time is 1730863693.56 2024-11-06T03:28:13Z- AHRS_M2 msg: 1730863693.94 epoch sec; seqNo:5424 2024-11-06T03:28:13Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:28:13Z- handleDepth()- Depth_Keller msg: 1730863693.94 epoch sec; seqNo:5777; depth 49.97 2024-11-06T03:28:13Z- handleDVL() - DVL msg epochMillisec: 1730863693.95 2024-11-06T03:28:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:28:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:28:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:28:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:28:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:28:13Z- handleDvl() - RDI_Pathfinder msg: 1730863693.95 sec; seqNo:1994; #beams:0 2024-11-06T03:28:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:28:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.01 2024-11-06T03:28:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863693.96 epoch sec; seqNo:5777; 4061902.95 north; 594647.91 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:28:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863691.13 2024-11-06T03:28:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:14Z- waiting for 1730863695.13, time is 1730863693.96 2024-11-06T03:28:14Z- AHRS_M2 msg: 1730863694.35 epoch sec; seqNo:5425 2024-11-06T03:28:14Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:28:14Z- handleDepth()- Depth_Keller msg: 1730863694.35 epoch sec; seqNo:5778; depth 49.98 2024-11-06T03:28:14Z- handleDVL() - DVL msg epochMillisec: 1730863694.37 2024-11-06T03:28:14Z- handleDvl() - RDI_Pathfinder msg: 1730863694.37 sec; seqNo:1995; #beams:4 2024-11-06T03:28:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.24 , 35.51 , 37.76 , 39.44 2024-11-06T03:28:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.08 2024-11-06T03:28:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863694.40 epoch sec; seqNo:5778; 4061903.33 north; 594647.90 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:28:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863691.13 2024-11-06T03:28:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:14Z- waiting for 1730863695.13, time is 1730863694.40 2024-11-06T03:28:14Z- AHRS_M2 msg: 1730863694.75 epoch sec; seqNo:5426 2024-11-06T03:28:14Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:28:14Z- handleDepth()- Depth_Keller msg: 1730863694.76 epoch sec; seqNo:5779; depth 49.99 2024-11-06T03:28:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863694.77 epoch sec; seqNo:5779; 4061903.71 north; 594647.90 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:28:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863691.13 2024-11-06T03:28:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:14Z- waiting for 1730863695.13, time is 1730863694.77 2024-11-06T03:28:15Z- AHRS_M2 msg: 1730863695.15 epoch sec; seqNo:5427 2024-11-06T03:28:15Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.03 psi 2024-11-06T03:28:15Z- handleDepth()- Depth_Keller msg: 1730863695.15 epoch sec; seqNo:5780; depth 49.98 2024-11-06T03:28:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863695.18 epoch sec; seqNo:5780; 4061904.09 north; 594647.89 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:28:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863691.13 2024-11-06T03:28:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:15Z- waiting for 1730863695.13, time is 1730863695.18 2024-11-06T03:28:15Z- LcmTrn::updateTrn() >>>> pose time 1730863695.18, meas time 1730863694.37 <<<< 2024-11-06T03:28:15Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863695.18 2024-11-06T03:28:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863694.37 sec, ping # 0; waiting for INS data more recent than 1730863691.13... 2024-11-06T03:28:15Z- TerrainNav::motionUpdate() - currEstimate = (4061904, 594647.89, 49.98) 2024-11-06T03:28:15Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:28:15Z- TerrainNav:: SubcloudNIS is 0.31, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:28:15Z- TNavPF::Projecting Measurements 2024-11-06T03:28:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:28:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:28:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:28:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:28:15Z- TNavFilter::4 good beams. 2024-11-06T03:28:15Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:28:15Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:28:15Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:28:15Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:28:15Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:28:15Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:28:15Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:28:15Z- TerrainMapOctree: (vehN, vehE ) = (4061903.41, 594648.14). 2024-11-06T03:28:15Z- TerrainMapOctree: Min Distance = 184.14. 2024-11-06T03:28:15Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:28:15Z- Forcing Subcloud Comparison 2024-11-06T03:28:15Z- Weighting particles with subcloud comparison 2024-11-06T03:28:15Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:15Z- meanExpectedMeasDiff: 2.753397 delta_rms_squared: 1.090641 alpha: 0.981992 2024-11-06T03:28:15Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:15Z- meanExpectedMeasDiff: -2.008276 delta_rms_squared: 1.070179 alpha: 0.981654 2024-11-06T03:28:15Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:15Z- meanExpectedMeasDiff: 0.227033 delta_rms_squared: 1.206407 alpha: 0.983692 2024-11-06T03:28:15Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:15Z- meanExpectedMeasDiff: 0.256366 delta_rms_squared: 1.207815 alpha: 0.983711 2024-11-06T03:28:15Z- Subcloud NIS: 0.312786 2024-11-06T03:28:15Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:28:15Z- TNavPF:: sumWeights = 0.180291 2024-11-06T03:28:15Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:28:15Z- TNavPF::First Term of Map Covariance Matrix is 0.94 2024-11-06T03:28:15Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.02 2024-11-06T03:28:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863694.37 sec. 2024-11-06T03:28:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:28:15Z- variances([0], [2], and psi_berg) = 0.827002 0.269731 0.000000 2024-11-06T03:28:15Z- publishEstimatesDecoder() - at 1730863695.18 sec 2024-11-06T03:28:15Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:28:15Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061933.12 594632.01 56.28 2024-11-06T03:28:15Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061933.32 594631.11 55.84 lat/lon: 36.70 -121.94 2024-11-06T03:28:15Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.83 0.27 0.72 0.00 2024-11-06T03:28:15Z- AHRS_M2 msg: 1730863695.56 epoch sec; seqNo:5428 2024-11-06T03:28:15Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.03 psi 2024-11-06T03:28:15Z- handleDepth()- Depth_Keller msg: 1730863695.57 epoch sec; seqNo:5781; depth 50.00 2024-11-06T03:28:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863695.57 epoch sec; seqNo:5781; 4061904.47 north; 594647.89 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:28:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863695.18 2024-11-06T03:28:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:15Z- waiting for 1730863699.18, time is 1730863695.57 2024-11-06T03:28:15Z- AHRS_M2 msg: 1730863695.96 epoch sec; seqNo:5429 2024-11-06T03:28:15Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.03 psi 2024-11-06T03:28:15Z- handleDepth()- Depth_Keller msg: 1730863695.96 epoch sec; seqNo:5782; depth 50.00 2024-11-06T03:28:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863695.97 epoch sec; seqNo:5782; 4061904.47 north; 594647.89 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:28:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863695.18 2024-11-06T03:28:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:16Z- waiting for 1730863699.18, time is 1730863695.97 2024-11-06T03:28:16Z- AHRS_M2 msg: 1730863696.36 epoch sec; seqNo:5430 2024-11-06T03:28:16Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.03 psi 2024-11-06T03:28:16Z- handleDepth()- Depth_Keller msg: 1730863696.37 epoch sec; seqNo:5783; depth 50.02 2024-11-06T03:28:16Z- handleDVL() - DVL msg epochMillisec: 1730863696.39 2024-11-06T03:28:16Z- handleDvl() - RDI_Pathfinder msg: 1730863696.39 sec; seqNo:1996; #beams:4 2024-11-06T03:28:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.68 , 35.32 , 37.75 , 39.06 2024-11-06T03:28:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.09 , -0.10 2024-11-06T03:28:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863696.40 epoch sec; seqNo:5783; 4061904.85 north; 594647.88 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:28:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863695.18 2024-11-06T03:28:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:16Z- waiting for 1730863699.18, time is 1730863696.40 2024-11-06T03:28:16Z- AHRS_M2 msg: 1730863696.77 epoch sec; seqNo:5431 2024-11-06T03:28:16Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.03 psi 2024-11-06T03:28:16Z- handleDepth()- Depth_Keller msg: 1730863696.77 epoch sec; seqNo:5784; depth 50.03 2024-11-06T03:28:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863696.79 epoch sec; seqNo:5784; 4061905.23 north; 594647.88 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:28:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863695.18 2024-11-06T03:28:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:16Z- waiting for 1730863699.18, time is 1730863696.79 2024-11-06T03:28:17Z- AHRS_M2 msg: 1730863697.17 epoch sec; seqNo:5432 2024-11-06T03:28:17Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.03 psi 2024-11-06T03:28:17Z- handleDepth()- Depth_Keller msg: 1730863697.18 epoch sec; seqNo:5785; depth 50.05 2024-11-06T03:28:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863697.19 epoch sec; seqNo:5785; 4061905.59 north; 594646.66 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:28:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863695.18 2024-11-06T03:28:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:17Z- waiting for 1730863699.18, time is 1730863697.19 2024-11-06T03:28:17Z- AHRS_M2 msg: 1730863697.58 epoch sec; seqNo:5433 2024-11-06T03:28:17Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 6.02 psi 2024-11-06T03:28:17Z- handleDepth()- Depth_Keller msg: 1730863697.58 epoch sec; seqNo:5786; depth 50.04 2024-11-06T03:28:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863697.59 epoch sec; seqNo:5786; 4061905.97 north; 594646.65 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:28:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863695.18 2024-11-06T03:28:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:17Z- waiting for 1730863699.18, time is 1730863697.59 2024-11-06T03:28:17Z- AHRS_M2 msg: 1730863697.98 epoch sec; seqNo:5434 2024-11-06T03:28:17Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.02 psi 2024-11-06T03:28:18Z- handleDepth()- Depth_Keller msg: 1730863698.01 epoch sec; seqNo:5787; depth 50.07 2024-11-06T03:28:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863698.03 epoch sec; seqNo:5787; 4061906.35 north; 594646.65 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:28:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863695.18 2024-11-06T03:28:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:18Z- waiting for 1730863699.18, time is 1730863698.03 2024-11-06T03:28:18Z- AHRS_M2 msg: 1730863698.38 epoch sec; seqNo:5435 2024-11-06T03:28:18Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.02 psi 2024-11-06T03:28:18Z- handleDepth()- Depth_Keller msg: 1730863698.39 epoch sec; seqNo:5788; depth 50.09 2024-11-06T03:28:18Z- handleDVL() - DVL msg epochMillisec: 1730863698.41 2024-11-06T03:28:18Z- handleDvl() - RDI_Pathfinder msg: 1730863698.41 sec; seqNo:1997; #beams:4 2024-11-06T03:28:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 35.14 , 37.57 , 39.06 2024-11-06T03:28:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.13 , -0.03 2024-11-06T03:28:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863698.43 epoch sec; seqNo:5788; 4061906.73 north; 594646.64 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:28:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863695.18 2024-11-06T03:28:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:18Z- waiting for 1730863699.18, time is 1730863698.43 2024-11-06T03:28:18Z- AHRS_M2 msg: 1730863698.79 epoch sec; seqNo:5436 2024-11-06T03:28:18Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.02 psi 2024-11-06T03:28:18Z- handleDepth()- Depth_Keller msg: 1730863698.80 epoch sec; seqNo:5789; depth 50.11 2024-11-06T03:28:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863698.81 epoch sec; seqNo:5789; 4061907.11 north; 594646.64 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:28:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863695.18 2024-11-06T03:28:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:18Z- waiting for 1730863699.18, time is 1730863698.81 2024-11-06T03:28:19Z- AHRS_M2 msg: 1730863699.19 epoch sec; seqNo:5437 2024-11-06T03:28:19Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 6.02 psi 2024-11-06T03:28:19Z- handleDepth()- Depth_Keller msg: 1730863699.20 epoch sec; seqNo:5790; depth 50.13 2024-11-06T03:28:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863699.21 epoch sec; seqNo:5790; 4061907.49 north; 594646.63 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:28:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863695.18 2024-11-06T03:28:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:19Z- waiting for 1730863699.18, time is 1730863699.21 2024-11-06T03:28:19Z- LcmTrn::updateTrn() >>>> pose time 1730863699.21, meas time 1730863698.41 <<<< 2024-11-06T03:28:19Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863699.21 2024-11-06T03:28:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863698.41 sec, ping # 0; waiting for INS data more recent than 1730863695.18... 2024-11-06T03:28:19Z- TerrainNav::motionUpdate() - currEstimate = (4061907, 594646.63, 50.13) 2024-11-06T03:28:19Z- TerrainNav:: Windowed NIS is 0.1, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:28:19Z- TerrainNav:: SubcloudNIS is 0.31, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:28:19Z- TNavPF::Projecting Measurements 2024-11-06T03:28:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:28:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:28:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:28:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:28:19Z- TNavFilter::4 good beams. 2024-11-06T03:28:19Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:28:19Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:28:19Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:28:19Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:28:19Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:28:19Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:28:19Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:28:19Z- TerrainMapOctree: (vehN, vehE ) = (4061906.81, 594646.88). 2024-11-06T03:28:19Z- TerrainMapOctree: Min Distance = 187.68. 2024-11-06T03:28:19Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:28:19Z- Forcing Subcloud Comparison 2024-11-06T03:28:19Z- Weighting particles with subcloud comparison 2024-11-06T03:28:19Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:19Z- meanExpectedMeasDiff: 2.973569 delta_rms_squared: 1.484602 alpha: 0.986707 2024-11-06T03:28:19Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:19Z- meanExpectedMeasDiff: -2.433354 delta_rms_squared: 1.233189 alpha: 0.984041 2024-11-06T03:28:19Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:19Z- meanExpectedMeasDiff: 0.205032 delta_rms_squared: 1.169167 alpha: 0.983182 2024-11-06T03:28:19Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:19Z- meanExpectedMeasDiff: -0.351874 delta_rms_squared: 1.277133 alpha: 0.984581 2024-11-06T03:28:19Z- Subcloud NIS: 0.316092 2024-11-06T03:28:19Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:28:19Z- TNavPF:: sumWeights = 0.113992 2024-11-06T03:28:19Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:28:19Z- TNavPF::First Term of Map Covariance Matrix is 0.85 2024-11-06T03:28:19Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T03:28:19Z- TerrainNav::Resampling particle filter... 2024-11-06T03:28:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863698.41 sec. 2024-11-06T03:28:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:28:19Z- variances([0], [2], and psi_berg) = 0.748812 0.269300 0.000000 2024-11-06T03:28:19Z- publishEstimatesDecoder() - at 1730863699.21 sec 2024-11-06T03:28:19Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:28:19Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061938.07 594629.29 56.63 2024-11-06T03:28:19Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061936.64 594629.83 55.96 lat/lon: 36.70 -121.94 2024-11-06T03:28:19Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.75 0.27 0.70 0.00 2024-11-06T03:28:19Z- handleDepth()- Depth_Keller msg: 1730863699.62 epoch sec; seqNo:5791; depth 50.16 2024-11-06T03:28:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863699.63 epoch sec; seqNo:5791; 4061907.86 north; 594646.63 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:28:19Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:28:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863699.21 2024-11-06T03:28:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:19Z- waiting for 1730863703.21, time is 1730863699.63 2024-11-06T03:28:20Z- AHRS_M2 msg: 1730863700.00 epoch sec; seqNo:5438 2024-11-06T03:28:20Z- AHRS_M2 msg: 0.11 phi; -0.03 theta; 6.00 psi 2024-11-06T03:28:20Z- handleDepth()- Depth_Keller msg: 1730863700.01 epoch sec; seqNo:5792; depth 50.17 2024-11-06T03:28:20Z- handleDVL() - DVL msg epochMillisec: 1730863700.01 2024-11-06T03:28:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:28:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:28:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:28:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:28:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:28:20Z- handleDvl() - RDI_Pathfinder msg: 1730863700.01 sec; seqNo:1998; #beams:0 2024-11-06T03:28:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:28:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.13 , -0.03 2024-11-06T03:28:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863700.02 epoch sec; seqNo:5792; 4061908.24 north; 594646.63 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:28:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863699.21 2024-11-06T03:28:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:20Z- waiting for 1730863703.21, time is 1730863700.02 2024-11-06T03:28:20Z- handleDepth()- Depth_Keller msg: 1730863700.42 epoch sec; seqNo:5793; depth 50.20 2024-11-06T03:28:20Z- handleDVL() - DVL msg epochMillisec: 1730863700.44 2024-11-06T03:28:20Z- handleDvl() - RDI_Pathfinder msg: 1730863700.44 sec; seqNo:1999; #beams:4 2024-11-06T03:28:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.12 , 35.33 , 37.57 , 38.69 2024-11-06T03:28:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.11 , 0.03 2024-11-06T03:28:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863700.45 epoch sec; seqNo:5793; 4061908.61 north; 594645.40 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:28:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863699.21 2024-11-06T03:28:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:20Z- waiting for 1730863703.21, time is 1730863700.45 2024-11-06T03:28:20Z- AHRS_M2 msg: 1730863700.81 epoch sec; seqNo:5439 2024-11-06T03:28:20Z- AHRS_M2 msg: 0.05 phi; -0.03 theta; 5.97 psi 2024-11-06T03:28:20Z- handleDepth()- Depth_Keller msg: 1730863700.82 epoch sec; seqNo:5794; depth 50.21 2024-11-06T03:28:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863700.84 epoch sec; seqNo:5794; 4061908.99 north; 594645.40 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:28:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863699.21 2024-11-06T03:28:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:20Z- waiting for 1730863703.21, time is 1730863700.84 2024-11-06T03:28:21Z- AHRS_M2 msg: 1730863701.21 epoch sec; seqNo:5440 2024-11-06T03:28:21Z- AHRS_M2 msg: 0.02 phi; -0.02 theta; 5.95 psi 2024-11-06T03:28:21Z- handleDepth()- Depth_Keller msg: 1730863701.25 epoch sec; seqNo:5795; depth 50.23 2024-11-06T03:28:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863701.26 epoch sec; seqNo:5795; 4061909.37 north; 594645.39 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:28:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863699.21 2024-11-06T03:28:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:21Z- waiting for 1730863703.21, time is 1730863701.26 2024-11-06T03:28:21Z- AHRS_M2 msg: 1730863701.62 epoch sec; seqNo:5441 2024-11-06T03:28:21Z- AHRS_M2 msg: -0.02 phi; -0.02 theta; 5.92 psi 2024-11-06T03:28:21Z- handleDepth()- Depth_Keller msg: 1730863701.63 epoch sec; seqNo:5796; depth 50.25 2024-11-06T03:28:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863701.64 epoch sec; seqNo:5796; 4061909.74 north; 594645.39 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:28:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863699.21 2024-11-06T03:28:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:21Z- waiting for 1730863703.21, time is 1730863701.64 2024-11-06T03:28:22Z- AHRS_M2 msg: 1730863702.02 epoch sec; seqNo:5442 2024-11-06T03:28:22Z- AHRS_M2 msg: -0.05 phi; -0.02 theta; 5.89 psi 2024-11-06T03:28:22Z- handleDepth()- Depth_Keller msg: 1730863702.03 epoch sec; seqNo:5797; depth 50.28 2024-11-06T03:28:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863702.04 epoch sec; seqNo:5797; 4061910.12 north; 594645.38 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:28:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863699.21 2024-11-06T03:28:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:22Z- waiting for 1730863703.21, time is 1730863702.04 2024-11-06T03:28:22Z- handleDepth()- Depth_Keller msg: 1730863702.45 epoch sec; seqNo:5798; depth 50.28 2024-11-06T03:28:22Z- handleDVL() - DVL msg epochMillisec: 1730863702.47 2024-11-06T03:28:22Z- handleDvl() - RDI_Pathfinder msg: 1730863702.47 sec; seqNo:2000; #beams:4 2024-11-06T03:28:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.76 , 37.57 , 37.76 , 38.32 2024-11-06T03:28:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.03 2024-11-06T03:28:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863702.48 epoch sec; seqNo:5798; 4061910.12 north; 594645.38 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:28:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863699.21 2024-11-06T03:28:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:22Z- waiting for 1730863703.21, time is 1730863702.48 2024-11-06T03:28:22Z- AHRS_M2 msg: 1730863702.83 epoch sec; seqNo:5443 2024-11-06T03:28:22Z- AHRS_M2 msg: -0.13 phi; -0.01 theta; 5.81 psi 2024-11-06T03:28:22Z- handleDepth()- Depth_Keller msg: 1730863702.84 epoch sec; seqNo:5799; depth 50.28 2024-11-06T03:28:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863702.85 epoch sec; seqNo:5799; 4061910.50 north; 594645.38 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:28:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863699.21 2024-11-06T03:28:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:22Z- waiting for 1730863703.21, time is 1730863702.85 2024-11-06T03:28:23Z- AHRS_M2 msg: 1730863703.23 epoch sec; seqNo:5444 2024-11-06T03:28:23Z- AHRS_M2 msg: -0.17 phi; -0.01 theta; 5.76 psi 2024-11-06T03:28:23Z- handleDepth()- Depth_Keller msg: 1730863703.25 epoch sec; seqNo:5800; depth 50.30 2024-11-06T03:28:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863703.25 epoch sec; seqNo:5800; 4061910.87 north; 594644.16 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:28:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863699.21 2024-11-06T03:28:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:23Z- waiting for 1730863703.21, time is 1730863703.25 2024-11-06T03:28:23Z- LcmTrn::updateTrn() >>>> pose time 1730863703.25, meas time 1730863702.47 <<<< 2024-11-06T03:28:23Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863703.25 2024-11-06T03:28:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863702.47 sec, ping # 0; waiting for INS data more recent than 1730863699.21... 2024-11-06T03:28:23Z- TerrainNav::motionUpdate() - currEstimate = (4061911, 594644.16, 50.30) 2024-11-06T03:28:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:28:23Z- TerrainNav:: SubcloudNIS is 0.32, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:28:23Z- TNavPF::Projecting Measurements 2024-11-06T03:28:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:28:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:28:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:28:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:28:23Z- TNavFilter::4 good beams. 2024-11-06T03:28:23Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:28:23Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:28:23Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:28:23Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:28:23Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:28:23Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:28:23Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:28:23Z- TerrainMapOctree: (vehN, vehE ) = (4061910.21, 594644.64). 2024-11-06T03:28:23Z- TerrainMapOctree: Min Distance = 191.75. 2024-11-06T03:28:23Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:28:23Z- Forcing Subcloud Comparison 2024-11-06T03:28:23Z- Weighting particles with subcloud comparison 2024-11-06T03:28:23Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:23Z- meanExpectedMeasDiff: 4.226559 delta_rms_squared: 1.018500 alpha: 0.980741 2024-11-06T03:28:23Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:23Z- meanExpectedMeasDiff: -6.507217 delta_rms_squared: 1.447089 alpha: 0.986368 2024-11-06T03:28:23Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:23Z- meanExpectedMeasDiff: -1.275433 delta_rms_squared: 1.860165 alpha: 0.989363 2024-11-06T03:28:23Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:23Z- meanExpectedMeasDiff: -2.182225 delta_rms_squared: 1.298381 alpha: 0.984830 2024-11-06T03:28:23Z- Subcloud NIS: 0.365713 2024-11-06T03:28:23Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:28:23Z- TNavPF:: sumWeights = 0.000447 2024-11-06T03:28:23Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:28:23Z- TNavPF::First Term of Map Covariance Matrix is 0.01 2024-11-06T03:28:23Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:28:23Z- TerrainNav::Resampling particle filter... 2024-11-06T03:28:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863702.47 sec. 2024-11-06T03:28:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:28:23Z- variances([0], [2], and psi_berg) = 0.898899 0.342667 0.000000 2024-11-06T03:28:23Z- publishEstimatesDecoder() - at 1730863703.25 sec 2024-11-06T03:28:23Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:28:23Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061941.52 594626.87 55.70 2024-11-06T03:28:23Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061940.15 594627.56 57.06 lat/lon: 36.70 -121.94 2024-11-06T03:28:23Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.90 0.34 0.74 0.00 2024-11-06T03:28:23Z- AHRS_M2 msg: 1730863703.64 epoch sec; seqNo:5445 2024-11-06T03:28:23Z- AHRS_M2 msg: -0.19 phi; -0.01 theta; 5.72 psi 2024-11-06T03:28:23Z- handleDepth()- Depth_Keller msg: 1730863703.65 epoch sec; seqNo:5801; depth 50.32 2024-11-06T03:28:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863703.69 epoch sec; seqNo:5801; 4061911.25 north; 594644.15 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:28:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863703.25 2024-11-06T03:28:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:23Z- waiting for 1730863707.25, time is 1730863703.69 2024-11-06T03:28:24Z- AHRS_M2 msg: 1730863704.04 epoch sec; seqNo:5446 2024-11-06T03:28:24Z- AHRS_M2 msg: -0.19 phi; -0.00 theta; 5.67 psi 2024-11-06T03:28:24Z- handleDepth()- Depth_Keller msg: 1730863704.04 epoch sec; seqNo:5802; depth 50.32 2024-11-06T03:28:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863704.05 epoch sec; seqNo:5802; 4061911.25 north; 594644.15 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:28:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863703.25 2024-11-06T03:28:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:24Z- waiting for 1730863707.25, time is 1730863704.05 2024-11-06T03:28:24Z- AHRS_M2 msg: 1730863704.44 epoch sec; seqNo:5447 2024-11-06T03:28:24Z- AHRS_M2 msg: -0.18 phi; 0.00 theta; 5.63 psi 2024-11-06T03:28:24Z- handleDepth()- Depth_Keller msg: 1730863704.45 epoch sec; seqNo:5803; depth 50.35 2024-11-06T03:28:24Z- handleDVL() - DVL msg epochMillisec: 1730863704.47 2024-11-06T03:28:24Z- handleDvl() - RDI_Pathfinder msg: 1730863704.47 sec; seqNo:2001; #beams:4 2024-11-06T03:28:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.00 , 43.33 , 38.57 , 38.57 2024-11-06T03:28:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.02 2024-11-06T03:28:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863704.48 epoch sec; seqNo:5803; 4061911.63 north; 594644.15 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:28:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863703.25 2024-11-06T03:28:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:24Z- waiting for 1730863707.25, time is 1730863704.48 2024-11-06T03:28:24Z- AHRS_M2 msg: 1730863704.85 epoch sec; seqNo:5448 2024-11-06T03:28:24Z- AHRS_M2 msg: -0.16 phi; 0.01 theta; 5.58 psi 2024-11-06T03:28:24Z- handleDepth()- Depth_Keller msg: 1730863704.85 epoch sec; seqNo:5804; depth 50.36 2024-11-06T03:28:24Z- handleDVL() - DVL msg epochMillisec: 1730863704.85 2024-11-06T03:28:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:28:24Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:28:24Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:28:24Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:28:24Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:28:24Z- handleDvl() - RDI_Pathfinder msg: 1730863704.85 sec; seqNo:2002; #beams:0 2024-11-06T03:28:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:28:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.02 2024-11-06T03:28:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863704.88 epoch sec; seqNo:5804; 4061912.00 north; 594644.14 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:28:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863703.25 2024-11-06T03:28:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:25Z- waiting for 1730863707.25, time is 1730863704.88 2024-11-06T03:28:25Z- AHRS_M2 msg: 1730863705.25 epoch sec; seqNo:5449 2024-11-06T03:28:25Z- AHRS_M2 msg: -0.14 phi; 0.01 theta; 5.54 psi 2024-11-06T03:28:25Z- handleDepth()- Depth_Keller msg: 1730863705.27 epoch sec; seqNo:5805; depth 50.36 2024-11-06T03:28:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863705.28 epoch sec; seqNo:5805; 4061912.38 north; 594644.14 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:28:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863703.25 2024-11-06T03:28:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:25Z- waiting for 1730863707.25, time is 1730863705.28 2024-11-06T03:28:25Z- AHRS_M2 msg: 1730863705.66 epoch sec; seqNo:5450 2024-11-06T03:28:25Z- AHRS_M2 msg: -0.11 phi; 0.02 theta; 5.50 psi 2024-11-06T03:28:25Z- handleDepth()- Depth_Keller msg: 1730863705.66 epoch sec; seqNo:5806; depth 50.39 2024-11-06T03:28:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863705.67 epoch sec; seqNo:5806; 4061912.37 north; 594642.92 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:28:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863703.25 2024-11-06T03:28:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:25Z- waiting for 1730863707.25, time is 1730863705.67 2024-11-06T03:28:26Z- AHRS_M2 msg: 1730863706.06 epoch sec; seqNo:5451 2024-11-06T03:28:26Z- AHRS_M2 msg: -0.10 phi; 0.03 theta; 5.45 psi 2024-11-06T03:28:26Z- handleDepth()- Depth_Keller msg: 1730863706.07 epoch sec; seqNo:5807; depth 50.40 2024-11-06T03:28:26Z- handleDVL() - DVL msg epochMillisec: 1730863706.07 2024-11-06T03:28:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:28:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:28:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:28:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:28:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:28:26Z- handleDvl() - RDI_Pathfinder msg: 1730863706.07 sec; seqNo:2003; #beams:0 2024-11-06T03:28:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:28:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.02 2024-11-06T03:28:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863706.08 epoch sec; seqNo:5807; 4061912.75 north; 594642.92 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:28:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863703.25 2024-11-06T03:28:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:26Z- waiting for 1730863707.25, time is 1730863706.08 2024-11-06T03:28:26Z- AHRS_M2 msg: 1730863706.46 epoch sec; seqNo:5452 2024-11-06T03:28:26Z- AHRS_M2 msg: -0.09 phi; 0.04 theta; 5.41 psi 2024-11-06T03:28:26Z- handleDepth()- Depth_Keller msg: 1730863706.47 epoch sec; seqNo:5808; depth 50.40 2024-11-06T03:28:26Z- handleDVL() - DVL msg epochMillisec: 1730863706.50 2024-11-06T03:28:26Z- handleDvl() - RDI_Pathfinder msg: 1730863706.50 sec; seqNo:2004; #beams:4 2024-11-06T03:28:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.70 , 40.56 , 38.69 , 37.57 2024-11-06T03:28:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , -0.04 2024-11-06T03:28:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863706.52 epoch sec; seqNo:5808; 4061912.75 north; 594642.92 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:28:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863703.25 2024-11-06T03:28:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:26Z- waiting for 1730863707.25, time is 1730863706.52 2024-11-06T03:28:26Z- AHRS_M2 msg: 1730863706.87 epoch sec; seqNo:5453 2024-11-06T03:28:26Z- AHRS_M2 msg: -0.08 phi; 0.05 theta; 5.37 psi 2024-11-06T03:28:26Z- handleDepth()- Depth_Keller msg: 1730863706.87 epoch sec; seqNo:5809; depth 50.39 2024-11-06T03:28:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863706.89 epoch sec; seqNo:5809; 4061913.13 north; 594642.91 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:28:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863703.25 2024-11-06T03:28:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:27Z- waiting for 1730863707.25, time is 1730863706.89 2024-11-06T03:28:27Z- AHRS_M2 msg: 1730863707.28 epoch sec; seqNo:5454 2024-11-06T03:28:27Z- AHRS_M2 msg: -0.08 phi; 0.06 theta; 5.33 psi 2024-11-06T03:28:27Z- handleDepth()- Depth_Keller msg: 1730863707.29 epoch sec; seqNo:5810; depth 50.40 2024-11-06T03:28:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863707.30 epoch sec; seqNo:5810; 4061913.11 north; 594641.69 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:28:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863703.25 2024-11-06T03:28:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:27Z- waiting for 1730863707.25, time is 1730863707.30 2024-11-06T03:28:27Z- LcmTrn::updateTrn() >>>> pose time 1730863707.30, meas time 1730863706.50 <<<< 2024-11-06T03:28:27Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863707.30 2024-11-06T03:28:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863706.50 sec, ping # 0; waiting for INS data more recent than 1730863703.25... 2024-11-06T03:28:27Z- TerrainNav::motionUpdate() - currEstimate = (4061913, 594641.69, 50.40) 2024-11-06T03:28:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:28:27Z- TerrainNav:: SubcloudNIS is 0.37, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:28:27Z- TNavPF::Projecting Measurements 2024-11-06T03:28:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:28:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:28:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:28:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:28:27Z- TNavFilter::4 good beams. 2024-11-06T03:28:27Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:28:27Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:28:27Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:28:27Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:28:27Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:28:27Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:28:27Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:28:27Z- TerrainMapOctree: (vehN, vehE ) = (4061912.67, 594642.18). 2024-11-06T03:28:27Z- TerrainMapOctree: Min Distance = 195.16. 2024-11-06T03:28:27Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:28:27Z- Forcing Subcloud Comparison 2024-11-06T03:28:27Z- Weighting particles with subcloud comparison 2024-11-06T03:28:27Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:27Z- meanExpectedMeasDiff: 3.072470 delta_rms_squared: 1.188876 alpha: 0.983456 2024-11-06T03:28:27Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:27Z- meanExpectedMeasDiff: -1.949980 delta_rms_squared: 1.482853 alpha: 0.986692 2024-11-06T03:28:27Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:27Z- meanExpectedMeasDiff: 2.786134 delta_rms_squared: 1.244668 alpha: 0.984186 2024-11-06T03:28:27Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:27Z- meanExpectedMeasDiff: -0.440694 delta_rms_squared: 1.249245 alpha: 0.984243 2024-11-06T03:28:27Z- Subcloud NIS: 0.380694 2024-11-06T03:28:27Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:28:27Z- TNavPF:: sumWeights = 0.058829 2024-11-06T03:28:27Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:28:27Z- TNavPF::First Term of Map Covariance Matrix is 0.40 2024-11-06T03:28:27Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T03:28:27Z- TerrainNav::Resampling particle filter... 2024-11-06T03:28:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863706.50 sec. 2024-11-06T03:28:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:28:27Z- variances([0], [2], and psi_berg) = 0.999167 0.369542 0.000000 2024-11-06T03:28:27Z- publishEstimatesDecoder() - at 1730863707.30 sec 2024-11-06T03:28:27Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:28:27Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061943.76 594624.38 54.07 2024-11-06T03:28:27Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061942.42 594625.21 56.80 lat/lon: 36.70 -121.94 2024-11-06T03:28:27Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.00 0.37 0.72 0.00 2024-11-06T03:28:27Z- AHRS_M2 msg: 1730863707.69 epoch sec; seqNo:5455 2024-11-06T03:28:27Z- AHRS_M2 msg: -0.08 phi; 0.08 theta; 5.25 psi 2024-11-06T03:28:27Z- handleDepth()- Depth_Keller msg: 1730863707.71 epoch sec; seqNo:5811; depth 50.41 2024-11-06T03:28:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863707.73 epoch sec; seqNo:5811; 4061913.49 north; 594641.69 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:28:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863707.30 2024-11-06T03:28:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:27Z- waiting for 1730863711.30, time is 1730863707.73 2024-11-06T03:28:28Z- AHRS_M2 msg: 1730863708.09 epoch sec; seqNo:5456 2024-11-06T03:28:28Z- AHRS_M2 msg: -0.08 phi; 0.09 theta; 5.21 psi 2024-11-06T03:28:28Z- handleDepth()- Depth_Keller msg: 1730863708.10 epoch sec; seqNo:5812; depth 50.41 2024-11-06T03:28:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863708.11 epoch sec; seqNo:5812; 4061913.49 north; 594641.69 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:28:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863707.30 2024-11-06T03:28:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:28Z- waiting for 1730863711.30, time is 1730863708.11 2024-11-06T03:28:28Z- AHRS_M2 msg: 1730863708.49 epoch sec; seqNo:5457 2024-11-06T03:28:28Z- AHRS_M2 msg: -0.08 phi; 0.10 theta; 5.17 psi 2024-11-06T03:28:28Z- handleDepth()- Depth_Keller msg: 1730863708.49 epoch sec; seqNo:5813; depth 50.40 2024-11-06T03:28:28Z- handleDVL() - DVL msg epochMillisec: 1730863708.53 2024-11-06T03:28:28Z- handleDvl() - RDI_Pathfinder msg: 1730863708.53 sec; seqNo:2005; #beams:4 2024-11-06T03:28:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.45 , 39.63 , 39.81 , 36.26 2024-11-06T03:28:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , -0.05 2024-11-06T03:28:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863708.54 epoch sec; seqNo:5813; 4061913.49 north; 594641.69 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:28:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863707.30 2024-11-06T03:28:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:28Z- waiting for 1730863711.30, time is 1730863708.54 2024-11-06T03:28:28Z- AHRS_M2 msg: 1730863708.92 epoch sec; seqNo:5458 2024-11-06T03:28:28Z- AHRS_M2 msg: -0.08 phi; 0.11 theta; 5.13 psi 2024-11-06T03:28:28Z- handleDepth()- Depth_Keller msg: 1730863708.92 epoch sec; seqNo:5814; depth 50.39 2024-11-06T03:28:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863708.94 epoch sec; seqNo:5814; 4061913.86 north; 594640.46 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:28:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863707.30 2024-11-06T03:28:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:29Z- waiting for 1730863711.30, time is 1730863708.94 2024-11-06T03:28:29Z- AHRS_M2 msg: 1730863709.32 epoch sec; seqNo:5459 2024-11-06T03:28:29Z- AHRS_M2 msg: -0.07 phi; 0.12 theta; 5.09 psi 2024-11-06T03:28:29Z- handleDepth()- Depth_Keller msg: 1730863709.33 epoch sec; seqNo:5815; depth 50.37 2024-11-06T03:28:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863709.35 epoch sec; seqNo:5815; 4061913.86 north; 594640.46 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:28:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863707.30 2024-11-06T03:28:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:29Z- waiting for 1730863711.30, time is 1730863709.35 2024-11-06T03:28:29Z- AHRS_M2 msg: 1730863709.72 epoch sec; seqNo:5460 2024-11-06T03:28:29Z- AHRS_M2 msg: -0.07 phi; 0.12 theta; 5.04 psi 2024-11-06T03:28:29Z- handleDepth()- Depth_Keller msg: 1730863709.73 epoch sec; seqNo:5816; depth 50.35 2024-11-06T03:28:29Z- handleDVL() - DVL msg epochMillisec: 1730863709.73 2024-11-06T03:28:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:28:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:28:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:28:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:28:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:28:29Z- handleDvl() - RDI_Pathfinder msg: 1730863709.73 sec; seqNo:2006; #beams:0 2024-11-06T03:28:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:28:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , -0.05 2024-11-06T03:28:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863709.74 epoch sec; seqNo:5816; 4061913.86 north; 594640.46 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:28:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863707.30 2024-11-06T03:28:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:29Z- waiting for 1730863711.30, time is 1730863709.74 2024-11-06T03:28:30Z- AHRS_M2 msg: 1730863710.13 epoch sec; seqNo:5461 2024-11-06T03:28:30Z- AHRS_M2 msg: -0.08 phi; 0.13 theta; 5.00 psi 2024-11-06T03:28:30Z- handleDepth()- Depth_Keller msg: 1730863710.13 epoch sec; seqNo:5817; depth 50.35 2024-11-06T03:28:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863710.15 epoch sec; seqNo:5817; 4061913.86 north; 594640.46 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:28:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863707.30 2024-11-06T03:28:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:30Z- waiting for 1730863711.30, time is 1730863710.15 2024-11-06T03:28:30Z- handleDepth()- Depth_Keller msg: 1730863710.56 epoch sec; seqNo:5818; depth 50.33 2024-11-06T03:28:30Z- handleDVL() - DVL msg epochMillisec: 1730863710.58 2024-11-06T03:28:30Z- handleDvl() - RDI_Pathfinder msg: 1730863710.58 sec; seqNo:2007; #beams:4 2024-11-06T03:28:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.70 , 40.10 , 41.29 , 35.51 2024-11-06T03:28:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.06 2024-11-06T03:28:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863710.59 epoch sec; seqNo:5818; 4061914.22 north; 594639.24 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:28:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863707.30 2024-11-06T03:28:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:30Z- waiting for 1730863711.30, time is 1730863710.59 2024-11-06T03:28:30Z- AHRS_M2 msg: 1730863710.94 epoch sec; seqNo:5462 2024-11-06T03:28:30Z- AHRS_M2 msg: -0.08 phi; 0.14 theta; 4.92 psi 2024-11-06T03:28:30Z- handleDepth()- Depth_Keller msg: 1730863710.95 epoch sec; seqNo:5819; depth 50.31 2024-11-06T03:28:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863710.97 epoch sec; seqNo:5819; 4061914.22 north; 594639.24 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:28:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863707.30 2024-11-06T03:28:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:31Z- waiting for 1730863711.30, time is 1730863710.97 2024-11-06T03:28:31Z- handleDepth()- Depth_Keller msg: 1730863711.37 epoch sec; seqNo:5820; depth 50.28 2024-11-06T03:28:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863711.38 epoch sec; seqNo:5820; 4061914.22 north; 594639.24 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:28:31Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:28:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863707.30 2024-11-06T03:28:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:31Z- waiting for 1730863711.30, time is 1730863711.38 2024-11-06T03:28:31Z- LcmTrn::updateTrn() >>>> pose time 1730863711.38, meas time 1730863710.58 <<<< 2024-11-06T03:28:31Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863711.38 2024-11-06T03:28:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863710.58 sec, ping # 0; waiting for INS data more recent than 1730863707.30... 2024-11-06T03:28:31Z- TerrainNav::motionUpdate() - currEstimate = (4061914, 594639.24, 50.28) 2024-11-06T03:28:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:28:31Z- TerrainNav:: SubcloudNIS is 0.38, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:28:31Z- TNavPF::Projecting Measurements 2024-11-06T03:28:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:28:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:28:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:28:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:28:31Z- TNavFilter::4 good beams. 2024-11-06T03:28:31Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:28:31Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:28:31Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:28:31Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:28:31Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:28:31Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:28:31Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:28:31Z- TerrainMapOctree: (vehN, vehE ) = (4061914.01, 594639.72). 2024-11-06T03:28:31Z- TerrainMapOctree: Min Distance = 197.64. 2024-11-06T03:28:31Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:28:31Z- Forcing Subcloud Comparison 2024-11-06T03:28:31Z- Weighting particles with subcloud comparison 2024-11-06T03:28:31Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:31Z- meanExpectedMeasDiff: 2.539956 delta_rms_squared: 1.770932 alpha: 0.988833 2024-11-06T03:28:31Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:31Z- meanExpectedMeasDiff: -3.449628 delta_rms_squared: 2.392371 alpha: 0.991709 2024-11-06T03:28:31Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:31Z- meanExpectedMeasDiff: 6.334166 delta_rms_squared: 1.177008 alpha: 0.983292 2024-11-06T03:28:31Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:31Z- meanExpectedMeasDiff: -1.469685 delta_rms_squared: 1.138409 alpha: 0.982735 2024-11-06T03:28:31Z- Subcloud NIS: 0.430552 2024-11-06T03:28:31Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:28:31Z- TNavPF:: sumWeights = 0.000910 2024-11-06T03:28:31Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:28:31Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:28:31Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:28:31Z- TerrainNav::Resampling particle filter... 2024-11-06T03:28:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863710.58 sec. 2024-11-06T03:28:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:28:31Z- variances([0], [2], and psi_berg) = 0.897000 0.304811 0.000000 2024-11-06T03:28:31Z- publishEstimatesDecoder() - at 1730863711.38 sec 2024-11-06T03:28:31Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:28:31Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061944.75 594621.86 56.48 2024-11-06T03:28:31Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061943.16 594622.91 56.46 lat/lon: 36.70 -121.94 2024-11-06T03:28:31Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.90 0.30 0.63 0.00 2024-11-06T03:28:31Z- AHRS_M2 msg: 1730863711.76 epoch sec; seqNo:5463 2024-11-06T03:28:31Z- AHRS_M2 msg: -0.08 phi; 0.15 theta; 4.84 psi 2024-11-06T03:28:31Z- handleDepth()- Depth_Keller msg: 1730863711.78 epoch sec; seqNo:5821; depth 50.26 2024-11-06T03:28:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863711.81 epoch sec; seqNo:5821; 4061914.22 north; 594639.24 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:28:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863711.38 2024-11-06T03:28:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:31Z- waiting for 1730863715.38, time is 1730863711.81 2024-11-06T03:28:32Z- AHRS_M2 msg: 1730863712.16 epoch sec; seqNo:5464 2024-11-06T03:28:32Z- AHRS_M2 msg: -0.08 phi; 0.15 theta; 4.80 psi 2024-11-06T03:28:32Z- handleDepth()- Depth_Keller msg: 1730863712.18 epoch sec; seqNo:5822; depth 50.24 2024-11-06T03:28:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863712.23 epoch sec; seqNo:5822; 4061914.21 north; 594638.02 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:28:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863711.38 2024-11-06T03:28:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:32Z- waiting for 1730863715.38, time is 1730863712.23 2024-11-06T03:28:32Z- AHRS_M2 msg: 1730863712.59 epoch sec; seqNo:5465 2024-11-06T03:28:32Z- AHRS_M2 msg: -0.08 phi; 0.15 theta; 4.76 psi 2024-11-06T03:28:32Z- handleDepth()- Depth_Keller msg: 1730863712.59 epoch sec; seqNo:5823; depth 50.21 2024-11-06T03:28:32Z- handleDVL() - DVL msg epochMillisec: 1730863712.62 2024-11-06T03:28:32Z- handleDvl() - RDI_Pathfinder msg: 1730863712.62 sec; seqNo:2008; #beams:4 2024-11-06T03:28:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.01 , 40.75 , 42.06 , 35.33 2024-11-06T03:28:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.02 , -0.04 2024-11-06T03:28:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863712.65 epoch sec; seqNo:5823; 4061914.21 north; 594638.02 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:28:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863711.38 2024-11-06T03:28:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:32Z- waiting for 1730863715.38, time is 1730863712.65 2024-11-06T03:28:33Z- AHRS_M2 msg: 1730863712.99 epoch sec; seqNo:5466 2024-11-06T03:28:33Z- AHRS_M2 msg: -0.07 phi; 0.15 theta; 4.73 psi 2024-11-06T03:28:33Z- handleDepth()- Depth_Keller msg: 1730863713.00 epoch sec; seqNo:5824; depth 50.17 2024-11-06T03:28:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863713.02 epoch sec; seqNo:5824; 4061914.21 north; 594638.02 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:28:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863711.38 2024-11-06T03:28:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:33Z- waiting for 1730863715.38, time is 1730863713.02 2024-11-06T03:28:33Z- AHRS_M2 msg: 1730863713.39 epoch sec; seqNo:5467 2024-11-06T03:28:33Z- AHRS_M2 msg: -0.07 phi; 0.15 theta; 4.69 psi 2024-11-06T03:28:33Z- handleDepth()- Depth_Keller msg: 1730863713.40 epoch sec; seqNo:5825; depth 50.14 2024-11-06T03:28:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863713.42 epoch sec; seqNo:5825; 4061914.20 north; 594636.80 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:28:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863711.38 2024-11-06T03:28:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:33Z- waiting for 1730863715.38, time is 1730863713.42 2024-11-06T03:28:33Z- AHRS_M2 msg: 1730863713.80 epoch sec; seqNo:5468 2024-11-06T03:28:33Z- AHRS_M2 msg: -0.07 phi; 0.15 theta; 4.65 psi 2024-11-06T03:28:33Z- handleDepth()- Depth_Keller msg: 1730863713.81 epoch sec; seqNo:5826; depth 50.11 2024-11-06T03:28:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863713.84 epoch sec; seqNo:5826; 4061914.20 north; 594636.80 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:28:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863711.38 2024-11-06T03:28:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:33Z- waiting for 1730863715.38, time is 1730863713.84 2024-11-06T03:28:34Z- AHRS_M2 msg: 1730863714.23 epoch sec; seqNo:5469 2024-11-06T03:28:34Z- AHRS_M2 msg: -0.08 phi; 0.15 theta; 4.61 psi 2024-11-06T03:28:34Z- handleDepth()- Depth_Keller msg: 1730863714.23 epoch sec; seqNo:5827; depth 50.07 2024-11-06T03:28:34Z- handleDVL() - DVL msg epochMillisec: 1730863714.26 2024-11-06T03:28:34Z- handleDvl() - RDI_Pathfinder msg: 1730863714.26 sec; seqNo:2009; #beams:4 2024-11-06T03:28:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.38 , 40.75 , 41.87 , 35.33 2024-11-06T03:28:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.00 , -0.02 2024-11-06T03:28:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863714.27 epoch sec; seqNo:5827; 4061914.20 north; 594636.80 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:28:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863711.38 2024-11-06T03:28:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:34Z- waiting for 1730863715.38, time is 1730863714.27 2024-11-06T03:28:34Z- AHRS_M2 msg: 1730863714.61 epoch sec; seqNo:5470 2024-11-06T03:28:34Z- AHRS_M2 msg: -0.08 phi; 0.14 theta; 4.58 psi 2024-11-06T03:28:34Z- handleDepth()- Depth_Keller msg: 1730863714.62 epoch sec; seqNo:5828; depth 50.03 2024-11-06T03:28:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863714.64 epoch sec; seqNo:5828; 4061914.20 north; 594636.80 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:28:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863711.38 2024-11-06T03:28:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:34Z- waiting for 1730863715.38, time is 1730863714.64 2024-11-06T03:28:35Z- AHRS_M2 msg: 1730863715.02 epoch sec; seqNo:5471 2024-11-06T03:28:35Z- AHRS_M2 msg: -0.07 phi; 0.14 theta; 4.54 psi 2024-11-06T03:28:35Z- handleDepth()- Depth_Keller msg: 1730863715.02 epoch sec; seqNo:5829; depth 50.02 2024-11-06T03:28:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863715.04 epoch sec; seqNo:5829; 4061914.18 north; 594635.58 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:28:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863711.38 2024-11-06T03:28:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:35Z- waiting for 1730863715.38, time is 1730863715.04 2024-11-06T03:28:35Z- AHRS_M2 msg: 1730863715.41 epoch sec; seqNo:5472 2024-11-06T03:28:35Z- AHRS_M2 msg: -0.07 phi; 0.13 theta; 4.50 psi 2024-11-06T03:28:35Z- handleDepth()- Depth_Keller msg: 1730863715.42 epoch sec; seqNo:5830; depth 49.96 2024-11-06T03:28:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863715.43 epoch sec; seqNo:5830; 4061913.80 north; 594635.58 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:28:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863711.38 2024-11-06T03:28:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:35Z- waiting for 1730863715.38, time is 1730863715.43 2024-11-06T03:28:35Z- LcmTrn::updateTrn() >>>> pose time 1730863715.43, meas time 1730863714.26 <<<< 2024-11-06T03:28:35Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863715.43 2024-11-06T03:28:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863714.26 sec, ping # 0; waiting for INS data more recent than 1730863711.38... 2024-11-06T03:28:35Z- TerrainNav::motionUpdate() - currEstimate = (4061914, 594635.58, 49.96) 2024-11-06T03:28:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:28:35Z- TerrainNav:: SubcloudNIS is 0.43, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:28:35Z- TNavPF::Projecting Measurements 2024-11-06T03:28:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:28:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:28:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:28:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:28:35Z- TNavFilter::4 good beams. 2024-11-06T03:28:35Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:28:35Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:28:35Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:28:35Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:28:35Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:28:35Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:28:35Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:28:35Z- TerrainMapOctree: (vehN, vehE ) = (4061913.93, 594636.64). 2024-11-06T03:28:35Z- TerrainMapOctree: Min Distance = 199.31. 2024-11-06T03:28:35Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:28:35Z- Forcing Subcloud Comparison 2024-11-06T03:28:35Z- Weighting particles with subcloud comparison 2024-11-06T03:28:35Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:35Z- meanExpectedMeasDiff: 2.102562 delta_rms_squared: 1.112013 alpha: 0.982332 2024-11-06T03:28:35Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:35Z- meanExpectedMeasDiff: -0.218775 delta_rms_squared: 1.764885 alpha: 0.988795 2024-11-06T03:28:35Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:35Z- meanExpectedMeasDiff: 5.937974 delta_rms_squared: 0.705659 alpha: 0.972439 2024-11-06T03:28:35Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:35Z- meanExpectedMeasDiff: -1.954851 delta_rms_squared: 1.037104 alpha: 0.981080 2024-11-06T03:28:35Z- Subcloud NIS: 0.432465 2024-11-06T03:28:35Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:28:35Z- TNavPF:: sumWeights = 0.007259 2024-11-06T03:28:35Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:28:35Z- TNavPF::First Term of Map Covariance Matrix is 0.02 2024-11-06T03:28:35Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:28:35Z- TerrainNav::Resampling particle filter... 2024-11-06T03:28:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863714.26 sec. 2024-11-06T03:28:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:28:35Z- variances([0], [2], and psi_berg) = 0.918898 0.269152 0.000000 2024-11-06T03:28:35Z- publishEstimatesDecoder() - at 1730863715.43 sec 2024-11-06T03:28:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:28:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061943.96 594617.80 56.45 2024-11-06T03:28:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061942.76 594619.44 55.75 lat/lon: 36.70 -121.94 2024-11-06T03:28:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.92 0.27 0.69 0.00 2024-11-06T03:28:35Z- AHRS_M2 msg: 1730863715.82 epoch sec; seqNo:5473 2024-11-06T03:28:35Z- AHRS_M2 msg: -0.07 phi; 0.13 theta; 4.46 psi 2024-11-06T03:28:35Z- handleDepth()- Depth_Keller msg: 1730863715.82 epoch sec; seqNo:5831; depth 49.96 2024-11-06T03:28:35Z- handleDVL() - DVL msg epochMillisec: 1730863715.83 2024-11-06T03:28:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:28:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:28:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:28:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:28:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:28:35Z- handleDvl() - RDI_Pathfinder msg: 1730863715.83 sec; seqNo:2010; #beams:0 2024-11-06T03:28:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:28:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.00 , -0.02 2024-11-06T03:28:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863715.83 epoch sec; seqNo:5831; 4061913.80 north; 594635.58 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:28:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863715.43 2024-11-06T03:28:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:35Z- waiting for 1730863719.43, time is 1730863715.83 2024-11-06T03:28:36Z- AHRS_M2 msg: 1730863716.24 epoch sec; seqNo:5474 2024-11-06T03:28:36Z- AHRS_M2 msg: -0.07 phi; 0.13 theta; 4.43 psi 2024-11-06T03:28:36Z- handleDepth()- Depth_Keller msg: 1730863716.24 epoch sec; seqNo:5832; depth 49.91 2024-11-06T03:28:36Z- handleDVL() - DVL msg epochMillisec: 1730863716.26 2024-11-06T03:28:36Z- handleDvl() - RDI_Pathfinder msg: 1730863716.26 sec; seqNo:2011; #beams:4 2024-11-06T03:28:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.38 , 40.75 , 41.31 , 35.70 2024-11-06T03:28:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.00 , -0.04 2024-11-06T03:28:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863716.29 epoch sec; seqNo:5832; 4061913.80 north; 594635.58 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:28:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863715.43 2024-11-06T03:28:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:36Z- waiting for 1730863719.43, time is 1730863716.29 2024-11-06T03:28:36Z- AHRS_M2 msg: 1730863716.63 epoch sec; seqNo:5475 2024-11-06T03:28:36Z- AHRS_M2 msg: -0.07 phi; 0.12 theta; 4.39 psi 2024-11-06T03:28:36Z- handleDepth()- Depth_Keller msg: 1730863716.64 epoch sec; seqNo:5833; depth 49.86 2024-11-06T03:28:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863716.65 epoch sec; seqNo:5833; 4061913.79 north; 594634.36 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:28:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863715.43 2024-11-06T03:28:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:36Z- waiting for 1730863719.43, time is 1730863716.65 2024-11-06T03:28:37Z- AHRS_M2 msg: 1730863717.03 epoch sec; seqNo:5476 2024-11-06T03:28:37Z- AHRS_M2 msg: -0.07 phi; 0.12 theta; 4.35 psi 2024-11-06T03:28:37Z- handleDepth()- Depth_Keller msg: 1730863717.04 epoch sec; seqNo:5834; depth 49.83 2024-11-06T03:28:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863717.06 epoch sec; seqNo:5834; 4061913.41 north; 594634.36 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:28:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863715.43 2024-11-06T03:28:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:37Z- waiting for 1730863719.43, time is 1730863717.06 2024-11-06T03:28:37Z- AHRS_M2 msg: 1730863717.44 epoch sec; seqNo:5477 2024-11-06T03:28:37Z- AHRS_M2 msg: -0.07 phi; 0.11 theta; 4.32 psi 2024-11-06T03:28:37Z- handleDepth()- Depth_Keller msg: 1730863717.44 epoch sec; seqNo:5835; depth 49.81 2024-11-06T03:28:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863717.45 epoch sec; seqNo:5835; 4061913.41 north; 594634.36 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:28:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863715.43 2024-11-06T03:28:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:37Z- waiting for 1730863719.43, time is 1730863717.45 2024-11-06T03:28:37Z- AHRS_M2 msg: 1730863717.84 epoch sec; seqNo:5478 2024-11-06T03:28:37Z- AHRS_M2 msg: -0.07 phi; 0.11 theta; 4.28 psi 2024-11-06T03:28:37Z- handleDepth()- Depth_Keller msg: 1730863717.84 epoch sec; seqNo:5836; depth 49.78 2024-11-06T03:28:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863717.85 epoch sec; seqNo:5836; 4061913.41 north; 594634.36 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:28:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863715.43 2024-11-06T03:28:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:37Z- waiting for 1730863719.43, time is 1730863717.85 2024-11-06T03:28:38Z- AHRS_M2 msg: 1730863718.24 epoch sec; seqNo:5479 2024-11-06T03:28:38Z- AHRS_M2 msg: -0.07 phi; 0.10 theta; 4.24 psi 2024-11-06T03:28:38Z- handleDepth()- Depth_Keller msg: 1730863718.25 epoch sec; seqNo:5837; depth 49.77 2024-11-06T03:28:38Z- handleDVL() - DVL msg epochMillisec: 1730863718.28 2024-11-06T03:28:38Z- handleDvl() - RDI_Pathfinder msg: 1730863718.28 sec; seqNo:2012; #beams:4 2024-11-06T03:28:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.38 , 40.56 , 40.56 , 36.08 2024-11-06T03:28:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.04 , -0.05 2024-11-06T03:28:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863718.29 epoch sec; seqNo:5837; 4061913.03 north; 594634.37 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:28:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863715.43 2024-11-06T03:28:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:38Z- waiting for 1730863719.43, time is 1730863718.29 2024-11-06T03:28:38Z- AHRS_M2 msg: 1730863718.65 epoch sec; seqNo:5480 2024-11-06T03:28:38Z- AHRS_M2 msg: -0.06 phi; 0.10 theta; 4.20 psi 2024-11-06T03:28:38Z- handleDepth()- Depth_Keller msg: 1730863718.65 epoch sec; seqNo:5838; depth 49.73 2024-11-06T03:28:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863718.67 epoch sec; seqNo:5838; 4061913.02 north; 594633.15 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:28:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863715.43 2024-11-06T03:28:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:38Z- waiting for 1730863719.43, time is 1730863718.67 2024-11-06T03:28:39Z- AHRS_M2 msg: 1730863719.05 epoch sec; seqNo:5481 2024-11-06T03:28:39Z- AHRS_M2 msg: -0.06 phi; 0.09 theta; 4.17 psi 2024-11-06T03:28:39Z- handleDepth()- Depth_Keller msg: 1730863719.06 epoch sec; seqNo:5839; depth 49.71 2024-11-06T03:28:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863719.07 epoch sec; seqNo:5839; 4061913.02 north; 594633.15 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:28:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863715.43 2024-11-06T03:28:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:39Z- waiting for 1730863719.43, time is 1730863719.07 2024-11-06T03:28:39Z- AHRS_M2 msg: 1730863719.47 epoch sec; seqNo:5482 2024-11-06T03:28:39Z- AHRS_M2 msg: -0.06 phi; 0.09 theta; 4.13 psi 2024-11-06T03:28:39Z- handleDepth()- Depth_Keller msg: 1730863719.48 epoch sec; seqNo:5840; depth 49.67 2024-11-06T03:28:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863719.49 epoch sec; seqNo:5840; 4061912.64 north; 594633.15 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:28:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863715.43 2024-11-06T03:28:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:39Z- waiting for 1730863719.43, time is 1730863719.49 2024-11-06T03:28:39Z- LcmTrn::updateTrn() >>>> pose time 1730863719.49, meas time 1730863718.28 <<<< 2024-11-06T03:28:39Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863719.49 2024-11-06T03:28:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863718.28 sec, ping # 0; waiting for INS data more recent than 1730863715.43... 2024-11-06T03:28:39Z- TerrainNav::motionUpdate() - currEstimate = (4061913, 594633.15, 49.67) 2024-11-06T03:28:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:28:39Z- TerrainNav:: SubcloudNIS is 0.43, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:28:39Z- TNavPF::Projecting Measurements 2024-11-06T03:28:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:28:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:28:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:28:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:28:39Z- TNavFilter::4 good beams. 2024-11-06T03:28:39Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:28:39Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:28:39Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:28:39Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:28:39Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:28:39Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:28:39Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:28:39Z- TerrainMapOctree: (vehN, vehE ) = (4061912.99, 594633.88). 2024-11-06T03:28:39Z- TerrainMapOctree: Min Distance = 200.12. 2024-11-06T03:28:39Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:28:39Z- Forcing Subcloud Comparison 2024-11-06T03:28:39Z- Weighting particles with subcloud comparison 2024-11-06T03:28:39Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:39Z- meanExpectedMeasDiff: 0.210566 delta_rms_squared: 1.285379 alpha: 0.984679 2024-11-06T03:28:39Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:39Z- meanExpectedMeasDiff: 3.003689 delta_rms_squared: 2.080028 alpha: 0.990476 2024-11-06T03:28:39Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:39Z- meanExpectedMeasDiff: 4.205461 delta_rms_squared: 0.824126 alpha: 0.976307 2024-11-06T03:28:39Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:39Z- meanExpectedMeasDiff: -1.750694 delta_rms_squared: 1.913105 alpha: 0.989654 2024-11-06T03:28:39Z- Subcloud NIS: 0.454511 2024-11-06T03:28:39Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:28:39Z- TNavPF:: sumWeights = 0.030068 2024-11-06T03:28:39Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:28:39Z- TNavPF::First Term of Map Covariance Matrix is 0.02 2024-11-06T03:28:39Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:28:39Z- TerrainNav::Resampling particle filter... 2024-11-06T03:28:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863718.28 sec. 2024-11-06T03:28:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:28:39Z- variances([0], [2], and psi_berg) = 0.621916 0.227487 0.000000 2024-11-06T03:28:39Z- publishEstimatesDecoder() - at 1730863719.49 sec 2024-11-06T03:28:39Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:28:39Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061942.35 594616.13 54.28 2024-11-06T03:28:39Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061941.39 594617.10 55.07 lat/lon: 36.70 -121.94 2024-11-06T03:28:39Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.62 0.23 0.70 0.00 2024-11-06T03:28:39Z- AHRS_M2 msg: 1730863719.86 epoch sec; seqNo:5483 2024-11-06T03:28:39Z- AHRS_M2 msg: -0.06 phi; 0.09 theta; 4.09 psi 2024-11-06T03:28:39Z- handleDepth()- Depth_Keller msg: 1730863719.88 epoch sec; seqNo:5841; depth 49.65 2024-11-06T03:28:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863719.90 epoch sec; seqNo:5841; 4061912.64 north; 594633.15 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:28:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863719.49 2024-11-06T03:28:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:40Z- waiting for 1730863723.49, time is 1730863719.90 2024-11-06T03:28:40Z- AHRS_M2 msg: 1730863720.26 epoch sec; seqNo:5484 2024-11-06T03:28:40Z- AHRS_M2 msg: -0.06 phi; 0.09 theta; 4.05 psi 2024-11-06T03:28:40Z- handleDepth()- Depth_Keller msg: 1730863720.27 epoch sec; seqNo:5842; depth 49.64 2024-11-06T03:28:40Z- handleDVL() - DVL msg epochMillisec: 1730863720.29 2024-11-06T03:28:40Z- handleDvl() - RDI_Pathfinder msg: 1730863720.29 sec; seqNo:2013; #beams:4 2024-11-06T03:28:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.57 , 40.56 , 40.93 , 36.63 2024-11-06T03:28:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.05 , -0.02 2024-11-06T03:28:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863720.32 epoch sec; seqNo:5842; 4061912.25 north; 594631.94 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:28:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863719.49 2024-11-06T03:28:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:40Z- waiting for 1730863723.49, time is 1730863720.32 2024-11-06T03:28:40Z- AHRS_M2 msg: 1730863720.67 epoch sec; seqNo:5485 2024-11-06T03:28:40Z- AHRS_M2 msg: -0.06 phi; 0.08 theta; 4.01 psi 2024-11-06T03:28:40Z- handleDepth()- Depth_Keller msg: 1730863720.67 epoch sec; seqNo:5843; depth 49.62 2024-11-06T03:28:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863720.69 epoch sec; seqNo:5843; 4061912.25 north; 594631.94 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:28:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863719.49 2024-11-06T03:28:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:40Z- waiting for 1730863723.49, time is 1730863720.69 2024-11-06T03:28:41Z- AHRS_M2 msg: 1730863721.08 epoch sec; seqNo:5486 2024-11-06T03:28:41Z- AHRS_M2 msg: -0.05 phi; 0.08 theta; 3.98 psi 2024-11-06T03:28:41Z- handleDepth()- Depth_Keller msg: 1730863721.08 epoch sec; seqNo:5844; depth 49.58 2024-11-06T03:28:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863721.11 epoch sec; seqNo:5844; 4061911.87 north; 594631.94 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:28:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863719.49 2024-11-06T03:28:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:41Z- waiting for 1730863723.49, time is 1730863721.11 2024-11-06T03:28:41Z- AHRS_M2 msg: 1730863721.47 epoch sec; seqNo:5487 2024-11-06T03:28:41Z- AHRS_M2 msg: -0.05 phi; 0.08 theta; 3.94 psi 2024-11-06T03:28:41Z- handleDepth()- Depth_Keller msg: 1730863721.48 epoch sec; seqNo:5845; depth 49.56 2024-11-06T03:28:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863721.49 epoch sec; seqNo:5845; 4061911.87 north; 594631.94 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:28:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863719.49 2024-11-06T03:28:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:41Z- waiting for 1730863723.49, time is 1730863721.49 2024-11-06T03:28:41Z- AHRS_M2 msg: 1730863721.88 epoch sec; seqNo:5488 2024-11-06T03:28:41Z- AHRS_M2 msg: -0.05 phi; 0.07 theta; 3.91 psi 2024-11-06T03:28:41Z- handleDepth()- Depth_Keller msg: 1730863721.89 epoch sec; seqNo:5846; depth 49.55 2024-11-06T03:28:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863721.91 epoch sec; seqNo:5846; 4061911.49 north; 594631.94 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:28:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863719.49 2024-11-06T03:28:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:42Z- waiting for 1730863723.49, time is 1730863721.91 2024-11-06T03:28:42Z- AHRS_M2 msg: 1730863722.28 epoch sec; seqNo:5489 2024-11-06T03:28:42Z- AHRS_M2 msg: -0.05 phi; 0.07 theta; 3.87 psi 2024-11-06T03:28:42Z- handleDepth()- Depth_Keller msg: 1730863722.29 epoch sec; seqNo:5847; depth 49.54 2024-11-06T03:28:42Z- handleDVL() - DVL msg epochMillisec: 1730863722.32 2024-11-06T03:28:42Z- handleDvl() - RDI_Pathfinder msg: 1730863722.32 sec; seqNo:2014; #beams:4 2024-11-06T03:28:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.76 , 40.19 , 40.75 , 37.01 2024-11-06T03:28:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.03 , -0.01 2024-11-06T03:28:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863722.34 epoch sec; seqNo:5847; 4061911.48 north; 594630.72 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T03:28:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863719.49 2024-11-06T03:28:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:42Z- waiting for 1730863723.49, time is 1730863722.34 2024-11-06T03:28:42Z- AHRS_M2 msg: 1730863722.69 epoch sec; seqNo:5490 2024-11-06T03:28:42Z- AHRS_M2 msg: -0.04 phi; 0.07 theta; 3.81 psi 2024-11-06T03:28:42Z- handleDepth()- Depth_Keller msg: 1730863722.70 epoch sec; seqNo:5848; depth 49.51 2024-11-06T03:28:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863722.71 epoch sec; seqNo:5848; 4061911.10 north; 594630.73 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T03:28:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863719.49 2024-11-06T03:28:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:42Z- waiting for 1730863723.49, time is 1730863722.71 2024-11-06T03:28:43Z- AHRS_M2 msg: 1730863723.11 epoch sec; seqNo:5491 2024-11-06T03:28:43Z- AHRS_M2 msg: -0.03 phi; 0.07 theta; 3.78 psi 2024-11-06T03:28:43Z- handleDepth()- Depth_Keller msg: 1730863723.11 epoch sec; seqNo:5849; depth 49.47 2024-11-06T03:28:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863723.12 epoch sec; seqNo:5849; 4061910.72 north; 594630.73 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:28:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863719.49 2024-11-06T03:28:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:43Z- waiting for 1730863723.49, time is 1730863723.12 2024-11-06T03:28:43Z- AHRS_M2 msg: 1730863723.50 epoch sec; seqNo:5492 2024-11-06T03:28:43Z- AHRS_M2 msg: -0.02 phi; 0.06 theta; 3.76 psi 2024-11-06T03:28:43Z- handleDepth()- Depth_Keller msg: 1730863723.52 epoch sec; seqNo:5850; depth 49.48 2024-11-06T03:28:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863723.53 epoch sec; seqNo:5850; 4061910.72 north; 594630.73 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:28:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863719.49 2024-11-06T03:28:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:43Z- waiting for 1730863723.49, time is 1730863723.53 2024-11-06T03:28:43Z- LcmTrn::updateTrn() >>>> pose time 1730863723.53, meas time 1730863722.32 <<<< 2024-11-06T03:28:43Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863723.53 2024-11-06T03:28:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863722.32 sec, ping # 0; waiting for INS data more recent than 1730863719.49... 2024-11-06T03:28:43Z- TerrainNav::motionUpdate() - currEstimate = (4061911, 594630.73, 49.48) 2024-11-06T03:28:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:28:43Z- TerrainNav:: SubcloudNIS is 0.45, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:28:43Z- TNavPF::Projecting Measurements 2024-11-06T03:28:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:28:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:28:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:28:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:28:43Z- TNavFilter::4 good beams. 2024-11-06T03:28:43Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:28:43Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:28:43Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:28:43Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:28:43Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:28:43Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:28:43Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:28:43Z- TerrainMapOctree: (vehN, vehE ) = (4061911.29, 594631.46). 2024-11-06T03:28:43Z- TerrainMapOctree: Min Distance = 200.17. 2024-11-06T03:28:43Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:28:43Z- Forcing Subcloud Comparison 2024-11-06T03:28:43Z- Weighting particles with subcloud comparison 2024-11-06T03:28:43Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:43Z- meanExpectedMeasDiff: -1.690312 delta_rms_squared: 1.434723 alpha: 0.986252 2024-11-06T03:28:43Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:43Z- meanExpectedMeasDiff: 6.270553 delta_rms_squared: 1.324305 alpha: 0.985122 2024-11-06T03:28:43Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:43Z- meanExpectedMeasDiff: 3.562899 delta_rms_squared: 1.341630 alpha: 0.985312 2024-11-06T03:28:43Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:43Z- meanExpectedMeasDiff: -1.009891 delta_rms_squared: 2.125422 alpha: 0.990678 2024-11-06T03:28:43Z- Subcloud NIS: 0.475882 2024-11-06T03:28:43Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:28:43Z- TNavPF:: sumWeights = 0.002126 2024-11-06T03:28:43Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:28:43Z- TNavPF::First Term of Map Covariance Matrix is 0.01 2024-11-06T03:28:43Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:28:43Z- TerrainNav::Resampling particle filter... 2024-11-06T03:28:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863722.32 sec. 2024-11-06T03:28:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:28:43Z- variances([0], [2], and psi_berg) = 0.551909 0.267098 0.000000 2024-11-06T03:28:43Z- publishEstimatesDecoder() - at 1730863723.53 sec 2024-11-06T03:28:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:28:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061940.48 594613.64 53.56 2024-11-06T03:28:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061939.31 594614.50 54.10 lat/lon: 36.70 -121.94 2024-11-06T03:28:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.55 0.27 0.67 0.00 2024-11-06T03:28:43Z- AHRS_M2 msg: 1730863723.91 epoch sec; seqNo:5493 2024-11-06T03:28:43Z- AHRS_M2 msg: -0.00 phi; 0.06 theta; 3.73 psi 2024-11-06T03:28:43Z- handleDepth()- Depth_Keller msg: 1730863723.92 epoch sec; seqNo:5851; depth 49.46 2024-11-06T03:28:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863723.97 epoch sec; seqNo:5851; 4061910.34 north; 594630.74 east; 36.70 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T03:28:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863723.53 2024-11-06T03:28:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:44Z- waiting for 1730863727.53, time is 1730863723.97 2024-11-06T03:28:44Z- AHRS_M2 msg: 1730863724.31 epoch sec; seqNo:5494 2024-11-06T03:28:44Z- AHRS_M2 msg: 0.01 phi; 0.06 theta; 3.71 psi 2024-11-06T03:28:44Z- handleDepth()- Depth_Keller msg: 1730863724.32 epoch sec; seqNo:5852; depth 49.43 2024-11-06T03:28:44Z- handleDVL() - DVL msg epochMillisec: 1730863724.34 2024-11-06T03:28:44Z- handleDvl() - RDI_Pathfinder msg: 1730863724.34 sec; seqNo:2015; #beams:4 2024-11-06T03:28:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.32 , 39.44 , 40.37 , 37.20 2024-11-06T03:28:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.03 , -0.03 2024-11-06T03:28:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863724.38 epoch sec; seqNo:5852; 4061909.96 north; 594630.74 east; 36.70 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T03:28:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863723.53 2024-11-06T03:28:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:44Z- waiting for 1730863727.53, time is 1730863724.38 2024-11-06T03:28:44Z- handleDepth()- Depth_Keller msg: 1730863724.74 epoch sec; seqNo:5853; depth 49.41 2024-11-06T03:28:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863724.75 epoch sec; seqNo:5853; 4061909.96 north; 594630.74 east; 36.70 lat; -121.94 lon; 49.41 depth; utmZone: 10 2024-11-06T03:28:44Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:28:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863723.53 2024-11-06T03:28:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:44Z- waiting for 1730863727.53, time is 1730863724.75 2024-11-06T03:28:45Z- AHRS_M2 msg: 1730863725.12 epoch sec; seqNo:5495 2024-11-06T03:28:45Z- AHRS_M2 msg: 0.01 phi; 0.05 theta; 3.67 psi 2024-11-06T03:28:45Z- handleDepth()- Depth_Keller msg: 1730863725.13 epoch sec; seqNo:5854; depth 49.40 2024-11-06T03:28:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863725.14 epoch sec; seqNo:5854; 4061909.57 north; 594629.53 east; 36.70 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T03:28:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863723.53 2024-11-06T03:28:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:45Z- waiting for 1730863727.53, time is 1730863725.14 2024-11-06T03:28:45Z- AHRS_M2 msg: 1730863725.52 epoch sec; seqNo:5496 2024-11-06T03:28:45Z- AHRS_M2 msg: 0.01 phi; 0.05 theta; 3.65 psi 2024-11-06T03:28:45Z- handleDepth()- Depth_Keller msg: 1730863725.53 epoch sec; seqNo:5855; depth 49.39 2024-11-06T03:28:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863725.53 epoch sec; seqNo:5855; 4061909.19 north; 594629.53 east; 36.70 lat; -121.94 lon; 49.39 depth; utmZone: 10 2024-11-06T03:28:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863723.53 2024-11-06T03:28:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:45Z- waiting for 1730863727.53, time is 1730863725.53 2024-11-06T03:28:45Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:28:45Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:28:45Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:28:45Z- AHRS_M2 msg: 1730863725.92 epoch sec; seqNo:5497 2024-11-06T03:28:45Z- AHRS_M2 msg: 0.01 phi; 0.05 theta; 3.65 psi 2024-11-06T03:28:45Z- handleDepth()- Depth_Keller msg: 1730863725.96 epoch sec; seqNo:5856; depth 49.36 2024-11-06T03:28:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863725.97 epoch sec; seqNo:5856; 4061908.81 north; 594629.53 east; 36.70 lat; -121.94 lon; 49.36 depth; utmZone: 10 2024-11-06T03:28:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863723.53 2024-11-06T03:28:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:46Z- waiting for 1730863727.53, time is 1730863725.97 2024-11-06T03:28:46Z- AHRS_M2 msg: 1730863726.33 epoch sec; seqNo:5498 2024-11-06T03:28:46Z- AHRS_M2 msg: 0.01 phi; 0.04 theta; 3.62 psi 2024-11-06T03:28:46Z- handleDepth()- Depth_Keller msg: 1730863726.34 epoch sec; seqNo:5857; depth 49.34 2024-11-06T03:28:46Z- handleDVL() - DVL msg epochMillisec: 1730863726.36 2024-11-06T03:28:46Z- handleDvl() - RDI_Pathfinder msg: 1730863726.36 sec; seqNo:2016; #beams:4 2024-11-06T03:28:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.88 , 38.13 , 40.19 , 37.38 2024-11-06T03:28:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.06 , -0.04 2024-11-06T03:28:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863726.38 epoch sec; seqNo:5857; 4061908.81 north; 594629.53 east; 36.70 lat; -121.94 lon; 49.34 depth; utmZone: 10 2024-11-06T03:28:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863723.53 2024-11-06T03:28:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:46Z- waiting for 1730863727.53, time is 1730863726.38 2024-11-06T03:28:46Z- AHRS_M2 msg: 1730863726.73 epoch sec; seqNo:5499 2024-11-06T03:28:46Z- AHRS_M2 msg: 0.01 phi; 0.04 theta; 3.61 psi 2024-11-06T03:28:46Z- handleDepth()- Depth_Keller msg: 1730863726.74 epoch sec; seqNo:5858; depth 49.33 2024-11-06T03:28:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863726.76 epoch sec; seqNo:5858; 4061908.43 north; 594629.54 east; 36.70 lat; -121.94 lon; 49.33 depth; utmZone: 10 2024-11-06T03:28:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863723.53 2024-11-06T03:28:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:46Z- waiting for 1730863727.53, time is 1730863726.76 2024-11-06T03:28:47Z- AHRS_M2 msg: 1730863727.14 epoch sec; seqNo:5500 2024-11-06T03:28:47Z- AHRS_M2 msg: 0.01 phi; 0.03 theta; 3.60 psi 2024-11-06T03:28:47Z- handleDepth()- Depth_Keller msg: 1730863727.14 epoch sec; seqNo:5859; depth 49.31 2024-11-06T03:28:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863727.15 epoch sec; seqNo:5859; 4061908.05 north; 594629.54 east; 36.70 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T03:28:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863723.53 2024-11-06T03:28:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:47Z- waiting for 1730863727.53, time is 1730863727.15 2024-11-06T03:28:47Z- AHRS_M2 msg: 1730863727.55 epoch sec; seqNo:5501 2024-11-06T03:28:47Z- AHRS_M2 msg: 0.02 phi; 0.02 theta; 3.59 psi 2024-11-06T03:28:47Z- handleDepth()- Depth_Keller msg: 1730863727.55 epoch sec; seqNo:5860; depth 49.31 2024-11-06T03:28:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863727.56 epoch sec; seqNo:5860; 4061907.68 north; 594629.55 east; 36.70 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T03:28:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863723.53 2024-11-06T03:28:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:47Z- waiting for 1730863727.53, time is 1730863727.56 2024-11-06T03:28:47Z- LcmTrn::updateTrn() >>>> pose time 1730863727.56, meas time 1730863726.36 <<<< 2024-11-06T03:28:47Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863727.56 2024-11-06T03:28:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863726.36 sec, ping # 0; waiting for INS data more recent than 1730863723.53... 2024-11-06T03:28:47Z- TerrainNav::motionUpdate() - currEstimate = (4061908, 594629.55, 49.31) 2024-11-06T03:28:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:28:47Z- TerrainNav:: SubcloudNIS is 0.48, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:28:47Z- TNavPF::Projecting Measurements 2024-11-06T03:28:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:28:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:28:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:28:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:28:47Z- TNavFilter::4 good beams. 2024-11-06T03:28:47Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:28:47Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:28:47Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:28:47Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:28:47Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:28:47Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:28:47Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:28:47Z- TerrainMapOctree: (vehN, vehE ) = (4061908.58, 594629.90). 2024-11-06T03:28:47Z- TerrainMapOctree: Min Distance = 198.93. 2024-11-06T03:28:47Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:28:47Z- Forcing Subcloud Comparison 2024-11-06T03:28:47Z- Weighting particles with subcloud comparison 2024-11-06T03:28:47Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:47Z- meanExpectedMeasDiff: -2.544508 delta_rms_squared: 1.229626 alpha: 0.983995 2024-11-06T03:28:47Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:47Z- meanExpectedMeasDiff: 3.095288 delta_rms_squared: 0.916725 alpha: 0.978649 2024-11-06T03:28:47Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:47Z- meanExpectedMeasDiff: 1.953217 delta_rms_squared: 1.007437 alpha: 0.980534 2024-11-06T03:28:47Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:47Z- meanExpectedMeasDiff: -1.998135 delta_rms_squared: 1.336470 alpha: 0.985256 2024-11-06T03:28:47Z- Subcloud NIS: 0.491925 2024-11-06T03:28:47Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:28:47Z- TNavPF:: sumWeights = 0.044687 2024-11-06T03:28:47Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:28:47Z- TNavPF::First Term of Map Covariance Matrix is 0.27 2024-11-06T03:28:47Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:28:47Z- TerrainNav::Resampling particle filter... 2024-11-06T03:28:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863726.36 sec. 2024-11-06T03:28:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:28:47Z- variances([0], [2], and psi_berg) = 0.395747 0.206762 0.000000 2024-11-06T03:28:47Z- publishEstimatesDecoder() - at 1730863727.56 sec 2024-11-06T03:28:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:28:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061937.32 594612.56 52.91 2024-11-06T03:28:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061936.23 594613.46 54.04 lat/lon: 36.70 -121.94 2024-11-06T03:28:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.40 0.21 0.69 0.00 2024-11-06T03:28:47Z- AHRS_M2 msg: 1730863727.95 epoch sec; seqNo:5502 2024-11-06T03:28:47Z- AHRS_M2 msg: 0.03 phi; 0.01 theta; 3.58 psi 2024-11-06T03:28:47Z- handleDepth()- Depth_Keller msg: 1730863727.96 epoch sec; seqNo:5861; depth 49.28 2024-11-06T03:28:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863727.97 epoch sec; seqNo:5861; 4061907.66 north; 594628.33 east; 36.70 lat; -121.94 lon; 49.28 depth; utmZone: 10 2024-11-06T03:28:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863727.56 2024-11-06T03:28:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:48Z- waiting for 1730863731.56, time is 1730863727.97 2024-11-06T03:28:48Z- AHRS_M2 msg: 1730863728.35 epoch sec; seqNo:5503 2024-11-06T03:28:48Z- AHRS_M2 msg: 0.04 phi; 0.00 theta; 3.58 psi 2024-11-06T03:28:48Z- handleDepth()- Depth_Keller msg: 1730863728.36 epoch sec; seqNo:5862; depth 49.26 2024-11-06T03:28:48Z- handleDVL() - DVL msg epochMillisec: 1730863728.40 2024-11-06T03:28:48Z- handleDvl() - RDI_Pathfinder msg: 1730863728.40 sec; seqNo:2017; #beams:4 2024-11-06T03:28:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.06 , 38.13 , 39.63 , 37.94 2024-11-06T03:28:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , 0.09 , -0.03 2024-11-06T03:28:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863728.41 epoch sec; seqNo:5862; 4061907.28 north; 594628.33 east; 36.70 lat; -121.94 lon; 49.26 depth; utmZone: 10 2024-11-06T03:28:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863727.56 2024-11-06T03:28:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:48Z- waiting for 1730863731.56, time is 1730863728.41 2024-11-06T03:28:48Z- AHRS_M2 msg: 1730863728.78 epoch sec; seqNo:5504 2024-11-06T03:28:48Z- AHRS_M2 msg: 0.05 phi; -0.01 theta; 3.57 psi 2024-11-06T03:28:48Z- handleDepth()- Depth_Keller msg: 1730863728.79 epoch sec; seqNo:5863; depth 49.26 2024-11-06T03:28:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863728.81 epoch sec; seqNo:5863; 4061906.90 north; 594628.33 east; 36.70 lat; -121.94 lon; 49.26 depth; utmZone: 10 2024-11-06T03:28:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863727.56 2024-11-06T03:28:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:48Z- waiting for 1730863731.56, time is 1730863728.81 2024-11-06T03:28:49Z- AHRS_M2 msg: 1730863729.16 epoch sec; seqNo:5505 2024-11-06T03:28:49Z- AHRS_M2 msg: 0.06 phi; -0.02 theta; 3.57 psi 2024-11-06T03:28:49Z- handleDepth()- Depth_Keller msg: 1730863729.17 epoch sec; seqNo:5864; depth 49.25 2024-11-06T03:28:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863729.18 epoch sec; seqNo:5864; 4061906.53 north; 594628.34 east; 36.70 lat; -121.94 lon; 49.25 depth; utmZone: 10 2024-11-06T03:28:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863727.56 2024-11-06T03:28:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:49Z- waiting for 1730863731.56, time is 1730863729.18 2024-11-06T03:28:49Z- AHRS_M2 msg: 1730863729.56 epoch sec; seqNo:5506 2024-11-06T03:28:49Z- AHRS_M2 msg: 0.07 phi; -0.03 theta; 3.56 psi 2024-11-06T03:28:49Z- handleDepth()- Depth_Keller msg: 1730863729.57 epoch sec; seqNo:5865; depth 49.24 2024-11-06T03:28:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863729.60 epoch sec; seqNo:5865; 4061906.53 north; 594628.34 east; 36.70 lat; -121.94 lon; 49.24 depth; utmZone: 10 2024-11-06T03:28:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863727.56 2024-11-06T03:28:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:49Z- waiting for 1730863731.56, time is 1730863729.60 2024-11-06T03:28:49Z- AHRS_M2 msg: 1730863729.98 epoch sec; seqNo:5507 2024-11-06T03:28:49Z- AHRS_M2 msg: 0.08 phi; -0.04 theta; 3.56 psi 2024-11-06T03:28:49Z- handleDepth()- Depth_Keller msg: 1730863729.98 epoch sec; seqNo:5866; depth 49.25 2024-11-06T03:28:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863730.00 epoch sec; seqNo:5866; 4061906.15 north; 594628.34 east; 36.70 lat; -121.94 lon; 49.25 depth; utmZone: 10 2024-11-06T03:28:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863727.56 2024-11-06T03:28:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:50Z- waiting for 1730863731.56, time is 1730863730.00 2024-11-06T03:28:50Z- AHRS_M2 msg: 1730863730.39 epoch sec; seqNo:5508 2024-11-06T03:28:50Z- AHRS_M2 msg: 0.09 phi; -0.05 theta; 3.55 psi 2024-11-06T03:28:50Z- handleDepth()- Depth_Keller msg: 1730863730.39 epoch sec; seqNo:5867; depth 49.24 2024-11-06T03:28:50Z- handleDVL() - DVL msg epochMillisec: 1730863730.41 2024-11-06T03:28:50Z- handleDvl() - RDI_Pathfinder msg: 1730863730.41 sec; seqNo:2018; #beams:4 2024-11-06T03:28:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.19 , 37.01 , 38.50 , 38.88 2024-11-06T03:28:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.08 , 0.02 2024-11-06T03:28:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863730.42 epoch sec; seqNo:5867; 4061905.77 north; 594628.35 east; 36.70 lat; -121.94 lon; 49.24 depth; utmZone: 10 2024-11-06T03:28:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863727.56 2024-11-06T03:28:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:50Z- waiting for 1730863731.56, time is 1730863730.42 2024-11-06T03:28:50Z- AHRS_M2 msg: 1730863730.78 epoch sec; seqNo:5509 2024-11-06T03:28:50Z- AHRS_M2 msg: 0.10 phi; -0.06 theta; 3.55 psi 2024-11-06T03:28:50Z- handleDepth()- Depth_Keller msg: 1730863730.79 epoch sec; seqNo:5868; depth 49.25 2024-11-06T03:28:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863730.81 epoch sec; seqNo:5868; 4061905.38 north; 594627.13 east; 36.70 lat; -121.94 lon; 49.25 depth; utmZone: 10 2024-11-06T03:28:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863727.56 2024-11-06T03:28:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:50Z- waiting for 1730863731.56, time is 1730863730.81 2024-11-06T03:28:51Z- AHRS_M2 msg: 1730863731.18 epoch sec; seqNo:5510 2024-11-06T03:28:51Z- AHRS_M2 msg: 0.10 phi; -0.07 theta; 3.55 psi 2024-11-06T03:28:51Z- handleDepth()- Depth_Keller msg: 1730863731.19 epoch sec; seqNo:5869; depth 49.25 2024-11-06T03:28:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863731.20 epoch sec; seqNo:5869; 4061905.38 north; 594627.13 east; 36.70 lat; -121.94 lon; 49.25 depth; utmZone: 10 2024-11-06T03:28:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863727.56 2024-11-06T03:28:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:51Z- waiting for 1730863731.56, time is 1730863731.20 2024-11-06T03:28:51Z- AHRS_M2 msg: 1730863731.61 epoch sec; seqNo:5511 2024-11-06T03:28:51Z- AHRS_M2 msg: 0.11 phi; -0.08 theta; 3.54 psi 2024-11-06T03:28:51Z- handleDepth()- Depth_Keller msg: 1730863731.62 epoch sec; seqNo:5870; depth 49.27 2024-11-06T03:28:51Z- handleDVL() - DVL msg epochMillisec: 1730863731.62 2024-11-06T03:28:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:28:51Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:28:51Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:28:51Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:28:51Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:28:51Z- handleDvl() - RDI_Pathfinder msg: 1730863731.62 sec; seqNo:2019; #beams:0 2024-11-06T03:28:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:28:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.08 , 0.02 2024-11-06T03:28:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863731.65 epoch sec; seqNo:5870; 4061905.00 north; 594627.13 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T03:28:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863727.56 2024-11-06T03:28:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:51Z- waiting for 1730863731.56, time is 1730863731.65 2024-11-06T03:28:51Z- LcmTrn::updateTrn() >>>> pose time 1730863731.65, meas time 1730863731.62 <<<< 2024-11-06T03:28:51Z- LcmTrn::updateTrn() equating pose 1730863731.62 and meas times 1730863731.62 2024-11-06T03:28:51Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863731.62 2024-11-06T03:28:51Z- TerrainNav::motionUpdate() - currEstimate = (4061905, 594627.13, 49.27) 2024-11-06T03:28:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:28:51Z- TerrainNav:: SubcloudNIS is 0.49, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:28:51Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:28:51Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:28:51Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:28:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:28:51Z- variances([0], [2], and psi_berg) = 0.398281 0.209889 0.000000 2024-11-06T03:28:51Z- publishEstimatesDecoder() - at 1730863731.62 sec 2024-11-06T03:28:51Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:28:51Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061934.65 594610.12 53.66 2024-11-06T03:28:51Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061933.55 594611.04 54.01 lat/lon: 36.70 -121.94 2024-11-06T03:28:51Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.40 0.21 0.94 0.00 2024-11-06T03:28:52Z- AHRS_M2 msg: 1730863732.01 epoch sec; seqNo:5512 2024-11-06T03:28:52Z- AHRS_M2 msg: 0.11 phi; -0.09 theta; 3.54 psi 2024-11-06T03:28:52Z- handleDepth()- Depth_Keller msg: 1730863732.01 epoch sec; seqNo:5871; depth 49.29 2024-11-06T03:28:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863732.02 epoch sec; seqNo:5871; 4061904.62 north; 594627.14 east; 36.70 lat; -121.94 lon; 49.29 depth; utmZone: 10 2024-11-06T03:28:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863731.62 2024-11-06T03:28:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:52Z- waiting for 1730863735.62, time is 1730863732.02 2024-11-06T03:28:52Z- AHRS_M2 msg: 1730863732.41 epoch sec; seqNo:5513 2024-11-06T03:28:52Z- AHRS_M2 msg: 0.11 phi; -0.09 theta; 3.54 psi 2024-11-06T03:28:52Z- handleDepth()- Depth_Keller msg: 1730863732.43 epoch sec; seqNo:5872; depth 49.31 2024-11-06T03:28:52Z- handleDVL() - DVL msg epochMillisec: 1730863732.45 2024-11-06T03:28:52Z- handleDvl() - RDI_Pathfinder msg: 1730863732.45 sec; seqNo:2020; #beams:4 2024-11-06T03:28:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.31 , 36.63 , 37.57 , 40.37 2024-11-06T03:28:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , 0.05 2024-11-06T03:28:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863732.46 epoch sec; seqNo:5872; 4061904.24 north; 594627.14 east; 36.70 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T03:28:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863731.62 2024-11-06T03:28:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:52Z- waiting for 1730863735.62, time is 1730863732.46 2024-11-06T03:28:52Z- AHRS_M2 msg: 1730863732.82 epoch sec; seqNo:5514 2024-11-06T03:28:52Z- AHRS_M2 msg: 0.11 phi; -0.10 theta; 3.53 psi 2024-11-06T03:28:52Z- handleDepth()- Depth_Keller msg: 1730863732.83 epoch sec; seqNo:5873; depth 49.31 2024-11-06T03:28:52Z- handleDVL() - DVL msg epochMillisec: 1730863732.84 2024-11-06T03:28:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:28:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:28:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:28:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:28:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:28:52Z- handleDvl() - RDI_Pathfinder msg: 1730863732.84 sec; seqNo:2021; #beams:0 2024-11-06T03:28:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:28:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , 0.05 2024-11-06T03:28:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863732.85 epoch sec; seqNo:5873; 4061903.86 north; 594627.15 east; 36.70 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T03:28:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863731.62 2024-11-06T03:28:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:52Z- waiting for 1730863735.62, time is 1730863732.85 2024-11-06T03:28:53Z- AHRS_M2 msg: 1730863733.22 epoch sec; seqNo:5515 2024-11-06T03:28:53Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 3.53 psi 2024-11-06T03:28:53Z- handleDepth()- Depth_Keller msg: 1730863733.22 epoch sec; seqNo:5874; depth 49.33 2024-11-06T03:28:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863733.23 epoch sec; seqNo:5874; 4061903.86 north; 594627.15 east; 36.70 lat; -121.94 lon; 49.33 depth; utmZone: 10 2024-11-06T03:28:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863731.62 2024-11-06T03:28:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:53Z- waiting for 1730863735.62, time is 1730863733.23 2024-11-06T03:28:53Z- AHRS_M2 msg: 1730863733.64 epoch sec; seqNo:5516 2024-11-06T03:28:53Z- AHRS_M2 msg: 0.13 phi; -0.12 theta; 3.53 psi 2024-11-06T03:28:53Z- handleDepth()- Depth_Keller msg: 1730863733.64 epoch sec; seqNo:5875; depth 49.36 2024-11-06T03:28:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863733.65 epoch sec; seqNo:5875; 4061903.48 north; 594627.15 east; 36.70 lat; -121.94 lon; 49.36 depth; utmZone: 10 2024-11-06T03:28:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863731.62 2024-11-06T03:28:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:53Z- waiting for 1730863735.62, time is 1730863733.65 2024-11-06T03:28:54Z- AHRS_M2 msg: 1730863734.03 epoch sec; seqNo:5517 2024-11-06T03:28:54Z- AHRS_M2 msg: 0.14 phi; -0.12 theta; 3.54 psi 2024-11-06T03:28:54Z- handleDepth()- Depth_Keller msg: 1730863734.04 epoch sec; seqNo:5876; depth 49.38 2024-11-06T03:28:54Z- handleDVL() - DVL msg epochMillisec: 1730863734.04 2024-11-06T03:28:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:28:54Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:28:54Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:28:54Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:28:54Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:28:54Z- handleDvl() - RDI_Pathfinder msg: 1730863734.04 sec; seqNo:2022; #beams:0 2024-11-06T03:28:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:28:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.04 , 0.05 2024-11-06T03:28:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863734.05 epoch sec; seqNo:5876; 4061903.10 north; 594627.16 east; 36.70 lat; -121.94 lon; 49.38 depth; utmZone: 10 2024-11-06T03:28:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863731.62 2024-11-06T03:28:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:54Z- waiting for 1730863735.62, time is 1730863734.05 2024-11-06T03:28:54Z- AHRS_M2 msg: 1730863734.43 epoch sec; seqNo:5518 2024-11-06T03:28:54Z- AHRS_M2 msg: 0.15 phi; -0.13 theta; 3.54 psi 2024-11-06T03:28:54Z- handleDepth()- Depth_Keller msg: 1730863734.43 epoch sec; seqNo:5877; depth 49.40 2024-11-06T03:28:54Z- handleDVL() - DVL msg epochMillisec: 1730863734.46 2024-11-06T03:28:54Z- handleDvl() - RDI_Pathfinder msg: 1730863734.46 sec; seqNo:2023; #beams:4 2024-11-06T03:28:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.06 , 36.45 , 37.20 , 41.49 2024-11-06T03:28:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , 0.05 2024-11-06T03:28:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863734.48 epoch sec; seqNo:5877; 4061902.71 north; 594625.94 east; 36.70 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T03:28:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863731.62 2024-11-06T03:28:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:54Z- waiting for 1730863735.62, time is 1730863734.48 2024-11-06T03:28:54Z- AHRS_M2 msg: 1730863734.84 epoch sec; seqNo:5519 2024-11-06T03:28:54Z- AHRS_M2 msg: 0.16 phi; -0.14 theta; 3.54 psi 2024-11-06T03:28:54Z- handleDepth()- Depth_Keller msg: 1730863734.85 epoch sec; seqNo:5878; depth 49.45 2024-11-06T03:28:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863734.86 epoch sec; seqNo:5878; 4061902.33 north; 594625.94 east; 36.70 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:28:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863731.62 2024-11-06T03:28:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:55Z- waiting for 1730863735.62, time is 1730863734.86 2024-11-06T03:28:55Z- AHRS_M2 msg: 1730863735.26 epoch sec; seqNo:5520 2024-11-06T03:28:55Z- AHRS_M2 msg: 0.17 phi; -0.14 theta; 3.55 psi 2024-11-06T03:28:55Z- handleDepth()- Depth_Keller msg: 1730863735.26 epoch sec; seqNo:5879; depth 49.47 2024-11-06T03:28:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863735.27 epoch sec; seqNo:5879; 4061901.95 north; 594625.95 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:28:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863731.62 2024-11-06T03:28:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:55Z- waiting for 1730863735.62, time is 1730863735.27 2024-11-06T03:28:55Z- AHRS_M2 msg: 1730863735.65 epoch sec; seqNo:5521 2024-11-06T03:28:55Z- AHRS_M2 msg: 0.17 phi; -0.15 theta; 3.55 psi 2024-11-06T03:28:55Z- handleDepth()- Depth_Keller msg: 1730863735.65 epoch sec; seqNo:5880; depth 49.52 2024-11-06T03:28:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863735.68 epoch sec; seqNo:5880; 4061901.95 north; 594625.95 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T03:28:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863731.62 2024-11-06T03:28:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:55Z- waiting for 1730863735.62, time is 1730863735.68 2024-11-06T03:28:55Z- LcmTrn::updateTrn() >>>> pose time 1730863735.68, meas time 1730863734.46 <<<< 2024-11-06T03:28:55Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863735.68 2024-11-06T03:28:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863734.46 sec, ping # 0; waiting for INS data more recent than 1730863731.62... 2024-11-06T03:28:55Z- TerrainNav::motionUpdate() - currEstimate = (4061902, 594625.95, 49.52) 2024-11-06T03:28:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:28:55Z- TerrainNav:: SubcloudNIS is 0.49, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:28:55Z- TNavPF::Projecting Measurements 2024-11-06T03:28:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:28:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:28:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:28:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:28:55Z- TNavFilter::4 good beams. 2024-11-06T03:28:55Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:28:55Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:28:55Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:28:55Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:28:55Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:28:55Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:28:55Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:28:55Z- TerrainMapOctree: (vehN, vehE ) = (4061902.87, 594626.30). 2024-11-06T03:28:55Z- TerrainMapOctree: Min Distance = 196.64. 2024-11-06T03:28:55Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:28:55Z- Forcing Subcloud Comparison 2024-11-06T03:28:55Z- Weighting particles with subcloud comparison 2024-11-06T03:28:55Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:55Z- meanExpectedMeasDiff: -5.167187 delta_rms_squared: 1.963423 alpha: 0.989916 2024-11-06T03:28:55Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:55Z- meanExpectedMeasDiff: 2.363633 delta_rms_squared: 1.250287 alpha: 0.984256 2024-11-06T03:28:55Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:55Z- meanExpectedMeasDiff: 0.504127 delta_rms_squared: 1.404823 alpha: 0.985963 2024-11-06T03:28:55Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:28:55Z- meanExpectedMeasDiff: -3.889358 delta_rms_squared: 5.001513 alpha: 0.996017 2024-11-06T03:28:55Z- Subcloud NIS: 0.526491 2024-11-06T03:28:55Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:28:55Z- TNavPF:: sumWeights = 0.008846 2024-11-06T03:28:55Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:28:55Z- TNavPF::First Term of Map Covariance Matrix is 0.14 2024-11-06T03:28:55Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:28:55Z- TerrainNav::Resampling particle filter... 2024-11-06T03:28:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863734.46 sec. 2024-11-06T03:28:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:28:56Z- variances([0], [2], and psi_berg) = 0.260423 0.191662 0.000000 2024-11-06T03:28:56Z- publishEstimatesDecoder() - at 1730863735.68 sec 2024-11-06T03:28:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:28:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061931.53 594608.95 53.97 2024-11-06T03:28:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061930.35 594609.83 55.18 lat/lon: 36.70 -121.94 2024-11-06T03:28:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.19 0.66 0.00 2024-11-06T03:28:56Z- AHRS_M2 msg: 1730863736.07 epoch sec; seqNo:5522 2024-11-06T03:28:56Z- AHRS_M2 msg: 0.18 phi; -0.15 theta; 3.56 psi 2024-11-06T03:28:56Z- handleDepth()- Depth_Keller msg: 1730863736.08 epoch sec; seqNo:5881; depth 49.56 2024-11-06T03:28:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863736.11 epoch sec; seqNo:5881; 4061901.57 north; 594625.95 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:28:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863735.68 2024-11-06T03:28:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:56Z- waiting for 1730863739.68, time is 1730863736.11 2024-11-06T03:28:56Z- AHRS_M2 msg: 1730863736.46 epoch sec; seqNo:5523 2024-11-06T03:28:56Z- AHRS_M2 msg: 0.18 phi; -0.16 theta; 3.56 psi 2024-11-06T03:28:56Z- handleDepth()- Depth_Keller msg: 1730863736.46 epoch sec; seqNo:5882; depth 49.60 2024-11-06T03:28:56Z- handleDVL() - DVL msg epochMillisec: 1730863736.48 2024-11-06T03:28:56Z- handleDvl() - RDI_Pathfinder msg: 1730863736.48 sec; seqNo:2024; #beams:4 2024-11-06T03:28:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.74 , 35.70 , 36.63 , 42.99 2024-11-06T03:28:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.02 2024-11-06T03:28:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863736.50 epoch sec; seqNo:5882; 4061901.19 north; 594625.96 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:28:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863735.68 2024-11-06T03:28:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:56Z- waiting for 1730863739.68, time is 1730863736.50 2024-11-06T03:28:56Z- handleDepth()- Depth_Keller msg: 1730863736.89 epoch sec; seqNo:5883; depth 49.63 2024-11-06T03:28:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863736.90 epoch sec; seqNo:5883; 4061900.82 north; 594625.96 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:28:57Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:28:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863735.68 2024-11-06T03:28:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:57Z- waiting for 1730863739.68, time is 1730863736.90 2024-11-06T03:28:57Z- AHRS_M2 msg: 1730863737.28 epoch sec; seqNo:5524 2024-11-06T03:28:57Z- AHRS_M2 msg: 0.18 phi; -0.17 theta; 3.57 psi 2024-11-06T03:28:57Z- handleDepth()- Depth_Keller msg: 1730863737.28 epoch sec; seqNo:5884; depth 49.67 2024-11-06T03:28:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863737.31 epoch sec; seqNo:5884; 4061900.44 north; 594625.96 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:28:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863735.68 2024-11-06T03:28:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:57Z- waiting for 1730863739.68, time is 1730863737.31 2024-11-06T03:28:57Z- AHRS_M2 msg: 1730863737.68 epoch sec; seqNo:5525 2024-11-06T03:28:57Z- AHRS_M2 msg: 0.19 phi; -0.17 theta; 3.58 psi 2024-11-06T03:28:57Z- handleDepth()- Depth_Keller msg: 1730863737.70 epoch sec; seqNo:5885; depth 49.72 2024-11-06T03:28:57Z- handleDVL() - DVL msg epochMillisec: 1730863737.70 2024-11-06T03:28:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:28:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:28:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:28:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:28:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:28:57Z- handleDvl() - RDI_Pathfinder msg: 1730863737.70 sec; seqNo:2025; #beams:0 2024-11-06T03:28:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:28:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.02 2024-11-06T03:28:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863737.72 epoch sec; seqNo:5885; 4061900.04 north; 594624.75 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:28:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863735.68 2024-11-06T03:28:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:57Z- waiting for 1730863739.68, time is 1730863737.72 2024-11-06T03:28:58Z- AHRS_M2 msg: 1730863738.09 epoch sec; seqNo:5526 2024-11-06T03:28:58Z- AHRS_M2 msg: 0.19 phi; -0.17 theta; 3.60 psi 2024-11-06T03:28:58Z- handleDepth()- Depth_Keller msg: 1730863738.09 epoch sec; seqNo:5886; depth 49.76 2024-11-06T03:28:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863738.11 epoch sec; seqNo:5886; 4061899.67 north; 594624.75 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:28:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863735.68 2024-11-06T03:28:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:58Z- waiting for 1730863739.68, time is 1730863738.11 2024-11-06T03:28:58Z- AHRS_M2 msg: 1730863738.49 epoch sec; seqNo:5527 2024-11-06T03:28:58Z- AHRS_M2 msg: 0.20 phi; -0.17 theta; 3.60 psi 2024-11-06T03:28:58Z- handleDepth()- Depth_Keller msg: 1730863738.50 epoch sec; seqNo:5887; depth 49.81 2024-11-06T03:28:58Z- handleDVL() - DVL msg epochMillisec: 1730863738.52 2024-11-06T03:28:58Z- handleDvl() - RDI_Pathfinder msg: 1730863738.52 sec; seqNo:2026; #beams:4 2024-11-06T03:28:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.30 , 35.33 , 36.26 , 43.18 2024-11-06T03:28:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , -0.01 2024-11-06T03:28:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863738.53 epoch sec; seqNo:5887; 4061899.29 north; 594624.76 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:28:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863735.68 2024-11-06T03:28:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:58Z- waiting for 1730863739.68, time is 1730863738.53 2024-11-06T03:28:58Z- AHRS_M2 msg: 1730863738.94 epoch sec; seqNo:5528 2024-11-06T03:28:58Z- AHRS_M2 msg: 0.21 phi; -0.17 theta; 3.61 psi 2024-11-06T03:28:58Z- handleDepth()- Depth_Keller msg: 1730863738.94 epoch sec; seqNo:5888; depth 49.86 2024-11-06T03:28:58Z- handleDVL() - DVL msg epochMillisec: 1730863738.94 2024-11-06T03:28:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:28:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:28:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:28:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:28:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:28:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:28:58Z- handleDvl() - RDI_Pathfinder msg: 1730863738.94 sec; seqNo:2027; #beams:0 2024-11-06T03:28:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:28:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , -0.01 2024-11-06T03:28:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863738.97 epoch sec; seqNo:5888; 4061898.91 north; 594624.76 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:28:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:28:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863735.68 2024-11-06T03:28:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:59Z- waiting for 1730863739.68, time is 1730863738.97 2024-11-06T03:28:59Z- AHRS_M2 msg: 1730863739.34 epoch sec; seqNo:5529 2024-11-06T03:28:59Z- AHRS_M2 msg: 0.21 phi; -0.17 theta; 3.62 psi 2024-11-06T03:28:59Z- handleDepth()- Depth_Keller msg: 1730863739.34 epoch sec; seqNo:5889; depth 49.91 2024-11-06T03:28:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863739.36 epoch sec; seqNo:5889; 4061898.91 north; 594624.76 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:28:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863735.68 2024-11-06T03:28:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:59Z- waiting for 1730863739.68, time is 1730863739.36 2024-11-06T03:28:59Z- AHRS_M2 msg: 1730863739.76 epoch sec; seqNo:5530 2024-11-06T03:28:59Z- AHRS_M2 msg: 0.21 phi; -0.17 theta; 3.62 psi 2024-11-06T03:28:59Z- handleDepth()- Depth_Keller msg: 1730863739.77 epoch sec; seqNo:5890; depth 49.96 2024-11-06T03:28:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863739.77 epoch sec; seqNo:5890; 4061898.53 north; 594624.77 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:28:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:28:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863735.68 2024-11-06T03:28:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:28:59Z- waiting for 1730863739.68, time is 1730863739.77 2024-11-06T03:28:59Z- LcmTrn::updateTrn() >>>> pose time 1730863739.77, meas time 1730863738.94 <<<< 2024-11-06T03:28:59Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863739.77 2024-11-06T03:28:59Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:28:59Z- TerrainNav::motionUpdate() - currEstimate = (4061899, 594624.77, 49.96) 2024-11-06T03:28:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:28:59Z- TerrainNav:: SubcloudNIS is 0.53, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:28:59Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:28:59Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:28:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:28:59Z- variances([0], [2], and psi_berg) = 0.262202 0.194040 0.000000 2024-11-06T03:28:59Z- publishEstimatesDecoder() - at 1730863739.77 sec 2024-11-06T03:28:59Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:28:59Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061928.09 594607.76 54.45 2024-11-06T03:28:59Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061926.93 594608.65 55.63 lat/lon: 36.70 -121.94 2024-11-06T03:28:59Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.19 0.89 0.00 2024-11-06T03:29:00Z- AHRS_M2 msg: 1730863740.16 epoch sec; seqNo:5531 2024-11-06T03:29:00Z- AHRS_M2 msg: 0.21 phi; -0.17 theta; 3.63 psi 2024-11-06T03:29:00Z- handleDepth()- Depth_Keller msg: 1730863740.18 epoch sec; seqNo:5891; depth 50.01 2024-11-06T03:29:00Z- handleDVL() - DVL msg epochMillisec: 1730863740.19 2024-11-06T03:29:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:00Z- handleDvl() - RDI_Pathfinder msg: 1730863740.19 sec; seqNo:2028; #beams:0 2024-11-06T03:29:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , -0.01 2024-11-06T03:29:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863740.24 epoch sec; seqNo:5891; 4061898.15 north; 594624.77 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:29:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863739.77 2024-11-06T03:29:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:00Z- waiting for 1730863743.77, time is 1730863740.24 2024-11-06T03:29:00Z- AHRS_M2 msg: 1730863740.55 epoch sec; seqNo:5532 2024-11-06T03:29:00Z- AHRS_M2 msg: 0.21 phi; -0.17 theta; 3.64 psi 2024-11-06T03:29:00Z- handleDepth()- Depth_Keller msg: 1730863740.56 epoch sec; seqNo:5892; depth 50.07 2024-11-06T03:29:00Z- handleDVL() - DVL msg epochMillisec: 1730863740.58 2024-11-06T03:29:00Z- handleDvl() - RDI_Pathfinder msg: 1730863740.58 sec; seqNo:2029; #beams:4 2024-11-06T03:29:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.67 , 34.77 , 36.08 , 43.55 2024-11-06T03:29:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.07 , 0.01 2024-11-06T03:29:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863740.60 epoch sec; seqNo:5892; 4061897.76 north; 594623.55 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:29:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863739.77 2024-11-06T03:29:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:00Z- waiting for 1730863743.77, time is 1730863740.60 2024-11-06T03:29:00Z- AHRS_M2 msg: 1730863740.96 epoch sec; seqNo:5533 2024-11-06T03:29:00Z- AHRS_M2 msg: 0.21 phi; -0.16 theta; 3.64 psi 2024-11-06T03:29:00Z- handleDepth()- Depth_Keller msg: 1730863740.96 epoch sec; seqNo:5893; depth 50.13 2024-11-06T03:29:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863740.98 epoch sec; seqNo:5893; 4061897.38 north; 594623.56 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:29:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863739.77 2024-11-06T03:29:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:01Z- waiting for 1730863743.77, time is 1730863740.98 2024-11-06T03:29:01Z- AHRS_M2 msg: 1730863741.36 epoch sec; seqNo:5534 2024-11-06T03:29:01Z- AHRS_M2 msg: 0.21 phi; -0.16 theta; 3.65 psi 2024-11-06T03:29:01Z- handleDepth()- Depth_Keller msg: 1730863741.36 epoch sec; seqNo:5894; depth 50.18 2024-11-06T03:29:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863741.38 epoch sec; seqNo:5894; 4061897.00 north; 594623.56 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:29:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863739.77 2024-11-06T03:29:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:01Z- waiting for 1730863743.77, time is 1730863741.38 2024-11-06T03:29:01Z- AHRS_M2 msg: 1730863741.76 epoch sec; seqNo:5535 2024-11-06T03:29:01Z- AHRS_M2 msg: 0.21 phi; -0.16 theta; 3.66 psi 2024-11-06T03:29:01Z- handleDepth()- Depth_Keller msg: 1730863741.77 epoch sec; seqNo:5895; depth 50.23 2024-11-06T03:29:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863741.78 epoch sec; seqNo:5895; 4061897.00 north; 594623.56 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:29:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863739.77 2024-11-06T03:29:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:01Z- waiting for 1730863743.77, time is 1730863741.78 2024-11-06T03:29:02Z- AHRS_M2 msg: 1730863742.21 epoch sec; seqNo:5536 2024-11-06T03:29:02Z- AHRS_M2 msg: 0.21 phi; -0.16 theta; 3.67 psi 2024-11-06T03:29:02Z- handleDepth()- Depth_Keller msg: 1730863742.23 epoch sec; seqNo:5896; depth 50.29 2024-11-06T03:29:02Z- handleDVL() - DVL msg epochMillisec: 1730863742.26 2024-11-06T03:29:02Z- handleDvl() - RDI_Pathfinder msg: 1730863742.26 sec; seqNo:2030; #beams:4 2024-11-06T03:29:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.67 , 34.58 , 35.89 , 42.62 2024-11-06T03:29:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.07 , 0.03 2024-11-06T03:29:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863742.27 epoch sec; seqNo:5896; 4061896.62 north; 594623.57 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:29:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863739.77 2024-11-06T03:29:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:02Z- waiting for 1730863743.77, time is 1730863742.27 2024-11-06T03:29:02Z- AHRS_M2 msg: 1730863742.57 epoch sec; seqNo:5537 2024-11-06T03:29:02Z- AHRS_M2 msg: 0.21 phi; -0.16 theta; 3.68 psi 2024-11-06T03:29:02Z- handleDepth()- Depth_Keller msg: 1730863742.58 epoch sec; seqNo:5897; depth 50.37 2024-11-06T03:29:02Z- handleDVL() - DVL msg epochMillisec: 1730863742.58 2024-11-06T03:29:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:02Z- handleDvl() - RDI_Pathfinder msg: 1730863742.58 sec; seqNo:2031; #beams:0 2024-11-06T03:29:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.07 , 0.03 2024-11-06T03:29:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863742.61 epoch sec; seqNo:5897; 4061896.24 north; 594623.57 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:29:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863739.77 2024-11-06T03:29:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:02Z- waiting for 1730863743.77, time is 1730863742.61 2024-11-06T03:29:03Z- AHRS_M2 msg: 1730863743.00 epoch sec; seqNo:5538 2024-11-06T03:29:03Z- AHRS_M2 msg: 0.22 phi; -0.15 theta; 3.69 psi 2024-11-06T03:29:03Z- handleDepth()- Depth_Keller msg: 1730863743.01 epoch sec; seqNo:5898; depth 50.41 2024-11-06T03:29:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863743.02 epoch sec; seqNo:5898; 4061895.85 north; 594622.35 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:29:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863739.77 2024-11-06T03:29:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:03Z- waiting for 1730863743.77, time is 1730863743.02 2024-11-06T03:29:03Z- AHRS_M2 msg: 1730863743.41 epoch sec; seqNo:5539 2024-11-06T03:29:03Z- AHRS_M2 msg: 0.22 phi; -0.15 theta; 3.69 psi 2024-11-06T03:29:03Z- handleDepth()- Depth_Keller msg: 1730863743.41 epoch sec; seqNo:5899; depth 50.47 2024-11-06T03:29:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863743.42 epoch sec; seqNo:5899; 4061895.47 north; 594622.36 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:29:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863739.77 2024-11-06T03:29:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:03Z- waiting for 1730863743.77, time is 1730863743.42 2024-11-06T03:29:03Z- AHRS_M2 msg: 1730863743.83 epoch sec; seqNo:5540 2024-11-06T03:29:03Z- AHRS_M2 msg: 0.22 phi; -0.14 theta; 3.70 psi 2024-11-06T03:29:03Z- handleDepth()- Depth_Keller msg: 1730863743.84 epoch sec; seqNo:5900; depth 50.52 2024-11-06T03:29:03Z- handleDVL() - DVL msg epochMillisec: 1730863743.84 2024-11-06T03:29:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:03Z- handleDvl() - RDI_Pathfinder msg: 1730863743.84 sec; seqNo:2032; #beams:0 2024-11-06T03:29:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.07 , 0.03 2024-11-06T03:29:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863743.85 epoch sec; seqNo:5900; 4061895.47 north; 594622.36 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:29:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863739.77 2024-11-06T03:29:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:03Z- waiting for 1730863743.77, time is 1730863743.85 2024-11-06T03:29:03Z- LcmTrn::updateTrn() >>>> pose time 1730863743.85, meas time 1730863743.84 <<<< 2024-11-06T03:29:03Z- LcmTrn::updateTrn() equating pose 1730863743.84 and meas times 1730863743.84 2024-11-06T03:29:03Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863743.84 2024-11-06T03:29:03Z- TerrainNav::motionUpdate() - currEstimate = (4061895, 594622.36, 50.52) 2024-11-06T03:29:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:29:03Z- TerrainNav:: SubcloudNIS is 0.53, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:29:03Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:29:03Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:29:04Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:29:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:29:04Z- variances([0], [2], and psi_berg) = 0.264712 0.196150 0.000000 2024-11-06T03:29:04Z- publishEstimatesDecoder() - at 1730863743.84 sec 2024-11-06T03:29:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:29:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061925.07 594605.36 54.45 2024-11-06T03:29:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061923.87 594606.24 56.18 lat/lon: 36.70 -121.94 2024-11-06T03:29:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.20 1.14 0.00 2024-11-06T03:29:04Z- handleDepth()- Depth_Keller msg: 1730863744.26 epoch sec; seqNo:5901; depth 50.60 2024-11-06T03:29:04Z- handleDVL() - DVL msg epochMillisec: 1730863744.29 2024-11-06T03:29:04Z- handleDvl() - RDI_Pathfinder msg: 1730863744.29 sec; seqNo:2033; #beams:4 2024-11-06T03:29:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.49 , 34.40 , 35.89 , 41.87 2024-11-06T03:29:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , 0.05 2024-11-06T03:29:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863744.31 epoch sec; seqNo:5901; 4061895.09 north; 594622.36 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:29:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863743.84 2024-11-06T03:29:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:04Z- waiting for 1730863747.84, time is 1730863744.31 2024-11-06T03:29:04Z- AHRS_M2 msg: 1730863744.62 epoch sec; seqNo:5541 2024-11-06T03:29:04Z- AHRS_M2 msg: 0.22 phi; -0.13 theta; 3.72 psi 2024-11-06T03:29:04Z- handleDepth()- Depth_Keller msg: 1730863744.63 epoch sec; seqNo:5902; depth 50.65 2024-11-06T03:29:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863744.64 epoch sec; seqNo:5902; 4061894.71 north; 594622.37 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:29:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863743.84 2024-11-06T03:29:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:04Z- waiting for 1730863747.84, time is 1730863744.64 2024-11-06T03:29:05Z- AHRS_M2 msg: 1730863745.02 epoch sec; seqNo:5542 2024-11-06T03:29:05Z- AHRS_M2 msg: 0.22 phi; -0.12 theta; 3.73 psi 2024-11-06T03:29:05Z- handleDepth()- Depth_Keller msg: 1730863745.02 epoch sec; seqNo:5903; depth 50.68 2024-11-06T03:29:05Z- handleDVL() - DVL msg epochMillisec: 1730863745.03 2024-11-06T03:29:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:05Z- handleDvl() - RDI_Pathfinder msg: 1730863745.03 sec; seqNo:2034; #beams:0 2024-11-06T03:29:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , 0.05 2024-11-06T03:29:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863745.04 epoch sec; seqNo:5903; 4061894.33 north; 594622.37 east; 36.70 lat; -121.94 lon; 50.68 depth; utmZone: 10 2024-11-06T03:29:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863743.84 2024-11-06T03:29:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:05Z- waiting for 1730863747.84, time is 1730863745.04 2024-11-06T03:29:05Z- AHRS_M2 msg: 1730863745.42 epoch sec; seqNo:5543 2024-11-06T03:29:05Z- AHRS_M2 msg: 0.22 phi; -0.12 theta; 3.74 psi 2024-11-06T03:29:05Z- handleDepth()- Depth_Keller msg: 1730863745.44 epoch sec; seqNo:5904; depth 50.74 2024-11-06T03:29:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863745.45 epoch sec; seqNo:5904; 4061893.94 north; 594621.15 east; 36.70 lat; -121.94 lon; 50.74 depth; utmZone: 10 2024-11-06T03:29:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863743.84 2024-11-06T03:29:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:05Z- waiting for 1730863747.84, time is 1730863745.45 2024-11-06T03:29:05Z- AHRS_M2 msg: 1730863745.83 epoch sec; seqNo:5544 2024-11-06T03:29:05Z- AHRS_M2 msg: 0.22 phi; -0.11 theta; 3.75 psi 2024-11-06T03:29:05Z- handleDepth()- Depth_Keller msg: 1730863745.84 epoch sec; seqNo:5905; depth 50.81 2024-11-06T03:29:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863745.86 epoch sec; seqNo:5905; 4061893.94 north; 594621.15 east; 36.70 lat; -121.94 lon; 50.81 depth; utmZone: 10 2024-11-06T03:29:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863743.84 2024-11-06T03:29:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:05Z- waiting for 1730863747.84, time is 1730863745.86 2024-11-06T03:29:06Z- AHRS_M2 msg: 1730863746.23 epoch sec; seqNo:5545 2024-11-06T03:29:06Z- AHRS_M2 msg: 0.22 phi; -0.10 theta; 3.76 psi 2024-11-06T03:29:06Z- handleDepth()- Depth_Keller msg: 1730863746.24 epoch sec; seqNo:5906; depth 50.85 2024-11-06T03:29:06Z- handleDVL() - DVL msg epochMillisec: 1730863746.27 2024-11-06T03:29:06Z- handleDvl() - RDI_Pathfinder msg: 1730863746.27 sec; seqNo:2035; #beams:4 2024-11-06T03:29:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.30 , 33.83 , 36.26 , 40.94 2024-11-06T03:29:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , 0.04 2024-11-06T03:29:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863746.29 epoch sec; seqNo:5906; 4061893.56 north; 594621.16 east; 36.70 lat; -121.94 lon; 50.85 depth; utmZone: 10 2024-11-06T03:29:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863743.84 2024-11-06T03:29:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:06Z- waiting for 1730863747.84, time is 1730863746.29 2024-11-06T03:29:06Z- AHRS_M2 msg: 1730863746.64 epoch sec; seqNo:5546 2024-11-06T03:29:06Z- AHRS_M2 msg: 0.23 phi; -0.10 theta; 3.77 psi 2024-11-06T03:29:06Z- handleDepth()- Depth_Keller msg: 1730863746.65 epoch sec; seqNo:5907; depth 50.91 2024-11-06T03:29:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863746.67 epoch sec; seqNo:5907; 4061893.18 north; 594621.16 east; 36.70 lat; -121.94 lon; 50.91 depth; utmZone: 10 2024-11-06T03:29:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863743.84 2024-11-06T03:29:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:06Z- waiting for 1730863747.84, time is 1730863746.67 2024-11-06T03:29:07Z- AHRS_M2 msg: 1730863747.05 epoch sec; seqNo:5547 2024-11-06T03:29:07Z- AHRS_M2 msg: 0.23 phi; -0.09 theta; 3.78 psi 2024-11-06T03:29:07Z- handleDepth()- Depth_Keller msg: 1730863747.05 epoch sec; seqNo:5908; depth 50.93 2024-11-06T03:29:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863747.07 epoch sec; seqNo:5908; 4061892.81 north; 594621.17 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T03:29:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863743.84 2024-11-06T03:29:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:07Z- waiting for 1730863747.84, time is 1730863747.07 2024-11-06T03:29:07Z- AHRS_M2 msg: 1730863747.45 epoch sec; seqNo:5548 2024-11-06T03:29:07Z- AHRS_M2 msg: 0.23 phi; -0.09 theta; 3.79 psi 2024-11-06T03:29:07Z- handleDepth()- Depth_Keller msg: 1730863747.45 epoch sec; seqNo:5909; depth 50.98 2024-11-06T03:29:07Z- handleDVL() - DVL msg epochMillisec: 1730863747.45 2024-11-06T03:29:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:07Z- handleDvl() - RDI_Pathfinder msg: 1730863747.45 sec; seqNo:2036; #beams:0 2024-11-06T03:29:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , 0.04 2024-11-06T03:29:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863747.48 epoch sec; seqNo:5909; 4061892.41 north; 594619.95 east; 36.70 lat; -121.94 lon; 50.98 depth; utmZone: 10 2024-11-06T03:29:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863743.84 2024-11-06T03:29:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:07Z- waiting for 1730863747.84, time is 1730863747.48 2024-11-06T03:29:07Z- AHRS_M2 msg: 1730863747.85 epoch sec; seqNo:5549 2024-11-06T03:29:07Z- AHRS_M2 msg: 0.22 phi; -0.08 theta; 3.80 psi 2024-11-06T03:29:07Z- handleDepth()- Depth_Keller msg: 1730863747.86 epoch sec; seqNo:5910; depth 51.03 2024-11-06T03:29:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863747.88 epoch sec; seqNo:5910; 4061892.41 north; 594619.95 east; 36.70 lat; -121.94 lon; 51.03 depth; utmZone: 10 2024-11-06T03:29:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863743.84 2024-11-06T03:29:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:08Z- waiting for 1730863747.84, time is 1730863747.88 2024-11-06T03:29:08Z- LcmTrn::updateTrn() >>>> pose time 1730863747.88, meas time 1730863747.45 <<<< 2024-11-06T03:29:08Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863747.88 2024-11-06T03:29:08Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:29:08Z- TerrainNav::motionUpdate() - currEstimate = (4061892, 594619.95, 51.03) 2024-11-06T03:29:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:29:08Z- TerrainNav:: SubcloudNIS is 0.53, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:29:08Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:29:08Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:29:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:29:08Z- variances([0], [2], and psi_berg) = 0.267593 0.198254 0.000000 2024-11-06T03:29:08Z- publishEstimatesDecoder() - at 1730863747.88 sec 2024-11-06T03:29:08Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:29:08Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061922.02 594602.95 55.06 2024-11-06T03:29:08Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061920.81 594603.84 56.69 lat/lon: 36.70 -121.94 2024-11-06T03:29:08Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.20 1.37 0.00 2024-11-06T03:29:08Z- AHRS_M2 msg: 1730863748.25 epoch sec; seqNo:5550 2024-11-06T03:29:08Z- AHRS_M2 msg: 0.22 phi; -0.08 theta; 3.81 psi 2024-11-06T03:29:08Z- handleDepth()- Depth_Keller msg: 1730863748.26 epoch sec; seqNo:5911; depth 51.04 2024-11-06T03:29:08Z- handleDVL() - DVL msg epochMillisec: 1730863748.28 2024-11-06T03:29:08Z- handleDvl() - RDI_Pathfinder msg: 1730863748.28 sec; seqNo:2037; #beams:4 2024-11-06T03:29:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.12 , 33.83 , 36.64 , 40.00 2024-11-06T03:29:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , 0.01 2024-11-06T03:29:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863748.30 epoch sec; seqNo:5911; 4061892.03 north; 594619.96 east; 36.70 lat; -121.94 lon; 51.04 depth; utmZone: 10 2024-11-06T03:29:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863747.88 2024-11-06T03:29:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:08Z- waiting for 1730863751.88, time is 1730863748.30 2024-11-06T03:29:08Z- AHRS_M2 msg: 1730863748.65 epoch sec; seqNo:5551 2024-11-06T03:29:08Z- AHRS_M2 msg: 0.22 phi; -0.07 theta; 3.82 psi 2024-11-06T03:29:08Z- handleDepth()- Depth_Keller msg: 1730863748.66 epoch sec; seqNo:5912; depth 51.09 2024-11-06T03:29:08Z- handleDVL() - DVL msg epochMillisec: 1730863748.66 2024-11-06T03:29:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:08Z- handleDvl() - RDI_Pathfinder msg: 1730863748.66 sec; seqNo:2038; #beams:0 2024-11-06T03:29:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , 0.01 2024-11-06T03:29:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863748.69 epoch sec; seqNo:5912; 4061891.66 north; 594619.96 east; 36.70 lat; -121.94 lon; 51.09 depth; utmZone: 10 2024-11-06T03:29:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863747.88 2024-11-06T03:29:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:08Z- waiting for 1730863751.88, time is 1730863748.69 2024-11-06T03:29:09Z- AHRS_M2 msg: 1730863749.06 epoch sec; seqNo:5552 2024-11-06T03:29:09Z- AHRS_M2 msg: 0.22 phi; -0.07 theta; 3.84 psi 2024-11-06T03:29:09Z- handleDepth()- Depth_Keller msg: 1730863749.08 epoch sec; seqNo:5913; depth 51.11 2024-11-06T03:29:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863749.10 epoch sec; seqNo:5913; 4061891.28 north; 594619.96 east; 36.70 lat; -121.94 lon; 51.11 depth; utmZone: 10 2024-11-06T03:29:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863747.88 2024-11-06T03:29:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:09Z- waiting for 1730863751.88, time is 1730863749.10 2024-11-06T03:29:09Z- AHRS_M2 msg: 1730863749.47 epoch sec; seqNo:5553 2024-11-06T03:29:09Z- AHRS_M2 msg: 0.22 phi; -0.06 theta; 3.84 psi 2024-11-06T03:29:09Z- handleDepth()- Depth_Keller msg: 1730863749.47 epoch sec; seqNo:5914; depth 51.13 2024-11-06T03:29:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863749.49 epoch sec; seqNo:5914; 4061891.28 north; 594619.96 east; 36.70 lat; -121.94 lon; 51.13 depth; utmZone: 10 2024-11-06T03:29:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863747.88 2024-11-06T03:29:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:09Z- waiting for 1730863751.88, time is 1730863749.49 2024-11-06T03:29:09Z- AHRS_M2 msg: 1730863749.87 epoch sec; seqNo:5554 2024-11-06T03:29:09Z- AHRS_M2 msg: 0.22 phi; -0.06 theta; 3.86 psi 2024-11-06T03:29:09Z- handleDepth()- Depth_Keller msg: 1730863749.88 epoch sec; seqNo:5915; depth 51.17 2024-11-06T03:29:09Z- handleDVL() - DVL msg epochMillisec: 1730863749.90 2024-11-06T03:29:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:09Z- handleDvl() - RDI_Pathfinder msg: 1730863749.90 sec; seqNo:2039; #beams:0 2024-11-06T03:29:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , 0.01 2024-11-06T03:29:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863749.92 epoch sec; seqNo:5915; 4061890.88 north; 594618.75 east; 36.70 lat; -121.94 lon; 51.17 depth; utmZone: 10 2024-11-06T03:29:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863747.88 2024-11-06T03:29:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:10Z- waiting for 1730863751.88, time is 1730863749.92 2024-11-06T03:29:10Z- AHRS_M2 msg: 1730863750.27 epoch sec; seqNo:5555 2024-11-06T03:29:10Z- AHRS_M2 msg: 0.22 phi; -0.06 theta; 3.87 psi 2024-11-06T03:29:10Z- handleDepth()- Depth_Keller msg: 1730863750.28 epoch sec; seqNo:5916; depth 51.18 2024-11-06T03:29:10Z- handleDVL() - DVL msg epochMillisec: 1730863750.30 2024-11-06T03:29:10Z- handleDvl() - RDI_Pathfinder msg: 1730863750.30 sec; seqNo:2040; #beams:4 2024-11-06T03:29:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.74 , 33.83 , 37.01 , 39.44 2024-11-06T03:29:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.04 , -0.00 2024-11-06T03:29:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863750.34 epoch sec; seqNo:5916; 4061890.51 north; 594618.75 east; 36.70 lat; -121.94 lon; 51.18 depth; utmZone: 10 2024-11-06T03:29:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863747.88 2024-11-06T03:29:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:10Z- waiting for 1730863751.88, time is 1730863750.34 2024-11-06T03:29:10Z- AHRS_M2 msg: 1730863750.68 epoch sec; seqNo:5556 2024-11-06T03:29:10Z- AHRS_M2 msg: 0.22 phi; -0.05 theta; 3.88 psi 2024-11-06T03:29:10Z- handleDepth()- Depth_Keller msg: 1730863750.69 epoch sec; seqNo:5917; depth 51.19 2024-11-06T03:29:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863750.70 epoch sec; seqNo:5917; 4061890.13 north; 594618.76 east; 36.70 lat; -121.94 lon; 51.19 depth; utmZone: 10 2024-11-06T03:29:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863747.88 2024-11-06T03:29:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:10Z- waiting for 1730863751.88, time is 1730863750.70 2024-11-06T03:29:11Z- AHRS_M2 msg: 1730863751.08 epoch sec; seqNo:5557 2024-11-06T03:29:11Z- AHRS_M2 msg: 0.22 phi; -0.05 theta; 3.89 psi 2024-11-06T03:29:11Z- handleDepth()- Depth_Keller msg: 1730863751.08 epoch sec; seqNo:5918; depth 51.20 2024-11-06T03:29:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863751.11 epoch sec; seqNo:5918; 4061889.75 north; 594618.76 east; 36.70 lat; -121.94 lon; 51.20 depth; utmZone: 10 2024-11-06T03:29:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863747.88 2024-11-06T03:29:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:11Z- waiting for 1730863751.88, time is 1730863751.11 2024-11-06T03:29:11Z- AHRS_M2 msg: 1730863751.48 epoch sec; seqNo:5558 2024-11-06T03:29:11Z- AHRS_M2 msg: 0.22 phi; -0.04 theta; 3.90 psi 2024-11-06T03:29:11Z- handleDepth()- Depth_Keller msg: 1730863751.49 epoch sec; seqNo:5919; depth 51.23 2024-11-06T03:29:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863751.50 epoch sec; seqNo:5919; 4061889.75 north; 594618.76 east; 36.70 lat; -121.94 lon; 51.23 depth; utmZone: 10 2024-11-06T03:29:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863747.88 2024-11-06T03:29:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:11Z- waiting for 1730863751.88, time is 1730863751.50 2024-11-06T03:29:11Z- AHRS_M2 msg: 1730863751.90 epoch sec; seqNo:5559 2024-11-06T03:29:11Z- AHRS_M2 msg: 0.22 phi; -0.04 theta; 3.91 psi 2024-11-06T03:29:11Z- handleDepth()- Depth_Keller msg: 1730863751.91 epoch sec; seqNo:5920; depth 51.24 2024-11-06T03:29:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863751.92 epoch sec; seqNo:5920; 4061889.36 north; 594617.54 east; 36.70 lat; -121.94 lon; 51.24 depth; utmZone: 10 2024-11-06T03:29:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863747.88 2024-11-06T03:29:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:12Z- waiting for 1730863751.88, time is 1730863751.92 2024-11-06T03:29:12Z- LcmTrn::updateTrn() >>>> pose time 1730863751.92, meas time 1730863750.30 <<<< 2024-11-06T03:29:12Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863751.92 2024-11-06T03:29:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863750.30 sec, ping # 0; waiting for INS data more recent than 1730863747.88... 2024-11-06T03:29:12Z- TerrainNav::motionUpdate() - currEstimate = (4061889, 594617.54, 51.24) 2024-11-06T03:29:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:29:12Z- TerrainNav:: SubcloudNIS is 0.53, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:29:12Z- TNavPF::Projecting Measurements 2024-11-06T03:29:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:29:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:29:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:29:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:29:12Z- TNavFilter::4 good beams. 2024-11-06T03:29:12Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:29:12Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:29:12Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:29:12Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:29:12Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:29:12Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:29:12Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:29:12Z- TerrainMapOctree: (vehN, vehE ) = (4061890.58, 594618.51). 2024-11-06T03:29:12Z- TerrainMapOctree: Min Distance = 192.49. 2024-11-06T03:29:12Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:29:12Z- Forcing Subcloud Comparison 2024-11-06T03:29:12Z- Weighting particles with subcloud comparison 2024-11-06T03:29:12Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:12Z- meanExpectedMeasDiff: 0.350902 delta_rms_squared: 2.985699 alpha: 0.993346 2024-11-06T03:29:12Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:12Z- meanExpectedMeasDiff: 4.596205 delta_rms_squared: 1.667828 alpha: 0.988150 2024-11-06T03:29:12Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:12Z- meanExpectedMeasDiff: 1.004905 delta_rms_squared: 3.715810 alpha: 0.994646 2024-11-06T03:29:12Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:12Z- meanExpectedMeasDiff: 1.190348 delta_rms_squared: 3.953475 alpha: 0.994967 2024-11-06T03:29:12Z- Subcloud NIS: 0.531216 2024-11-06T03:29:12Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:29:12Z- TNavPF:: sumWeights = 0.051622 2024-11-06T03:29:12Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:29:12Z- TNavPF::First Term of Map Covariance Matrix is 0.08 2024-11-06T03:29:12Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:29:12Z- TerrainNav::Resampling particle filter... 2024-11-06T03:29:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863750.30 sec. 2024-11-06T03:29:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:29:12Z- variances([0], [2], and psi_berg) = 0.275487 0.184698 0.000000 2024-11-06T03:29:12Z- publishEstimatesDecoder() - at 1730863751.92 sec 2024-11-06T03:29:12Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:29:12Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061918.92 594600.51 54.49 2024-11-06T03:29:12Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061917.74 594601.44 55.97 lat/lon: 36.70 -121.94 2024-11-06T03:29:12Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.28 0.18 0.68 0.00 2024-11-06T03:29:12Z- AHRS_M2 msg: 1730863752.30 epoch sec; seqNo:5560 2024-11-06T03:29:12Z- AHRS_M2 msg: 0.22 phi; -0.03 theta; 3.92 psi 2024-11-06T03:29:12Z- handleDepth()- Depth_Keller msg: 1730863752.33 epoch sec; seqNo:5921; depth 51.25 2024-11-06T03:29:12Z- handleDVL() - DVL msg epochMillisec: 1730863752.37 2024-11-06T03:29:12Z- handleDvl() - RDI_Pathfinder msg: 1730863752.37 sec; seqNo:2041; #beams:4 2024-11-06T03:29:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.74 , 33.83 , 37.20 , 38.88 2024-11-06T03:29:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.00 , -0.02 2024-11-06T03:29:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863752.39 epoch sec; seqNo:5921; 4061888.98 north; 594617.55 east; 36.70 lat; -121.94 lon; 51.25 depth; utmZone: 10 2024-11-06T03:29:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863751.92 2024-11-06T03:29:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:12Z- waiting for 1730863755.92, time is 1730863752.39 2024-11-06T03:29:12Z- handleDepth()- Depth_Keller msg: 1730863752.73 epoch sec; seqNo:5922; depth 51.26 2024-11-06T03:29:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863752.74 epoch sec; seqNo:5922; 4061888.60 north; 594617.55 east; 36.70 lat; -121.94 lon; 51.26 depth; utmZone: 10 2024-11-06T03:29:12Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:29:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863751.92 2024-11-06T03:29:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:12Z- waiting for 1730863755.92, time is 1730863752.74 2024-11-06T03:29:13Z- AHRS_M2 msg: 1730863753.10 epoch sec; seqNo:5561 2024-11-06T03:29:13Z- AHRS_M2 msg: 0.21 phi; -0.03 theta; 3.94 psi 2024-11-06T03:29:13Z- handleDepth()- Depth_Keller msg: 1730863753.11 epoch sec; seqNo:5923; depth 51.28 2024-11-06T03:29:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863753.12 epoch sec; seqNo:5923; 4061888.60 north; 594617.55 east; 36.70 lat; -121.94 lon; 51.28 depth; utmZone: 10 2024-11-06T03:29:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863751.92 2024-11-06T03:29:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:13Z- waiting for 1730863755.92, time is 1730863753.12 2024-11-06T03:29:13Z- handleDepth()- Depth_Keller msg: 1730863753.53 epoch sec; seqNo:5924; depth 51.27 2024-11-06T03:29:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863753.54 epoch sec; seqNo:5924; 4061888.21 north; 594616.34 east; 36.70 lat; -121.94 lon; 51.27 depth; utmZone: 10 2024-11-06T03:29:13Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:29:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863751.92 2024-11-06T03:29:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:13Z- waiting for 1730863755.92, time is 1730863753.54 2024-11-06T03:29:13Z- AHRS_M2 msg: 1730863753.91 epoch sec; seqNo:5562 2024-11-06T03:29:13Z- AHRS_M2 msg: 0.21 phi; -0.02 theta; 3.96 psi 2024-11-06T03:29:13Z- handleDepth()- Depth_Keller msg: 1730863753.91 epoch sec; seqNo:5925; depth 51.29 2024-11-06T03:29:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863753.92 epoch sec; seqNo:5925; 4061887.83 north; 594616.34 east; 36.70 lat; -121.94 lon; 51.29 depth; utmZone: 10 2024-11-06T03:29:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863751.92 2024-11-06T03:29:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:14Z- waiting for 1730863755.92, time is 1730863753.92 2024-11-06T03:29:14Z- AHRS_M2 msg: 1730863754.31 epoch sec; seqNo:5563 2024-11-06T03:29:14Z- AHRS_M2 msg: 0.21 phi; -0.02 theta; 3.96 psi 2024-11-06T03:29:14Z- handleDepth()- Depth_Keller msg: 1730863754.32 epoch sec; seqNo:5926; depth 51.29 2024-11-06T03:29:14Z- handleDVL() - DVL msg epochMillisec: 1730863754.34 2024-11-06T03:29:14Z- handleDvl() - RDI_Pathfinder msg: 1730863754.34 sec; seqNo:2042; #beams:4 2024-11-06T03:29:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.37 , 33.65 , 37.39 , 38.32 2024-11-06T03:29:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.03 , -0.03 2024-11-06T03:29:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863754.37 epoch sec; seqNo:5926; 4061887.83 north; 594616.34 east; 36.70 lat; -121.94 lon; 51.29 depth; utmZone: 10 2024-11-06T03:29:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863751.92 2024-11-06T03:29:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:14Z- waiting for 1730863755.92, time is 1730863754.37 2024-11-06T03:29:14Z- AHRS_M2 msg: 1730863754.72 epoch sec; seqNo:5564 2024-11-06T03:29:14Z- AHRS_M2 msg: 0.21 phi; -0.01 theta; 3.97 psi 2024-11-06T03:29:14Z- handleDepth()- Depth_Keller msg: 1730863754.72 epoch sec; seqNo:5927; depth 51.28 2024-11-06T03:29:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863754.74 epoch sec; seqNo:5927; 4061887.45 north; 594616.34 east; 36.70 lat; -121.94 lon; 51.28 depth; utmZone: 10 2024-11-06T03:29:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863751.92 2024-11-06T03:29:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:14Z- waiting for 1730863755.92, time is 1730863754.74 2024-11-06T03:29:15Z- AHRS_M2 msg: 1730863755.12 epoch sec; seqNo:5565 2024-11-06T03:29:15Z- AHRS_M2 msg: 0.20 phi; -0.01 theta; 3.97 psi 2024-11-06T03:29:15Z- handleDepth()- Depth_Keller msg: 1730863755.12 epoch sec; seqNo:5928; depth 51.29 2024-11-06T03:29:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863755.14 epoch sec; seqNo:5928; 4061887.06 north; 594615.13 east; 36.70 lat; -121.94 lon; 51.29 depth; utmZone: 10 2024-11-06T03:29:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863751.92 2024-11-06T03:29:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:15Z- waiting for 1730863755.92, time is 1730863755.14 2024-11-06T03:29:15Z- AHRS_M2 msg: 1730863755.53 epoch sec; seqNo:5566 2024-11-06T03:29:15Z- AHRS_M2 msg: 0.20 phi; -0.01 theta; 3.98 psi 2024-11-06T03:29:15Z- handleDepth()- Depth_Keller msg: 1730863755.54 epoch sec; seqNo:5929; depth 51.32 2024-11-06T03:29:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863755.56 epoch sec; seqNo:5929; 4061887.06 north; 594615.13 east; 36.70 lat; -121.94 lon; 51.32 depth; utmZone: 10 2024-11-06T03:29:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863751.92 2024-11-06T03:29:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:15Z- waiting for 1730863755.92, time is 1730863755.56 2024-11-06T03:29:15Z- AHRS_M2 msg: 1730863755.93 epoch sec; seqNo:5567 2024-11-06T03:29:15Z- AHRS_M2 msg: 0.20 phi; -0.00 theta; 3.98 psi 2024-11-06T03:29:15Z- handleDepth()- Depth_Keller msg: 1730863755.93 epoch sec; seqNo:5930; depth 51.31 2024-11-06T03:29:15Z- handleDVL() - DVL msg epochMillisec: 1730863755.93 2024-11-06T03:29:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:15Z- handleDvl() - RDI_Pathfinder msg: 1730863755.93 sec; seqNo:2043; #beams:0 2024-11-06T03:29:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.03 , -0.03 2024-11-06T03:29:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863755.94 epoch sec; seqNo:5930; 4061886.68 north; 594615.13 east; 36.70 lat; -121.94 lon; 51.31 depth; utmZone: 10 2024-11-06T03:29:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863751.92 2024-11-06T03:29:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:16Z- waiting for 1730863755.92, time is 1730863755.94 2024-11-06T03:29:16Z- LcmTrn::updateTrn() >>>> pose time 1730863755.94, meas time 1730863755.93 <<<< 2024-11-06T03:29:16Z- LcmTrn::updateTrn() equating pose 1730863755.93 and meas times 1730863755.93 2024-11-06T03:29:16Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863755.93 2024-11-06T03:29:16Z- TerrainNav::motionUpdate() - currEstimate = (4061887, 594615.13, 51.31) 2024-11-06T03:29:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:29:16Z- TerrainNav:: SubcloudNIS is 0.53, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:29:16Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:29:16Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:29:16Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:29:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:29:16Z- variances([0], [2], and psi_berg) = 0.278253 0.186927 0.000000 2024-11-06T03:29:16Z- publishEstimatesDecoder() - at 1730863755.93 sec 2024-11-06T03:29:16Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:29:16Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061916.14 594598.09 53.83 2024-11-06T03:29:16Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061915.06 594599.03 56.04 lat/lon: 36.70 -121.94 2024-11-06T03:29:16Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.28 0.19 0.94 0.00 2024-11-06T03:29:16Z- AHRS_M2 msg: 1730863756.33 epoch sec; seqNo:5568 2024-11-06T03:29:16Z- AHRS_M2 msg: 0.19 phi; 0.00 theta; 3.98 psi 2024-11-06T03:29:16Z- handleDepth()- Depth_Keller msg: 1730863756.34 epoch sec; seqNo:5931; depth 51.32 2024-11-06T03:29:16Z- handleDVL() - DVL msg epochMillisec: 1730863756.37 2024-11-06T03:29:16Z- handleDvl() - RDI_Pathfinder msg: 1730863756.37 sec; seqNo:2044; #beams:4 2024-11-06T03:29:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.81 , 33.83 , 37.57 , 37.76 2024-11-06T03:29:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.05 , 0.00 2024-11-06T03:29:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863756.38 epoch sec; seqNo:5931; 4061886.30 north; 594615.14 east; 36.70 lat; -121.94 lon; 51.32 depth; utmZone: 10 2024-11-06T03:29:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863755.93 2024-11-06T03:29:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:16Z- waiting for 1730863759.93, time is 1730863756.38 2024-11-06T03:29:16Z- handleDepth()- Depth_Keller msg: 1730863756.75 epoch sec; seqNo:5932; depth 51.32 2024-11-06T03:29:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863756.77 epoch sec; seqNo:5932; 4061886.28 north; 594613.92 east; 36.70 lat; -121.94 lon; 51.32 depth; utmZone: 10 2024-11-06T03:29:16Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:29:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863755.93 2024-11-06T03:29:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:16Z- waiting for 1730863759.93, time is 1730863756.77 2024-11-06T03:29:17Z- AHRS_M2 msg: 1730863757.14 epoch sec; seqNo:5569 2024-11-06T03:29:17Z- AHRS_M2 msg: 0.19 phi; 0.01 theta; 3.99 psi 2024-11-06T03:29:17Z- handleDepth()- Depth_Keller msg: 1730863757.15 epoch sec; seqNo:5933; depth 51.33 2024-11-06T03:29:17Z- handleDVL() - DVL msg epochMillisec: 1730863757.16 2024-11-06T03:29:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:17Z- handleDvl() - RDI_Pathfinder msg: 1730863757.16 sec; seqNo:2045; #beams:0 2024-11-06T03:29:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.05 , 0.00 2024-11-06T03:29:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863757.17 epoch sec; seqNo:5933; 4061885.91 north; 594613.92 east; 36.70 lat; -121.94 lon; 51.33 depth; utmZone: 10 2024-11-06T03:29:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863755.93 2024-11-06T03:29:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:17Z- waiting for 1730863759.93, time is 1730863757.17 2024-11-06T03:29:17Z- AHRS_M2 msg: 1730863757.54 epoch sec; seqNo:5570 2024-11-06T03:29:17Z- AHRS_M2 msg: 0.19 phi; 0.01 theta; 4.00 psi 2024-11-06T03:29:17Z- handleDepth()- Depth_Keller msg: 1730863757.55 epoch sec; seqNo:5934; depth 51.33 2024-11-06T03:29:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863757.56 epoch sec; seqNo:5934; 4061885.91 north; 594613.92 east; 36.70 lat; -121.94 lon; 51.33 depth; utmZone: 10 2024-11-06T03:29:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863755.93 2024-11-06T03:29:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:17Z- waiting for 1730863759.93, time is 1730863757.56 2024-11-06T03:29:17Z- AHRS_M2 msg: 1730863757.95 epoch sec; seqNo:5571 2024-11-06T03:29:17Z- AHRS_M2 msg: 0.20 phi; 0.02 theta; 4.00 psi 2024-11-06T03:29:17Z- handleDepth()- Depth_Keller msg: 1730863757.95 epoch sec; seqNo:5935; depth 51.32 2024-11-06T03:29:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863757.96 epoch sec; seqNo:5935; 4061885.53 north; 594613.93 east; 36.70 lat; -121.94 lon; 51.32 depth; utmZone: 10 2024-11-06T03:29:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863755.93 2024-11-06T03:29:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:18Z- waiting for 1730863759.93, time is 1730863757.96 2024-11-06T03:29:18Z- handleDepth()- Depth_Keller msg: 1730863758.37 epoch sec; seqNo:5936; depth 51.33 2024-11-06T03:29:18Z- handleDVL() - DVL msg epochMillisec: 1730863758.39 2024-11-06T03:29:18Z- handleDvl() - RDI_Pathfinder msg: 1730863758.39 sec; seqNo:2046; #beams:4 2024-11-06T03:29:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.49 , 33.99 , 37.90 , 37.22 2024-11-06T03:29:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.03 , 0.03 2024-11-06T03:29:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863758.40 epoch sec; seqNo:5936; 4061885.13 north; 594612.71 east; 36.70 lat; -121.94 lon; 51.33 depth; utmZone: 10 2024-11-06T03:29:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863755.93 2024-11-06T03:29:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:18Z- waiting for 1730863759.93, time is 1730863758.40 2024-11-06T03:29:18Z- AHRS_M2 msg: 1730863758.76 epoch sec; seqNo:5572 2024-11-06T03:29:18Z- AHRS_M2 msg: 0.20 phi; 0.02 theta; 4.01 psi 2024-11-06T03:29:18Z- handleDepth()- Depth_Keller msg: 1730863758.76 epoch sec; seqNo:5937; depth 51.33 2024-11-06T03:29:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863758.81 epoch sec; seqNo:5937; 4061885.13 north; 594612.71 east; 36.70 lat; -121.94 lon; 51.33 depth; utmZone: 10 2024-11-06T03:29:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863755.93 2024-11-06T03:29:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:18Z- waiting for 1730863759.93, time is 1730863758.81 2024-11-06T03:29:19Z- AHRS_M2 msg: 1730863759.16 epoch sec; seqNo:5573 2024-11-06T03:29:19Z- AHRS_M2 msg: 0.20 phi; 0.03 theta; 4.02 psi 2024-11-06T03:29:19Z- handleDepth()- Depth_Keller msg: 1730863759.16 epoch sec; seqNo:5938; depth 51.32 2024-11-06T03:29:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863759.17 epoch sec; seqNo:5938; 4061884.76 north; 594612.71 east; 36.70 lat; -121.94 lon; 51.32 depth; utmZone: 10 2024-11-06T03:29:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863755.93 2024-11-06T03:29:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:19Z- waiting for 1730863759.93, time is 1730863759.17 2024-11-06T03:29:19Z- AHRS_M2 msg: 1730863759.57 epoch sec; seqNo:5574 2024-11-06T03:29:19Z- AHRS_M2 msg: 0.19 phi; 0.03 theta; 4.03 psi 2024-11-06T03:29:19Z- handleDepth()- Depth_Keller msg: 1730863759.57 epoch sec; seqNo:5939; depth 51.32 2024-11-06T03:29:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863759.58 epoch sec; seqNo:5939; 4061884.38 north; 594612.72 east; 36.70 lat; -121.94 lon; 51.32 depth; utmZone: 10 2024-11-06T03:29:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863755.93 2024-11-06T03:29:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:19Z- waiting for 1730863759.93, time is 1730863759.58 2024-11-06T03:29:19Z- AHRS_M2 msg: 1730863759.97 epoch sec; seqNo:5575 2024-11-06T03:29:19Z- AHRS_M2 msg: 0.19 phi; 0.03 theta; 4.03 psi 2024-11-06T03:29:19Z- handleDepth()- Depth_Keller msg: 1730863759.98 epoch sec; seqNo:5940; depth 51.33 2024-11-06T03:29:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863759.99 epoch sec; seqNo:5940; 4061884.36 north; 594611.50 east; 36.70 lat; -121.94 lon; 51.33 depth; utmZone: 10 2024-11-06T03:29:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863755.93 2024-11-06T03:29:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:20Z- waiting for 1730863759.93, time is 1730863759.99 2024-11-06T03:29:20Z- LcmTrn::updateTrn() >>>> pose time 1730863759.99, meas time 1730863758.39 <<<< 2024-11-06T03:29:20Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863759.99 2024-11-06T03:29:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863758.39 sec, ping # 0; waiting for INS data more recent than 1730863755.93... 2024-11-06T03:29:20Z- TerrainNav::motionUpdate() - currEstimate = (4061884, 594611.50, 51.33) 2024-11-06T03:29:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:29:20Z- TerrainNav:: SubcloudNIS is 0.53, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:29:20Z- TNavPF::Projecting Measurements 2024-11-06T03:29:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:29:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:29:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:29:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:29:20Z- TNavFilter::4 good beams. 2024-11-06T03:29:20Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:29:20Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:29:20Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:29:20Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:29:20Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:29:20Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:29:20Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:29:20Z- TerrainMapOctree: (vehN, vehE ) = (4061885.28, 594612.93). 2024-11-06T03:29:20Z- TerrainMapOctree: Min Distance = 192.58. 2024-11-06T03:29:20Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:29:20Z- Forcing Subcloud Comparison 2024-11-06T03:29:20Z- Weighting particles with subcloud comparison 2024-11-06T03:29:20Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:20Z- meanExpectedMeasDiff: -0.847804 delta_rms_squared: 2.092160 alpha: 0.990531 2024-11-06T03:29:20Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:20Z- meanExpectedMeasDiff: 4.042142 delta_rms_squared: 1.024328 alpha: 0.980849 2024-11-06T03:29:20Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:20Z- meanExpectedMeasDiff: -2.110054 delta_rms_squared: 1.740224 alpha: 0.988638 2024-11-06T03:29:20Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:20Z- meanExpectedMeasDiff: 0.386044 delta_rms_squared: 1.606199 alpha: 0.987701 2024-11-06T03:29:20Z- Subcloud NIS: 0.534454 2024-11-06T03:29:20Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:29:20Z- TNavPF:: sumWeights = 0.038926 2024-11-06T03:29:20Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:29:20Z- TNavPF::First Term of Map Covariance Matrix is 0.06 2024-11-06T03:29:20Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:29:20Z- TerrainNav::Resampling particle filter... 2024-11-06T03:29:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863758.39 sec. 2024-11-06T03:29:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:29:20Z- variances([0], [2], and psi_berg) = 0.200986 0.153055 0.000000 2024-11-06T03:29:20Z- publishEstimatesDecoder() - at 1730863759.99 sec 2024-11-06T03:29:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:29:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061914.69 594595.16 54.87 2024-11-06T03:29:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061912.65 594595.44 55.97 lat/lon: 36.70 -121.94 2024-11-06T03:29:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.20 0.15 0.70 0.00 2024-11-06T03:29:20Z- handleDepth()- Depth_Keller msg: 1730863760.41 epoch sec; seqNo:5941; depth 51.31 2024-11-06T03:29:20Z- handleDVL() - DVL msg epochMillisec: 1730863760.43 2024-11-06T03:29:20Z- handleDvl() - RDI_Pathfinder msg: 1730863760.43 sec; seqNo:2047; #beams:4 2024-11-06T03:29:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.65 , 33.99 , 38.24 , 37.22 2024-11-06T03:29:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , 0.03 2024-11-06T03:29:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863760.45 epoch sec; seqNo:5941; 4061883.98 north; 594611.50 east; 36.70 lat; -121.94 lon; 51.31 depth; utmZone: 10 2024-11-06T03:29:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863759.99 2024-11-06T03:29:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:20Z- waiting for 1730863763.99, time is 1730863760.45 2024-11-06T03:29:20Z- AHRS_M2 msg: 1730863760.77 epoch sec; seqNo:5576 2024-11-06T03:29:20Z- AHRS_M2 msg: 0.19 phi; 0.04 theta; 4.04 psi 2024-11-06T03:29:20Z- handleDepth()- Depth_Keller msg: 1730863760.78 epoch sec; seqNo:5942; depth 51.30 2024-11-06T03:29:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863760.79 epoch sec; seqNo:5942; 4061883.61 north; 594611.51 east; 36.70 lat; -121.94 lon; 51.30 depth; utmZone: 10 2024-11-06T03:29:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863759.99 2024-11-06T03:29:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:20Z- waiting for 1730863763.99, time is 1730863760.79 2024-11-06T03:29:21Z- AHRS_M2 msg: 1730863761.19 epoch sec; seqNo:5577 2024-11-06T03:29:21Z- AHRS_M2 msg: 0.19 phi; 0.04 theta; 4.05 psi 2024-11-06T03:29:21Z- handleDepth()- Depth_Keller msg: 1730863761.19 epoch sec; seqNo:5943; depth 51.30 2024-11-06T03:29:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863761.20 epoch sec; seqNo:5943; 4061883.61 north; 594611.51 east; 36.70 lat; -121.94 lon; 51.30 depth; utmZone: 10 2024-11-06T03:29:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863759.99 2024-11-06T03:29:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:21Z- waiting for 1730863763.99, time is 1730863761.20 2024-11-06T03:29:21Z- AHRS_M2 msg: 1730863761.58 epoch sec; seqNo:5578 2024-11-06T03:29:21Z- AHRS_M2 msg: 0.19 phi; 0.04 theta; 4.05 psi 2024-11-06T03:29:21Z- handleDepth()- Depth_Keller msg: 1730863761.59 epoch sec; seqNo:5944; depth 51.29 2024-11-06T03:29:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863761.61 epoch sec; seqNo:5944; 4061883.21 north; 594610.29 east; 36.70 lat; -121.94 lon; 51.29 depth; utmZone: 10 2024-11-06T03:29:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863759.99 2024-11-06T03:29:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:21Z- waiting for 1730863763.99, time is 1730863761.61 2024-11-06T03:29:21Z- AHRS_M2 msg: 1730863761.99 epoch sec; seqNo:5579 2024-11-06T03:29:22Z- AHRS_M2 msg: 0.19 phi; 0.04 theta; 4.06 psi 2024-11-06T03:29:22Z- handleDepth()- Depth_Keller msg: 1730863762.00 epoch sec; seqNo:5945; depth 51.28 2024-11-06T03:29:22Z- handleDVL() - DVL msg epochMillisec: 1730863762.00 2024-11-06T03:29:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:22Z- handleDvl() - RDI_Pathfinder msg: 1730863762.00 sec; seqNo:2048; #beams:0 2024-11-06T03:29:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , 0.03 2024-11-06T03:29:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863762.01 epoch sec; seqNo:5945; 4061883.21 north; 594610.29 east; 36.70 lat; -121.94 lon; 51.28 depth; utmZone: 10 2024-11-06T03:29:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863759.99 2024-11-06T03:29:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:22Z- waiting for 1730863763.99, time is 1730863762.01 2024-11-06T03:29:22Z- AHRS_M2 msg: 1730863762.39 epoch sec; seqNo:5580 2024-11-06T03:29:22Z- AHRS_M2 msg: 0.18 phi; 0.05 theta; 4.07 psi 2024-11-06T03:29:22Z- handleDepth()- Depth_Keller msg: 1730863762.40 epoch sec; seqNo:5946; depth 51.26 2024-11-06T03:29:22Z- handleDVL() - DVL msg epochMillisec: 1730863762.42 2024-11-06T03:29:22Z- handleDvl() - RDI_Pathfinder msg: 1730863762.42 sec; seqNo:2049; #beams:4 2024-11-06T03:29:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.31 , 34.15 , 38.91 , 36.87 2024-11-06T03:29:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , -0.00 2024-11-06T03:29:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863762.45 epoch sec; seqNo:5946; 4061882.83 north; 594610.29 east; 36.70 lat; -121.94 lon; 51.26 depth; utmZone: 10 2024-11-06T03:29:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863759.99 2024-11-06T03:29:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:22Z- waiting for 1730863763.99, time is 1730863762.45 2024-11-06T03:29:22Z- AHRS_M2 msg: 1730863762.79 epoch sec; seqNo:5581 2024-11-06T03:29:22Z- AHRS_M2 msg: 0.18 phi; 0.05 theta; 4.07 psi 2024-11-06T03:29:22Z- handleDepth()- Depth_Keller msg: 1730863762.80 epoch sec; seqNo:5947; depth 51.25 2024-11-06T03:29:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863762.82 epoch sec; seqNo:5947; 4061882.46 north; 594610.30 east; 36.70 lat; -121.94 lon; 51.25 depth; utmZone: 10 2024-11-06T03:29:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863759.99 2024-11-06T03:29:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:22Z- waiting for 1730863763.99, time is 1730863762.82 2024-11-06T03:29:23Z- AHRS_M2 msg: 1730863763.20 epoch sec; seqNo:5582 2024-11-06T03:29:23Z- AHRS_M2 msg: 0.18 phi; 0.05 theta; 4.08 psi 2024-11-06T03:29:23Z- handleDepth()- Depth_Keller msg: 1730863763.21 epoch sec; seqNo:5948; depth 51.23 2024-11-06T03:29:23Z- handleDVL() - DVL msg epochMillisec: 1730863763.21 2024-11-06T03:29:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:23Z- handleDvl() - RDI_Pathfinder msg: 1730863763.21 sec; seqNo:2050; #beams:0 2024-11-06T03:29:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , -0.00 2024-11-06T03:29:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863763.22 epoch sec; seqNo:5948; 4061882.44 north; 594609.08 east; 36.70 lat; -121.94 lon; 51.23 depth; utmZone: 10 2024-11-06T03:29:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863759.99 2024-11-06T03:29:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:23Z- waiting for 1730863763.99, time is 1730863763.22 2024-11-06T03:29:23Z- AHRS_M2 msg: 1730863763.60 epoch sec; seqNo:5583 2024-11-06T03:29:23Z- AHRS_M2 msg: 0.18 phi; 0.05 theta; 4.08 psi 2024-11-06T03:29:23Z- handleDepth()- Depth_Keller msg: 1730863763.61 epoch sec; seqNo:5949; depth 51.20 2024-11-06T03:29:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863763.63 epoch sec; seqNo:5949; 4061882.06 north; 594609.08 east; 36.70 lat; -121.94 lon; 51.20 depth; utmZone: 10 2024-11-06T03:29:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863759.99 2024-11-06T03:29:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:23Z- waiting for 1730863763.99, time is 1730863763.63 2024-11-06T03:29:24Z- AHRS_M2 msg: 1730863764.01 epoch sec; seqNo:5584 2024-11-06T03:29:24Z- AHRS_M2 msg: 0.18 phi; 0.06 theta; 4.09 psi 2024-11-06T03:29:24Z- handleDepth()- Depth_Keller msg: 1730863764.01 epoch sec; seqNo:5950; depth 51.19 2024-11-06T03:29:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863764.02 epoch sec; seqNo:5950; 4061881.68 north; 594609.09 east; 36.70 lat; -121.94 lon; 51.19 depth; utmZone: 10 2024-11-06T03:29:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863759.99 2024-11-06T03:29:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:24Z- waiting for 1730863763.99, time is 1730863764.02 2024-11-06T03:29:24Z- LcmTrn::updateTrn() >>>> pose time 1730863764.02, meas time 1730863763.21 <<<< 2024-11-06T03:29:24Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863764.02 2024-11-06T03:29:24Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:29:24Z- TerrainNav::motionUpdate() - currEstimate = (4061882, 594609.09, 51.19) 2024-11-06T03:29:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:29:24Z- TerrainNav:: SubcloudNIS is 0.53, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:29:24Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:29:24Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:29:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:29:24Z- variances([0], [2], and psi_berg) = 0.203064 0.155703 0.000000 2024-11-06T03:29:24Z- publishEstimatesDecoder() - at 1730863764.02 sec 2024-11-06T03:29:24Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:29:24Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061912.06 594592.80 54.50 2024-11-06T03:29:24Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061909.98 594593.03 55.83 lat/lon: 36.70 -121.94 2024-11-06T03:29:24Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.20 0.16 0.95 0.00 2024-11-06T03:29:24Z- AHRS_M2 msg: 1730863764.41 epoch sec; seqNo:5585 2024-11-06T03:29:24Z- AHRS_M2 msg: 0.18 phi; 0.06 theta; 4.09 psi 2024-11-06T03:29:24Z- handleDepth()- Depth_Keller msg: 1730863764.42 epoch sec; seqNo:5951; depth 51.16 2024-11-06T03:29:24Z- handleDVL() - DVL msg epochMillisec: 1730863764.45 2024-11-06T03:29:24Z- handleDvl() - RDI_Pathfinder msg: 1730863764.45 sec; seqNo:2051; #beams:4 2024-11-06T03:29:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.15 , 34.50 , 39.09 , 36.71 2024-11-06T03:29:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , -0.04 2024-11-06T03:29:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863764.46 epoch sec; seqNo:5951; 4061881.68 north; 594609.09 east; 36.70 lat; -121.94 lon; 51.16 depth; utmZone: 10 2024-11-06T03:29:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863764.02 2024-11-06T03:29:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:24Z- waiting for 1730863768.02, time is 1730863764.46 2024-11-06T03:29:24Z- AHRS_M2 msg: 1730863764.81 epoch sec; seqNo:5586 2024-11-06T03:29:24Z- AHRS_M2 msg: 0.18 phi; 0.06 theta; 4.10 psi 2024-11-06T03:29:24Z- handleDepth()- Depth_Keller msg: 1730863764.83 epoch sec; seqNo:5952; depth 51.14 2024-11-06T03:29:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863764.85 epoch sec; seqNo:5952; 4061881.29 north; 594607.87 east; 36.70 lat; -121.94 lon; 51.14 depth; utmZone: 10 2024-11-06T03:29:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863764.02 2024-11-06T03:29:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:24Z- waiting for 1730863768.02, time is 1730863764.85 2024-11-06T03:29:25Z- AHRS_M2 msg: 1730863765.22 epoch sec; seqNo:5587 2024-11-06T03:29:25Z- AHRS_M2 msg: 0.18 phi; 0.07 theta; 4.10 psi 2024-11-06T03:29:25Z- handleDepth()- Depth_Keller msg: 1730863765.23 epoch sec; seqNo:5953; depth 51.10 2024-11-06T03:29:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863765.24 epoch sec; seqNo:5953; 4061881.29 north; 594607.87 east; 36.70 lat; -121.94 lon; 51.10 depth; utmZone: 10 2024-11-06T03:29:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863764.02 2024-11-06T03:29:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:25Z- waiting for 1730863768.02, time is 1730863765.24 2024-11-06T03:29:25Z- AHRS_M2 msg: 1730863765.63 epoch sec; seqNo:5588 2024-11-06T03:29:25Z- AHRS_M2 msg: 0.18 phi; 0.07 theta; 4.11 psi 2024-11-06T03:29:25Z- handleDepth()- Depth_Keller msg: 1730863765.63 epoch sec; seqNo:5954; depth 51.08 2024-11-06T03:29:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863765.64 epoch sec; seqNo:5954; 4061880.91 north; 594607.87 east; 36.70 lat; -121.94 lon; 51.08 depth; utmZone: 10 2024-11-06T03:29:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863764.02 2024-11-06T03:29:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:25Z- waiting for 1730863768.02, time is 1730863765.64 2024-11-06T03:29:26Z- AHRS_M2 msg: 1730863766.03 epoch sec; seqNo:5589 2024-11-06T03:29:26Z- AHRS_M2 msg: 0.17 phi; 0.07 theta; 4.11 psi 2024-11-06T03:29:26Z- handleDepth()- Depth_Keller msg: 1730863766.04 epoch sec; seqNo:5955; depth 51.06 2024-11-06T03:29:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863766.05 epoch sec; seqNo:5955; 4061880.91 north; 594607.87 east; 36.70 lat; -121.94 lon; 51.06 depth; utmZone: 10 2024-11-06T03:29:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863764.02 2024-11-06T03:29:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:26Z- waiting for 1730863768.02, time is 1730863766.05 2024-11-06T03:29:26Z- AHRS_M2 msg: 1730863766.43 epoch sec; seqNo:5590 2024-11-06T03:29:26Z- AHRS_M2 msg: 0.17 phi; 0.07 theta; 4.11 psi 2024-11-06T03:29:26Z- handleDepth()- Depth_Keller msg: 1730863766.44 epoch sec; seqNo:5956; depth 51.04 2024-11-06T03:29:26Z- handleDVL() - DVL msg epochMillisec: 1730863766.46 2024-11-06T03:29:26Z- handleDvl() - RDI_Pathfinder msg: 1730863766.46 sec; seqNo:2052; #beams:4 2024-11-06T03:29:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.43 , 34.58 , 39.44 , 36.45 2024-11-06T03:29:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , -0.03 2024-11-06T03:29:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863766.48 epoch sec; seqNo:5956; 4061880.52 north; 594606.66 east; 36.70 lat; -121.94 lon; 51.04 depth; utmZone: 10 2024-11-06T03:29:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863764.02 2024-11-06T03:29:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:26Z- waiting for 1730863768.02, time is 1730863766.48 2024-11-06T03:29:26Z- AHRS_M2 msg: 1730863766.83 epoch sec; seqNo:5591 2024-11-06T03:29:26Z- AHRS_M2 msg: 0.17 phi; 0.08 theta; 4.11 psi 2024-11-06T03:29:26Z- handleDepth()- Depth_Keller msg: 1730863766.84 epoch sec; seqNo:5957; depth 51.00 2024-11-06T03:29:26Z- handleDVL() - DVL msg epochMillisec: 1730863766.84 2024-11-06T03:29:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:26Z- handleDvl() - RDI_Pathfinder msg: 1730863766.84 sec; seqNo:2053; #beams:0 2024-11-06T03:29:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , -0.03 2024-11-06T03:29:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863766.86 epoch sec; seqNo:5957; 4061880.14 north; 594606.66 east; 36.70 lat; -121.94 lon; 51.00 depth; utmZone: 10 2024-11-06T03:29:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863764.02 2024-11-06T03:29:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:26Z- waiting for 1730863768.02, time is 1730863766.86 2024-11-06T03:29:27Z- AHRS_M2 msg: 1730863767.24 epoch sec; seqNo:5592 2024-11-06T03:29:27Z- AHRS_M2 msg: 0.17 phi; 0.08 theta; 4.12 psi 2024-11-06T03:29:27Z- handleDepth()- Depth_Keller msg: 1730863767.25 epoch sec; seqNo:5958; depth 50.98 2024-11-06T03:29:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863767.27 epoch sec; seqNo:5958; 4061880.14 north; 594606.66 east; 36.70 lat; -121.94 lon; 50.98 depth; utmZone: 10 2024-11-06T03:29:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863764.02 2024-11-06T03:29:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:27Z- waiting for 1730863768.02, time is 1730863767.27 2024-11-06T03:29:27Z- AHRS_M2 msg: 1730863767.65 epoch sec; seqNo:5593 2024-11-06T03:29:27Z- AHRS_M2 msg: 0.16 phi; 0.08 theta; 4.12 psi 2024-11-06T03:29:27Z- handleDepth()- Depth_Keller msg: 1730863767.65 epoch sec; seqNo:5959; depth 50.95 2024-11-06T03:29:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863767.66 epoch sec; seqNo:5959; 4061879.76 north; 594606.67 east; 36.70 lat; -121.94 lon; 50.95 depth; utmZone: 10 2024-11-06T03:29:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863764.02 2024-11-06T03:29:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:27Z- waiting for 1730863768.02, time is 1730863767.66 2024-11-06T03:29:28Z- AHRS_M2 msg: 1730863768.05 epoch sec; seqNo:5594 2024-11-06T03:29:28Z- AHRS_M2 msg: 0.16 phi; 0.08 theta; 4.12 psi 2024-11-06T03:29:28Z- handleDepth()- Depth_Keller msg: 1730863768.05 epoch sec; seqNo:5960; depth 50.92 2024-11-06T03:29:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863768.06 epoch sec; seqNo:5960; 4061879.75 north; 594605.45 east; 36.70 lat; -121.94 lon; 50.92 depth; utmZone: 10 2024-11-06T03:29:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863764.02 2024-11-06T03:29:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:28Z- waiting for 1730863768.02, time is 1730863768.06 2024-11-06T03:29:28Z- LcmTrn::updateTrn() >>>> pose time 1730863768.06, meas time 1730863766.84 <<<< 2024-11-06T03:29:28Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863768.06 2024-11-06T03:29:28Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:29:28Z- TerrainNav::motionUpdate() - currEstimate = (4061880, 594605.45, 50.92) 2024-11-06T03:29:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:29:28Z- TerrainNav:: SubcloudNIS is 0.53, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:29:28Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:29:28Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:29:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:29:28Z- variances([0], [2], and psi_berg) = 0.205949 0.158874 0.000000 2024-11-06T03:29:28Z- publishEstimatesDecoder() - at 1730863768.06 sec 2024-11-06T03:29:28Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:29:28Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061910.09 594589.13 54.41 2024-11-06T03:29:28Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061908.04 594589.39 55.56 lat/lon: 36.70 -121.94 2024-11-06T03:29:28Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.16 1.21 0.00 2024-11-06T03:29:28Z- AHRS_M2 msg: 1730863768.45 epoch sec; seqNo:5595 2024-11-06T03:29:28Z- AHRS_M2 msg: 0.16 phi; 0.08 theta; 4.12 psi 2024-11-06T03:29:28Z- handleDepth()- Depth_Keller msg: 1730863768.46 epoch sec; seqNo:5961; depth 50.89 2024-11-06T03:29:28Z- handleDVL() - DVL msg epochMillisec: 1730863768.49 2024-11-06T03:29:28Z- handleDvl() - RDI_Pathfinder msg: 1730863768.49 sec; seqNo:2054; #beams:4 2024-11-06T03:29:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 34.77 , 39.63 , 36.26 2024-11-06T03:29:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.03 , 0.01 2024-11-06T03:29:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863768.51 epoch sec; seqNo:5961; 4061879.37 north; 594605.45 east; 36.70 lat; -121.94 lon; 50.89 depth; utmZone: 10 2024-11-06T03:29:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863768.06 2024-11-06T03:29:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:28Z- waiting for 1730863772.06, time is 1730863768.51 2024-11-06T03:29:28Z- AHRS_M2 msg: 1730863768.85 epoch sec; seqNo:5596 2024-11-06T03:29:28Z- AHRS_M2 msg: 0.16 phi; 0.08 theta; 4.12 psi 2024-11-06T03:29:28Z- handleDepth()- Depth_Keller msg: 1730863768.87 epoch sec; seqNo:5962; depth 50.86 2024-11-06T03:29:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863768.89 epoch sec; seqNo:5962; 4061879.37 north; 594605.45 east; 36.70 lat; -121.94 lon; 50.86 depth; utmZone: 10 2024-11-06T03:29:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863768.06 2024-11-06T03:29:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:29Z- waiting for 1730863772.06, time is 1730863768.89 2024-11-06T03:29:29Z- AHRS_M2 msg: 1730863769.27 epoch sec; seqNo:5597 2024-11-06T03:29:29Z- AHRS_M2 msg: 0.16 phi; 0.09 theta; 4.13 psi 2024-11-06T03:29:29Z- handleDepth()- Depth_Keller msg: 1730863769.27 epoch sec; seqNo:5963; depth 50.84 2024-11-06T03:29:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863769.29 epoch sec; seqNo:5963; 4061878.99 north; 594605.45 east; 36.70 lat; -121.94 lon; 50.84 depth; utmZone: 10 2024-11-06T03:29:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863768.06 2024-11-06T03:29:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:29Z- waiting for 1730863772.06, time is 1730863769.29 2024-11-06T03:29:29Z- AHRS_M2 msg: 1730863769.67 epoch sec; seqNo:5598 2024-11-06T03:29:29Z- AHRS_M2 msg: 0.16 phi; 0.09 theta; 4.13 psi 2024-11-06T03:29:29Z- handleDepth()- Depth_Keller msg: 1730863769.67 epoch sec; seqNo:5964; depth 50.78 2024-11-06T03:29:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863769.69 epoch sec; seqNo:5964; 4061878.98 north; 594604.23 east; 36.70 lat; -121.94 lon; 50.78 depth; utmZone: 10 2024-11-06T03:29:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863768.06 2024-11-06T03:29:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:29Z- waiting for 1730863772.06, time is 1730863769.69 2024-11-06T03:29:30Z- AHRS_M2 msg: 1730863770.07 epoch sec; seqNo:5599 2024-11-06T03:29:30Z- AHRS_M2 msg: 0.16 phi; 0.09 theta; 4.13 psi 2024-11-06T03:29:30Z- handleDepth()- Depth_Keller msg: 1730863770.07 epoch sec; seqNo:5965; depth 50.76 2024-11-06T03:29:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863770.08 epoch sec; seqNo:5965; 4061878.60 north; 594604.24 east; 36.70 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T03:29:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863768.06 2024-11-06T03:29:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:30Z- waiting for 1730863772.06, time is 1730863770.08 2024-11-06T03:29:30Z- AHRS_M2 msg: 1730863770.47 epoch sec; seqNo:5600 2024-11-06T03:29:30Z- AHRS_M2 msg: 0.16 phi; 0.09 theta; 4.13 psi 2024-11-06T03:29:30Z- handleDepth()- Depth_Keller msg: 1730863770.48 epoch sec; seqNo:5966; depth 50.72 2024-11-06T03:29:30Z- handleDVL() - DVL msg epochMillisec: 1730863770.50 2024-11-06T03:29:30Z- handleDvl() - RDI_Pathfinder msg: 1730863770.50 sec; seqNo:2055; #beams:4 2024-11-06T03:29:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.25 , 35.14 , 40.19 , 36.26 2024-11-06T03:29:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.01 , 0.03 2024-11-06T03:29:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863770.52 epoch sec; seqNo:5966; 4061878.22 north; 594604.24 east; 36.70 lat; -121.94 lon; 50.72 depth; utmZone: 10 2024-11-06T03:29:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863768.06 2024-11-06T03:29:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:30Z- waiting for 1730863772.06, time is 1730863770.52 2024-11-06T03:29:30Z- AHRS_M2 msg: 1730863770.88 epoch sec; seqNo:5601 2024-11-06T03:29:30Z- AHRS_M2 msg: 0.16 phi; 0.09 theta; 4.14 psi 2024-11-06T03:29:30Z- handleDepth()- Depth_Keller msg: 1730863770.88 epoch sec; seqNo:5967; depth 50.69 2024-11-06T03:29:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863770.89 epoch sec; seqNo:5967; 4061878.22 north; 594604.24 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:29:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863768.06 2024-11-06T03:29:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:31Z- waiting for 1730863772.06, time is 1730863770.89 2024-11-06T03:29:31Z- AHRS_M2 msg: 1730863771.28 epoch sec; seqNo:5602 2024-11-06T03:29:31Z- AHRS_M2 msg: 0.15 phi; 0.09 theta; 4.14 psi 2024-11-06T03:29:31Z- handleDepth()- Depth_Keller msg: 1730863771.29 epoch sec; seqNo:5968; depth 50.67 2024-11-06T03:29:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863771.30 epoch sec; seqNo:5968; 4061877.83 north; 594603.03 east; 36.70 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T03:29:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863768.06 2024-11-06T03:29:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:31Z- waiting for 1730863772.06, time is 1730863771.30 2024-11-06T03:29:31Z- AHRS_M2 msg: 1730863771.68 epoch sec; seqNo:5603 2024-11-06T03:29:31Z- AHRS_M2 msg: 0.15 phi; 0.09 theta; 4.14 psi 2024-11-06T03:29:31Z- handleDepth()- Depth_Keller msg: 1730863771.69 epoch sec; seqNo:5969; depth 50.62 2024-11-06T03:29:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863771.70 epoch sec; seqNo:5969; 4061877.83 north; 594603.03 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:29:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863768.06 2024-11-06T03:29:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:31Z- waiting for 1730863772.06, time is 1730863771.70 2024-11-06T03:29:32Z- AHRS_M2 msg: 1730863772.09 epoch sec; seqNo:5604 2024-11-06T03:29:32Z- AHRS_M2 msg: 0.15 phi; 0.09 theta; 4.14 psi 2024-11-06T03:29:32Z- handleDepth()- Depth_Keller msg: 1730863772.09 epoch sec; seqNo:5970; depth 50.58 2024-11-06T03:29:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863772.11 epoch sec; seqNo:5970; 4061877.45 north; 594603.03 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T03:29:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863768.06 2024-11-06T03:29:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:32Z- waiting for 1730863772.06, time is 1730863772.11 2024-11-06T03:29:32Z- LcmTrn::updateTrn() >>>> pose time 1730863772.11, meas time 1730863770.50 <<<< 2024-11-06T03:29:32Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863772.11 2024-11-06T03:29:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863770.50 sec, ping # 0; waiting for INS data more recent than 1730863768.06... 2024-11-06T03:29:32Z- TerrainNav::motionUpdate() - currEstimate = (4061877, 594603.03, 50.58) 2024-11-06T03:29:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:29:32Z- TerrainNav:: SubcloudNIS is 0.53, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:29:32Z- TNavPF::Projecting Measurements 2024-11-06T03:29:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:29:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:29:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:29:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:29:32Z- TNavFilter::4 good beams. 2024-11-06T03:29:32Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:29:32Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:29:32Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:29:32Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:29:32Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:29:32Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:29:32Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:29:32Z- TerrainMapOctree: (vehN, vehE ) = (4061878.36, 594603.99). 2024-11-06T03:29:32Z- TerrainMapOctree: Min Distance = 194.41. 2024-11-06T03:29:32Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:29:32Z- Forcing Subcloud Comparison 2024-11-06T03:29:32Z- Weighting particles with subcloud comparison 2024-11-06T03:29:32Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:32Z- meanExpectedMeasDiff: -0.827177 delta_rms_squared: 2.393075 alpha: 0.991712 2024-11-06T03:29:32Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:32Z- meanExpectedMeasDiff: 2.527251 delta_rms_squared: 1.674890 alpha: 0.988200 2024-11-06T03:29:32Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:32Z- meanExpectedMeasDiff: -2.325549 delta_rms_squared: 2.196873 alpha: 0.990978 2024-11-06T03:29:32Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:32Z- meanExpectedMeasDiff: 1.305226 delta_rms_squared: 1.782704 alpha: 0.988906 2024-11-06T03:29:32Z- Subcloud NIS: 0.535878 2024-11-06T03:29:32Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:29:32Z- TNavPF:: sumWeights = 0.100666 2024-11-06T03:29:32Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:29:32Z- TNavPF::First Term of Map Covariance Matrix is 0.16 2024-11-06T03:29:32Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T03:29:32Z- TerrainNav::Resampling particle filter... 2024-11-06T03:29:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863770.50 sec. 2024-11-06T03:29:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:29:32Z- variances([0], [2], and psi_berg) = 0.199799 0.156353 0.000000 2024-11-06T03:29:32Z- publishEstimatesDecoder() - at 1730863772.11 sec 2024-11-06T03:29:32Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:29:32Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061907.79 594586.76 53.90 2024-11-06T03:29:32Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061905.73 594586.98 55.10 lat/lon: 36.70 -121.94 2024-11-06T03:29:32Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.20 0.16 0.74 0.00 2024-11-06T03:29:32Z- AHRS_M2 msg: 1730863772.49 epoch sec; seqNo:5605 2024-11-06T03:29:32Z- AHRS_M2 msg: 0.15 phi; 0.09 theta; 4.14 psi 2024-11-06T03:29:32Z- handleDepth()- Depth_Keller msg: 1730863772.50 epoch sec; seqNo:5971; depth 50.55 2024-11-06T03:29:32Z- handleDVL() - DVL msg epochMillisec: 1730863772.54 2024-11-06T03:29:32Z- handleDvl() - RDI_Pathfinder msg: 1730863772.54 sec; seqNo:2056; #beams:4 2024-11-06T03:29:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.24 , 35.33 , 40.37 , 36.45 2024-11-06T03:29:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , 0.01 2024-11-06T03:29:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863772.55 epoch sec; seqNo:5971; 4061877.44 north; 594601.81 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:29:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863772.11 2024-11-06T03:29:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:32Z- waiting for 1730863776.11, time is 1730863772.55 2024-11-06T03:29:32Z- AHRS_M2 msg: 1730863772.90 epoch sec; seqNo:5606 2024-11-06T03:29:32Z- AHRS_M2 msg: 0.15 phi; 0.09 theta; 4.14 psi 2024-11-06T03:29:32Z- handleDepth()- Depth_Keller msg: 1730863772.91 epoch sec; seqNo:5972; depth 50.52 2024-11-06T03:29:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863772.92 epoch sec; seqNo:5972; 4061877.06 north; 594601.81 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:29:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863772.11 2024-11-06T03:29:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:33Z- waiting for 1730863776.11, time is 1730863772.92 2024-11-06T03:29:33Z- AHRS_M2 msg: 1730863773.31 epoch sec; seqNo:5607 2024-11-06T03:29:33Z- AHRS_M2 msg: 0.15 phi; 0.09 theta; 4.14 psi 2024-11-06T03:29:33Z- handleDepth()- Depth_Keller msg: 1730863773.31 epoch sec; seqNo:5973; depth 50.46 2024-11-06T03:29:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863773.32 epoch sec; seqNo:5973; 4061876.68 north; 594601.82 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:29:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863772.11 2024-11-06T03:29:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:33Z- waiting for 1730863776.11, time is 1730863773.32 2024-11-06T03:29:33Z- AHRS_M2 msg: 1730863773.71 epoch sec; seqNo:5608 2024-11-06T03:29:33Z- AHRS_M2 msg: 0.15 phi; 0.09 theta; 4.14 psi 2024-11-06T03:29:33Z- handleDepth()- Depth_Keller msg: 1730863773.72 epoch sec; seqNo:5974; depth 50.42 2024-11-06T03:29:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863773.73 epoch sec; seqNo:5974; 4061876.68 north; 594601.82 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:29:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863772.11 2024-11-06T03:29:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:33Z- waiting for 1730863776.11, time is 1730863773.73 2024-11-06T03:29:34Z- AHRS_M2 msg: 1730863774.11 epoch sec; seqNo:5609 2024-11-06T03:29:34Z- AHRS_M2 msg: 0.15 phi; 0.09 theta; 4.14 psi 2024-11-06T03:29:34Z- handleDepth()- Depth_Keller msg: 1730863774.11 epoch sec; seqNo:5975; depth 50.39 2024-11-06T03:29:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863774.12 epoch sec; seqNo:5975; 4061876.29 north; 594600.60 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:29:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863772.11 2024-11-06T03:29:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:34Z- waiting for 1730863776.11, time is 1730863774.12 2024-11-06T03:29:34Z- AHRS_M2 msg: 1730863774.52 epoch sec; seqNo:5610 2024-11-06T03:29:34Z- AHRS_M2 msg: 0.15 phi; 0.09 theta; 4.14 psi 2024-11-06T03:29:34Z- handleDepth()- Depth_Keller msg: 1730863774.52 epoch sec; seqNo:5976; depth 50.35 2024-11-06T03:29:34Z- handleDVL() - DVL msg epochMillisec: 1730863774.54 2024-11-06T03:29:34Z- handleDvl() - RDI_Pathfinder msg: 1730863774.54 sec; seqNo:2057; #beams:4 2024-11-06T03:29:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.43 , 35.33 , 40.56 , 36.45 2024-11-06T03:29:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.00 , -0.02 2024-11-06T03:29:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863774.56 epoch sec; seqNo:5976; 4061876.29 north; 594600.60 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:29:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863772.11 2024-11-06T03:29:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:34Z- waiting for 1730863776.11, time is 1730863774.56 2024-11-06T03:29:34Z- AHRS_M2 msg: 1730863774.91 epoch sec; seqNo:5611 2024-11-06T03:29:34Z- AHRS_M2 msg: 0.15 phi; 0.09 theta; 4.14 psi 2024-11-06T03:29:34Z- handleDepth()- Depth_Keller msg: 1730863774.92 epoch sec; seqNo:5977; depth 50.31 2024-11-06T03:29:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863774.94 epoch sec; seqNo:5977; 4061875.91 north; 594600.61 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:29:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863772.11 2024-11-06T03:29:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:35Z- waiting for 1730863776.11, time is 1730863774.94 2024-11-06T03:29:35Z- AHRS_M2 msg: 1730863775.32 epoch sec; seqNo:5612 2024-11-06T03:29:35Z- AHRS_M2 msg: 0.15 phi; 0.09 theta; 4.14 psi 2024-11-06T03:29:35Z- handleDepth()- Depth_Keller msg: 1730863775.33 epoch sec; seqNo:5978; depth 50.26 2024-11-06T03:29:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863775.34 epoch sec; seqNo:5978; 4061875.91 north; 594600.61 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:29:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863772.11 2024-11-06T03:29:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:35Z- waiting for 1730863776.11, time is 1730863775.34 2024-11-06T03:29:35Z- AHRS_M2 msg: 1730863775.72 epoch sec; seqNo:5613 2024-11-06T03:29:35Z- AHRS_M2 msg: 0.15 phi; 0.09 theta; 4.14 psi 2024-11-06T03:29:35Z- handleDepth()- Depth_Keller msg: 1730863775.73 epoch sec; seqNo:5979; depth 50.25 2024-11-06T03:29:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863775.74 epoch sec; seqNo:5979; 4061875.51 north; 594599.39 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:29:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863772.11 2024-11-06T03:29:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:35Z- waiting for 1730863776.11, time is 1730863775.74 2024-11-06T03:29:36Z- AHRS_M2 msg: 1730863776.13 epoch sec; seqNo:5614 2024-11-06T03:29:36Z- AHRS_M2 msg: 0.15 phi; 0.09 theta; 4.14 psi 2024-11-06T03:29:36Z- handleDepth()- Depth_Keller msg: 1730863776.13 epoch sec; seqNo:5980; depth 50.19 2024-11-06T03:29:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863776.14 epoch sec; seqNo:5980; 4061875.51 north; 594599.39 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:29:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863772.11 2024-11-06T03:29:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:36Z- waiting for 1730863776.11, time is 1730863776.14 2024-11-06T03:29:36Z- LcmTrn::updateTrn() >>>> pose time 1730863776.14, meas time 1730863774.54 <<<< 2024-11-06T03:29:36Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863776.14 2024-11-06T03:29:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863774.54 sec, ping # 0; waiting for INS data more recent than 1730863772.11... 2024-11-06T03:29:36Z- TerrainNav::motionUpdate() - currEstimate = (4061876, 594599.39, 50.19) 2024-11-06T03:29:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:29:36Z- TerrainNav:: SubcloudNIS is 0.54, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:29:36Z- TNavPF::Projecting Measurements 2024-11-06T03:29:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:29:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:29:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:29:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:29:36Z- TNavFilter::4 good beams. 2024-11-06T03:29:36Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:29:36Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:29:36Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:29:36Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:29:36Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:29:36Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:29:36Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:29:36Z- TerrainMapOctree: (vehN, vehE ) = (4061876.28, 594600.84). 2024-11-06T03:29:36Z- TerrainMapOctree: Min Distance = 195.44. 2024-11-06T03:29:36Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:29:36Z- Forcing Subcloud Comparison 2024-11-06T03:29:36Z- Weighting particles with subcloud comparison 2024-11-06T03:29:36Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:36Z- meanExpectedMeasDiff: -1.032508 delta_rms_squared: 1.659874 alpha: 0.988094 2024-11-06T03:29:36Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:36Z- meanExpectedMeasDiff: -0.084792 delta_rms_squared: 2.128459 alpha: 0.990691 2024-11-06T03:29:36Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:36Z- meanExpectedMeasDiff: -2.533623 delta_rms_squared: 1.538756 alpha: 0.987169 2024-11-06T03:29:36Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:36Z- meanExpectedMeasDiff: 0.953910 delta_rms_squared: 1.446305 alpha: 0.986360 2024-11-06T03:29:36Z- Subcloud NIS: 0.546560 2024-11-06T03:29:36Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:29:36Z- TNavPF:: sumWeights = 0.258772 2024-11-06T03:29:36Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:29:36Z- TNavPF::First Term of Map Covariance Matrix is 0.24 2024-11-06T03:29:36Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T03:29:36Z- TerrainNav::Resampling particle filter... 2024-11-06T03:29:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863774.54 sec. 2024-11-06T03:29:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:29:36Z- variances([0], [2], and psi_berg) = 0.187719 0.151206 0.000000 2024-11-06T03:29:36Z- publishEstimatesDecoder() - at 1730863776.14 sec 2024-11-06T03:29:36Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:29:36Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061905.89 594583.06 53.88 2024-11-06T03:29:36Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061903.77 594583.32 55.02 lat/lon: 36.70 -121.94 2024-11-06T03:29:36Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.19 0.15 0.67 0.00 2024-11-06T03:29:36Z- AHRS_M2 msg: 1730863776.55 epoch sec; seqNo:5615 2024-11-06T03:29:36Z- AHRS_M2 msg: 0.15 phi; 0.09 theta; 4.14 psi 2024-11-06T03:29:36Z- handleDepth()- Depth_Keller msg: 1730863776.56 epoch sec; seqNo:5981; depth 50.15 2024-11-06T03:29:36Z- handleDVL() - DVL msg epochMillisec: 1730863776.61 2024-11-06T03:29:36Z- handleDvl() - RDI_Pathfinder msg: 1730863776.61 sec; seqNo:2058; #beams:4 2024-11-06T03:29:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.43 , 35.33 , 40.56 , 36.63 2024-11-06T03:29:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.03 , -0.02 2024-11-06T03:29:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863776.64 epoch sec; seqNo:5981; 4061875.14 north; 594599.39 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:29:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863776.14 2024-11-06T03:29:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:36Z- waiting for 1730863780.14, time is 1730863776.64 2024-11-06T03:29:36Z- AHRS_M2 msg: 1730863776.94 epoch sec; seqNo:5616 2024-11-06T03:29:36Z- AHRS_M2 msg: 0.15 phi; 0.09 theta; 4.14 psi 2024-11-06T03:29:36Z- handleDepth()- Depth_Keller msg: 1730863776.95 epoch sec; seqNo:5982; depth 50.09 2024-11-06T03:29:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863776.97 epoch sec; seqNo:5982; 4061875.14 north; 594599.39 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:29:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863776.14 2024-11-06T03:29:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:37Z- waiting for 1730863780.14, time is 1730863776.97 2024-11-06T03:29:37Z- AHRS_M2 msg: 1730863777.34 epoch sec; seqNo:5617 2024-11-06T03:29:37Z- AHRS_M2 msg: 0.15 phi; 0.08 theta; 4.14 psi 2024-11-06T03:29:37Z- handleDepth()- Depth_Keller msg: 1730863777.35 epoch sec; seqNo:5983; depth 50.06 2024-11-06T03:29:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863777.37 epoch sec; seqNo:5983; 4061874.74 north; 594598.18 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:29:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863776.14 2024-11-06T03:29:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:37Z- waiting for 1730863780.14, time is 1730863777.37 2024-11-06T03:29:37Z- AHRS_M2 msg: 1730863777.74 epoch sec; seqNo:5618 2024-11-06T03:29:37Z- AHRS_M2 msg: 0.15 phi; 0.08 theta; 4.14 psi 2024-11-06T03:29:37Z- handleDepth()- Depth_Keller msg: 1730863777.75 epoch sec; seqNo:5984; depth 50.04 2024-11-06T03:29:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863777.76 epoch sec; seqNo:5984; 4061874.74 north; 594598.18 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:29:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863776.14 2024-11-06T03:29:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:37Z- waiting for 1730863780.14, time is 1730863777.76 2024-11-06T03:29:38Z- handleDepth()- Depth_Keller msg: 1730863778.18 epoch sec; seqNo:5985; depth 50.00 2024-11-06T03:29:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863778.23 epoch sec; seqNo:5985; 4061874.36 north; 594598.18 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:29:38Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:29:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863776.14 2024-11-06T03:29:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:38Z- waiting for 1730863780.14, time is 1730863778.23 2024-11-06T03:29:38Z- AHRS_M2 msg: 1730863778.55 epoch sec; seqNo:5619 2024-11-06T03:29:38Z- AHRS_M2 msg: 0.15 phi; 0.08 theta; 4.13 psi 2024-11-06T03:29:38Z- handleDepth()- Depth_Keller msg: 1730863778.56 epoch sec; seqNo:5986; depth 49.96 2024-11-06T03:29:38Z- handleDVL() - DVL msg epochMillisec: 1730863778.61 2024-11-06T03:29:38Z- handleDvl() - RDI_Pathfinder msg: 1730863778.61 sec; seqNo:2059; #beams:4 2024-11-06T03:29:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.43 , 35.14 , 40.56 , 37.01 2024-11-06T03:29:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.04 , 0.01 2024-11-06T03:29:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863778.64 epoch sec; seqNo:5986; 4061873.97 north; 594596.97 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:29:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863776.14 2024-11-06T03:29:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:38Z- waiting for 1730863780.14, time is 1730863778.64 2024-11-06T03:29:38Z- AHRS_M2 msg: 1730863778.95 epoch sec; seqNo:5620 2024-11-06T03:29:38Z- AHRS_M2 msg: 0.15 phi; 0.08 theta; 4.13 psi 2024-11-06T03:29:38Z- handleDepth()- Depth_Keller msg: 1730863778.96 epoch sec; seqNo:5987; depth 49.92 2024-11-06T03:29:38Z- handleDVL() - DVL msg epochMillisec: 1730863778.96 2024-11-06T03:29:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:38Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:38Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:38Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:38Z- handleDvl() - RDI_Pathfinder msg: 1730863778.96 sec; seqNo:2060; #beams:0 2024-11-06T03:29:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.04 , 0.01 2024-11-06T03:29:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863778.98 epoch sec; seqNo:5987; 4061873.97 north; 594596.97 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:29:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863776.14 2024-11-06T03:29:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:39Z- waiting for 1730863780.14, time is 1730863778.98 2024-11-06T03:29:39Z- AHRS_M2 msg: 1730863779.36 epoch sec; seqNo:5621 2024-11-06T03:29:39Z- AHRS_M2 msg: 0.15 phi; 0.08 theta; 4.13 psi 2024-11-06T03:29:39Z- handleDepth()- Depth_Keller msg: 1730863779.37 epoch sec; seqNo:5988; depth 49.89 2024-11-06T03:29:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863779.38 epoch sec; seqNo:5988; 4061873.59 north; 594596.97 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:29:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863776.14 2024-11-06T03:29:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:39Z- waiting for 1730863780.14, time is 1730863779.38 2024-11-06T03:29:39Z- AHRS_M2 msg: 1730863779.76 epoch sec; seqNo:5622 2024-11-06T03:29:39Z- AHRS_M2 msg: 0.15 phi; 0.07 theta; 4.13 psi 2024-11-06T03:29:39Z- handleDepth()- Depth_Keller msg: 1730863779.77 epoch sec; seqNo:5989; depth 49.86 2024-11-06T03:29:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863779.79 epoch sec; seqNo:5989; 4061873.59 north; 594596.97 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T03:29:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863776.14 2024-11-06T03:29:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:39Z- waiting for 1730863780.14, time is 1730863779.79 2024-11-06T03:29:40Z- AHRS_M2 msg: 1730863780.20 epoch sec; seqNo:5623 2024-11-06T03:29:40Z- AHRS_M2 msg: 0.15 phi; 0.07 theta; 4.13 psi 2024-11-06T03:29:40Z- handleDepth()- Depth_Keller msg: 1730863780.22 epoch sec; seqNo:5990; depth 49.83 2024-11-06T03:29:40Z- handleDVL() - DVL msg epochMillisec: 1730863780.25 2024-11-06T03:29:40Z- handleDvl() - RDI_Pathfinder msg: 1730863780.25 sec; seqNo:2061; #beams:4 2024-11-06T03:29:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.24 , 35.51 , 40.37 , 37.20 2024-11-06T03:29:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.00 , 0.05 2024-11-06T03:29:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863780.27 epoch sec; seqNo:5990; 4061873.20 north; 594595.75 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:29:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863776.14 2024-11-06T03:29:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:40Z- waiting for 1730863780.14, time is 1730863780.27 2024-11-06T03:29:40Z- LcmTrn::updateTrn() >>>> pose time 1730863780.27, meas time 1730863780.25 <<<< 2024-11-06T03:29:40Z- LcmTrn::updateTrn() equating pose 1730863780.25 and meas times 1730863780.25 2024-11-06T03:29:40Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863780.25 2024-11-06T03:29:40Z- TerrainNav::motionUpdate() - currEstimate = (4061873, 594595.75, 49.83) 2024-11-06T03:29:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:29:40Z- TerrainNav:: SubcloudNIS is 0.55, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:29:40Z- TNavPF::Projecting Measurements 2024-11-06T03:29:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:29:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:29:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:29:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:29:40Z- TNavFilter::4 good beams. 2024-11-06T03:29:40Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:29:40Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:29:40Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:29:40Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:29:40Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:29:40Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:29:40Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:29:40Z- TerrainMapOctree: (vehN, vehE ) = (4061873.20, 594595.75). 2024-11-06T03:29:40Z- TerrainMapOctree: Min Distance = 197.41. 2024-11-06T03:29:40Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:29:40Z- Forcing Subcloud Comparison 2024-11-06T03:29:40Z- Weighting particles with subcloud comparison 2024-11-06T03:29:40Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:40Z- meanExpectedMeasDiff: -1.237651 delta_rms_squared: 1.488985 alpha: 0.986746 2024-11-06T03:29:40Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:40Z- meanExpectedMeasDiff: -2.241006 delta_rms_squared: 1.057001 alpha: 0.981430 2024-11-06T03:29:40Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:40Z- meanExpectedMeasDiff: -2.364223 delta_rms_squared: 1.356673 alpha: 0.985472 2024-11-06T03:29:40Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:40Z- meanExpectedMeasDiff: 1.626297 delta_rms_squared: 1.212780 alpha: 0.983777 2024-11-06T03:29:40Z- Subcloud NIS: 0.559107 2024-11-06T03:29:40Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:29:40Z- TNavPF:: sumWeights = 0.157056 2024-11-06T03:29:40Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:29:40Z- TNavPF::First Term of Map Covariance Matrix is 0.15 2024-11-06T03:29:40Z- TNavPF::Calculated NIS Value : 0.07 numBeams normalized NIS: 0.02 2024-11-06T03:29:40Z- TerrainNav::Resampling particle filter... 2024-11-06T03:29:40Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863780.25 sec, ping # 0. 2024-11-06T03:29:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:29:40Z- variances([0], [2], and psi_berg) = 0.154869 0.153206 0.000000 2024-11-06T03:29:40Z- publishEstimatesDecoder() - at 1730863780.25 sec 2024-11-06T03:29:40Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:29:40Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061901.87 594578.67 54.25 2024-11-06T03:29:40Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061901.37 594579.67 55.21 lat/lon: 36.70 -121.94 2024-11-06T03:29:40Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.15 0.44 0.00 2024-11-06T03:29:40Z- AHRS_M2 msg: 1730863780.58 epoch sec; seqNo:5624 2024-11-06T03:29:40Z- AHRS_M2 msg: 0.15 phi; 0.07 theta; 4.13 psi 2024-11-06T03:29:40Z- handleDepth()- Depth_Keller msg: 1730863780.59 epoch sec; seqNo:5991; depth 49.78 2024-11-06T03:29:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863780.64 epoch sec; seqNo:5991; 4061873.20 north; 594595.75 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:29:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863780.25 2024-11-06T03:29:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:40Z- waiting for 1730863784.25, time is 1730863780.64 2024-11-06T03:29:40Z- handleDepth()- Depth_Keller msg: 1730863780.99 epoch sec; seqNo:5992; depth 49.75 2024-11-06T03:29:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863781.00 epoch sec; seqNo:5992; 4061872.82 north; 594595.76 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:29:41Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:29:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863780.25 2024-11-06T03:29:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:41Z- waiting for 1730863784.25, time is 1730863781.00 2024-11-06T03:29:41Z- AHRS_M2 msg: 1730863781.38 epoch sec; seqNo:5625 2024-11-06T03:29:41Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 4.13 psi 2024-11-06T03:29:41Z- handleDepth()- Depth_Keller msg: 1730863781.39 epoch sec; seqNo:5993; depth 49.73 2024-11-06T03:29:41Z- handleDVL() - DVL msg epochMillisec: 1730863781.39 2024-11-06T03:29:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:41Z- handleDvl() - RDI_Pathfinder msg: 1730863781.39 sec; seqNo:2062; #beams:0 2024-11-06T03:29:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.00 , 0.05 2024-11-06T03:29:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863781.40 epoch sec; seqNo:5993; 4061872.82 north; 594595.76 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:29:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863780.25 2024-11-06T03:29:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:41Z- waiting for 1730863784.25, time is 1730863781.40 2024-11-06T03:29:41Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:29:41Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:29:41Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:29:41Z- AHRS_M2 msg: 1730863781.77 epoch sec; seqNo:5626 2024-11-06T03:29:41Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 4.13 psi 2024-11-06T03:29:41Z- handleDepth()- Depth_Keller msg: 1730863781.81 epoch sec; seqNo:5994; depth 49.70 2024-11-06T03:29:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863781.82 epoch sec; seqNo:5994; 4061872.43 north; 594594.54 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:29:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863780.25 2024-11-06T03:29:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:41Z- waiting for 1730863784.25, time is 1730863781.82 2024-11-06T03:29:42Z- AHRS_M2 msg: 1730863782.22 epoch sec; seqNo:5627 2024-11-06T03:29:42Z- AHRS_M2 msg: 0.15 phi; 0.06 theta; 4.13 psi 2024-11-06T03:29:42Z- handleDepth()- Depth_Keller msg: 1730863782.23 epoch sec; seqNo:5995; depth 49.66 2024-11-06T03:29:42Z- handleDVL() - DVL msg epochMillisec: 1730863782.25 2024-11-06T03:29:42Z- handleDvl() - RDI_Pathfinder msg: 1730863782.25 sec; seqNo:2063; #beams:4 2024-11-06T03:29:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.43 , 34.95 , 40.37 , 37.57 2024-11-06T03:29:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , 0.03 2024-11-06T03:29:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863782.28 epoch sec; seqNo:5995; 4061872.05 north; 594594.55 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:29:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863780.25 2024-11-06T03:29:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:42Z- waiting for 1730863784.25, time is 1730863782.28 2024-11-06T03:29:42Z- AHRS_M2 msg: 1730863782.61 epoch sec; seqNo:5628 2024-11-06T03:29:42Z- AHRS_M2 msg: 0.15 phi; 0.05 theta; 4.14 psi 2024-11-06T03:29:42Z- handleDepth()- Depth_Keller msg: 1730863782.62 epoch sec; seqNo:5996; depth 49.62 2024-11-06T03:29:42Z- handleDVL() - DVL msg epochMillisec: 1730863782.64 2024-11-06T03:29:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:42Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:42Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:42Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:42Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:42Z- handleDvl() - RDI_Pathfinder msg: 1730863782.64 sec; seqNo:2064; #beams:0 2024-11-06T03:29:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , 0.03 2024-11-06T03:29:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863782.66 epoch sec; seqNo:5996; 4061872.05 north; 594594.55 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:29:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863780.25 2024-11-06T03:29:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:42Z- waiting for 1730863784.25, time is 1730863782.66 2024-11-06T03:29:43Z- AHRS_M2 msg: 1730863783.00 epoch sec; seqNo:5629 2024-11-06T03:29:43Z- AHRS_M2 msg: 0.15 phi; 0.05 theta; 4.14 psi 2024-11-06T03:29:43Z- handleDepth()- Depth_Keller msg: 1730863783.01 epoch sec; seqNo:5997; depth 49.58 2024-11-06T03:29:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863783.02 epoch sec; seqNo:5997; 4061871.67 north; 594594.55 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:29:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863780.25 2024-11-06T03:29:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:43Z- waiting for 1730863784.25, time is 1730863783.02 2024-11-06T03:29:43Z- AHRS_M2 msg: 1730863783.40 epoch sec; seqNo:5630 2024-11-06T03:29:43Z- AHRS_M2 msg: 0.15 phi; 0.05 theta; 4.14 psi 2024-11-06T03:29:43Z- handleDepth()- Depth_Keller msg: 1730863783.41 epoch sec; seqNo:5998; depth 49.55 2024-11-06T03:29:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863783.42 epoch sec; seqNo:5998; 4061871.66 north; 594593.33 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:29:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863780.25 2024-11-06T03:29:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:43Z- waiting for 1730863784.25, time is 1730863783.42 2024-11-06T03:29:43Z- AHRS_M2 msg: 1730863783.84 epoch sec; seqNo:5631 2024-11-06T03:29:43Z- AHRS_M2 msg: 0.16 phi; 0.04 theta; 4.14 psi 2024-11-06T03:29:43Z- handleDepth()- Depth_Keller msg: 1730863783.84 epoch sec; seqNo:5999; depth 49.53 2024-11-06T03:29:43Z- handleDVL() - DVL msg epochMillisec: 1730863783.84 2024-11-06T03:29:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:43Z- handleDvl() - RDI_Pathfinder msg: 1730863783.84 sec; seqNo:2065; #beams:0 2024-11-06T03:29:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , 0.03 2024-11-06T03:29:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863783.85 epoch sec; seqNo:5999; 4061871.28 north; 594593.33 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T03:29:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863780.25 2024-11-06T03:29:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:43Z- waiting for 1730863784.25, time is 1730863783.85 2024-11-06T03:29:44Z- AHRS_M2 msg: 1730863784.23 epoch sec; seqNo:5632 2024-11-06T03:29:44Z- AHRS_M2 msg: 0.16 phi; 0.04 theta; 4.14 psi 2024-11-06T03:29:44Z- handleDepth()- Depth_Keller msg: 1730863784.23 epoch sec; seqNo:6000; depth 49.50 2024-11-06T03:29:44Z- handleDVL() - DVL msg epochMillisec: 1730863784.27 2024-11-06T03:29:44Z- handleDvl() - RDI_Pathfinder msg: 1730863784.27 sec; seqNo:2066; #beams:4 2024-11-06T03:29:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.80 , 35.51 , 40.19 , 38.13 2024-11-06T03:29:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , -0.02 2024-11-06T03:29:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863784.28 epoch sec; seqNo:6000; 4061870.90 north; 594593.34 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:29:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863780.25 2024-11-06T03:29:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:44Z- waiting for 1730863784.25, time is 1730863784.28 2024-11-06T03:29:44Z- LcmTrn::updateTrn() >>>> pose time 1730863784.28, meas time 1730863784.27 <<<< 2024-11-06T03:29:44Z- LcmTrn::updateTrn() equating pose 1730863784.27 and meas times 1730863784.27 2024-11-06T03:29:44Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863784.27 2024-11-06T03:29:44Z- TerrainNav::motionUpdate() - currEstimate = (4061871, 594593.34, 49.50) 2024-11-06T03:29:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:29:44Z- TerrainNav:: SubcloudNIS is 0.56, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:29:44Z- TNavPF::Projecting Measurements 2024-11-06T03:29:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:29:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:29:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:29:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:29:44Z- TNavFilter::4 good beams. 2024-11-06T03:29:44Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:29:44Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:29:44Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:29:44Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:29:44Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:29:44Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:29:44Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:29:44Z- TerrainMapOctree: (vehN, vehE ) = (4061870.90, 594593.34). 2024-11-06T03:29:44Z- TerrainMapOctree: Min Distance = 197.89. 2024-11-06T03:29:44Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:29:44Z- Forcing Subcloud Comparison 2024-11-06T03:29:44Z- Weighting particles with subcloud comparison 2024-11-06T03:29:44Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:44Z- meanExpectedMeasDiff: -0.794518 delta_rms_squared: 1.127690 alpha: 0.982574 2024-11-06T03:29:44Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:44Z- meanExpectedMeasDiff: -1.768424 delta_rms_squared: 0.774810 alpha: 0.974837 2024-11-06T03:29:44Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:44Z- meanExpectedMeasDiff: -1.563383 delta_rms_squared: 0.974186 alpha: 0.979883 2024-11-06T03:29:44Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:44Z- meanExpectedMeasDiff: 0.841562 delta_rms_squared: 0.806400 alpha: 0.975799 2024-11-06T03:29:44Z- Subcloud NIS: 0.542452 2024-11-06T03:29:44Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:29:44Z- TNavPF:: sumWeights = 0.374962 2024-11-06T03:29:44Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:29:44Z- TNavPF::First Term of Map Covariance Matrix is 0.25 2024-11-06T03:29:44Z- TNavPF::Calculated NIS Value : 0.21 numBeams normalized NIS: 0.05 2024-11-06T03:29:44Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863784.27 sec, ping # 0. 2024-11-06T03:29:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:29:44Z- variances([0], [2], and psi_berg) = 0.135595 0.154288 0.000000 2024-11-06T03:29:44Z- publishEstimatesDecoder() - at 1730863784.27 sec 2024-11-06T03:29:44Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:29:44Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061898.14 594577.26 55.85 2024-11-06T03:29:44Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061899.04 594577.25 55.23 lat/lon: 36.70 -121.94 2024-11-06T03:29:44Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.15 0.37 0.00 2024-11-06T03:29:44Z- AHRS_M2 msg: 1730863784.62 epoch sec; seqNo:5633 2024-11-06T03:29:44Z- AHRS_M2 msg: 0.16 phi; 0.04 theta; 4.14 psi 2024-11-06T03:29:44Z- handleDepth()- Depth_Keller msg: 1730863784.63 epoch sec; seqNo:6001; depth 49.47 2024-11-06T03:29:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863784.66 epoch sec; seqNo:6001; 4061870.89 north; 594592.12 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:29:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863784.27 2024-11-06T03:29:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:44Z- waiting for 1730863788.27, time is 1730863784.66 2024-11-06T03:29:45Z- AHRS_M2 msg: 1730863785.02 epoch sec; seqNo:5634 2024-11-06T03:29:45Z- AHRS_M2 msg: 0.17 phi; 0.04 theta; 4.14 psi 2024-11-06T03:29:45Z- handleDepth()- Depth_Keller msg: 1730863785.02 epoch sec; seqNo:6002; depth 49.43 2024-11-06T03:29:45Z- handleDVL() - DVL msg epochMillisec: 1730863785.03 2024-11-06T03:29:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:45Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:45Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:45Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:45Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:45Z- handleDvl() - RDI_Pathfinder msg: 1730863785.03 sec; seqNo:2067; #beams:0 2024-11-06T03:29:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , -0.02 2024-11-06T03:29:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863785.04 epoch sec; seqNo:6002; 4061870.51 north; 594592.12 east; 36.70 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T03:29:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863784.27 2024-11-06T03:29:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:45Z- waiting for 1730863788.27, time is 1730863785.04 2024-11-06T03:29:45Z- AHRS_M2 msg: 1730863785.42 epoch sec; seqNo:5635 2024-11-06T03:29:45Z- AHRS_M2 msg: 0.16 phi; 0.03 theta; 4.14 psi 2024-11-06T03:29:45Z- handleDepth()- Depth_Keller msg: 1730863785.44 epoch sec; seqNo:6003; depth 49.40 2024-11-06T03:29:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863785.45 epoch sec; seqNo:6003; 4061870.51 north; 594592.12 east; 36.70 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T03:29:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863784.27 2024-11-06T03:29:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:45Z- waiting for 1730863788.27, time is 1730863785.45 2024-11-06T03:29:45Z- AHRS_M2 msg: 1730863785.83 epoch sec; seqNo:5636 2024-11-06T03:29:45Z- AHRS_M2 msg: 0.16 phi; 0.03 theta; 4.14 psi 2024-11-06T03:29:45Z- handleDepth()- Depth_Keller msg: 1730863785.84 epoch sec; seqNo:6004; depth 49.40 2024-11-06T03:29:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863785.86 epoch sec; seqNo:6004; 4061870.13 north; 594592.13 east; 36.70 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T03:29:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863784.27 2024-11-06T03:29:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:45Z- waiting for 1730863788.27, time is 1730863785.86 2024-11-06T03:29:46Z- AHRS_M2 msg: 1730863786.24 epoch sec; seqNo:5637 2024-11-06T03:29:46Z- AHRS_M2 msg: 0.16 phi; 0.03 theta; 4.14 psi 2024-11-06T03:29:46Z- handleDepth()- Depth_Keller msg: 1730863786.25 epoch sec; seqNo:6005; depth 49.36 2024-11-06T03:29:46Z- handleDVL() - DVL msg epochMillisec: 1730863786.27 2024-11-06T03:29:46Z- handleDvl() - RDI_Pathfinder msg: 1730863786.27 sec; seqNo:2068; #beams:4 2024-11-06T03:29:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.36 , 35.51 , 39.81 , 38.69 2024-11-06T03:29:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.04 , -0.01 2024-11-06T03:29:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863786.29 epoch sec; seqNo:6005; 4061870.12 north; 594590.91 east; 36.70 lat; -121.94 lon; 49.36 depth; utmZone: 10 2024-11-06T03:29:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863784.27 2024-11-06T03:29:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:46Z- waiting for 1730863788.27, time is 1730863786.29 2024-11-06T03:29:46Z- handleDepth()- Depth_Keller msg: 1730863786.67 epoch sec; seqNo:6006; depth 49.34 2024-11-06T03:29:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863786.69 epoch sec; seqNo:6006; 4061869.74 north; 594590.91 east; 36.70 lat; -121.94 lon; 49.34 depth; utmZone: 10 2024-11-06T03:29:46Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:29:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863784.27 2024-11-06T03:29:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:46Z- waiting for 1730863788.27, time is 1730863786.69 2024-11-06T03:29:47Z- AHRS_M2 msg: 1730863787.04 epoch sec; seqNo:5638 2024-11-06T03:29:47Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 4.14 psi 2024-11-06T03:29:47Z- handleDepth()- Depth_Keller msg: 1730863787.04 epoch sec; seqNo:6007; depth 49.31 2024-11-06T03:29:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863787.06 epoch sec; seqNo:6007; 4061869.74 north; 594590.91 east; 36.70 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T03:29:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863784.27 2024-11-06T03:29:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:47Z- waiting for 1730863788.27, time is 1730863787.06 2024-11-06T03:29:47Z- AHRS_M2 msg: 1730863787.44 epoch sec; seqNo:5639 2024-11-06T03:29:47Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 4.14 psi 2024-11-06T03:29:47Z- handleDepth()- Depth_Keller msg: 1730863787.45 epoch sec; seqNo:6008; depth 49.30 2024-11-06T03:29:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863787.46 epoch sec; seqNo:6008; 4061869.36 north; 594590.92 east; 36.70 lat; -121.94 lon; 49.30 depth; utmZone: 10 2024-11-06T03:29:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863784.27 2024-11-06T03:29:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:47Z- waiting for 1730863788.27, time is 1730863787.46 2024-11-06T03:29:47Z- AHRS_M2 msg: 1730863787.85 epoch sec; seqNo:5640 2024-11-06T03:29:47Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 4.14 psi 2024-11-06T03:29:47Z- handleDepth()- Depth_Keller msg: 1730863787.85 epoch sec; seqNo:6009; depth 49.26 2024-11-06T03:29:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863787.86 epoch sec; seqNo:6009; 4061869.35 north; 594589.69 east; 36.70 lat; -121.94 lon; 49.26 depth; utmZone: 10 2024-11-06T03:29:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863784.27 2024-11-06T03:29:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:48Z- waiting for 1730863788.27, time is 1730863787.86 2024-11-06T03:29:48Z- AHRS_M2 msg: 1730863788.25 epoch sec; seqNo:5641 2024-11-06T03:29:48Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 4.14 psi 2024-11-06T03:29:48Z- handleDepth()- Depth_Keller msg: 1730863788.26 epoch sec; seqNo:6010; depth 49.25 2024-11-06T03:29:48Z- handleDVL() - DVL msg epochMillisec: 1730863788.28 2024-11-06T03:29:48Z- handleDvl() - RDI_Pathfinder msg: 1730863788.28 sec; seqNo:2069; #beams:4 2024-11-06T03:29:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.99 , 35.33 , 39.63 , 38.88 2024-11-06T03:29:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.05 , 0.03 2024-11-06T03:29:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863788.30 epoch sec; seqNo:6010; 4061868.97 north; 594589.70 east; 36.70 lat; -121.94 lon; 49.25 depth; utmZone: 10 2024-11-06T03:29:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863784.27 2024-11-06T03:29:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:48Z- waiting for 1730863788.27, time is 1730863788.30 2024-11-06T03:29:48Z- LcmTrn::updateTrn() >>>> pose time 1730863788.30, meas time 1730863788.28 <<<< 2024-11-06T03:29:48Z- LcmTrn::updateTrn() equating pose 1730863788.28 and meas times 1730863788.28 2024-11-06T03:29:48Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863788.28 2024-11-06T03:29:48Z- TerrainNav::motionUpdate() - currEstimate = (4061869, 594589.70, 49.25) 2024-11-06T03:29:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:29:48Z- TerrainNav:: SubcloudNIS is 0.54, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:29:48Z- TNavPF::Projecting Measurements 2024-11-06T03:29:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:29:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:29:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:29:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:29:48Z- TNavFilter::4 good beams. 2024-11-06T03:29:48Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:29:48Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:29:48Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:29:48Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:29:48Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:29:48Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:29:48Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:29:48Z- TerrainMapOctree: (vehN, vehE ) = (4061868.97, 594589.70). 2024-11-06T03:29:48Z- TerrainMapOctree: Min Distance = 199.62. 2024-11-06T03:29:48Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:29:48Z- Forcing Subcloud Comparison 2024-11-06T03:29:48Z- Weighting particles with subcloud comparison 2024-11-06T03:29:48Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:48Z- meanExpectedMeasDiff: -0.536504 delta_rms_squared: 1.014441 alpha: 0.980666 2024-11-06T03:29:48Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:48Z- meanExpectedMeasDiff: -1.784318 delta_rms_squared: 0.692415 alpha: 0.971926 2024-11-06T03:29:48Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:48Z- meanExpectedMeasDiff: -1.339608 delta_rms_squared: 0.845893 alpha: 0.976902 2024-11-06T03:29:48Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:48Z- meanExpectedMeasDiff: 0.898548 delta_rms_squared: 0.901324 alpha: 0.978292 2024-11-06T03:29:48Z- Subcloud NIS: 0.529756 2024-11-06T03:29:48Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:29:48Z- TNavPF:: sumWeights = 0.389163 2024-11-06T03:29:48Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:29:48Z- TNavPF::First Term of Map Covariance Matrix is 0.22 2024-11-06T03:29:48Z- TNavPF::Calculated NIS Value : 0.24 numBeams normalized NIS: 0.06 2024-11-06T03:29:48Z- TerrainNav::Resampling particle filter... 2024-11-06T03:29:48Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863788.28 sec, ping # 0. 2024-11-06T03:29:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:29:48Z- variances([0], [2], and psi_berg) = 0.134376 0.156150 0.000000 2024-11-06T03:29:48Z- publishEstimatesDecoder() - at 1730863788.28 sec 2024-11-06T03:29:48Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:29:48Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061897.64 594572.50 54.60 2024-11-06T03:29:48Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061897.09 594573.61 55.24 lat/lon: 36.70 -121.94 2024-11-06T03:29:48Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.13 0.16 0.34 0.00 2024-11-06T03:29:48Z- AHRS_M2 msg: 1730863788.68 epoch sec; seqNo:5642 2024-11-06T03:29:48Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 4.14 psi 2024-11-06T03:29:48Z- handleDepth()- Depth_Keller msg: 1730863788.69 epoch sec; seqNo:6011; depth 49.24 2024-11-06T03:29:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863788.70 epoch sec; seqNo:6011; 4061868.97 north; 594589.70 east; 36.70 lat; -121.94 lon; 49.24 depth; utmZone: 10 2024-11-06T03:29:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863788.28 2024-11-06T03:29:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:48Z- waiting for 1730863792.28, time is 1730863788.70 2024-11-06T03:29:49Z- AHRS_M2 msg: 1730863789.06 epoch sec; seqNo:5643 2024-11-06T03:29:49Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 4.14 psi 2024-11-06T03:29:49Z- handleDepth()- Depth_Keller msg: 1730863789.06 epoch sec; seqNo:6012; depth 49.20 2024-11-06T03:29:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863789.07 epoch sec; seqNo:6012; 4061868.57 north; 594588.48 east; 36.70 lat; -121.94 lon; 49.20 depth; utmZone: 10 2024-11-06T03:29:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863788.28 2024-11-06T03:29:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:49Z- waiting for 1730863792.28, time is 1730863789.07 2024-11-06T03:29:49Z- AHRS_M2 msg: 1730863789.46 epoch sec; seqNo:5644 2024-11-06T03:29:49Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 4.14 psi 2024-11-06T03:29:49Z- handleDepth()- Depth_Keller msg: 1730863789.47 epoch sec; seqNo:6013; depth 49.20 2024-11-06T03:29:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863789.48 epoch sec; seqNo:6013; 4061868.57 north; 594588.48 east; 36.70 lat; -121.94 lon; 49.20 depth; utmZone: 10 2024-11-06T03:29:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863788.28 2024-11-06T03:29:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:49Z- waiting for 1730863792.28, time is 1730863789.48 2024-11-06T03:29:49Z- AHRS_M2 msg: 1730863789.87 epoch sec; seqNo:5645 2024-11-06T03:29:49Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 4.14 psi 2024-11-06T03:29:49Z- handleDepth()- Depth_Keller msg: 1730863789.88 epoch sec; seqNo:6014; depth 49.18 2024-11-06T03:29:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863789.89 epoch sec; seqNo:6014; 4061868.20 north; 594588.49 east; 36.70 lat; -121.94 lon; 49.18 depth; utmZone: 10 2024-11-06T03:29:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863788.28 2024-11-06T03:29:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:50Z- waiting for 1730863792.28, time is 1730863789.89 2024-11-06T03:29:50Z- AHRS_M2 msg: 1730863790.27 epoch sec; seqNo:5646 2024-11-06T03:29:50Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 4.14 psi 2024-11-06T03:29:50Z- handleDepth()- Depth_Keller msg: 1730863790.28 epoch sec; seqNo:6015; depth 49.18 2024-11-06T03:29:50Z- handleDVL() - DVL msg epochMillisec: 1730863790.29 2024-11-06T03:29:50Z- handleDvl() - RDI_Pathfinder msg: 1730863790.29 sec; seqNo:2070; #beams:4 2024-11-06T03:29:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.18 , 34.95 , 39.44 , 39.25 2024-11-06T03:29:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.02 , 0.05 2024-11-06T03:29:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863790.32 epoch sec; seqNo:6015; 4061867.82 north; 594588.49 east; 36.70 lat; -121.94 lon; 49.18 depth; utmZone: 10 2024-11-06T03:29:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863788.28 2024-11-06T03:29:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:50Z- waiting for 1730863792.28, time is 1730863790.32 2024-11-06T03:29:50Z- AHRS_M2 msg: 1730863790.70 epoch sec; seqNo:5647 2024-11-06T03:29:50Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 4.14 psi 2024-11-06T03:29:50Z- handleDepth()- Depth_Keller msg: 1730863790.71 epoch sec; seqNo:6016; depth 49.17 2024-11-06T03:29:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863790.73 epoch sec; seqNo:6016; 4061867.80 north; 594587.27 east; 36.70 lat; -121.94 lon; 49.17 depth; utmZone: 10 2024-11-06T03:29:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863788.28 2024-11-06T03:29:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:50Z- waiting for 1730863792.28, time is 1730863790.73 2024-11-06T03:29:51Z- AHRS_M2 msg: 1730863791.08 epoch sec; seqNo:5648 2024-11-06T03:29:51Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 4.14 psi 2024-11-06T03:29:51Z- handleDepth()- Depth_Keller msg: 1730863791.09 epoch sec; seqNo:6017; depth 49.16 2024-11-06T03:29:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863791.10 epoch sec; seqNo:6017; 4061867.42 north; 594587.28 east; 36.70 lat; -121.94 lon; 49.16 depth; utmZone: 10 2024-11-06T03:29:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863788.28 2024-11-06T03:29:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:51Z- waiting for 1730863792.28, time is 1730863791.10 2024-11-06T03:29:51Z- AHRS_M2 msg: 1730863791.48 epoch sec; seqNo:5649 2024-11-06T03:29:51Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 4.14 psi 2024-11-06T03:29:51Z- handleDepth()- Depth_Keller msg: 1730863791.49 epoch sec; seqNo:6018; depth 49.15 2024-11-06T03:29:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863791.51 epoch sec; seqNo:6018; 4061867.42 north; 594587.28 east; 36.70 lat; -121.94 lon; 49.15 depth; utmZone: 10 2024-11-06T03:29:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863788.28 2024-11-06T03:29:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:51Z- waiting for 1730863792.28, time is 1730863791.51 2024-11-06T03:29:51Z- AHRS_M2 msg: 1730863791.89 epoch sec; seqNo:5650 2024-11-06T03:29:51Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 4.14 psi 2024-11-06T03:29:51Z- handleDepth()- Depth_Keller msg: 1730863791.90 epoch sec; seqNo:6019; depth 49.15 2024-11-06T03:29:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863791.91 epoch sec; seqNo:6019; 4061867.05 north; 594587.28 east; 36.70 lat; -121.94 lon; 49.15 depth; utmZone: 10 2024-11-06T03:29:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863788.28 2024-11-06T03:29:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:52Z- waiting for 1730863792.28, time is 1730863791.91 2024-11-06T03:29:52Z- AHRS_M2 msg: 1730863792.29 epoch sec; seqNo:5651 2024-11-06T03:29:52Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.14 psi 2024-11-06T03:29:52Z- handleDepth()- Depth_Keller msg: 1730863792.30 epoch sec; seqNo:6020; depth 49.12 2024-11-06T03:29:52Z- handleDVL() - DVL msg epochMillisec: 1730863792.32 2024-11-06T03:29:52Z- handleDvl() - RDI_Pathfinder msg: 1730863792.32 sec; seqNo:2071; #beams:4 2024-11-06T03:29:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.36 , 34.58 , 39.06 , 39.63 2024-11-06T03:29:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.00 , 0.04 2024-11-06T03:29:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863792.35 epoch sec; seqNo:6020; 4061867.03 north; 594586.06 east; 36.70 lat; -121.94 lon; 49.12 depth; utmZone: 10 2024-11-06T03:29:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863788.28 2024-11-06T03:29:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:52Z- waiting for 1730863792.28, time is 1730863792.35 2024-11-06T03:29:52Z- LcmTrn::updateTrn() >>>> pose time 1730863792.35, meas time 1730863792.32 <<<< 2024-11-06T03:29:52Z- LcmTrn::updateTrn() equating pose 1730863792.32 and meas times 1730863792.32 2024-11-06T03:29:52Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863792.32 2024-11-06T03:29:52Z- TerrainNav::motionUpdate() - currEstimate = (4061867, 594586.06, 49.12) 2024-11-06T03:29:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:29:52Z- TerrainNav:: SubcloudNIS is 0.53, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:29:52Z- TNavPF::Projecting Measurements 2024-11-06T03:29:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:29:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:29:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:29:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:29:52Z- TNavFilter::4 good beams. 2024-11-06T03:29:52Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:29:52Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:29:52Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:29:52Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:29:52Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:29:52Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:29:52Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:29:52Z- TerrainMapOctree: (vehN, vehE ) = (4061867.03, 594586.06). 2024-11-06T03:29:52Z- TerrainMapOctree: Min Distance = 201.43. 2024-11-06T03:29:52Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:29:52Z- Forcing Subcloud Comparison 2024-11-06T03:29:52Z- Weighting particles with subcloud comparison 2024-11-06T03:29:52Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:52Z- meanExpectedMeasDiff: -0.089566 delta_rms_squared: 0.977983 alpha: 0.979960 2024-11-06T03:29:52Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:52Z- meanExpectedMeasDiff: -2.354659 delta_rms_squared: 0.663114 alpha: 0.970722 2024-11-06T03:29:52Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:52Z- meanExpectedMeasDiff: -1.262850 delta_rms_squared: 0.792517 alpha: 0.975385 2024-11-06T03:29:52Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:52Z- meanExpectedMeasDiff: 0.302386 delta_rms_squared: 0.663720 alpha: 0.970748 2024-11-06T03:29:52Z- Subcloud NIS: 0.526918 2024-11-06T03:29:52Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:29:52Z- TNavPF:: sumWeights = 0.350160 2024-11-06T03:29:52Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:29:52Z- TNavPF::First Term of Map Covariance Matrix is 0.22 2024-11-06T03:29:52Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.06 2024-11-06T03:29:52Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863792.32 sec, ping # 0. 2024-11-06T03:29:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:29:52Z- variances([0], [2], and psi_berg) = 0.133894 0.156526 0.000000 2024-11-06T03:29:52Z- publishEstimatesDecoder() - at 1730863792.32 sec 2024-11-06T03:29:52Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:29:52Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061895.22 594569.53 55.88 2024-11-06T03:29:52Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061895.16 594569.97 55.44 lat/lon: 36.70 -121.94 2024-11-06T03:29:52Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.13 0.16 0.34 0.00 2024-11-06T03:29:52Z- AHRS_M2 msg: 1730863792.70 epoch sec; seqNo:5652 2024-11-06T03:29:52Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.14 psi 2024-11-06T03:29:52Z- handleDepth()- Depth_Keller msg: 1730863792.71 epoch sec; seqNo:6021; depth 49.13 2024-11-06T03:29:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863792.75 epoch sec; seqNo:6021; 4061866.65 north; 594586.06 east; 36.70 lat; -121.94 lon; 49.13 depth; utmZone: 10 2024-11-06T03:29:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863792.32 2024-11-06T03:29:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:52Z- waiting for 1730863796.32, time is 1730863792.75 2024-11-06T03:29:53Z- AHRS_M2 msg: 1730863793.10 epoch sec; seqNo:5653 2024-11-06T03:29:53Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.14 psi 2024-11-06T03:29:53Z- handleDepth()- Depth_Keller msg: 1730863793.10 epoch sec; seqNo:6022; depth 49.11 2024-11-06T03:29:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863793.12 epoch sec; seqNo:6022; 4061866.65 north; 594586.06 east; 36.70 lat; -121.94 lon; 49.11 depth; utmZone: 10 2024-11-06T03:29:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863792.32 2024-11-06T03:29:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:53Z- waiting for 1730863796.32, time is 1730863793.12 2024-11-06T03:29:53Z- AHRS_M2 msg: 1730863793.50 epoch sec; seqNo:5654 2024-11-06T03:29:53Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.14 psi 2024-11-06T03:29:53Z- handleDepth()- Depth_Keller msg: 1730863793.51 epoch sec; seqNo:6023; depth 49.13 2024-11-06T03:29:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863793.52 epoch sec; seqNo:6023; 4061866.26 north; 594584.85 east; 36.70 lat; -121.94 lon; 49.13 depth; utmZone: 10 2024-11-06T03:29:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863792.32 2024-11-06T03:29:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:53Z- waiting for 1730863796.32, time is 1730863793.52 2024-11-06T03:29:53Z- AHRS_M2 msg: 1730863793.91 epoch sec; seqNo:5655 2024-11-06T03:29:53Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.15 psi 2024-11-06T03:29:53Z- handleDepth()- Depth_Keller msg: 1730863793.92 epoch sec; seqNo:6024; depth 49.11 2024-11-06T03:29:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863793.93 epoch sec; seqNo:6024; 4061866.26 north; 594584.85 east; 36.70 lat; -121.94 lon; 49.11 depth; utmZone: 10 2024-11-06T03:29:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863792.32 2024-11-06T03:29:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:54Z- waiting for 1730863796.32, time is 1730863793.93 2024-11-06T03:29:54Z- AHRS_M2 msg: 1730863794.31 epoch sec; seqNo:5656 2024-11-06T03:29:54Z- AHRS_M2 msg: 0.17 phi; -0.01 theta; 4.15 psi 2024-11-06T03:29:54Z- handleDepth()- Depth_Keller msg: 1730863794.32 epoch sec; seqNo:6025; depth 49.09 2024-11-06T03:29:54Z- handleDVL() - DVL msg epochMillisec: 1730863794.34 2024-11-06T03:29:54Z- handleDvl() - RDI_Pathfinder msg: 1730863794.34 sec; seqNo:2072; #beams:4 2024-11-06T03:29:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.55 , 34.77 , 39.25 , 39.81 2024-11-06T03:29:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , 0.02 2024-11-06T03:29:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863794.36 epoch sec; seqNo:6025; 4061865.88 north; 594584.85 east; 36.70 lat; -121.94 lon; 49.09 depth; utmZone: 10 2024-11-06T03:29:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863792.32 2024-11-06T03:29:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:54Z- waiting for 1730863796.32, time is 1730863794.36 2024-11-06T03:29:54Z- AHRS_M2 msg: 1730863794.71 epoch sec; seqNo:5657 2024-11-06T03:29:54Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 4.15 psi 2024-11-06T03:29:54Z- handleDepth()- Depth_Keller msg: 1730863794.73 epoch sec; seqNo:6026; depth 49.10 2024-11-06T03:29:54Z- handleDVL() - DVL msg epochMillisec: 1730863794.73 2024-11-06T03:29:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:54Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:54Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:54Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:54Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:54Z- handleDvl() - RDI_Pathfinder msg: 1730863794.73 sec; seqNo:2073; #beams:0 2024-11-06T03:29:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , 0.02 2024-11-06T03:29:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863794.76 epoch sec; seqNo:6026; 4061865.50 north; 594584.86 east; 36.70 lat; -121.94 lon; 49.10 depth; utmZone: 10 2024-11-06T03:29:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863792.32 2024-11-06T03:29:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:54Z- waiting for 1730863796.32, time is 1730863794.76 2024-11-06T03:29:55Z- AHRS_M2 msg: 1730863795.12 epoch sec; seqNo:5658 2024-11-06T03:29:55Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 4.15 psi 2024-11-06T03:29:55Z- handleDepth()- Depth_Keller msg: 1730863795.12 epoch sec; seqNo:6027; depth 49.09 2024-11-06T03:29:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863795.13 epoch sec; seqNo:6027; 4061865.49 north; 594583.64 east; 36.70 lat; -121.94 lon; 49.09 depth; utmZone: 10 2024-11-06T03:29:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863792.32 2024-11-06T03:29:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:55Z- waiting for 1730863796.32, time is 1730863795.13 2024-11-06T03:29:55Z- AHRS_M2 msg: 1730863795.52 epoch sec; seqNo:5659 2024-11-06T03:29:55Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.15 psi 2024-11-06T03:29:55Z- handleDepth()- Depth_Keller msg: 1730863795.53 epoch sec; seqNo:6028; depth 49.07 2024-11-06T03:29:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863795.55 epoch sec; seqNo:6028; 4061865.11 north; 594583.64 east; 36.70 lat; -121.94 lon; 49.07 depth; utmZone: 10 2024-11-06T03:29:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863792.32 2024-11-06T03:29:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:55Z- waiting for 1730863796.32, time is 1730863795.55 2024-11-06T03:29:55Z- AHRS_M2 msg: 1730863795.93 epoch sec; seqNo:5660 2024-11-06T03:29:55Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.15 psi 2024-11-06T03:29:55Z- handleDepth()- Depth_Keller msg: 1730863795.93 epoch sec; seqNo:6029; depth 49.08 2024-11-06T03:29:55Z- handleDVL() - DVL msg epochMillisec: 1730863795.94 2024-11-06T03:29:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:55Z- handleDvl() - RDI_Pathfinder msg: 1730863795.94 sec; seqNo:2074; #beams:0 2024-11-06T03:29:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , 0.02 2024-11-06T03:29:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863795.94 epoch sec; seqNo:6029; 4061865.11 north; 594583.64 east; 36.70 lat; -121.94 lon; 49.08 depth; utmZone: 10 2024-11-06T03:29:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863792.32 2024-11-06T03:29:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:56Z- waiting for 1730863796.32, time is 1730863795.94 2024-11-06T03:29:56Z- AHRS_M2 msg: 1730863796.33 epoch sec; seqNo:5661 2024-11-06T03:29:56Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.15 psi 2024-11-06T03:29:56Z- handleDepth()- Depth_Keller msg: 1730863796.35 epoch sec; seqNo:6030; depth 49.06 2024-11-06T03:29:56Z- handleDVL() - DVL msg epochMillisec: 1730863796.40 2024-11-06T03:29:56Z- handleDvl() - RDI_Pathfinder msg: 1730863796.40 sec; seqNo:2075; #beams:4 2024-11-06T03:29:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.74 , 35.14 , 39.06 , 40.19 2024-11-06T03:29:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.02 , 0.03 2024-11-06T03:29:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863796.41 epoch sec; seqNo:6030; 4061864.73 north; 594583.64 east; 36.70 lat; -121.94 lon; 49.06 depth; utmZone: 10 2024-11-06T03:29:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863792.32 2024-11-06T03:29:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:56Z- waiting for 1730863796.32, time is 1730863796.41 2024-11-06T03:29:56Z- LcmTrn::updateTrn() >>>> pose time 1730863796.41, meas time 1730863796.40 <<<< 2024-11-06T03:29:56Z- LcmTrn::updateTrn() equating pose 1730863796.40 and meas times 1730863796.40 2024-11-06T03:29:56Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863796.40 2024-11-06T03:29:56Z- TerrainNav::motionUpdate() - currEstimate = (4061865, 594583.64, 49.06) 2024-11-06T03:29:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:29:56Z- TerrainNav:: SubcloudNIS is 0.53, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:29:56Z- TNavPF::Projecting Measurements 2024-11-06T03:29:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:29:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:29:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:29:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:29:56Z- TNavFilter::4 good beams. 2024-11-06T03:29:56Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:29:56Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:29:56Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:29:56Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:29:56Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:29:56Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:29:56Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:29:56Z- TerrainMapOctree: (vehN, vehE ) = (4061864.73, 594583.64). 2024-11-06T03:29:56Z- TerrainMapOctree: Min Distance = 202.08. 2024-11-06T03:29:56Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:29:56Z- Forcing Subcloud Comparison 2024-11-06T03:29:56Z- Weighting particles with subcloud comparison 2024-11-06T03:29:56Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:56Z- meanExpectedMeasDiff: 0.716346 delta_rms_squared: 0.953125 alpha: 0.979448 2024-11-06T03:29:56Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:56Z- meanExpectedMeasDiff: -1.566400 delta_rms_squared: 0.667538 alpha: 0.970911 2024-11-06T03:29:56Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:56Z- meanExpectedMeasDiff: -0.604984 delta_rms_squared: 0.781360 alpha: 0.975042 2024-11-06T03:29:56Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:29:56Z- meanExpectedMeasDiff: 0.691047 delta_rms_squared: 0.732445 alpha: 0.973420 2024-11-06T03:29:56Z- Subcloud NIS: 0.521616 2024-11-06T03:29:56Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:29:56Z- TNavPF:: sumWeights = 0.478133 2024-11-06T03:29:56Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:29:56Z- TNavPF::First Term of Map Covariance Matrix is 0.36 2024-11-06T03:29:56Z- TNavPF::Calculated NIS Value : 0.31 numBeams normalized NIS: 0.08 2024-11-06T03:29:56Z- TerrainNav::Resampling particle filter... 2024-11-06T03:29:56Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863796.40 sec, ping # 0. 2024-11-06T03:29:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:29:56Z- variances([0], [2], and psi_berg) = 0.136290 0.158686 0.000000 2024-11-06T03:29:56Z- publishEstimatesDecoder() - at 1730863796.40 sec 2024-11-06T03:29:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:29:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061893.45 594567.01 55.24 2024-11-06T03:29:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061892.86 594567.54 55.46 lat/lon: 36.70 -121.94 2024-11-06T03:29:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.16 0.35 0.00 2024-11-06T03:29:56Z- AHRS_M2 msg: 1730863796.73 epoch sec; seqNo:5662 2024-11-06T03:29:56Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.15 psi 2024-11-06T03:29:56Z- handleDepth()- Depth_Keller msg: 1730863796.74 epoch sec; seqNo:6031; depth 49.05 2024-11-06T03:29:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863796.77 epoch sec; seqNo:6031; 4061864.72 north; 594582.42 east; 36.70 lat; -121.94 lon; 49.05 depth; utmZone: 10 2024-11-06T03:29:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863796.40 2024-11-06T03:29:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:56Z- waiting for 1730863800.40, time is 1730863796.77 2024-11-06T03:29:57Z- handleDepth()- Depth_Keller msg: 1730863797.16 epoch sec; seqNo:6032; depth 49.06 2024-11-06T03:29:57Z- handleDVL() - DVL msg epochMillisec: 1730863797.16 2024-11-06T03:29:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:57Z- handleDvl() - RDI_Pathfinder msg: 1730863797.16 sec; seqNo:2076; #beams:0 2024-11-06T03:29:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.02 , 0.03 2024-11-06T03:29:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863797.17 epoch sec; seqNo:6032; 4061864.34 north; 594582.43 east; 36.70 lat; -121.94 lon; 49.06 depth; utmZone: 10 2024-11-06T03:29:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863796.40 2024-11-06T03:29:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:57Z- waiting for 1730863800.40, time is 1730863797.17 2024-11-06T03:29:57Z- AHRS_M2 msg: 1730863797.55 epoch sec; seqNo:5663 2024-11-06T03:29:57Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.15 psi 2024-11-06T03:29:57Z- handleDepth()- Depth_Keller msg: 1730863797.55 epoch sec; seqNo:6033; depth 49.05 2024-11-06T03:29:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863797.56 epoch sec; seqNo:6033; 4061864.34 north; 594582.43 east; 36.70 lat; -121.94 lon; 49.05 depth; utmZone: 10 2024-11-06T03:29:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863796.40 2024-11-06T03:29:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:57Z- waiting for 1730863800.40, time is 1730863797.56 2024-11-06T03:29:57Z- AHRS_M2 msg: 1730863797.95 epoch sec; seqNo:5664 2024-11-06T03:29:57Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.15 psi 2024-11-06T03:29:57Z- handleDepth()- Depth_Keller msg: 1730863797.96 epoch sec; seqNo:6034; depth 49.04 2024-11-06T03:29:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863797.97 epoch sec; seqNo:6034; 4061863.96 north; 594582.43 east; 36.70 lat; -121.94 lon; 49.04 depth; utmZone: 10 2024-11-06T03:29:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863796.40 2024-11-06T03:29:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:58Z- waiting for 1730863800.40, time is 1730863797.97 2024-11-06T03:29:58Z- AHRS_M2 msg: 1730863798.35 epoch sec; seqNo:5665 2024-11-06T03:29:58Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:29:58Z- handleDepth()- Depth_Keller msg: 1730863798.36 epoch sec; seqNo:6035; depth 49.04 2024-11-06T03:29:58Z- handleDVL() - DVL msg epochMillisec: 1730863798.40 2024-11-06T03:29:58Z- handleDvl() - RDI_Pathfinder msg: 1730863798.40 sec; seqNo:2077; #beams:4 2024-11-06T03:29:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.55 , 35.14 , 38.88 , 40.00 2024-11-06T03:29:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , 0.01 2024-11-06T03:29:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863798.41 epoch sec; seqNo:6035; 4061863.57 north; 594581.22 east; 36.70 lat; -121.94 lon; 49.04 depth; utmZone: 10 2024-11-06T03:29:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863796.40 2024-11-06T03:29:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:58Z- waiting for 1730863800.40, time is 1730863798.41 2024-11-06T03:29:58Z- AHRS_M2 msg: 1730863798.77 epoch sec; seqNo:5666 2024-11-06T03:29:58Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:29:58Z- handleDepth()- Depth_Keller msg: 1730863798.77 epoch sec; seqNo:6036; depth 49.01 2024-11-06T03:29:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863798.79 epoch sec; seqNo:6036; 4061863.57 north; 594581.22 east; 36.70 lat; -121.94 lon; 49.01 depth; utmZone: 10 2024-11-06T03:29:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863796.40 2024-11-06T03:29:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:58Z- waiting for 1730863800.40, time is 1730863798.79 2024-11-06T03:29:59Z- AHRS_M2 msg: 1730863799.16 epoch sec; seqNo:5667 2024-11-06T03:29:59Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:29:59Z- handleDepth()- Depth_Keller msg: 1730863799.17 epoch sec; seqNo:6037; depth 49.02 2024-11-06T03:29:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863799.18 epoch sec; seqNo:6037; 4061863.19 north; 594581.22 east; 36.70 lat; -121.94 lon; 49.02 depth; utmZone: 10 2024-11-06T03:29:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:29:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863796.40 2024-11-06T03:29:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:59Z- waiting for 1730863800.40, time is 1730863799.18 2024-11-06T03:29:59Z- AHRS_M2 msg: 1730863799.56 epoch sec; seqNo:5668 2024-11-06T03:29:59Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:29:59Z- handleDepth()- Depth_Keller msg: 1730863799.57 epoch sec; seqNo:6038; depth 49.01 2024-11-06T03:29:59Z- handleDVL() - DVL msg epochMillisec: 1730863799.57 2024-11-06T03:29:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:29:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:29:59Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:29:59Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:29:59Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:29:59Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:29:59Z- handleDvl() - RDI_Pathfinder msg: 1730863799.57 sec; seqNo:2078; #beams:0 2024-11-06T03:29:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:29:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.03 , 0.01 2024-11-06T03:29:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863799.58 epoch sec; seqNo:6038; 4061863.19 north; 594581.22 east; 36.70 lat; -121.94 lon; 49.01 depth; utmZone: 10 2024-11-06T03:29:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:29:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863796.40 2024-11-06T03:29:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:29:59Z- waiting for 1730863800.40, time is 1730863799.58 2024-11-06T03:29:59Z- AHRS_M2 msg: 1730863799.97 epoch sec; seqNo:5669 2024-11-06T03:29:59Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:29:59Z- handleDepth()- Depth_Keller msg: 1730863799.97 epoch sec; seqNo:6039; depth 49.01 2024-11-06T03:29:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863799.98 epoch sec; seqNo:6039; 4061862.80 north; 594580.00 east; 36.70 lat; -121.94 lon; 49.01 depth; utmZone: 10 2024-11-06T03:30:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863796.40 2024-11-06T03:30:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:00Z- waiting for 1730863800.40, time is 1730863799.98 2024-11-06T03:30:00Z- AHRS_M2 msg: 1730863800.37 epoch sec; seqNo:5670 2024-11-06T03:30:00Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:30:00Z- handleDepth()- Depth_Keller msg: 1730863800.38 epoch sec; seqNo:6040; depth 49.02 2024-11-06T03:30:00Z- handleDVL() - DVL msg epochMillisec: 1730863800.40 2024-11-06T03:30:00Z- handleDvl() - RDI_Pathfinder msg: 1730863800.40 sec; seqNo:2079; #beams:4 2024-11-06T03:30:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.55 , 34.39 , 38.50 , 40.37 2024-11-06T03:30:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , -0.00 2024-11-06T03:30:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863800.41 epoch sec; seqNo:6040; 4061862.80 north; 594580.00 east; 36.70 lat; -121.94 lon; 49.02 depth; utmZone: 10 2024-11-06T03:30:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863796.40 2024-11-06T03:30:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:00Z- waiting for 1730863800.40, time is 1730863800.41 2024-11-06T03:30:00Z- LcmTrn::updateTrn() >>>> pose time 1730863800.41, meas time 1730863800.40 <<<< 2024-11-06T03:30:00Z- LcmTrn::updateTrn() equating pose 1730863800.40 and meas times 1730863800.40 2024-11-06T03:30:00Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863800.40 2024-11-06T03:30:00Z- TerrainNav::motionUpdate() - currEstimate = (4061863, 594580.00, 49.02) 2024-11-06T03:30:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:30:00Z- TerrainNav:: SubcloudNIS is 0.52, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:30:00Z- TNavPF::Projecting Measurements 2024-11-06T03:30:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:30:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:30:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:30:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:30:00Z- TNavFilter::4 good beams. 2024-11-06T03:30:00Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:30:00Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:30:00Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:30:00Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:30:00Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:30:00Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:30:00Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:30:00Z- TerrainMapOctree: (vehN, vehE ) = (4061862.80, 594580.00). 2024-11-06T03:30:00Z- TerrainMapOctree: Min Distance = 204.01. 2024-11-06T03:30:00Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:30:00Z- Forcing Subcloud Comparison 2024-11-06T03:30:00Z- Weighting particles with subcloud comparison 2024-11-06T03:30:00Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:00Z- meanExpectedMeasDiff: 0.200889 delta_rms_squared: 0.550471 alpha: 0.964941 2024-11-06T03:30:00Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:00Z- meanExpectedMeasDiff: -2.260556 delta_rms_squared: 0.676912 alpha: 0.971302 2024-11-06T03:30:00Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:00Z- meanExpectedMeasDiff: -0.835866 delta_rms_squared: 0.781332 alpha: 0.975042 2024-11-06T03:30:00Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:00Z- meanExpectedMeasDiff: -0.552355 delta_rms_squared: 0.839843 alpha: 0.976740 2024-11-06T03:30:00Z- Subcloud NIS: 0.520514 2024-11-06T03:30:00Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:30:00Z- TNavPF:: sumWeights = 0.411009 2024-11-06T03:30:00Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:30:00Z- TNavPF::First Term of Map Covariance Matrix is 0.17 2024-11-06T03:30:00Z- TNavPF::Calculated NIS Value : 0.25 numBeams normalized NIS: 0.06 2024-11-06T03:30:00Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863800.40 sec, ping # 0. 2024-11-06T03:30:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:30:00Z- variances([0], [2], and psi_berg) = 0.136751 0.159599 0.000000 2024-11-06T03:30:00Z- publishEstimatesDecoder() - at 1730863800.40 sec 2024-11-06T03:30:00Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:30:00Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061891.09 594563.47 56.37 2024-11-06T03:30:00Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061890.94 594563.90 55.78 lat/lon: 36.70 -121.94 2024-11-06T03:30:00Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.16 0.37 0.00 2024-11-06T03:30:00Z- AHRS_M2 msg: 1730863800.78 epoch sec; seqNo:5671 2024-11-06T03:30:00Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:30:00Z- handleDepth()- Depth_Keller msg: 1730863800.80 epoch sec; seqNo:6041; depth 48.98 2024-11-06T03:30:00Z- handleDVL() - DVL msg epochMillisec: 1730863800.80 2024-11-06T03:30:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:30:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:30:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:30:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:30:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:30:00Z- handleDvl() - RDI_Pathfinder msg: 1730863800.80 sec; seqNo:2080; #beams:0 2024-11-06T03:30:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:30:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , -0.00 2024-11-06T03:30:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863800.82 epoch sec; seqNo:6041; 4061862.42 north; 594580.01 east; 36.70 lat; -121.94 lon; 48.98 depth; utmZone: 10 2024-11-06T03:30:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863800.40 2024-11-06T03:30:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:00Z- waiting for 1730863804.40, time is 1730863800.82 2024-11-06T03:30:01Z- handleDepth()- Depth_Keller msg: 1730863801.20 epoch sec; seqNo:6042; depth 49.00 2024-11-06T03:30:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863801.21 epoch sec; seqNo:6042; 4061862.40 north; 594578.79 east; 36.70 lat; -121.94 lon; 49.00 depth; utmZone: 10 2024-11-06T03:30:01Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:30:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863800.40 2024-11-06T03:30:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:01Z- waiting for 1730863804.40, time is 1730863801.21 2024-11-06T03:30:01Z- AHRS_M2 msg: 1730863801.58 epoch sec; seqNo:5672 2024-11-06T03:30:01Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.17 psi 2024-11-06T03:30:01Z- handleDepth()- Depth_Keller msg: 1730863801.59 epoch sec; seqNo:6043; depth 49.01 2024-11-06T03:30:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863801.60 epoch sec; seqNo:6043; 4061862.03 north; 594578.79 east; 36.70 lat; -121.94 lon; 49.01 depth; utmZone: 10 2024-11-06T03:30:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863800.40 2024-11-06T03:30:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:01Z- waiting for 1730863804.40, time is 1730863801.60 2024-11-06T03:30:02Z- AHRS_M2 msg: 1730863802.00 epoch sec; seqNo:5673 2024-11-06T03:30:02Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.17 psi 2024-11-06T03:30:02Z- handleDepth()- Depth_Keller msg: 1730863802.01 epoch sec; seqNo:6044; depth 48.98 2024-11-06T03:30:02Z- handleDVL() - DVL msg epochMillisec: 1730863802.01 2024-11-06T03:30:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:30:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:30:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:30:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:30:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:30:02Z- handleDvl() - RDI_Pathfinder msg: 1730863802.01 sec; seqNo:2081; #beams:0 2024-11-06T03:30:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:30:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , -0.00 2024-11-06T03:30:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863802.02 epoch sec; seqNo:6044; 4061862.03 north; 594578.79 east; 36.70 lat; -121.94 lon; 48.98 depth; utmZone: 10 2024-11-06T03:30:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863800.40 2024-11-06T03:30:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:02Z- waiting for 1730863804.40, time is 1730863802.02 2024-11-06T03:30:02Z- AHRS_M2 msg: 1730863802.39 epoch sec; seqNo:5674 2024-11-06T03:30:02Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.17 psi 2024-11-06T03:30:02Z- handleDepth()- Depth_Keller msg: 1730863802.40 epoch sec; seqNo:6045; depth 49.00 2024-11-06T03:30:02Z- handleDVL() - DVL msg epochMillisec: 1730863802.43 2024-11-06T03:30:02Z- handleDvl() - RDI_Pathfinder msg: 1730863802.43 sec; seqNo:2082; #beams:4 2024-11-06T03:30:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.55 , 34.95 , 38.50 , 40.56 2024-11-06T03:30:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.03 , -0.00 2024-11-06T03:30:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863802.44 epoch sec; seqNo:6045; 4061861.65 north; 594578.80 east; 36.70 lat; -121.94 lon; 49.00 depth; utmZone: 10 2024-11-06T03:30:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863800.40 2024-11-06T03:30:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:02Z- waiting for 1730863804.40, time is 1730863802.44 2024-11-06T03:30:02Z- AHRS_M2 msg: 1730863802.81 epoch sec; seqNo:5675 2024-11-06T03:30:02Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.16 psi 2024-11-06T03:30:02Z- handleDepth()- Depth_Keller msg: 1730863802.82 epoch sec; seqNo:6046; depth 49.00 2024-11-06T03:30:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863802.83 epoch sec; seqNo:6046; 4061861.63 north; 594577.58 east; 36.70 lat; -121.94 lon; 49.00 depth; utmZone: 10 2024-11-06T03:30:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863800.40 2024-11-06T03:30:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:02Z- waiting for 1730863804.40, time is 1730863802.83 2024-11-06T03:30:03Z- AHRS_M2 msg: 1730863803.20 epoch sec; seqNo:5676 2024-11-06T03:30:03Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.17 psi 2024-11-06T03:30:03Z- handleDepth()- Depth_Keller msg: 1730863803.21 epoch sec; seqNo:6047; depth 49.00 2024-11-06T03:30:03Z- handleDVL() - DVL msg epochMillisec: 1730863803.22 2024-11-06T03:30:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:30:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:30:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:30:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:30:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:30:03Z- handleDvl() - RDI_Pathfinder msg: 1730863803.22 sec; seqNo:2083; #beams:0 2024-11-06T03:30:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:30:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.03 , -0.00 2024-11-06T03:30:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863803.23 epoch sec; seqNo:6047; 4061861.25 north; 594577.58 east; 36.70 lat; -121.94 lon; 49.00 depth; utmZone: 10 2024-11-06T03:30:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863800.40 2024-11-06T03:30:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:03Z- waiting for 1730863804.40, time is 1730863803.23 2024-11-06T03:30:03Z- AHRS_M2 msg: 1730863803.61 epoch sec; seqNo:5677 2024-11-06T03:30:03Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.17 psi 2024-11-06T03:30:03Z- handleDepth()- Depth_Keller msg: 1730863803.62 epoch sec; seqNo:6048; depth 49.02 2024-11-06T03:30:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863803.64 epoch sec; seqNo:6048; 4061861.25 north; 594577.58 east; 36.70 lat; -121.94 lon; 49.02 depth; utmZone: 10 2024-11-06T03:30:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863800.40 2024-11-06T03:30:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:03Z- waiting for 1730863804.40, time is 1730863803.64 2024-11-06T03:30:04Z- AHRS_M2 msg: 1730863804.02 epoch sec; seqNo:5678 2024-11-06T03:30:04Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.17 psi 2024-11-06T03:30:04Z- handleDepth()- Depth_Keller msg: 1730863804.03 epoch sec; seqNo:6049; depth 49.01 2024-11-06T03:30:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863804.04 epoch sec; seqNo:6049; 4061860.88 north; 594577.58 east; 36.70 lat; -121.94 lon; 49.01 depth; utmZone: 10 2024-11-06T03:30:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863800.40 2024-11-06T03:30:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:04Z- waiting for 1730863804.40, time is 1730863804.04 2024-11-06T03:30:04Z- AHRS_M2 msg: 1730863804.41 epoch sec; seqNo:5679 2024-11-06T03:30:04Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.17 psi 2024-11-06T03:30:04Z- handleDepth()- Depth_Keller msg: 1730863804.42 epoch sec; seqNo:6050; depth 49.03 2024-11-06T03:30:04Z- handleDVL() - DVL msg epochMillisec: 1730863804.44 2024-11-06T03:30:04Z- handleDvl() - RDI_Pathfinder msg: 1730863804.44 sec; seqNo:2084; #beams:4 2024-11-06T03:30:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.36 , 34.77 , 38.32 , 40.37 2024-11-06T03:30:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.04 , 0.03 2024-11-06T03:30:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863804.45 epoch sec; seqNo:6050; 4061860.86 north; 594576.36 east; 36.70 lat; -121.94 lon; 49.03 depth; utmZone: 10 2024-11-06T03:30:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863800.40 2024-11-06T03:30:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:04Z- waiting for 1730863804.40, time is 1730863804.45 2024-11-06T03:30:04Z- LcmTrn::updateTrn() >>>> pose time 1730863804.45, meas time 1730863804.44 <<<< 2024-11-06T03:30:04Z- LcmTrn::updateTrn() equating pose 1730863804.44 and meas times 1730863804.44 2024-11-06T03:30:04Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863804.44 2024-11-06T03:30:04Z- TerrainNav::motionUpdate() - currEstimate = (4061861, 594576.36, 49.03) 2024-11-06T03:30:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:30:04Z- TerrainNav:: SubcloudNIS is 0.52, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:30:04Z- TNavPF::Projecting Measurements 2024-11-06T03:30:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:30:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:30:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:30:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:30:04Z- TNavFilter::4 good beams. 2024-11-06T03:30:04Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:30:04Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:30:04Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:30:04Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:30:04Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:30:04Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:30:04Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:30:04Z- TerrainMapOctree: (vehN, vehE ) = (4061860.86, 594576.36). 2024-11-06T03:30:04Z- TerrainMapOctree: Min Distance = 206.01. 2024-11-06T03:30:04Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:30:04Z- Forcing Subcloud Comparison 2024-11-06T03:30:04Z- Weighting particles with subcloud comparison 2024-11-06T03:30:04Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:04Z- meanExpectedMeasDiff: -0.336152 delta_rms_squared: 0.969105 alpha: 0.979780 2024-11-06T03:30:04Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:04Z- meanExpectedMeasDiff: -1.567472 delta_rms_squared: 0.687409 alpha: 0.971728 2024-11-06T03:30:04Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:04Z- meanExpectedMeasDiff: -0.349569 delta_rms_squared: 0.779892 alpha: 0.974997 2024-11-06T03:30:04Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:04Z- meanExpectedMeasDiff: -0.376368 delta_rms_squared: 0.867277 alpha: 0.977459 2024-11-06T03:30:04Z- Subcloud NIS: 0.513204 2024-11-06T03:30:04Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:30:04Z- TNavPF:: sumWeights = 0.567566 2024-11-06T03:30:04Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:30:04Z- TNavPF::First Term of Map Covariance Matrix is 0.32 2024-11-06T03:30:04Z- TNavPF::Calculated NIS Value : 0.24 numBeams normalized NIS: 0.06 2024-11-06T03:30:04Z- TerrainNav::Resampling particle filter... 2024-11-06T03:30:04Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863804.44 sec, ping # 0. 2024-11-06T03:30:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:30:04Z- variances([0], [2], and psi_berg) = 0.138824 0.160668 0.000000 2024-11-06T03:30:04Z- publishEstimatesDecoder() - at 1730863804.44 sec 2024-11-06T03:30:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:30:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061889.59 594559.64 55.42 2024-11-06T03:30:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061889.01 594560.26 56.03 lat/lon: 36.70 -121.94 2024-11-06T03:30:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.16 0.34 0.00 2024-11-06T03:30:04Z- AHRS_M2 msg: 1730863804.82 epoch sec; seqNo:5680 2024-11-06T03:30:04Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.17 psi 2024-11-06T03:30:04Z- handleDepth()- Depth_Keller msg: 1730863804.83 epoch sec; seqNo:6051; depth 49.04 2024-11-06T03:30:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863804.84 epoch sec; seqNo:6051; 4061860.48 north; 594576.37 east; 36.70 lat; -121.94 lon; 49.04 depth; utmZone: 10 2024-11-06T03:30:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863804.44 2024-11-06T03:30:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:04Z- waiting for 1730863808.44, time is 1730863804.84 2024-11-06T03:30:05Z- handleDepth()- Depth_Keller msg: 1730863805.24 epoch sec; seqNo:6052; depth 49.04 2024-11-06T03:30:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863805.25 epoch sec; seqNo:6052; 4061860.48 north; 594576.37 east; 36.70 lat; -121.94 lon; 49.04 depth; utmZone: 10 2024-11-06T03:30:05Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:30:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863804.44 2024-11-06T03:30:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:05Z- waiting for 1730863808.44, time is 1730863805.25 2024-11-06T03:30:05Z- AHRS_M2 msg: 1730863805.63 epoch sec; seqNo:5681 2024-11-06T03:30:05Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.18 psi 2024-11-06T03:30:05Z- handleDepth()- Depth_Keller msg: 1730863805.63 epoch sec; seqNo:6053; depth 49.06 2024-11-06T03:30:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863805.65 epoch sec; seqNo:6053; 4061860.09 north; 594575.15 east; 36.70 lat; -121.94 lon; 49.06 depth; utmZone: 10 2024-11-06T03:30:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863804.44 2024-11-06T03:30:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:05Z- waiting for 1730863808.44, time is 1730863805.65 2024-11-06T03:30:06Z- AHRS_M2 msg: 1730863806.03 epoch sec; seqNo:5682 2024-11-06T03:30:06Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.18 psi 2024-11-06T03:30:06Z- handleDepth()- Depth_Keller msg: 1730863806.03 epoch sec; seqNo:6054; depth 49.08 2024-11-06T03:30:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863806.05 epoch sec; seqNo:6054; 4061860.09 north; 594575.15 east; 36.70 lat; -121.94 lon; 49.08 depth; utmZone: 10 2024-11-06T03:30:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863804.44 2024-11-06T03:30:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:06Z- waiting for 1730863808.44, time is 1730863806.05 2024-11-06T03:30:06Z- AHRS_M2 msg: 1730863806.43 epoch sec; seqNo:5683 2024-11-06T03:30:06Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.18 psi 2024-11-06T03:30:06Z- handleDepth()- Depth_Keller msg: 1730863806.44 epoch sec; seqNo:6055; depth 49.10 2024-11-06T03:30:06Z- handleDVL() - DVL msg epochMillisec: 1730863806.46 2024-11-06T03:30:06Z- handleDvl() - RDI_Pathfinder msg: 1730863806.46 sec; seqNo:2085; #beams:4 2024-11-06T03:30:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.99 , 34.95 , 38.32 , 40.19 2024-11-06T03:30:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.02 , 0.05 2024-11-06T03:30:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863806.47 epoch sec; seqNo:6055; 4061859.71 north; 594575.16 east; 36.70 lat; -121.94 lon; 49.10 depth; utmZone: 10 2024-11-06T03:30:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863804.44 2024-11-06T03:30:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:06Z- waiting for 1730863808.44, time is 1730863806.47 2024-11-06T03:30:06Z- AHRS_M2 msg: 1730863806.84 epoch sec; seqNo:5684 2024-11-06T03:30:06Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.19 psi 2024-11-06T03:30:06Z- handleDepth()- Depth_Keller msg: 1730863806.84 epoch sec; seqNo:6056; depth 49.10 2024-11-06T03:30:06Z- handleDVL() - DVL msg epochMillisec: 1730863806.84 2024-11-06T03:30:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:30:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:30:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:30:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:30:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:30:06Z- handleDvl() - RDI_Pathfinder msg: 1730863806.84 sec; seqNo:2086; #beams:0 2024-11-06T03:30:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:30:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.02 , 0.05 2024-11-06T03:30:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863806.87 epoch sec; seqNo:6056; 4061859.71 north; 594575.16 east; 36.70 lat; -121.94 lon; 49.10 depth; utmZone: 10 2024-11-06T03:30:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863804.44 2024-11-06T03:30:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:06Z- waiting for 1730863808.44, time is 1730863806.87 2024-11-06T03:30:07Z- AHRS_M2 msg: 1730863807.24 epoch sec; seqNo:5685 2024-11-06T03:30:07Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.19 psi 2024-11-06T03:30:07Z- handleDepth()- Depth_Keller msg: 1730863807.25 epoch sec; seqNo:6057; depth 49.11 2024-11-06T03:30:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863807.26 epoch sec; seqNo:6057; 4061859.32 north; 594573.94 east; 36.70 lat; -121.94 lon; 49.11 depth; utmZone: 10 2024-11-06T03:30:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863804.44 2024-11-06T03:30:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:07Z- waiting for 1730863808.44, time is 1730863807.26 2024-11-06T03:30:07Z- AHRS_M2 msg: 1730863807.65 epoch sec; seqNo:5686 2024-11-06T03:30:07Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.19 psi 2024-11-06T03:30:07Z- handleDepth()- Depth_Keller msg: 1730863807.65 epoch sec; seqNo:6058; depth 49.11 2024-11-06T03:30:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863807.66 epoch sec; seqNo:6058; 4061859.32 north; 594573.94 east; 36.70 lat; -121.94 lon; 49.11 depth; utmZone: 10 2024-11-06T03:30:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863804.44 2024-11-06T03:30:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:07Z- waiting for 1730863808.44, time is 1730863807.66 2024-11-06T03:30:08Z- AHRS_M2 msg: 1730863808.05 epoch sec; seqNo:5687 2024-11-06T03:30:08Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.20 psi 2024-11-06T03:30:08Z- handleDepth()- Depth_Keller msg: 1730863808.06 epoch sec; seqNo:6059; depth 49.14 2024-11-06T03:30:08Z- handleDVL() - DVL msg epochMillisec: 1730863808.06 2024-11-06T03:30:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:30:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:30:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:30:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:30:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:30:08Z- handleDvl() - RDI_Pathfinder msg: 1730863808.06 sec; seqNo:2087; #beams:0 2024-11-06T03:30:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:30:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.02 , 0.05 2024-11-06T03:30:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863808.07 epoch sec; seqNo:6059; 4061858.94 north; 594573.94 east; 36.70 lat; -121.94 lon; 49.14 depth; utmZone: 10 2024-11-06T03:30:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863804.44 2024-11-06T03:30:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:08Z- waiting for 1730863808.44, time is 1730863808.07 2024-11-06T03:30:08Z- AHRS_M2 msg: 1730863808.45 epoch sec; seqNo:5688 2024-11-06T03:30:08Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.20 psi 2024-11-06T03:30:08Z- handleDepth()- Depth_Keller msg: 1730863808.45 epoch sec; seqNo:6060; depth 49.14 2024-11-06T03:30:08Z- handleDVL() - DVL msg epochMillisec: 1730863808.48 2024-11-06T03:30:08Z- handleDvl() - RDI_Pathfinder msg: 1730863808.48 sec; seqNo:2088; #beams:4 2024-11-06T03:30:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.36 , 34.58 , 38.13 , 40.37 2024-11-06T03:30:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , 0.06 2024-11-06T03:30:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863808.50 epoch sec; seqNo:6060; 4061858.93 north; 594572.72 east; 36.70 lat; -121.94 lon; 49.14 depth; utmZone: 10 2024-11-06T03:30:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863804.44 2024-11-06T03:30:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:08Z- waiting for 1730863808.44, time is 1730863808.50 2024-11-06T03:30:08Z- LcmTrn::updateTrn() >>>> pose time 1730863808.50, meas time 1730863808.48 <<<< 2024-11-06T03:30:08Z- LcmTrn::updateTrn() equating pose 1730863808.48 and meas times 1730863808.48 2024-11-06T03:30:08Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863808.48 2024-11-06T03:30:08Z- TerrainNav::motionUpdate() - currEstimate = (4061859, 594572.72, 49.14) 2024-11-06T03:30:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:30:08Z- TerrainNav:: SubcloudNIS is 0.51, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:30:08Z- TNavPF::Projecting Measurements 2024-11-06T03:30:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:30:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:30:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:30:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:30:08Z- TNavFilter::4 good beams. 2024-11-06T03:30:08Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:30:08Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:30:08Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:30:08Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:30:08Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:30:08Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:30:08Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:30:08Z- TerrainMapOctree: (vehN, vehE ) = (4061858.93, 594572.72). 2024-11-06T03:30:08Z- TerrainMapOctree: Min Distance = 208.07. 2024-11-06T03:30:08Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:30:08Z- Forcing Subcloud Comparison 2024-11-06T03:30:08Z- Weighting particles with subcloud comparison 2024-11-06T03:30:08Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:08Z- meanExpectedMeasDiff: -0.098456 delta_rms_squared: 0.988405 alpha: 0.980167 2024-11-06T03:30:08Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:08Z- meanExpectedMeasDiff: -1.293760 delta_rms_squared: 0.670155 alpha: 0.971021 2024-11-06T03:30:08Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:08Z- meanExpectedMeasDiff: -0.089381 delta_rms_squared: 0.762091 alpha: 0.974428 2024-11-06T03:30:08Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:08Z- meanExpectedMeasDiff: -0.078059 delta_rms_squared: 0.854887 alpha: 0.977140 2024-11-06T03:30:08Z- Subcloud NIS: 0.494389 2024-11-06T03:30:08Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:30:08Z- TNavPF:: sumWeights = 0.620268 2024-11-06T03:30:08Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:30:08Z- TNavPF::First Term of Map Covariance Matrix is 0.35 2024-11-06T03:30:08Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T03:30:08Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863808.48 sec, ping # 0. 2024-11-06T03:30:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:30:08Z- variances([0], [2], and psi_berg) = 0.141151 0.164520 0.000000 2024-11-06T03:30:08Z- publishEstimatesDecoder() - at 1730863808.48 sec 2024-11-06T03:30:08Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:30:08Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061886.97 594556.64 56.48 2024-11-06T03:30:08Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061887.07 594556.62 56.29 lat/lon: 36.70 -121.94 2024-11-06T03:30:08Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.16 0.34 0.00 2024-11-06T03:30:08Z- AHRS_M2 msg: 1730863808.87 epoch sec; seqNo:5689 2024-11-06T03:30:08Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.21 psi 2024-11-06T03:30:08Z- handleDepth()- Depth_Keller msg: 1730863808.88 epoch sec; seqNo:6061; depth 49.15 2024-11-06T03:30:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863808.90 epoch sec; seqNo:6061; 4061858.55 north; 594572.73 east; 36.70 lat; -121.94 lon; 49.15 depth; utmZone: 10 2024-11-06T03:30:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863808.48 2024-11-06T03:30:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:09Z- waiting for 1730863812.48, time is 1730863808.90 2024-11-06T03:30:09Z- handleDepth()- Depth_Keller msg: 1730863809.28 epoch sec; seqNo:6062; depth 49.16 2024-11-06T03:30:09Z- handleDVL() - DVL msg epochMillisec: 1730863809.28 2024-11-06T03:30:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:30:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:30:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:30:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:30:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:30:09Z- handleDvl() - RDI_Pathfinder msg: 1730863809.28 sec; seqNo:2089; #beams:0 2024-11-06T03:30:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:30:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , 0.06 2024-11-06T03:30:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863809.29 epoch sec; seqNo:6062; 4061858.55 north; 594572.73 east; 36.70 lat; -121.94 lon; 49.16 depth; utmZone: 10 2024-11-06T03:30:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863808.48 2024-11-06T03:30:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:09Z- waiting for 1730863812.48, time is 1730863809.29 2024-11-06T03:30:09Z- AHRS_M2 msg: 1730863809.67 epoch sec; seqNo:5690 2024-11-06T03:30:09Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.21 psi 2024-11-06T03:30:09Z- handleDepth()- Depth_Keller msg: 1730863809.67 epoch sec; seqNo:6063; depth 49.16 2024-11-06T03:30:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863809.69 epoch sec; seqNo:6063; 4061858.17 north; 594572.73 east; 36.70 lat; -121.94 lon; 49.16 depth; utmZone: 10 2024-11-06T03:30:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863808.48 2024-11-06T03:30:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:09Z- waiting for 1730863812.48, time is 1730863809.69 2024-11-06T03:30:10Z- handleDepth()- Depth_Keller msg: 1730863810.09 epoch sec; seqNo:6064; depth 49.16 2024-11-06T03:30:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863810.10 epoch sec; seqNo:6064; 4061858.16 north; 594571.51 east; 36.70 lat; -121.94 lon; 49.16 depth; utmZone: 10 2024-11-06T03:30:10Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:30:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863808.48 2024-11-06T03:30:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:10Z- waiting for 1730863812.48, time is 1730863810.10 2024-11-06T03:30:10Z- AHRS_M2 msg: 1730863810.47 epoch sec; seqNo:5691 2024-11-06T03:30:10Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.22 psi 2024-11-06T03:30:10Z- handleDepth()- Depth_Keller msg: 1730863810.48 epoch sec; seqNo:6065; depth 49.17 2024-11-06T03:30:10Z- handleDVL() - DVL msg epochMillisec: 1730863810.50 2024-11-06T03:30:10Z- handleDvl() - RDI_Pathfinder msg: 1730863810.50 sec; seqNo:2090; #beams:4 2024-11-06T03:30:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.36 , 34.95 , 38.13 , 40.37 2024-11-06T03:30:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.07 , 0.02 2024-11-06T03:30:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863810.52 epoch sec; seqNo:6065; 4061857.78 north; 594571.52 east; 36.70 lat; -121.94 lon; 49.17 depth; utmZone: 10 2024-11-06T03:30:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863808.48 2024-11-06T03:30:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:10Z- waiting for 1730863812.48, time is 1730863810.52 2024-11-06T03:30:10Z- AHRS_M2 msg: 1730863810.88 epoch sec; seqNo:5692 2024-11-06T03:30:10Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.23 psi 2024-11-06T03:30:10Z- handleDepth()- Depth_Keller msg: 1730863810.89 epoch sec; seqNo:6066; depth 49.17 2024-11-06T03:30:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863810.91 epoch sec; seqNo:6066; 4061857.78 north; 594571.52 east; 36.70 lat; -121.94 lon; 49.17 depth; utmZone: 10 2024-11-06T03:30:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863808.48 2024-11-06T03:30:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:11Z- waiting for 1730863812.48, time is 1730863810.91 2024-11-06T03:30:11Z- AHRS_M2 msg: 1730863811.28 epoch sec; seqNo:5693 2024-11-06T03:30:11Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.23 psi 2024-11-06T03:30:11Z- handleDepth()- Depth_Keller msg: 1730863811.28 epoch sec; seqNo:6067; depth 49.19 2024-11-06T03:30:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863811.30 epoch sec; seqNo:6067; 4061857.40 north; 594571.52 east; 36.70 lat; -121.94 lon; 49.19 depth; utmZone: 10 2024-11-06T03:30:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863808.48 2024-11-06T03:30:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:11Z- waiting for 1730863812.48, time is 1730863811.30 2024-11-06T03:30:11Z- AHRS_M2 msg: 1730863811.68 epoch sec; seqNo:5694 2024-11-06T03:30:11Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.24 psi 2024-11-06T03:30:11Z- handleDepth()- Depth_Keller msg: 1730863811.70 epoch sec; seqNo:6068; depth 49.18 2024-11-06T03:30:11Z- handleDVL() - DVL msg epochMillisec: 1730863811.70 2024-11-06T03:30:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:30:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:30:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:30:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:30:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:30:11Z- handleDvl() - RDI_Pathfinder msg: 1730863811.70 sec; seqNo:2091; #beams:0 2024-11-06T03:30:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:30:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.07 , 0.02 2024-11-06T03:30:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863811.72 epoch sec; seqNo:6068; 4061857.01 north; 594570.30 east; 36.70 lat; -121.94 lon; 49.18 depth; utmZone: 10 2024-11-06T03:30:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863808.48 2024-11-06T03:30:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:11Z- waiting for 1730863812.48, time is 1730863811.72 2024-11-06T03:30:12Z- AHRS_M2 msg: 1730863812.09 epoch sec; seqNo:5695 2024-11-06T03:30:12Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.24 psi 2024-11-06T03:30:12Z- handleDepth()- Depth_Keller msg: 1730863812.09 epoch sec; seqNo:6069; depth 49.17 2024-11-06T03:30:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863812.10 epoch sec; seqNo:6069; 4061857.01 north; 594570.30 east; 36.70 lat; -121.94 lon; 49.17 depth; utmZone: 10 2024-11-06T03:30:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863808.48 2024-11-06T03:30:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:12Z- waiting for 1730863812.48, time is 1730863812.10 2024-11-06T03:30:12Z- handleDepth()- Depth_Keller msg: 1730863812.52 epoch sec; seqNo:6070; depth 49.17 2024-11-06T03:30:12Z- handleDVL() - DVL msg epochMillisec: 1730863812.53 2024-11-06T03:30:12Z- handleDvl() - RDI_Pathfinder msg: 1730863812.53 sec; seqNo:2092; #beams:4 2024-11-06T03:30:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.36 , 34.77 , 38.13 , 40.75 2024-11-06T03:30:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.02 2024-11-06T03:30:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863812.55 epoch sec; seqNo:6070; 4061856.63 north; 594570.31 east; 36.70 lat; -121.94 lon; 49.17 depth; utmZone: 10 2024-11-06T03:30:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863808.48 2024-11-06T03:30:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:12Z- waiting for 1730863812.48, time is 1730863812.55 2024-11-06T03:30:12Z- LcmTrn::updateTrn() >>>> pose time 1730863812.55, meas time 1730863812.53 <<<< 2024-11-06T03:30:12Z- LcmTrn::updateTrn() equating pose 1730863812.53 and meas times 1730863812.53 2024-11-06T03:30:12Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863812.53 2024-11-06T03:30:12Z- TerrainNav::motionUpdate() - currEstimate = (4061857, 594570.31, 49.17) 2024-11-06T03:30:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:30:12Z- TerrainNav:: SubcloudNIS is 0.49, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:30:12Z- TNavPF::Projecting Measurements 2024-11-06T03:30:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:30:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:30:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:30:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:30:12Z- TNavFilter::4 good beams. 2024-11-06T03:30:12Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:30:12Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:30:12Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:30:12Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:30:12Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:30:12Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:30:12Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:30:12Z- TerrainMapOctree: (vehN, vehE ) = (4061856.63, 594570.31). 2024-11-06T03:30:12Z- TerrainMapOctree: Min Distance = 208.95. 2024-11-06T03:30:12Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:30:12Z- Forcing Subcloud Comparison 2024-11-06T03:30:12Z- Weighting particles with subcloud comparison 2024-11-06T03:30:12Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:12Z- meanExpectedMeasDiff: 0.172024 delta_rms_squared: 0.963928 alpha: 0.979673 2024-11-06T03:30:12Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:12Z- meanExpectedMeasDiff: -0.927218 delta_rms_squared: 0.659393 alpha: 0.970562 2024-11-06T03:30:12Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:12Z- meanExpectedMeasDiff: 0.137746 delta_rms_squared: 0.748443 alpha: 0.973973 2024-11-06T03:30:12Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:12Z- meanExpectedMeasDiff: 0.501992 delta_rms_squared: 0.841008 alpha: 0.976771 2024-11-06T03:30:12Z- Subcloud NIS: 0.463689 2024-11-06T03:30:12Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:30:12Z- TNavPF:: sumWeights = 0.652491 2024-11-06T03:30:12Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:30:12Z- TNavPF::First Term of Map Covariance Matrix is 0.37 2024-11-06T03:30:12Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T03:30:12Z- TerrainNav::Resampling particle filter... 2024-11-06T03:30:12Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863812.53 sec, ping # 0. 2024-11-06T03:30:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:30:12Z- variances([0], [2], and psi_berg) = 0.141854 0.166193 0.000000 2024-11-06T03:30:12Z- publishEstimatesDecoder() - at 1730863812.53 sec 2024-11-06T03:30:12Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:30:12Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061885.44 594553.78 57.19 2024-11-06T03:30:12Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061884.77 594554.20 56.34 lat/lon: 36.70 -121.94 2024-11-06T03:30:12Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.17 0.33 0.00 2024-11-06T03:30:12Z- AHRS_M2 msg: 1730863812.90 epoch sec; seqNo:5696 2024-11-06T03:30:12Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.24 psi 2024-11-06T03:30:12Z- handleDepth()- Depth_Keller msg: 1730863812.91 epoch sec; seqNo:6071; depth 49.16 2024-11-06T03:30:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863812.92 epoch sec; seqNo:6071; 4061856.63 north; 594570.31 east; 36.70 lat; -121.94 lon; 49.16 depth; utmZone: 10 2024-11-06T03:30:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863812.53 2024-11-06T03:30:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:13Z- waiting for 1730863816.53, time is 1730863812.92 2024-11-06T03:30:13Z- AHRS_M2 msg: 1730863813.30 epoch sec; seqNo:5697 2024-11-06T03:30:13Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.25 psi 2024-11-06T03:30:13Z- handleDepth()- Depth_Keller msg: 1730863813.30 epoch sec; seqNo:6072; depth 49.16 2024-11-06T03:30:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863813.31 epoch sec; seqNo:6072; 4061856.61 north; 594569.09 east; 36.70 lat; -121.94 lon; 49.16 depth; utmZone: 10 2024-11-06T03:30:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863812.53 2024-11-06T03:30:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:13Z- waiting for 1730863816.53, time is 1730863813.31 2024-11-06T03:30:13Z- AHRS_M2 msg: 1730863813.71 epoch sec; seqNo:5698 2024-11-06T03:30:13Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.25 psi 2024-11-06T03:30:13Z- handleDepth()- Depth_Keller msg: 1730863813.71 epoch sec; seqNo:6073; depth 49.17 2024-11-06T03:30:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863813.72 epoch sec; seqNo:6073; 4061856.24 north; 594569.09 east; 36.70 lat; -121.94 lon; 49.17 depth; utmZone: 10 2024-11-06T03:30:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863812.53 2024-11-06T03:30:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:13Z- waiting for 1730863816.53, time is 1730863813.72 2024-11-06T03:30:14Z- AHRS_M2 msg: 1730863814.11 epoch sec; seqNo:5699 2024-11-06T03:30:14Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.25 psi 2024-11-06T03:30:14Z- handleDepth()- Depth_Keller msg: 1730863814.12 epoch sec; seqNo:6074; depth 49.17 2024-11-06T03:30:14Z- handleDVL() - DVL msg epochMillisec: 1730863814.12 2024-11-06T03:30:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:30:14Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:30:14Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:30:14Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:30:14Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:30:14Z- handleDvl() - RDI_Pathfinder msg: 1730863814.12 sec; seqNo:2093; #beams:0 2024-11-06T03:30:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:30:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.02 2024-11-06T03:30:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863814.14 epoch sec; seqNo:6074; 4061856.24 north; 594569.09 east; 36.70 lat; -121.94 lon; 49.17 depth; utmZone: 10 2024-11-06T03:30:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863812.53 2024-11-06T03:30:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:14Z- waiting for 1730863816.53, time is 1730863814.14 2024-11-06T03:30:14Z- AHRS_M2 msg: 1730863814.51 epoch sec; seqNo:5700 2024-11-06T03:30:14Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.25 psi 2024-11-06T03:30:14Z- handleDepth()- Depth_Keller msg: 1730863814.52 epoch sec; seqNo:6075; depth 49.16 2024-11-06T03:30:14Z- handleDVL() - DVL msg epochMillisec: 1730863814.54 2024-11-06T03:30:14Z- handleDvl() - RDI_Pathfinder msg: 1730863814.54 sec; seqNo:2094; #beams:4 2024-11-06T03:30:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.18 , 34.77 , 37.94 , 40.19 2024-11-06T03:30:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.01 , -0.02 2024-11-06T03:30:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863814.56 epoch sec; seqNo:6075; 4061855.86 north; 594569.10 east; 36.70 lat; -121.94 lon; 49.16 depth; utmZone: 10 2024-11-06T03:30:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863812.53 2024-11-06T03:30:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:14Z- waiting for 1730863816.53, time is 1730863814.56 2024-11-06T03:30:14Z- AHRS_M2 msg: 1730863814.92 epoch sec; seqNo:5701 2024-11-06T03:30:14Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.25 psi 2024-11-06T03:30:14Z- handleDepth()- Depth_Keller msg: 1730863814.92 epoch sec; seqNo:6076; depth 49.16 2024-11-06T03:30:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863814.94 epoch sec; seqNo:6076; 4061855.84 north; 594567.88 east; 36.70 lat; -121.94 lon; 49.16 depth; utmZone: 10 2024-11-06T03:30:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863812.53 2024-11-06T03:30:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:15Z- waiting for 1730863816.53, time is 1730863814.94 2024-11-06T03:30:15Z- AHRS_M2 msg: 1730863815.32 epoch sec; seqNo:5702 2024-11-06T03:30:15Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.25 psi 2024-11-06T03:30:15Z- handleDepth()- Depth_Keller msg: 1730863815.33 epoch sec; seqNo:6077; depth 49.15 2024-11-06T03:30:15Z- handleDVL() - DVL msg epochMillisec: 1730863815.33 2024-11-06T03:30:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:30:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:30:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:30:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:30:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:30:15Z- handleDvl() - RDI_Pathfinder msg: 1730863815.33 sec; seqNo:2095; #beams:0 2024-11-06T03:30:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:30:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.01 , -0.02 2024-11-06T03:30:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863815.34 epoch sec; seqNo:6077; 4061855.46 north; 594567.88 east; 36.70 lat; -121.94 lon; 49.15 depth; utmZone: 10 2024-11-06T03:30:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863812.53 2024-11-06T03:30:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:15Z- waiting for 1730863816.53, time is 1730863815.34 2024-11-06T03:30:15Z- AHRS_M2 msg: 1730863815.73 epoch sec; seqNo:5703 2024-11-06T03:30:15Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.25 psi 2024-11-06T03:30:15Z- handleDepth()- Depth_Keller msg: 1730863815.73 epoch sec; seqNo:6078; depth 49.16 2024-11-06T03:30:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863815.75 epoch sec; seqNo:6078; 4061855.46 north; 594567.88 east; 36.70 lat; -121.94 lon; 49.16 depth; utmZone: 10 2024-11-06T03:30:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863812.53 2024-11-06T03:30:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:15Z- waiting for 1730863816.53, time is 1730863815.75 2024-11-06T03:30:16Z- AHRS_M2 msg: 1730863816.13 epoch sec; seqNo:5704 2024-11-06T03:30:16Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.26 psi 2024-11-06T03:30:16Z- handleDepth()- Depth_Keller msg: 1730863816.13 epoch sec; seqNo:6079; depth 49.16 2024-11-06T03:30:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863816.15 epoch sec; seqNo:6079; 4061855.07 north; 594566.66 east; 36.70 lat; -121.94 lon; 49.16 depth; utmZone: 10 2024-11-06T03:30:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863812.53 2024-11-06T03:30:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:16Z- waiting for 1730863816.53, time is 1730863816.15 2024-11-06T03:30:16Z- AHRS_M2 msg: 1730863816.53 epoch sec; seqNo:5705 2024-11-06T03:30:16Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 4.26 psi 2024-11-06T03:30:16Z- handleDepth()- Depth_Keller msg: 1730863816.54 epoch sec; seqNo:6080; depth 49.17 2024-11-06T03:30:16Z- handleDVL() - DVL msg epochMillisec: 1730863816.56 2024-11-06T03:30:16Z- handleDvl() - RDI_Pathfinder msg: 1730863816.56 sec; seqNo:2096; #beams:4 2024-11-06T03:30:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.61 , 34.39 , 37.57 , 40.00 2024-11-06T03:30:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.04 , -0.00 2024-11-06T03:30:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863816.57 epoch sec; seqNo:6080; 4061855.07 north; 594566.66 east; 36.70 lat; -121.94 lon; 49.17 depth; utmZone: 10 2024-11-06T03:30:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863812.53 2024-11-06T03:30:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:16Z- waiting for 1730863816.53, time is 1730863816.57 2024-11-06T03:30:16Z- LcmTrn::updateTrn() >>>> pose time 1730863816.57, meas time 1730863816.56 <<<< 2024-11-06T03:30:16Z- LcmTrn::updateTrn() equating pose 1730863816.56 and meas times 1730863816.56 2024-11-06T03:30:16Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863816.56 2024-11-06T03:30:16Z- TerrainNav::motionUpdate() - currEstimate = (4061855, 594566.66, 49.17) 2024-11-06T03:30:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:30:16Z- TerrainNav:: SubcloudNIS is 0.46, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:30:16Z- TNavPF::Projecting Measurements 2024-11-06T03:30:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:30:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:30:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:30:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:30:16Z- TNavFilter::4 good beams. 2024-11-06T03:30:16Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:30:16Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:30:16Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:30:16Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:30:16Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:30:16Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:30:16Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:30:16Z- TerrainMapOctree: (vehN, vehE ) = (4061855.07, 594566.66). 2024-11-06T03:30:16Z- TerrainMapOctree: Min Distance = 211.31. 2024-11-06T03:30:16Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:30:16Z- Forcing Subcloud Comparison 2024-11-06T03:30:16Z- Weighting particles with subcloud comparison 2024-11-06T03:30:16Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:16Z- meanExpectedMeasDiff: 0.113294 delta_rms_squared: 0.877219 alpha: 0.977709 2024-11-06T03:30:16Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:16Z- meanExpectedMeasDiff: -1.333932 delta_rms_squared: 0.663524 alpha: 0.970740 2024-11-06T03:30:16Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:16Z- meanExpectedMeasDiff: -0.244531 delta_rms_squared: 0.744322 alpha: 0.973833 2024-11-06T03:30:16Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:16Z- meanExpectedMeasDiff: -0.379853 delta_rms_squared: 0.850404 alpha: 0.977022 2024-11-06T03:30:16Z- Subcloud NIS: 0.443962 2024-11-06T03:30:16Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:30:16Z- TNavPF:: sumWeights = 0.603385 2024-11-06T03:30:16Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:30:16Z- TNavPF::First Term of Map Covariance Matrix is 0.37 2024-11-06T03:30:16Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T03:30:16Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863816.56 sec, ping # 0. 2024-11-06T03:30:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:30:16Z- variances([0], [2], and psi_berg) = 0.141699 0.169831 0.000000 2024-11-06T03:30:16Z- publishEstimatesDecoder() - at 1730863816.56 sec 2024-11-06T03:30:16Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:30:16Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061883.97 594550.16 56.86 2024-11-06T03:30:16Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061883.23 594550.56 56.53 lat/lon: 36.70 -121.94 2024-11-06T03:30:16Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.17 0.36 0.00 2024-11-06T03:30:16Z- AHRS_M2 msg: 1730863816.94 epoch sec; seqNo:5706 2024-11-06T03:30:16Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 4.26 psi 2024-11-06T03:30:16Z- handleDepth()- Depth_Keller msg: 1730863816.95 epoch sec; seqNo:6081; depth 49.17 2024-11-06T03:30:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863816.97 epoch sec; seqNo:6081; 4061854.69 north; 594566.67 east; 36.70 lat; -121.94 lon; 49.17 depth; utmZone: 10 2024-11-06T03:30:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863816.56 2024-11-06T03:30:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:17Z- waiting for 1730863820.56, time is 1730863816.97 2024-11-06T03:30:17Z- AHRS_M2 msg: 1730863817.34 epoch sec; seqNo:5707 2024-11-06T03:30:17Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 4.26 psi 2024-11-06T03:30:17Z- handleDepth()- Depth_Keller msg: 1730863817.35 epoch sec; seqNo:6082; depth 49.19 2024-11-06T03:30:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863817.37 epoch sec; seqNo:6082; 4061854.69 north; 594566.67 east; 36.70 lat; -121.94 lon; 49.19 depth; utmZone: 10 2024-11-06T03:30:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863816.56 2024-11-06T03:30:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:17Z- waiting for 1730863820.56, time is 1730863817.37 2024-11-06T03:30:17Z- AHRS_M2 msg: 1730863817.74 epoch sec; seqNo:5708 2024-11-06T03:30:17Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 4.26 psi 2024-11-06T03:30:17Z- handleDepth()- Depth_Keller msg: 1730863817.75 epoch sec; seqNo:6083; depth 49.21 2024-11-06T03:30:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863817.76 epoch sec; seqNo:6083; 4061854.68 north; 594565.45 east; 36.70 lat; -121.94 lon; 49.21 depth; utmZone: 10 2024-11-06T03:30:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863816.56 2024-11-06T03:30:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:17Z- waiting for 1730863820.56, time is 1730863817.76 2024-11-06T03:30:18Z- AHRS_M2 msg: 1730863818.17 epoch sec; seqNo:5709 2024-11-06T03:30:18Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 4.25 psi 2024-11-06T03:30:18Z- handleDepth()- Depth_Keller msg: 1730863818.18 epoch sec; seqNo:6084; depth 49.21 2024-11-06T03:30:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863818.23 epoch sec; seqNo:6084; 4061854.30 north; 594565.45 east; 36.70 lat; -121.94 lon; 49.21 depth; utmZone: 10 2024-11-06T03:30:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863816.56 2024-11-06T03:30:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:18Z- waiting for 1730863820.56, time is 1730863818.23 2024-11-06T03:30:18Z- AHRS_M2 msg: 1730863818.55 epoch sec; seqNo:5710 2024-11-06T03:30:18Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 4.25 psi 2024-11-06T03:30:18Z- handleDepth()- Depth_Keller msg: 1730863818.56 epoch sec; seqNo:6085; depth 49.24 2024-11-06T03:30:18Z- handleDVL() - DVL msg epochMillisec: 1730863818.58 2024-11-06T03:30:18Z- handleDvl() - RDI_Pathfinder msg: 1730863818.58 sec; seqNo:2097; #beams:4 2024-11-06T03:30:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.80 , 34.58 , 37.20 , 40.56 2024-11-06T03:30:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.04 , 0.05 2024-11-06T03:30:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863818.59 epoch sec; seqNo:6085; 4061854.30 north; 594565.45 east; 36.70 lat; -121.94 lon; 49.24 depth; utmZone: 10 2024-11-06T03:30:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863816.56 2024-11-06T03:30:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:18Z- waiting for 1730863820.56, time is 1730863818.59 2024-11-06T03:30:18Z- AHRS_M2 msg: 1730863818.95 epoch sec; seqNo:5711 2024-11-06T03:30:18Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 4.25 psi 2024-11-06T03:30:18Z- handleDepth()- Depth_Keller msg: 1730863818.96 epoch sec; seqNo:6086; depth 49.27 2024-11-06T03:30:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863818.98 epoch sec; seqNo:6086; 4061853.91 north; 594564.24 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T03:30:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863816.56 2024-11-06T03:30:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:19Z- waiting for 1730863820.56, time is 1730863818.98 2024-11-06T03:30:19Z- handleDepth()- Depth_Keller msg: 1730863819.38 epoch sec; seqNo:6087; depth 49.27 2024-11-06T03:30:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863819.39 epoch sec; seqNo:6087; 4061853.91 north; 594564.24 east; 36.70 lat; -121.94 lon; 49.27 depth; utmZone: 10 2024-11-06T03:30:19Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:30:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863816.56 2024-11-06T03:30:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:19Z- waiting for 1730863820.56, time is 1730863819.39 2024-11-06T03:30:19Z- AHRS_M2 msg: 1730863819.77 epoch sec; seqNo:5712 2024-11-06T03:30:19Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 4.24 psi 2024-11-06T03:30:19Z- handleDepth()- Depth_Keller msg: 1730863819.78 epoch sec; seqNo:6088; depth 49.29 2024-11-06T03:30:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863819.80 epoch sec; seqNo:6088; 4061853.53 north; 594564.24 east; 36.70 lat; -121.94 lon; 49.29 depth; utmZone: 10 2024-11-06T03:30:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863816.56 2024-11-06T03:30:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:19Z- waiting for 1730863820.56, time is 1730863819.80 2024-11-06T03:30:20Z- AHRS_M2 msg: 1730863820.21 epoch sec; seqNo:5713 2024-11-06T03:30:20Z- AHRS_M2 msg: 0.14 phi; -0.08 theta; 4.24 psi 2024-11-06T03:30:20Z- handleDepth()- Depth_Keller msg: 1730863820.23 epoch sec; seqNo:6089; depth 49.34 2024-11-06T03:30:20Z- handleDVL() - DVL msg epochMillisec: 1730863820.25 2024-11-06T03:30:20Z- handleDvl() - RDI_Pathfinder msg: 1730863820.25 sec; seqNo:2098; #beams:4 2024-11-06T03:30:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.62 , 34.58 , 36.82 , 40.37 2024-11-06T03:30:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , 0.08 2024-11-06T03:30:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863820.27 epoch sec; seqNo:6089; 4061853.52 north; 594563.02 east; 36.70 lat; -121.94 lon; 49.34 depth; utmZone: 10 2024-11-06T03:30:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863816.56 2024-11-06T03:30:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:20Z- waiting for 1730863820.56, time is 1730863820.27 2024-11-06T03:30:20Z- AHRS_M2 msg: 1730863820.58 epoch sec; seqNo:5714 2024-11-06T03:30:20Z- AHRS_M2 msg: 0.14 phi; -0.09 theta; 4.24 psi 2024-11-06T03:30:20Z- handleDepth()- Depth_Keller msg: 1730863820.58 epoch sec; seqNo:6090; depth 49.35 2024-11-06T03:30:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863820.61 epoch sec; seqNo:6090; 4061853.14 north; 594563.02 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T03:30:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863816.56 2024-11-06T03:30:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:20Z- waiting for 1730863820.56, time is 1730863820.61 2024-11-06T03:30:20Z- LcmTrn::updateTrn() >>>> pose time 1730863820.61, meas time 1730863820.25 <<<< 2024-11-06T03:30:20Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863820.61 2024-11-06T03:30:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863820.25 sec, ping # 0; waiting for INS data more recent than 1730863816.56... 2024-11-06T03:30:20Z- TerrainNav::motionUpdate() - currEstimate = (4061853, 594563.02, 49.35) 2024-11-06T03:30:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:30:20Z- TerrainNav:: SubcloudNIS is 0.44, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:30:20Z- TNavPF::Projecting Measurements 2024-11-06T03:30:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:30:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:30:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:30:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:30:20Z- TNavFilter::4 good beams. 2024-11-06T03:30:20Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:30:20Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:30:20Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:30:20Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:30:20Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:30:20Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:30:20Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:30:20Z- TerrainMapOctree: (vehN, vehE ) = (4061853.31, 594563.35). 2024-11-06T03:30:20Z- TerrainMapOctree: Min Distance = 213.34. 2024-11-06T03:30:20Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:30:20Z- Forcing Subcloud Comparison 2024-11-06T03:30:20Z- Weighting particles with subcloud comparison 2024-11-06T03:30:20Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:20Z- meanExpectedMeasDiff: 0.851575 delta_rms_squared: 0.670229 alpha: 0.971024 2024-11-06T03:30:20Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:20Z- meanExpectedMeasDiff: -1.144295 delta_rms_squared: 0.703792 alpha: 0.972368 2024-11-06T03:30:20Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:20Z- meanExpectedMeasDiff: 0.425124 delta_rms_squared: 0.504457 alpha: 0.961865 2024-11-06T03:30:20Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:20Z- meanExpectedMeasDiff: -0.181451 delta_rms_squared: 0.909955 alpha: 0.978494 2024-11-06T03:30:20Z- Subcloud NIS: 0.411263 2024-11-06T03:30:20Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:30:20Z- TNavPF:: sumWeights = 0.589433 2024-11-06T03:30:20Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:30:20Z- TNavPF::First Term of Map Covariance Matrix is 0.33 2024-11-06T03:30:20Z- TNavPF::Calculated NIS Value : 0.30 numBeams normalized NIS: 0.07 2024-11-06T03:30:20Z- TerrainNav::Resampling particle filter... 2024-11-06T03:30:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863820.25 sec. 2024-11-06T03:30:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:30:20Z- variances([0], [2], and psi_berg) = 0.136751 0.171074 0.000000 2024-11-06T03:30:20Z- publishEstimatesDecoder() - at 1730863820.61 sec 2024-11-06T03:30:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:30:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061882.04 594546.52 56.47 2024-11-06T03:30:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061881.31 594546.94 56.75 lat/lon: 36.70 -121.94 2024-11-06T03:30:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.17 0.62 0.00 2024-11-06T03:30:20Z- AHRS_M2 msg: 1730863820.98 epoch sec; seqNo:5715 2024-11-06T03:30:20Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 4.24 psi 2024-11-06T03:30:20Z- handleDepth()- Depth_Keller msg: 1730863820.99 epoch sec; seqNo:6091; depth 49.37 2024-11-06T03:30:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863821.01 epoch sec; seqNo:6091; 4061853.14 north; 594563.02 east; 36.70 lat; -121.94 lon; 49.37 depth; utmZone: 10 2024-11-06T03:30:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863820.61 2024-11-06T03:30:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:21Z- waiting for 1730863824.61, time is 1730863821.01 2024-11-06T03:30:21Z- AHRS_M2 msg: 1730863821.38 epoch sec; seqNo:5716 2024-11-06T03:30:21Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 4.24 psi 2024-11-06T03:30:21Z- handleDepth()- Depth_Keller msg: 1730863821.39 epoch sec; seqNo:6092; depth 49.40 2024-11-06T03:30:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863821.40 epoch sec; seqNo:6092; 4061853.14 north; 594563.02 east; 36.70 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T03:30:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863820.61 2024-11-06T03:30:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:21Z- waiting for 1730863824.61, time is 1730863821.40 2024-11-06T03:30:21Z- AHRS_M2 msg: 1730863821.78 epoch sec; seqNo:5717 2024-11-06T03:30:21Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 4.24 psi 2024-11-06T03:30:21Z- handleDepth()- Depth_Keller msg: 1730863821.79 epoch sec; seqNo:6093; depth 49.44 2024-11-06T03:30:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863821.81 epoch sec; seqNo:6093; 4061852.75 north; 594561.81 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:30:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863820.61 2024-11-06T03:30:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:21Z- waiting for 1730863824.61, time is 1730863821.81 2024-11-06T03:30:22Z- AHRS_M2 msg: 1730863822.22 epoch sec; seqNo:5718 2024-11-06T03:30:22Z- AHRS_M2 msg: 0.13 phi; -0.10 theta; 4.25 psi 2024-11-06T03:30:22Z- handleDepth()- Depth_Keller msg: 1730863822.23 epoch sec; seqNo:6094; depth 49.45 2024-11-06T03:30:22Z- handleDVL() - DVL msg epochMillisec: 1730863822.25 2024-11-06T03:30:22Z- handleDvl() - RDI_Pathfinder msg: 1730863822.25 sec; seqNo:2099; #beams:4 2024-11-06T03:30:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 35.14 , 36.63 , 40.75 2024-11-06T03:30:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , 0.06 2024-11-06T03:30:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863822.28 epoch sec; seqNo:6094; 4061852.75 north; 594561.81 east; 36.70 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:30:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863820.61 2024-11-06T03:30:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:22Z- waiting for 1730863824.61, time is 1730863822.28 2024-11-06T03:30:22Z- AHRS_M2 msg: 1730863822.59 epoch sec; seqNo:5719 2024-11-06T03:30:22Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 4.25 psi 2024-11-06T03:30:22Z- handleDepth()- Depth_Keller msg: 1730863822.60 epoch sec; seqNo:6095; depth 49.49 2024-11-06T03:30:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863822.62 epoch sec; seqNo:6095; 4061852.37 north; 594561.81 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:30:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863820.61 2024-11-06T03:30:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:22Z- waiting for 1730863824.61, time is 1730863822.62 2024-11-06T03:30:23Z- AHRS_M2 msg: 1730863822.99 epoch sec; seqNo:5720 2024-11-06T03:30:23Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 4.25 psi 2024-11-06T03:30:23Z- handleDepth()- Depth_Keller msg: 1730863823.00 epoch sec; seqNo:6096; depth 49.52 2024-11-06T03:30:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863823.01 epoch sec; seqNo:6096; 4061852.37 north; 594561.81 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T03:30:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863820.61 2024-11-06T03:30:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:23Z- waiting for 1730863824.61, time is 1730863823.01 2024-11-06T03:30:23Z- AHRS_M2 msg: 1730863823.40 epoch sec; seqNo:5721 2024-11-06T03:30:23Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 4.26 psi 2024-11-06T03:30:23Z- handleDepth()- Depth_Keller msg: 1730863823.41 epoch sec; seqNo:6097; depth 49.52 2024-11-06T03:30:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863823.42 epoch sec; seqNo:6097; 4061851.97 north; 594560.60 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T03:30:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863820.61 2024-11-06T03:30:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:23Z- waiting for 1730863824.61, time is 1730863823.42 2024-11-06T03:30:23Z- AHRS_M2 msg: 1730863823.80 epoch sec; seqNo:5722 2024-11-06T03:30:23Z- AHRS_M2 msg: 0.15 phi; -0.11 theta; 4.26 psi 2024-11-06T03:30:23Z- handleDepth()- Depth_Keller msg: 1730863823.81 epoch sec; seqNo:6098; depth 49.55 2024-11-06T03:30:23Z- handleDVL() - DVL msg epochMillisec: 1730863823.82 2024-11-06T03:30:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:30:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:30:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:30:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:30:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:30:23Z- handleDvl() - RDI_Pathfinder msg: 1730863823.82 sec; seqNo:2100; #beams:0 2024-11-06T03:30:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:30:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , 0.06 2024-11-06T03:30:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863823.83 epoch sec; seqNo:6098; 4061851.97 north; 594560.60 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:30:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863820.61 2024-11-06T03:30:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:23Z- waiting for 1730863824.61, time is 1730863823.83 2024-11-06T03:30:24Z- AHRS_M2 msg: 1730863824.23 epoch sec; seqNo:5723 2024-11-06T03:30:24Z- AHRS_M2 msg: 0.15 phi; -0.11 theta; 4.26 psi 2024-11-06T03:30:24Z- handleDepth()- Depth_Keller msg: 1730863824.23 epoch sec; seqNo:6099; depth 49.58 2024-11-06T03:30:24Z- handleDVL() - DVL msg epochMillisec: 1730863824.27 2024-11-06T03:30:24Z- handleDvl() - RDI_Pathfinder msg: 1730863824.27 sec; seqNo:2101; #beams:4 2024-11-06T03:30:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.06 , 35.14 , 36.26 , 41.49 2024-11-06T03:30:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , 0.01 2024-11-06T03:30:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863824.28 epoch sec; seqNo:6099; 4061851.60 north; 594560.60 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:30:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863820.61 2024-11-06T03:30:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:24Z- waiting for 1730863824.61, time is 1730863824.28 2024-11-06T03:30:24Z- AHRS_M2 msg: 1730863824.62 epoch sec; seqNo:5724 2024-11-06T03:30:24Z- AHRS_M2 msg: 0.16 phi; -0.11 theta; 4.26 psi 2024-11-06T03:30:24Z- handleDepth()- Depth_Keller msg: 1730863824.62 epoch sec; seqNo:6100; depth 49.62 2024-11-06T03:30:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863824.64 epoch sec; seqNo:6100; 4061851.60 north; 594560.60 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:30:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863820.61 2024-11-06T03:30:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:24Z- waiting for 1730863824.61, time is 1730863824.64 2024-11-06T03:30:24Z- LcmTrn::updateTrn() >>>> pose time 1730863824.64, meas time 1730863824.27 <<<< 2024-11-06T03:30:24Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863824.64 2024-11-06T03:30:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863824.27 sec, ping # 0; waiting for INS data more recent than 1730863820.61... 2024-11-06T03:30:24Z- TerrainNav::motionUpdate() - currEstimate = (4061852, 594560.60, 49.62) 2024-11-06T03:30:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:30:24Z- TerrainNav:: SubcloudNIS is 0.41, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:30:24Z- TNavPF::Projecting Measurements 2024-11-06T03:30:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:30:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:30:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:30:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:30:24Z- TNavFilter::4 good beams. 2024-11-06T03:30:24Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:30:24Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:30:24Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:30:24Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:30:24Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:30:24Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:30:24Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:30:24Z- TerrainMapOctree: (vehN, vehE ) = (4061851.74, 594560.82). 2024-11-06T03:30:24Z- TerrainMapOctree: Min Distance = 214.80. 2024-11-06T03:30:24Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:30:24Z- Forcing Subcloud Comparison 2024-11-06T03:30:24Z- Weighting particles with subcloud comparison 2024-11-06T03:30:24Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:24Z- meanExpectedMeasDiff: 1.107383 delta_rms_squared: 1.426195 alpha: 0.986171 2024-11-06T03:30:24Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:24Z- meanExpectedMeasDiff: -0.100496 delta_rms_squared: 0.996794 alpha: 0.980330 2024-11-06T03:30:24Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:24Z- meanExpectedMeasDiff: 1.207372 delta_rms_squared: 1.041151 alpha: 0.981153 2024-11-06T03:30:24Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:24Z- meanExpectedMeasDiff: 0.504908 delta_rms_squared: 1.352117 alpha: 0.985424 2024-11-06T03:30:24Z- Subcloud NIS: 0.388139 2024-11-06T03:30:24Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:30:24Z- TNavPF:: sumWeights = 0.538169 2024-11-06T03:30:24Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:30:24Z- TNavPF::First Term of Map Covariance Matrix is 0.42 2024-11-06T03:30:24Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T03:30:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863824.27 sec. 2024-11-06T03:30:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:30:24Z- variances([0], [2], and psi_berg) = 0.141746 0.170142 0.000000 2024-11-06T03:30:24Z- publishEstimatesDecoder() - at 1730863824.64 sec 2024-11-06T03:30:24Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:30:24Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061881.36 594544.90 56.84 2024-11-06T03:30:24Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061879.77 594544.51 56.72 lat/lon: 36.70 -121.94 2024-11-06T03:30:24Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.17 0.64 0.00 2024-11-06T03:30:25Z- AHRS_M2 msg: 1730863825.02 epoch sec; seqNo:5725 2024-11-06T03:30:25Z- AHRS_M2 msg: 0.16 phi; -0.11 theta; 4.27 psi 2024-11-06T03:30:25Z- handleDepth()- Depth_Keller msg: 1730863825.02 epoch sec; seqNo:6101; depth 49.63 2024-11-06T03:30:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863825.03 epoch sec; seqNo:6101; 4061851.20 north; 594559.38 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:30:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863824.64 2024-11-06T03:30:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:25Z- waiting for 1730863828.64, time is 1730863825.03 2024-11-06T03:30:25Z- AHRS_M2 msg: 1730863825.42 epoch sec; seqNo:5726 2024-11-06T03:30:25Z- AHRS_M2 msg: 0.16 phi; -0.11 theta; 4.27 psi 2024-11-06T03:30:25Z- handleDepth()- Depth_Keller msg: 1730863825.45 epoch sec; seqNo:6102; depth 49.66 2024-11-06T03:30:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863825.46 epoch sec; seqNo:6102; 4061851.20 north; 594559.38 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:30:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863824.64 2024-11-06T03:30:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:25Z- waiting for 1730863828.64, time is 1730863825.46 2024-11-06T03:30:25Z- AHRS_M2 msg: 1730863825.83 epoch sec; seqNo:5727 2024-11-06T03:30:25Z- AHRS_M2 msg: 0.16 phi; -0.11 theta; 4.27 psi 2024-11-06T03:30:25Z- handleDepth()- Depth_Keller msg: 1730863825.83 epoch sec; seqNo:6103; depth 49.68 2024-11-06T03:30:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863825.84 epoch sec; seqNo:6103; 4061850.82 north; 594559.39 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:30:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863824.64 2024-11-06T03:30:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:25Z- waiting for 1730863828.64, time is 1730863825.84 2024-11-06T03:30:26Z- AHRS_M2 msg: 1730863826.24 epoch sec; seqNo:5728 2024-11-06T03:30:26Z- AHRS_M2 msg: 0.16 phi; -0.11 theta; 4.27 psi 2024-11-06T03:30:26Z- handleDepth()- Depth_Keller msg: 1730863826.25 epoch sec; seqNo:6104; depth 49.70 2024-11-06T03:30:26Z- handleDVL() - DVL msg epochMillisec: 1730863826.27 2024-11-06T03:30:26Z- handleDvl() - RDI_Pathfinder msg: 1730863826.27 sec; seqNo:2102; #beams:4 2024-11-06T03:30:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.62 , 34.77 , 36.08 , 41.31 2024-11-06T03:30:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , -0.02 2024-11-06T03:30:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863826.28 epoch sec; seqNo:6104; 4061850.82 north; 594559.39 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:30:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863824.64 2024-11-06T03:30:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:26Z- waiting for 1730863828.64, time is 1730863826.28 2024-11-06T03:30:26Z- AHRS_M2 msg: 1730863826.64 epoch sec; seqNo:5729 2024-11-06T03:30:26Z- AHRS_M2 msg: 0.15 phi; -0.11 theta; 4.27 psi 2024-11-06T03:30:26Z- handleDepth()- Depth_Keller msg: 1730863826.65 epoch sec; seqNo:6105; depth 49.72 2024-11-06T03:30:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863826.66 epoch sec; seqNo:6105; 4061850.43 north; 594558.17 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:30:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863824.64 2024-11-06T03:30:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:26Z- waiting for 1730863828.64, time is 1730863826.66 2024-11-06T03:30:27Z- AHRS_M2 msg: 1730863827.03 epoch sec; seqNo:5730 2024-11-06T03:30:27Z- AHRS_M2 msg: 0.15 phi; -0.11 theta; 4.27 psi 2024-11-06T03:30:27Z- handleDepth()- Depth_Keller msg: 1730863827.04 epoch sec; seqNo:6106; depth 49.74 2024-11-06T03:30:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863827.05 epoch sec; seqNo:6106; 4061850.43 north; 594558.17 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:30:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863824.64 2024-11-06T03:30:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:27Z- waiting for 1730863828.64, time is 1730863827.05 2024-11-06T03:30:27Z- AHRS_M2 msg: 1730863827.44 epoch sec; seqNo:5731 2024-11-06T03:30:27Z- AHRS_M2 msg: 0.15 phi; -0.11 theta; 4.27 psi 2024-11-06T03:30:27Z- handleDepth()- Depth_Keller msg: 1730863827.44 epoch sec; seqNo:6107; depth 49.76 2024-11-06T03:30:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863827.45 epoch sec; seqNo:6107; 4061850.43 north; 594558.17 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:30:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863824.64 2024-11-06T03:30:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:27Z- waiting for 1730863828.64, time is 1730863827.45 2024-11-06T03:30:27Z- AHRS_M2 msg: 1730863827.84 epoch sec; seqNo:5732 2024-11-06T03:30:27Z- AHRS_M2 msg: 0.15 phi; -0.10 theta; 4.27 psi 2024-11-06T03:30:27Z- handleDepth()- Depth_Keller msg: 1730863827.85 epoch sec; seqNo:6108; depth 49.78 2024-11-06T03:30:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863827.86 epoch sec; seqNo:6108; 4061850.04 north; 594556.96 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:30:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863824.64 2024-11-06T03:30:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:28Z- waiting for 1730863828.64, time is 1730863827.86 2024-11-06T03:30:28Z- AHRS_M2 msg: 1730863828.25 epoch sec; seqNo:5733 2024-11-06T03:30:28Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 4.27 psi 2024-11-06T03:30:28Z- handleDepth()- Depth_Keller msg: 1730863828.25 epoch sec; seqNo:6109; depth 49.81 2024-11-06T03:30:28Z- handleDVL() - DVL msg epochMillisec: 1730863828.28 2024-11-06T03:30:28Z- handleDvl() - RDI_Pathfinder msg: 1730863828.28 sec; seqNo:2103; #beams:4 2024-11-06T03:30:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.06 , 34.58 , 35.51 , 40.75 2024-11-06T03:30:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , -0.02 2024-11-06T03:30:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863828.29 epoch sec; seqNo:6109; 4061850.04 north; 594556.96 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:30:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863824.64 2024-11-06T03:30:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:28Z- waiting for 1730863828.64, time is 1730863828.29 2024-11-06T03:30:28Z- AHRS_M2 msg: 1730863828.65 epoch sec; seqNo:5734 2024-11-06T03:30:28Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 4.26 psi 2024-11-06T03:30:28Z- handleDepth()- Depth_Keller msg: 1730863828.65 epoch sec; seqNo:6110; depth 49.84 2024-11-06T03:30:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863828.68 epoch sec; seqNo:6110; 4061849.66 north; 594556.96 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:30:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863824.64 2024-11-06T03:30:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:28Z- waiting for 1730863828.64, time is 1730863828.68 2024-11-06T03:30:28Z- LcmTrn::updateTrn() >>>> pose time 1730863828.68, meas time 1730863828.28 <<<< 2024-11-06T03:30:28Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863828.68 2024-11-06T03:30:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863828.28 sec, ping # 0; waiting for INS data more recent than 1730863824.64... 2024-11-06T03:30:28Z- TerrainNav::motionUpdate() - currEstimate = (4061850, 594556.96, 49.84) 2024-11-06T03:30:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:30:28Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:30:28Z- TNavPF::Projecting Measurements 2024-11-06T03:30:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:30:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:30:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:30:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:30:28Z- TNavFilter::4 good beams. 2024-11-06T03:30:28Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:30:28Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:30:28Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:30:28Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:30:28Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:30:28Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:30:28Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:30:28Z- TerrainMapOctree: (vehN, vehE ) = (4061849.85, 594557.32). 2024-11-06T03:30:28Z- TerrainMapOctree: Min Distance = 217.01. 2024-11-06T03:30:28Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:30:28Z- Forcing Subcloud Comparison 2024-11-06T03:30:28Z- Weighting particles with subcloud comparison 2024-11-06T03:30:28Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:28Z- meanExpectedMeasDiff: 1.474421 delta_rms_squared: 1.428494 alpha: 0.986193 2024-11-06T03:30:28Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:28Z- meanExpectedMeasDiff: -0.906064 delta_rms_squared: 1.023750 alpha: 0.980838 2024-11-06T03:30:28Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:28Z- meanExpectedMeasDiff: 0.360551 delta_rms_squared: 1.069409 alpha: 0.981641 2024-11-06T03:30:28Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:28Z- meanExpectedMeasDiff: -0.082612 delta_rms_squared: 1.358594 alpha: 0.985492 2024-11-06T03:30:28Z- Subcloud NIS: 0.365312 2024-11-06T03:30:28Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:30:28Z- TNavPF:: sumWeights = 0.475078 2024-11-06T03:30:28Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:30:28Z- TNavPF::First Term of Map Covariance Matrix is 0.56 2024-11-06T03:30:28Z- TNavPF::Calculated NIS Value : 0.24 numBeams normalized NIS: 0.06 2024-11-06T03:30:28Z- TerrainNav::Resampling particle filter... 2024-11-06T03:30:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863828.28 sec. 2024-11-06T03:30:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:30:28Z- variances([0], [2], and psi_berg) = 0.145067 0.171321 0.000000 2024-11-06T03:30:28Z- publishEstimatesDecoder() - at 1730863828.68 sec 2024-11-06T03:30:28Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:30:28Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061878.66 594540.39 56.56 2024-11-06T03:30:28Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061877.83 594540.87 56.85 lat/lon: 36.70 -121.94 2024-11-06T03:30:28Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.17 0.65 0.00 2024-11-06T03:30:29Z- AHRS_M2 msg: 1730863829.06 epoch sec; seqNo:5735 2024-11-06T03:30:29Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 4.26 psi 2024-11-06T03:30:29Z- handleDepth()- Depth_Keller msg: 1730863829.06 epoch sec; seqNo:6111; depth 49.85 2024-11-06T03:30:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863829.07 epoch sec; seqNo:6111; 4061849.66 north; 594556.96 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:30:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863828.68 2024-11-06T03:30:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:29Z- waiting for 1730863832.68, time is 1730863829.07 2024-11-06T03:30:29Z- AHRS_M2 msg: 1730863829.46 epoch sec; seqNo:5736 2024-11-06T03:30:29Z- AHRS_M2 msg: 0.13 phi; -0.10 theta; 4.26 psi 2024-11-06T03:30:29Z- handleDepth()- Depth_Keller msg: 1730863829.47 epoch sec; seqNo:6112; depth 49.87 2024-11-06T03:30:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863829.48 epoch sec; seqNo:6112; 4061849.27 north; 594555.74 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:30:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863828.68 2024-11-06T03:30:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:29Z- waiting for 1730863832.68, time is 1730863829.48 2024-11-06T03:30:29Z- AHRS_M2 msg: 1730863829.86 epoch sec; seqNo:5737 2024-11-06T03:30:29Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 4.26 psi 2024-11-06T03:30:29Z- handleDepth()- Depth_Keller msg: 1730863829.88 epoch sec; seqNo:6113; depth 49.91 2024-11-06T03:30:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863829.89 epoch sec; seqNo:6113; 4061849.27 north; 594555.74 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:30:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863828.68 2024-11-06T03:30:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:30Z- waiting for 1730863832.68, time is 1730863829.89 2024-11-06T03:30:30Z- AHRS_M2 msg: 1730863830.27 epoch sec; seqNo:5738 2024-11-06T03:30:30Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 4.25 psi 2024-11-06T03:30:30Z- handleDepth()- Depth_Keller msg: 1730863830.27 epoch sec; seqNo:6114; depth 49.92 2024-11-06T03:30:30Z- handleDVL() - DVL msg epochMillisec: 1730863830.30 2024-11-06T03:30:30Z- handleDvl() - RDI_Pathfinder msg: 1730863830.30 sec; seqNo:2104; #beams:4 2024-11-06T03:30:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.31 , 34.58 , 35.51 , 40.56 2024-11-06T03:30:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.05 , -0.01 2024-11-06T03:30:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863830.32 epoch sec; seqNo:6114; 4061849.27 north; 594555.74 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:30:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863828.68 2024-11-06T03:30:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:30Z- waiting for 1730863832.68, time is 1730863830.32 2024-11-06T03:30:30Z- AHRS_M2 msg: 1730863830.67 epoch sec; seqNo:5739 2024-11-06T03:30:30Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 4.25 psi 2024-11-06T03:30:30Z- handleDepth()- Depth_Keller msg: 1730863830.67 epoch sec; seqNo:6115; depth 49.94 2024-11-06T03:30:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863830.69 epoch sec; seqNo:6115; 4061848.88 north; 594554.53 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:30:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863828.68 2024-11-06T03:30:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:30Z- waiting for 1730863832.68, time is 1730863830.69 2024-11-06T03:30:31Z- AHRS_M2 msg: 1730863831.08 epoch sec; seqNo:5740 2024-11-06T03:30:31Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 4.25 psi 2024-11-06T03:30:31Z- handleDepth()- Depth_Keller msg: 1730863831.09 epoch sec; seqNo:6116; depth 49.98 2024-11-06T03:30:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863831.10 epoch sec; seqNo:6116; 4061848.88 north; 594554.53 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:30:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863828.68 2024-11-06T03:30:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:31Z- waiting for 1730863832.68, time is 1730863831.10 2024-11-06T03:30:31Z- AHRS_M2 msg: 1730863831.48 epoch sec; seqNo:5741 2024-11-06T03:30:31Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 4.24 psi 2024-11-06T03:30:31Z- handleDepth()- Depth_Keller msg: 1730863831.49 epoch sec; seqNo:6117; depth 50.01 2024-11-06T03:30:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863831.51 epoch sec; seqNo:6117; 4061848.50 north; 594554.53 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:30:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863828.68 2024-11-06T03:30:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:31Z- waiting for 1730863832.68, time is 1730863831.51 2024-11-06T03:30:31Z- AHRS_M2 msg: 1730863831.88 epoch sec; seqNo:5742 2024-11-06T03:30:31Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 4.24 psi 2024-11-06T03:30:31Z- handleDepth()- Depth_Keller msg: 1730863831.89 epoch sec; seqNo:6118; depth 50.05 2024-11-06T03:30:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863831.90 epoch sec; seqNo:6118; 4061848.50 north; 594554.53 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:30:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863828.68 2024-11-06T03:30:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:32Z- waiting for 1730863832.68, time is 1730863831.90 2024-11-06T03:30:32Z- AHRS_M2 msg: 1730863832.29 epoch sec; seqNo:5743 2024-11-06T03:30:32Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 4.24 psi 2024-11-06T03:30:32Z- handleDepth()- Depth_Keller msg: 1730863832.30 epoch sec; seqNo:6119; depth 50.08 2024-11-06T03:30:32Z- handleDVL() - DVL msg epochMillisec: 1730863832.33 2024-11-06T03:30:32Z- handleDvl() - RDI_Pathfinder msg: 1730863832.33 sec; seqNo:2105; #beams:4 2024-11-06T03:30:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.95 , 34.66 , 35.51 , 39.93 2024-11-06T03:30:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.09 , 0.02 2024-11-06T03:30:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863832.34 epoch sec; seqNo:6119; 4061848.48 north; 594553.31 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:30:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863828.68 2024-11-06T03:30:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:32Z- waiting for 1730863832.68, time is 1730863832.34 2024-11-06T03:30:32Z- AHRS_M2 msg: 1730863832.69 epoch sec; seqNo:5744 2024-11-06T03:30:32Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 4.23 psi 2024-11-06T03:30:32Z- handleDepth()- Depth_Keller msg: 1730863832.70 epoch sec; seqNo:6120; depth 50.12 2024-11-06T03:30:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863832.71 epoch sec; seqNo:6120; 4061848.10 north; 594553.32 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:30:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863828.68 2024-11-06T03:30:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:32Z- waiting for 1730863832.68, time is 1730863832.71 2024-11-06T03:30:32Z- LcmTrn::updateTrn() >>>> pose time 1730863832.71, meas time 1730863832.33 <<<< 2024-11-06T03:30:32Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863832.71 2024-11-06T03:30:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863832.33 sec, ping # 0; waiting for INS data more recent than 1730863828.68... 2024-11-06T03:30:32Z- TerrainNav::motionUpdate() - currEstimate = (4061848, 594553.32, 50.12) 2024-11-06T03:30:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:30:32Z- TerrainNav:: SubcloudNIS is 0.37, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:30:32Z- TNavPF::Projecting Measurements 2024-11-06T03:30:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:30:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:30:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:30:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:30:32Z- TNavFilter::4 good beams. 2024-11-06T03:30:32Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:30:32Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:30:32Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:30:32Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:30:32Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:30:32Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:30:32Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:30:32Z- TerrainMapOctree: (vehN, vehE ) = (4061848.25, 594553.66). 2024-11-06T03:30:32Z- TerrainMapOctree: Min Distance = 219.55. 2024-11-06T03:30:32Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:30:32Z- Forcing Subcloud Comparison 2024-11-06T03:30:32Z- Weighting particles with subcloud comparison 2024-11-06T03:30:32Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:32Z- meanExpectedMeasDiff: 1.156369 delta_rms_squared: 1.416581 alpha: 0.986078 2024-11-06T03:30:32Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:32Z- meanExpectedMeasDiff: -0.783987 delta_rms_squared: 1.053551 alpha: 0.981370 2024-11-06T03:30:32Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:32Z- meanExpectedMeasDiff: 0.607423 delta_rms_squared: 0.783820 alpha: 0.975119 2024-11-06T03:30:32Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:32Z- meanExpectedMeasDiff: -0.057890 delta_rms_squared: 1.343512 alpha: 0.985332 2024-11-06T03:30:32Z- Subcloud NIS: 0.331248 2024-11-06T03:30:32Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:30:32Z- TNavPF:: sumWeights = 0.523784 2024-11-06T03:30:32Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:30:32Z- TNavPF::First Term of Map Covariance Matrix is 0.48 2024-11-06T03:30:32Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T03:30:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863832.33 sec. 2024-11-06T03:30:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:30:33Z- variances([0], [2], and psi_berg) = 0.148215 0.170131 0.000000 2024-11-06T03:30:33Z- publishEstimatesDecoder() - at 1730863832.71 sec 2024-11-06T03:30:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:30:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061876.18 594537.24 57.46 2024-11-06T03:30:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061876.28 594537.23 57.05 lat/lon: 36.70 -121.94 2024-11-06T03:30:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.17 0.68 0.00 2024-11-06T03:30:33Z- AHRS_M2 msg: 1730863833.10 epoch sec; seqNo:5745 2024-11-06T03:30:33Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 4.23 psi 2024-11-06T03:30:33Z- handleDepth()- Depth_Keller msg: 1730863833.11 epoch sec; seqNo:6121; depth 50.15 2024-11-06T03:30:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863833.12 epoch sec; seqNo:6121; 4061848.10 north; 594553.32 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:30:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863832.71 2024-11-06T03:30:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:33Z- waiting for 1730863836.71, time is 1730863833.12 2024-11-06T03:30:33Z- AHRS_M2 msg: 1730863833.50 epoch sec; seqNo:5746 2024-11-06T03:30:33Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 4.23 psi 2024-11-06T03:30:33Z- handleDepth()- Depth_Keller msg: 1730863833.50 epoch sec; seqNo:6122; depth 50.19 2024-11-06T03:30:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863833.54 epoch sec; seqNo:6122; 4061847.71 north; 594552.10 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:30:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863832.71 2024-11-06T03:30:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:33Z- waiting for 1730863836.71, time is 1730863833.54 2024-11-06T03:30:33Z- AHRS_M2 msg: 1730863833.90 epoch sec; seqNo:5747 2024-11-06T03:30:33Z- AHRS_M2 msg: 0.12 phi; -0.07 theta; 4.22 psi 2024-11-06T03:30:33Z- handleDepth()- Depth_Keller msg: 1730863833.91 epoch sec; seqNo:6123; depth 50.24 2024-11-06T03:30:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863833.92 epoch sec; seqNo:6123; 4061847.71 north; 594552.10 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:30:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863832.71 2024-11-06T03:30:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:34Z- waiting for 1730863836.71, time is 1730863833.92 2024-11-06T03:30:34Z- AHRS_M2 msg: 1730863834.31 epoch sec; seqNo:5748 2024-11-06T03:30:34Z- AHRS_M2 msg: 0.12 phi; -0.07 theta; 4.22 psi 2024-11-06T03:30:34Z- handleDepth()- Depth_Keller msg: 1730863834.32 epoch sec; seqNo:6124; depth 50.27 2024-11-06T03:30:34Z- handleDVL() - DVL msg epochMillisec: 1730863834.34 2024-11-06T03:30:34Z- handleDvl() - RDI_Pathfinder msg: 1730863834.34 sec; seqNo:2106; #beams:4 2024-11-06T03:30:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.61 , 34.49 , 35.85 , 39.25 2024-11-06T03:30:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.04 , 0.07 2024-11-06T03:30:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863834.35 epoch sec; seqNo:6124; 4061847.33 north; 594552.10 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:30:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863832.71 2024-11-06T03:30:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:34Z- waiting for 1730863836.71, time is 1730863834.35 2024-11-06T03:30:34Z- AHRS_M2 msg: 1730863834.71 epoch sec; seqNo:5749 2024-11-06T03:30:34Z- AHRS_M2 msg: 0.12 phi; -0.07 theta; 4.22 psi 2024-11-06T03:30:34Z- handleDepth()- Depth_Keller msg: 1730863834.72 epoch sec; seqNo:6125; depth 50.31 2024-11-06T03:30:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863834.74 epoch sec; seqNo:6125; 4061847.32 north; 594550.88 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:30:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863832.71 2024-11-06T03:30:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:34Z- waiting for 1730863836.71, time is 1730863834.74 2024-11-06T03:30:35Z- AHRS_M2 msg: 1730863835.11 epoch sec; seqNo:5750 2024-11-06T03:30:35Z- AHRS_M2 msg: 0.12 phi; -0.07 theta; 4.22 psi 2024-11-06T03:30:35Z- handleDepth()- Depth_Keller msg: 1730863835.12 epoch sec; seqNo:6126; depth 50.35 2024-11-06T03:30:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863835.13 epoch sec; seqNo:6126; 4061847.32 north; 594550.88 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:30:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863832.71 2024-11-06T03:30:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:35Z- waiting for 1730863836.71, time is 1730863835.13 2024-11-06T03:30:35Z- AHRS_M2 msg: 1730863835.52 epoch sec; seqNo:5751 2024-11-06T03:30:35Z- AHRS_M2 msg: 0.12 phi; -0.07 theta; 4.21 psi 2024-11-06T03:30:35Z- handleDepth()- Depth_Keller msg: 1730863835.53 epoch sec; seqNo:6127; depth 50.38 2024-11-06T03:30:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863835.54 epoch sec; seqNo:6127; 4061846.94 north; 594550.89 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:30:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863832.71 2024-11-06T03:30:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:35Z- waiting for 1730863836.71, time is 1730863835.54 2024-11-06T03:30:35Z- AHRS_M2 msg: 1730863835.92 epoch sec; seqNo:5752 2024-11-06T03:30:35Z- AHRS_M2 msg: 0.12 phi; -0.06 theta; 4.21 psi 2024-11-06T03:30:35Z- handleDepth()- Depth_Keller msg: 1730863835.93 epoch sec; seqNo:6128; depth 50.40 2024-11-06T03:30:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863835.94 epoch sec; seqNo:6128; 4061846.94 north; 594550.89 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:30:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863832.71 2024-11-06T03:30:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:36Z- waiting for 1730863836.71, time is 1730863835.94 2024-11-06T03:30:36Z- AHRS_M2 msg: 1730863836.33 epoch sec; seqNo:5753 2024-11-06T03:30:36Z- AHRS_M2 msg: 0.12 phi; -0.06 theta; 4.21 psi 2024-11-06T03:30:36Z- handleDepth()- Depth_Keller msg: 1730863836.33 epoch sec; seqNo:6129; depth 50.44 2024-11-06T03:30:36Z- handleDVL() - DVL msg epochMillisec: 1730863836.36 2024-11-06T03:30:36Z- handleDvl() - RDI_Pathfinder msg: 1730863836.36 sec; seqNo:2107; #beams:4 2024-11-06T03:30:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.44 , 34.66 , 36.02 , 39.08 2024-11-06T03:30:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , 0.08 2024-11-06T03:30:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863836.38 epoch sec; seqNo:6129; 4061846.55 north; 594549.67 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:30:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863832.71 2024-11-06T03:30:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:36Z- waiting for 1730863836.71, time is 1730863836.38 2024-11-06T03:30:36Z- AHRS_M2 msg: 1730863836.73 epoch sec; seqNo:5754 2024-11-06T03:30:36Z- AHRS_M2 msg: 0.12 phi; -0.06 theta; 4.21 psi 2024-11-06T03:30:36Z- handleDepth()- Depth_Keller msg: 1730863836.73 epoch sec; seqNo:6130; depth 50.47 2024-11-06T03:30:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863836.76 epoch sec; seqNo:6130; 4061846.55 north; 594549.67 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:30:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863832.71 2024-11-06T03:30:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:36Z- waiting for 1730863836.71, time is 1730863836.76 2024-11-06T03:30:36Z- LcmTrn::updateTrn() >>>> pose time 1730863836.76, meas time 1730863836.36 <<<< 2024-11-06T03:30:36Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863836.76 2024-11-06T03:30:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863836.36 sec, ping # 0; waiting for INS data more recent than 1730863832.71... 2024-11-06T03:30:36Z- TerrainNav::motionUpdate() - currEstimate = (4061847, 594549.67, 50.47) 2024-11-06T03:30:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:30:36Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:30:36Z- TNavPF::Projecting Measurements 2024-11-06T03:30:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:30:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:30:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:30:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:30:36Z- TNavFilter::4 good beams. 2024-11-06T03:30:36Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:30:36Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:30:36Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:30:36Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:30:36Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:30:36Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:30:36Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:30:36Z- TerrainMapOctree: (vehN, vehE ) = (4061846.70, 594550.03). 2024-11-06T03:30:36Z- TerrainMapOctree: Min Distance = 222.13. 2024-11-06T03:30:36Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:30:36Z- Forcing Subcloud Comparison 2024-11-06T03:30:36Z- Weighting particles with subcloud comparison 2024-11-06T03:30:36Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:37Z- meanExpectedMeasDiff: 1.082820 delta_rms_squared: 1.459384 alpha: 0.986481 2024-11-06T03:30:37Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:37Z- meanExpectedMeasDiff: -0.499809 delta_rms_squared: 1.091439 alpha: 0.982005 2024-11-06T03:30:37Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:37Z- meanExpectedMeasDiff: 1.983568 delta_rms_squared: 0.915667 alpha: 0.978625 2024-11-06T03:30:37Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:37Z- meanExpectedMeasDiff: 0.027559 delta_rms_squared: 1.348609 alpha: 0.985387 2024-11-06T03:30:37Z- Subcloud NIS: 0.309194 2024-11-06T03:30:37Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:30:37Z- TNavPF:: sumWeights = 0.387766 2024-11-06T03:30:37Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:30:37Z- TNavPF::First Term of Map Covariance Matrix is 0.33 2024-11-06T03:30:37Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.05 2024-11-06T03:30:37Z- TerrainNav::Resampling particle filter... 2024-11-06T03:30:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863836.36 sec. 2024-11-06T03:30:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:30:37Z- variances([0], [2], and psi_berg) = 0.149356 0.166306 0.000000 2024-11-06T03:30:37Z- publishEstimatesDecoder() - at 1730863836.76 sec 2024-11-06T03:30:37Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:30:37Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061874.79 594532.89 58.54 2024-11-06T03:30:37Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061874.74 594533.61 57.11 lat/lon: 36.70 -121.94 2024-11-06T03:30:37Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.17 0.67 0.00 2024-11-06T03:30:37Z- AHRS_M2 msg: 1730863837.13 epoch sec; seqNo:5755 2024-11-06T03:30:37Z- AHRS_M2 msg: 0.12 phi; -0.05 theta; 4.21 psi 2024-11-06T03:30:37Z- handleDepth()- Depth_Keller msg: 1730863837.14 epoch sec; seqNo:6131; depth 50.49 2024-11-06T03:30:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863837.15 epoch sec; seqNo:6131; 4061846.17 north; 594549.68 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:30:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863836.76 2024-11-06T03:30:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:37Z- waiting for 1730863840.76, time is 1730863837.15 2024-11-06T03:30:37Z- AHRS_M2 msg: 1730863837.54 epoch sec; seqNo:5756 2024-11-06T03:30:37Z- AHRS_M2 msg: 0.12 phi; -0.05 theta; 4.21 psi 2024-11-06T03:30:37Z- handleDepth()- Depth_Keller msg: 1730863837.56 epoch sec; seqNo:6132; depth 50.50 2024-11-06T03:30:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863837.57 epoch sec; seqNo:6132; 4061846.17 north; 594549.68 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:30:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863836.76 2024-11-06T03:30:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:37Z- waiting for 1730863840.76, time is 1730863837.57 2024-11-06T03:30:37Z- AHRS_M2 msg: 1730863837.95 epoch sec; seqNo:5757 2024-11-06T03:30:37Z- AHRS_M2 msg: 0.13 phi; -0.05 theta; 4.21 psi 2024-11-06T03:30:37Z- handleDepth()- Depth_Keller msg: 1730863837.95 epoch sec; seqNo:6133; depth 50.52 2024-11-06T03:30:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863837.96 epoch sec; seqNo:6133; 4061845.78 north; 594548.46 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:30:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863836.76 2024-11-06T03:30:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:38Z- waiting for 1730863840.76, time is 1730863837.96 2024-11-06T03:30:38Z- AHRS_M2 msg: 1730863838.35 epoch sec; seqNo:5758 2024-11-06T03:30:38Z- AHRS_M2 msg: 0.13 phi; -0.05 theta; 4.21 psi 2024-11-06T03:30:38Z- handleDepth()- Depth_Keller msg: 1730863838.35 epoch sec; seqNo:6134; depth 50.53 2024-11-06T03:30:38Z- handleDVL() - DVL msg epochMillisec: 1730863838.37 2024-11-06T03:30:38Z- handleDvl() - RDI_Pathfinder msg: 1730863838.37 sec; seqNo:2108; #beams:4 2024-11-06T03:30:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.27 , 34.66 , 36.02 , 38.91 2024-11-06T03:30:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.09 , 0.03 2024-11-06T03:30:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863838.40 epoch sec; seqNo:6134; 4061845.78 north; 594548.46 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:30:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863836.76 2024-11-06T03:30:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:38Z- waiting for 1730863840.76, time is 1730863838.40 2024-11-06T03:30:38Z- AHRS_M2 msg: 1730863838.75 epoch sec; seqNo:5759 2024-11-06T03:30:38Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.21 psi 2024-11-06T03:30:38Z- handleDepth()- Depth_Keller msg: 1730863838.76 epoch sec; seqNo:6135; depth 50.54 2024-11-06T03:30:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863838.77 epoch sec; seqNo:6135; 4061845.40 north; 594548.46 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:30:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863836.76 2024-11-06T03:30:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:38Z- waiting for 1730863840.76, time is 1730863838.77 2024-11-06T03:30:39Z- AHRS_M2 msg: 1730863839.15 epoch sec; seqNo:5760 2024-11-06T03:30:39Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.21 psi 2024-11-06T03:30:39Z- handleDepth()- Depth_Keller msg: 1730863839.17 epoch sec; seqNo:6136; depth 50.55 2024-11-06T03:30:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863839.18 epoch sec; seqNo:6136; 4061845.40 north; 594548.46 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:30:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863836.76 2024-11-06T03:30:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:39Z- waiting for 1730863840.76, time is 1730863839.18 2024-11-06T03:30:39Z- AHRS_M2 msg: 1730863839.56 epoch sec; seqNo:5761 2024-11-06T03:30:39Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.21 psi 2024-11-06T03:30:39Z- handleDepth()- Depth_Keller msg: 1730863839.57 epoch sec; seqNo:6137; depth 50.55 2024-11-06T03:30:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863839.58 epoch sec; seqNo:6137; 4061845.01 north; 594547.25 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:30:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863836.76 2024-11-06T03:30:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:39Z- waiting for 1730863840.76, time is 1730863839.58 2024-11-06T03:30:39Z- AHRS_M2 msg: 1730863839.96 epoch sec; seqNo:5762 2024-11-06T03:30:39Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.21 psi 2024-11-06T03:30:39Z- handleDepth()- Depth_Keller msg: 1730863839.97 epoch sec; seqNo:6138; depth 50.56 2024-11-06T03:30:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863839.98 epoch sec; seqNo:6138; 4061845.01 north; 594547.25 east; 36.70 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T03:30:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863836.76 2024-11-06T03:30:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:40Z- waiting for 1730863840.76, time is 1730863839.98 2024-11-06T03:30:40Z- AHRS_M2 msg: 1730863840.37 epoch sec; seqNo:5763 2024-11-06T03:30:40Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.22 psi 2024-11-06T03:30:40Z- handleDepth()- Depth_Keller msg: 1730863840.38 epoch sec; seqNo:6139; depth 50.54 2024-11-06T03:30:40Z- handleDVL() - DVL msg epochMillisec: 1730863840.40 2024-11-06T03:30:40Z- handleDvl() - RDI_Pathfinder msg: 1730863840.40 sec; seqNo:2109; #beams:4 2024-11-06T03:30:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.44 , 34.32 , 36.02 , 38.40 2024-11-06T03:30:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , -0.04 2024-11-06T03:30:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863840.41 epoch sec; seqNo:6139; 4061844.63 north; 594547.25 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:30:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863836.76 2024-11-06T03:30:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:40Z- waiting for 1730863840.76, time is 1730863840.41 2024-11-06T03:30:40Z- AHRS_M2 msg: 1730863840.77 epoch sec; seqNo:5764 2024-11-06T03:30:40Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.22 psi 2024-11-06T03:30:40Z- handleDepth()- Depth_Keller msg: 1730863840.78 epoch sec; seqNo:6140; depth 50.54 2024-11-06T03:30:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863840.79 epoch sec; seqNo:6140; 4061844.63 north; 594547.25 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:30:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863836.76 2024-11-06T03:30:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:40Z- waiting for 1730863840.76, time is 1730863840.79 2024-11-06T03:30:40Z- LcmTrn::updateTrn() >>>> pose time 1730863840.79, meas time 1730863840.40 <<<< 2024-11-06T03:30:40Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863840.79 2024-11-06T03:30:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863840.40 sec, ping # 0; waiting for INS data more recent than 1730863836.76... 2024-11-06T03:30:40Z- TerrainNav::motionUpdate() - currEstimate = (4061845, 594547.25, 50.54) 2024-11-06T03:30:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:30:40Z- TerrainNav:: SubcloudNIS is 0.31, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:30:40Z- TNavPF::Projecting Measurements 2024-11-06T03:30:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:30:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:30:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:30:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:30:40Z- TNavFilter::4 good beams. 2024-11-06T03:30:40Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:30:40Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:30:40Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:30:40Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:30:40Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:30:40Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:30:40Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:30:40Z- TerrainMapOctree: (vehN, vehE ) = (4061844.81, 594547.49). 2024-11-06T03:30:40Z- TerrainMapOctree: Min Distance = 223.61. 2024-11-06T03:30:40Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:30:41Z- Forcing Subcloud Comparison 2024-11-06T03:30:41Z- Weighting particles with subcloud comparison 2024-11-06T03:30:41Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:41Z- meanExpectedMeasDiff: 0.671815 delta_rms_squared: 1.442005 alpha: 0.986320 2024-11-06T03:30:41Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:41Z- meanExpectedMeasDiff: -0.892164 delta_rms_squared: 1.055940 alpha: 0.981412 2024-11-06T03:30:41Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:41Z- meanExpectedMeasDiff: 2.414871 delta_rms_squared: 1.123345 alpha: 0.982507 2024-11-06T03:30:41Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:41Z- meanExpectedMeasDiff: -0.407340 delta_rms_squared: 1.284577 alpha: 0.984669 2024-11-06T03:30:41Z- Subcloud NIS: 0.285039 2024-11-06T03:30:41Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:30:41Z- TNavPF:: sumWeights = 0.280202 2024-11-06T03:30:41Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:30:41Z- TNavPF::First Term of Map Covariance Matrix is 0.21 2024-11-06T03:30:41Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T03:30:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863840.40 sec. 2024-11-06T03:30:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:30:41Z- variances([0], [2], and psi_berg) = 0.149899 0.166046 0.000000 2024-11-06T03:30:41Z- publishEstimatesDecoder() - at 1730863840.79 sec 2024-11-06T03:30:41Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:30:41Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061872.95 594531.02 56.56 2024-11-06T03:30:41Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061872.82 594531.20 56.97 lat/lon: 36.70 -121.94 2024-11-06T03:30:41Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.17 0.64 0.00 2024-11-06T03:30:41Z- AHRS_M2 msg: 1730863841.17 epoch sec; seqNo:5765 2024-11-06T03:30:41Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.22 psi 2024-11-06T03:30:41Z- handleDepth()- Depth_Keller msg: 1730863841.18 epoch sec; seqNo:6141; depth 50.53 2024-11-06T03:30:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863841.20 epoch sec; seqNo:6141; 4061844.24 north; 594546.04 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:30:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863840.79 2024-11-06T03:30:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:41Z- waiting for 1730863844.79, time is 1730863841.20 2024-11-06T03:30:41Z- AHRS_M2 msg: 1730863841.59 epoch sec; seqNo:5766 2024-11-06T03:30:41Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.22 psi 2024-11-06T03:30:41Z- handleDepth()- Depth_Keller msg: 1730863841.59 epoch sec; seqNo:6142; depth 50.54 2024-11-06T03:30:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863841.60 epoch sec; seqNo:6142; 4061844.24 north; 594546.04 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:30:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863840.79 2024-11-06T03:30:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:41Z- waiting for 1730863844.79, time is 1730863841.60 2024-11-06T03:30:41Z- AHRS_M2 msg: 1730863841.98 epoch sec; seqNo:5767 2024-11-06T03:30:41Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.22 psi 2024-11-06T03:30:41Z- handleDepth()- Depth_Keller msg: 1730863841.99 epoch sec; seqNo:6143; depth 50.52 2024-11-06T03:30:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863842.00 epoch sec; seqNo:6143; 4061843.86 north; 594546.04 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:30:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863840.79 2024-11-06T03:30:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:42Z- waiting for 1730863844.79, time is 1730863842.00 2024-11-06T03:30:42Z- AHRS_M2 msg: 1730863842.39 epoch sec; seqNo:5768 2024-11-06T03:30:42Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.22 psi 2024-11-06T03:30:42Z- handleDepth()- Depth_Keller msg: 1730863842.39 epoch sec; seqNo:6144; depth 50.53 2024-11-06T03:30:42Z- handleDVL() - DVL msg epochMillisec: 1730863842.41 2024-11-06T03:30:42Z- handleDvl() - RDI_Pathfinder msg: 1730863842.41 sec; seqNo:2110; #beams:4 2024-11-06T03:30:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.61 , 33.98 , 36.19 , 38.40 2024-11-06T03:30:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , -0.04 2024-11-06T03:30:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863842.44 epoch sec; seqNo:6144; 4061843.86 north; 594546.04 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:30:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863840.79 2024-11-06T03:30:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:42Z- waiting for 1730863844.79, time is 1730863842.44 2024-11-06T03:30:42Z- AHRS_M2 msg: 1730863842.79 epoch sec; seqNo:5769 2024-11-06T03:30:42Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.21 psi 2024-11-06T03:30:42Z- handleDepth()- Depth_Keller msg: 1730863842.80 epoch sec; seqNo:6145; depth 50.51 2024-11-06T03:30:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863842.81 epoch sec; seqNo:6145; 4061843.46 north; 594544.82 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:30:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863840.79 2024-11-06T03:30:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:42Z- waiting for 1730863844.79, time is 1730863842.81 2024-11-06T03:30:43Z- AHRS_M2 msg: 1730863843.19 epoch sec; seqNo:5770 2024-11-06T03:30:43Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.21 psi 2024-11-06T03:30:43Z- handleDepth()- Depth_Keller msg: 1730863843.20 epoch sec; seqNo:6146; depth 50.51 2024-11-06T03:30:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863843.21 epoch sec; seqNo:6146; 4061843.46 north; 594544.82 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:30:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863840.79 2024-11-06T03:30:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:43Z- waiting for 1730863844.79, time is 1730863843.21 2024-11-06T03:30:43Z- handleDepth()- Depth_Keller msg: 1730863843.63 epoch sec; seqNo:6147; depth 50.51 2024-11-06T03:30:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863843.65 epoch sec; seqNo:6147; 4061843.09 north; 594544.83 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:30:43Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:30:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863840.79 2024-11-06T03:30:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:43Z- waiting for 1730863844.79, time is 1730863843.65 2024-11-06T03:30:44Z- handleDepth()- Depth_Keller msg: 1730863844.02 epoch sec; seqNo:6148; depth 50.51 2024-11-06T03:30:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863844.03 epoch sec; seqNo:6148; 4061843.09 north; 594544.83 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:30:44Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:30:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863840.79 2024-11-06T03:30:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:44Z- waiting for 1730863844.79, time is 1730863844.03 2024-11-06T03:30:44Z- AHRS_M2 msg: 1730863844.41 epoch sec; seqNo:5771 2024-11-06T03:30:44Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.21 psi 2024-11-06T03:30:44Z- handleDepth()- Depth_Keller msg: 1730863844.41 epoch sec; seqNo:6149; depth 50.52 2024-11-06T03:30:44Z- handleDVL() - DVL msg epochMillisec: 1730863844.44 2024-11-06T03:30:44Z- handleDvl() - RDI_Pathfinder msg: 1730863844.44 sec; seqNo:2111; #beams:4 2024-11-06T03:30:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.61 , 33.81 , 36.02 , 38.06 2024-11-06T03:30:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.06 , 0.01 2024-11-06T03:30:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863844.45 epoch sec; seqNo:6149; 4061842.69 north; 594543.61 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:30:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863840.79 2024-11-06T03:30:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:44Z- waiting for 1730863844.79, time is 1730863844.45 2024-11-06T03:30:44Z- AHRS_M2 msg: 1730863844.81 epoch sec; seqNo:5772 2024-11-06T03:30:44Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.20 psi 2024-11-06T03:30:44Z- handleDepth()- Depth_Keller msg: 1730863844.82 epoch sec; seqNo:6150; depth 50.51 2024-11-06T03:30:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863844.83 epoch sec; seqNo:6150; 4061842.69 north; 594543.61 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:30:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863840.79 2024-11-06T03:30:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:44Z- waiting for 1730863844.79, time is 1730863844.83 2024-11-06T03:30:44Z- LcmTrn::updateTrn() >>>> pose time 1730863844.83, meas time 1730863844.44 <<<< 2024-11-06T03:30:44Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863844.83 2024-11-06T03:30:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863844.44 sec, ping # 0; waiting for INS data more recent than 1730863840.79... 2024-11-06T03:30:44Z- TerrainNav::motionUpdate() - currEstimate = (4061843, 594543.61, 50.51) 2024-11-06T03:30:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:30:44Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:30:45Z- TNavPF::Projecting Measurements 2024-11-06T03:30:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:30:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:30:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:30:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:30:45Z- TNavFilter::4 good beams. 2024-11-06T03:30:45Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:30:45Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:30:45Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:30:45Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:30:45Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:30:45Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:30:45Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:30:45Z- TerrainMapOctree: (vehN, vehE ) = (4061842.88, 594543.96). 2024-11-06T03:30:45Z- TerrainMapOctree: Min Distance = 226.00. 2024-11-06T03:30:45Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:30:45Z- Forcing Subcloud Comparison 2024-11-06T03:30:45Z- Weighting particles with subcloud comparison 2024-11-06T03:30:45Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:45Z- meanExpectedMeasDiff: 1.545383 delta_rms_squared: 1.437460 alpha: 0.986277 2024-11-06T03:30:45Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:45Z- meanExpectedMeasDiff: -1.522306 delta_rms_squared: 1.034668 alpha: 0.981037 2024-11-06T03:30:45Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:45Z- meanExpectedMeasDiff: 3.208396 delta_rms_squared: 1.141635 alpha: 0.982783 2024-11-06T03:30:45Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:45Z- meanExpectedMeasDiff: -1.034332 delta_rms_squared: 1.268836 alpha: 0.984482 2024-11-06T03:30:45Z- Subcloud NIS: 0.294988 2024-11-06T03:30:45Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:30:45Z- TNavPF:: sumWeights = 0.093277 2024-11-06T03:30:45Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:30:45Z- TNavPF::First Term of Map Covariance Matrix is 0.18 2024-11-06T03:30:45Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T03:30:45Z- TerrainNav::Resampling particle filter... 2024-11-06T03:30:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863844.44 sec. 2024-11-06T03:30:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:30:45Z- variances([0], [2], and psi_berg) = 0.158667 0.169313 0.000000 2024-11-06T03:30:45Z- publishEstimatesDecoder() - at 1730863844.83 sec 2024-11-06T03:30:45Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:30:45Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061870.77 594526.94 56.87 2024-11-06T03:30:45Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061870.89 594527.56 56.69 lat/lon: 36.70 -121.94 2024-11-06T03:30:45Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.17 0.66 0.00 2024-11-06T03:30:45Z- AHRS_M2 msg: 1730863845.24 epoch sec; seqNo:5773 2024-11-06T03:30:45Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.20 psi 2024-11-06T03:30:45Z- handleDepth()- Depth_Keller msg: 1730863845.26 epoch sec; seqNo:6151; depth 50.52 2024-11-06T03:30:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863845.27 epoch sec; seqNo:6151; 4061842.31 north; 594543.62 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:30:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863844.83 2024-11-06T03:30:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:45Z- waiting for 1730863848.83, time is 1730863845.27 2024-11-06T03:30:45Z- AHRS_M2 msg: 1730863845.63 epoch sec; seqNo:5774 2024-11-06T03:30:45Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.20 psi 2024-11-06T03:30:45Z- handleDepth()- Depth_Keller msg: 1730863845.63 epoch sec; seqNo:6152; depth 50.52 2024-11-06T03:30:45Z- handleDVL() - DVL msg epochMillisec: 1730863845.64 2024-11-06T03:30:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:30:45Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:30:45Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:30:45Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:30:45Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:30:45Z- handleDvl() - RDI_Pathfinder msg: 1730863845.64 sec; seqNo:2112; #beams:0 2024-11-06T03:30:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:30:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.06 , 0.01 2024-11-06T03:30:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863845.65 epoch sec; seqNo:6152; 4061842.30 north; 594542.40 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:30:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863844.83 2024-11-06T03:30:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:45Z- waiting for 1730863848.83, time is 1730863845.65 2024-11-06T03:30:46Z- AHRS_M2 msg: 1730863846.02 epoch sec; seqNo:5775 2024-11-06T03:30:46Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.20 psi 2024-11-06T03:30:46Z- handleDepth()- Depth_Keller msg: 1730863846.03 epoch sec; seqNo:6153; depth 50.55 2024-11-06T03:30:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863846.05 epoch sec; seqNo:6153; 4061841.92 north; 594542.40 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:30:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863844.83 2024-11-06T03:30:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:46Z- waiting for 1730863848.83, time is 1730863846.05 2024-11-06T03:30:46Z- AHRS_M2 msg: 1730863846.43 epoch sec; seqNo:5776 2024-11-06T03:30:46Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.19 psi 2024-11-06T03:30:46Z- handleDepth()- Depth_Keller msg: 1730863846.44 epoch sec; seqNo:6154; depth 50.54 2024-11-06T03:30:46Z- handleDVL() - DVL msg epochMillisec: 1730863846.46 2024-11-06T03:30:46Z- handleDvl() - RDI_Pathfinder msg: 1730863846.46 sec; seqNo:2113; #beams:4 2024-11-06T03:30:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.27 , 33.64 , 36.02 , 37.89 2024-11-06T03:30:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.05 , 0.04 2024-11-06T03:30:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863846.48 epoch sec; seqNo:6154; 4061841.92 north; 594542.40 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:30:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863844.83 2024-11-06T03:30:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:46Z- waiting for 1730863848.83, time is 1730863846.48 2024-11-06T03:30:46Z- AHRS_M2 msg: 1730863846.83 epoch sec; seqNo:5777 2024-11-06T03:30:46Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.19 psi 2024-11-06T03:30:46Z- handleDepth()- Depth_Keller msg: 1730863846.84 epoch sec; seqNo:6155; depth 50.57 2024-11-06T03:30:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863846.89 epoch sec; seqNo:6155; 4061841.53 north; 594541.18 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T03:30:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863844.83 2024-11-06T03:30:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:46Z- waiting for 1730863848.83, time is 1730863846.89 2024-11-06T03:30:47Z- AHRS_M2 msg: 1730863847.24 epoch sec; seqNo:5778 2024-11-06T03:30:47Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.19 psi 2024-11-06T03:30:47Z- handleDepth()- Depth_Keller msg: 1730863847.24 epoch sec; seqNo:6156; depth 50.58 2024-11-06T03:30:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863847.25 epoch sec; seqNo:6156; 4061841.53 north; 594541.18 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T03:30:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863844.83 2024-11-06T03:30:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:47Z- waiting for 1730863848.83, time is 1730863847.25 2024-11-06T03:30:47Z- AHRS_M2 msg: 1730863847.64 epoch sec; seqNo:5779 2024-11-06T03:30:47Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.19 psi 2024-11-06T03:30:47Z- handleDepth()- Depth_Keller msg: 1730863847.65 epoch sec; seqNo:6157; depth 50.59 2024-11-06T03:30:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863847.67 epoch sec; seqNo:6157; 4061841.53 north; 594541.18 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:30:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863844.83 2024-11-06T03:30:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:47Z- waiting for 1730863848.83, time is 1730863847.67 2024-11-06T03:30:48Z- AHRS_M2 msg: 1730863848.04 epoch sec; seqNo:5780 2024-11-06T03:30:48Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.19 psi 2024-11-06T03:30:48Z- handleDepth()- Depth_Keller msg: 1730863848.05 epoch sec; seqNo:6158; depth 50.61 2024-11-06T03:30:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863848.06 epoch sec; seqNo:6158; 4061841.15 north; 594541.19 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:30:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863844.83 2024-11-06T03:30:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:48Z- waiting for 1730863848.83, time is 1730863848.06 2024-11-06T03:30:48Z- AHRS_M2 msg: 1730863848.45 epoch sec; seqNo:5781 2024-11-06T03:30:48Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.20 psi 2024-11-06T03:30:48Z- handleDepth()- Depth_Keller msg: 1730863848.46 epoch sec; seqNo:6159; depth 50.62 2024-11-06T03:30:48Z- handleDVL() - DVL msg epochMillisec: 1730863848.48 2024-11-06T03:30:48Z- handleDvl() - RDI_Pathfinder msg: 1730863848.48 sec; seqNo:2114; #beams:4 2024-11-06T03:30:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.10 , 33.81 , 36.02 , 37.55 2024-11-06T03:30:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , 0.05 2024-11-06T03:30:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863848.50 epoch sec; seqNo:6159; 4061841.14 north; 594539.97 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:30:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863844.83 2024-11-06T03:30:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:48Z- waiting for 1730863848.83, time is 1730863848.50 2024-11-06T03:30:48Z- AHRS_M2 msg: 1730863848.85 epoch sec; seqNo:5782 2024-11-06T03:30:48Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.19 psi 2024-11-06T03:30:48Z- handleDepth()- Depth_Keller msg: 1730863848.89 epoch sec; seqNo:6160; depth 50.63 2024-11-06T03:30:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863848.91 epoch sec; seqNo:6160; 4061840.76 north; 594539.97 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:30:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863844.83 2024-11-06T03:30:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:49Z- waiting for 1730863848.83, time is 1730863848.91 2024-11-06T03:30:49Z- LcmTrn::updateTrn() >>>> pose time 1730863848.91, meas time 1730863848.48 <<<< 2024-11-06T03:30:49Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863848.91 2024-11-06T03:30:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863848.48 sec, ping # 0; waiting for INS data more recent than 1730863844.83... 2024-11-06T03:30:49Z- TerrainNav::motionUpdate() - currEstimate = (4061841, 594539.97, 50.63) 2024-11-06T03:30:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:30:49Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:30:49Z- TNavPF::Projecting Measurements 2024-11-06T03:30:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:30:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:30:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:30:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:30:49Z- TNavFilter::4 good beams. 2024-11-06T03:30:49Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:30:49Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:30:49Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:30:49Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:30:49Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:30:49Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:30:49Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:30:49Z- TerrainMapOctree: (vehN, vehE ) = (4061840.96, 594540.36). 2024-11-06T03:30:49Z- TerrainMapOctree: Min Distance = 228.52. 2024-11-06T03:30:49Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:30:49Z- Forcing Subcloud Comparison 2024-11-06T03:30:49Z- Weighting particles with subcloud comparison 2024-11-06T03:30:49Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:49Z- meanExpectedMeasDiff: 0.849492 delta_rms_squared: 1.450356 alpha: 0.986398 2024-11-06T03:30:49Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:49Z- meanExpectedMeasDiff: -0.574698 delta_rms_squared: 1.060482 alpha: 0.981490 2024-11-06T03:30:49Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:49Z- meanExpectedMeasDiff: 2.726123 delta_rms_squared: 1.188486 alpha: 0.983450 2024-11-06T03:30:49Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:49Z- meanExpectedMeasDiff: -1.283069 delta_rms_squared: 1.250942 alpha: 0.984264 2024-11-06T03:30:49Z- Subcloud NIS: 0.289253 2024-11-06T03:30:49Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:30:49Z- TNavPF:: sumWeights = 0.193365 2024-11-06T03:30:49Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:30:49Z- TNavPF::First Term of Map Covariance Matrix is 0.17 2024-11-06T03:30:49Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T03:30:49Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863848.48 sec. 2024-11-06T03:30:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:30:49Z- variances([0], [2], and psi_berg) = 0.155829 0.169833 0.000000 2024-11-06T03:30:49Z- publishEstimatesDecoder() - at 1730863848.91 sec 2024-11-06T03:30:49Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:30:49Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061868.69 594523.58 56.27 2024-11-06T03:30:49Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061868.95 594523.92 56.60 lat/lon: 36.70 -121.94 2024-11-06T03:30:49Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.17 0.67 0.00 2024-11-06T03:30:49Z- AHRS_M2 msg: 1730863849.26 epoch sec; seqNo:5783 2024-11-06T03:30:49Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.19 psi 2024-11-06T03:30:49Z- handleDepth()- Depth_Keller msg: 1730863849.27 epoch sec; seqNo:6161; depth 50.64 2024-11-06T03:30:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863849.28 epoch sec; seqNo:6161; 4061840.76 north; 594539.97 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:30:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863848.91 2024-11-06T03:30:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:49Z- waiting for 1730863852.91, time is 1730863849.28 2024-11-06T03:30:49Z- AHRS_M2 msg: 1730863849.66 epoch sec; seqNo:5784 2024-11-06T03:30:49Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 4.19 psi 2024-11-06T03:30:49Z- handleDepth()- Depth_Keller msg: 1730863849.66 epoch sec; seqNo:6162; depth 50.66 2024-11-06T03:30:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863849.68 epoch sec; seqNo:6162; 4061840.37 north; 594538.76 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T03:30:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863848.91 2024-11-06T03:30:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:49Z- waiting for 1730863852.91, time is 1730863849.68 2024-11-06T03:30:50Z- AHRS_M2 msg: 1730863850.06 epoch sec; seqNo:5785 2024-11-06T03:30:50Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 4.19 psi 2024-11-06T03:30:50Z- handleDepth()- Depth_Keller msg: 1730863850.07 epoch sec; seqNo:6163; depth 50.67 2024-11-06T03:30:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863850.09 epoch sec; seqNo:6163; 4061840.37 north; 594538.76 east; 36.70 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T03:30:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863848.91 2024-11-06T03:30:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:50Z- waiting for 1730863852.91, time is 1730863850.09 2024-11-06T03:30:50Z- AHRS_M2 msg: 1730863850.47 epoch sec; seqNo:5786 2024-11-06T03:30:50Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 4.19 psi 2024-11-06T03:30:50Z- handleDepth()- Depth_Keller msg: 1730863850.48 epoch sec; seqNo:6164; depth 50.67 2024-11-06T03:30:50Z- handleDVL() - DVL msg epochMillisec: 1730863850.51 2024-11-06T03:30:50Z- handleDvl() - RDI_Pathfinder msg: 1730863850.51 sec; seqNo:2115; #beams:4 2024-11-06T03:30:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.44 , 33.64 , 36.36 , 37.55 2024-11-06T03:30:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.06 2024-11-06T03:30:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863850.52 epoch sec; seqNo:6164; 4061839.99 north; 594538.76 east; 36.70 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T03:30:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863848.91 2024-11-06T03:30:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:50Z- waiting for 1730863852.91, time is 1730863850.52 2024-11-06T03:30:50Z- AHRS_M2 msg: 1730863850.88 epoch sec; seqNo:5787 2024-11-06T03:30:50Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 4.19 psi 2024-11-06T03:30:50Z- handleDepth()- Depth_Keller msg: 1730863850.88 epoch sec; seqNo:6165; depth 50.69 2024-11-06T03:30:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863850.90 epoch sec; seqNo:6165; 4061839.99 north; 594538.76 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:30:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863848.91 2024-11-06T03:30:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:51Z- waiting for 1730863852.91, time is 1730863850.90 2024-11-06T03:30:51Z- AHRS_M2 msg: 1730863851.28 epoch sec; seqNo:5788 2024-11-06T03:30:51Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 4.19 psi 2024-11-06T03:30:51Z- handleDepth()- Depth_Keller msg: 1730863851.30 epoch sec; seqNo:6166; depth 50.69 2024-11-06T03:30:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863851.31 epoch sec; seqNo:6166; 4061839.59 north; 594537.54 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:30:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863848.91 2024-11-06T03:30:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:51Z- waiting for 1730863852.91, time is 1730863851.31 2024-11-06T03:30:51Z- AHRS_M2 msg: 1730863851.68 epoch sec; seqNo:5789 2024-11-06T03:30:51Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 4.19 psi 2024-11-06T03:30:51Z- handleDepth()- Depth_Keller msg: 1730863851.69 epoch sec; seqNo:6167; depth 50.69 2024-11-06T03:30:51Z- handleDVL() - DVL msg epochMillisec: 1730863851.69 2024-11-06T03:30:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:30:51Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:30:51Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:30:51Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:30:51Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:30:51Z- handleDvl() - RDI_Pathfinder msg: 1730863851.69 sec; seqNo:2116; #beams:0 2024-11-06T03:30:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:30:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.06 2024-11-06T03:30:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863851.70 epoch sec; seqNo:6167; 4061839.59 north; 594537.54 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:30:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863848.91 2024-11-06T03:30:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:51Z- waiting for 1730863852.91, time is 1730863851.70 2024-11-06T03:30:52Z- AHRS_M2 msg: 1730863852.09 epoch sec; seqNo:5790 2024-11-06T03:30:52Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 4.20 psi 2024-11-06T03:30:52Z- handleDepth()- Depth_Keller msg: 1730863852.10 epoch sec; seqNo:6168; depth 50.69 2024-11-06T03:30:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863852.11 epoch sec; seqNo:6168; 4061839.22 north; 594537.55 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:30:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863848.91 2024-11-06T03:30:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:52Z- waiting for 1730863852.91, time is 1730863852.11 2024-11-06T03:30:52Z- AHRS_M2 msg: 1730863852.49 epoch sec; seqNo:5791 2024-11-06T03:30:52Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 4.20 psi 2024-11-06T03:30:52Z- handleDepth()- Depth_Keller msg: 1730863852.51 epoch sec; seqNo:6169; depth 50.69 2024-11-06T03:30:52Z- handleDVL() - DVL msg epochMillisec: 1730863852.53 2024-11-06T03:30:52Z- handleDvl() - RDI_Pathfinder msg: 1730863852.53 sec; seqNo:2117; #beams:4 2024-11-06T03:30:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.78 , 33.98 , 36.70 , 37.55 2024-11-06T03:30:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.04 2024-11-06T03:30:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863852.54 epoch sec; seqNo:6169; 4061839.22 north; 594537.55 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:30:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863848.91 2024-11-06T03:30:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:52Z- waiting for 1730863852.91, time is 1730863852.54 2024-11-06T03:30:52Z- AHRS_M2 msg: 1730863852.89 epoch sec; seqNo:5792 2024-11-06T03:30:52Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 4.20 psi 2024-11-06T03:30:52Z- handleDepth()- Depth_Keller msg: 1730863852.89 epoch sec; seqNo:6170; depth 50.70 2024-11-06T03:30:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863852.91 epoch sec; seqNo:6170; 4061838.82 north; 594536.33 east; 36.70 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T03:30:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863848.91 2024-11-06T03:30:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:53Z- waiting for 1730863852.91, time is 1730863852.91 2024-11-06T03:30:53Z- AHRS_M2 msg: 1730863853.29 epoch sec; seqNo:5793 2024-11-06T03:30:53Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 4.20 psi 2024-11-06T03:30:53Z- handleDepth()- Depth_Keller msg: 1730863853.30 epoch sec; seqNo:6171; depth 50.69 2024-11-06T03:30:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863853.31 epoch sec; seqNo:6171; 4061838.82 north; 594536.33 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T03:30:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863848.91 2024-11-06T03:30:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:53Z- waiting for 1730863852.91, time is 1730863853.31 2024-11-06T03:30:53Z- LcmTrn::updateTrn() >>>> pose time 1730863853.31, meas time 1730863852.53 <<<< 2024-11-06T03:30:53Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863853.31 2024-11-06T03:30:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863852.53 sec, ping # 0; waiting for INS data more recent than 1730863848.91... 2024-11-06T03:30:53Z- TerrainNav::motionUpdate() - currEstimate = (4061839, 594536.33, 50.69) 2024-11-06T03:30:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:30:53Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:30:53Z- TNavPF::Projecting Measurements 2024-11-06T03:30:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:30:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:30:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:30:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:30:53Z- TNavFilter::4 good beams. 2024-11-06T03:30:53Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:30:53Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:30:53Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:30:53Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:30:53Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:30:53Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:30:53Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:30:53Z- TerrainMapOctree: (vehN, vehE ) = (4061839.17, 594536.98). 2024-11-06T03:30:53Z- TerrainMapOctree: Min Distance = 230.93. 2024-11-06T03:30:53Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:30:53Z- Forcing Subcloud Comparison 2024-11-06T03:30:53Z- Weighting particles with subcloud comparison 2024-11-06T03:30:53Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:53Z- meanExpectedMeasDiff: 2.608943 delta_rms_squared: 1.493144 alpha: 0.986782 2024-11-06T03:30:53Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:53Z- meanExpectedMeasDiff: -0.557495 delta_rms_squared: 1.069176 alpha: 0.981637 2024-11-06T03:30:53Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:53Z- meanExpectedMeasDiff: 3.014945 delta_rms_squared: 1.233830 alpha: 0.984049 2024-11-06T03:30:53Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:53Z- meanExpectedMeasDiff: -1.199233 delta_rms_squared: 1.266381 alpha: 0.984453 2024-11-06T03:30:53Z- Subcloud NIS: 0.291413 2024-11-06T03:30:53Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:30:53Z- TNavPF:: sumWeights = 0.101892 2024-11-06T03:30:53Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:30:53Z- TNavPF::First Term of Map Covariance Matrix is 0.40 2024-11-06T03:30:53Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T03:30:53Z- TerrainNav::Resampling particle filter... 2024-11-06T03:30:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863852.53 sec. 2024-11-06T03:30:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:30:53Z- variances([0], [2], and psi_berg) = 0.157195 0.174601 0.000000 2024-11-06T03:30:53Z- publishEstimatesDecoder() - at 1730863853.31 sec 2024-11-06T03:30:53Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:30:53Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061866.84 594519.66 54.38 2024-11-06T03:30:53Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061867.02 594520.28 56.19 lat/lon: 36.70 -121.94 2024-11-06T03:30:53Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.17 0.66 0.00 2024-11-06T03:30:53Z- AHRS_M2 msg: 1730863853.71 epoch sec; seqNo:5794 2024-11-06T03:30:53Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 4.20 psi 2024-11-06T03:30:53Z- handleDepth()- Depth_Keller msg: 1730863853.72 epoch sec; seqNo:6172; depth 50.68 2024-11-06T03:30:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863853.75 epoch sec; seqNo:6172; 4061838.44 north; 594536.34 east; 36.70 lat; -121.94 lon; 50.68 depth; utmZone: 10 2024-11-06T03:30:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863853.31 2024-11-06T03:30:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:53Z- waiting for 1730863857.31, time is 1730863853.75 2024-11-06T03:30:54Z- AHRS_M2 msg: 1730863854.10 epoch sec; seqNo:5795 2024-11-06T03:30:54Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 4.20 psi 2024-11-06T03:30:54Z- handleDepth()- Depth_Keller msg: 1730863854.11 epoch sec; seqNo:6173; depth 50.66 2024-11-06T03:30:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863854.14 epoch sec; seqNo:6173; 4061838.44 north; 594536.34 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T03:30:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863853.31 2024-11-06T03:30:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:54Z- waiting for 1730863857.31, time is 1730863854.14 2024-11-06T03:30:54Z- AHRS_M2 msg: 1730863854.51 epoch sec; seqNo:5796 2024-11-06T03:30:54Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 4.21 psi 2024-11-06T03:30:54Z- handleDepth()- Depth_Keller msg: 1730863854.52 epoch sec; seqNo:6174; depth 50.66 2024-11-06T03:30:54Z- handleDVL() - DVL msg epochMillisec: 1730863854.55 2024-11-06T03:30:54Z- handleDvl() - RDI_Pathfinder msg: 1730863854.55 sec; seqNo:2118; #beams:4 2024-11-06T03:30:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.61 , 33.98 , 36.87 , 37.55 2024-11-06T03:30:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , -0.03 2024-11-06T03:30:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863854.56 epoch sec; seqNo:6174; 4061838.05 north; 594535.12 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T03:30:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863853.31 2024-11-06T03:30:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:54Z- waiting for 1730863857.31, time is 1730863854.56 2024-11-06T03:30:54Z- handleDepth()- Depth_Keller msg: 1730863854.95 epoch sec; seqNo:6175; depth 50.65 2024-11-06T03:30:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863854.96 epoch sec; seqNo:6175; 4061838.05 north; 594535.12 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:30:55Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:30:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863853.31 2024-11-06T03:30:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:55Z- waiting for 1730863857.31, time is 1730863854.96 2024-11-06T03:30:55Z- AHRS_M2 msg: 1730863855.32 epoch sec; seqNo:5797 2024-11-06T03:30:55Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 4.21 psi 2024-11-06T03:30:55Z- handleDepth()- Depth_Keller msg: 1730863855.33 epoch sec; seqNo:6176; depth 50.65 2024-11-06T03:30:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863855.34 epoch sec; seqNo:6176; 4061837.67 north; 594535.12 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:30:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863853.31 2024-11-06T03:30:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:55Z- waiting for 1730863857.31, time is 1730863855.34 2024-11-06T03:30:55Z- AHRS_M2 msg: 1730863855.72 epoch sec; seqNo:5798 2024-11-06T03:30:55Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 4.21 psi 2024-11-06T03:30:55Z- handleDepth()- Depth_Keller msg: 1730863855.72 epoch sec; seqNo:6177; depth 50.64 2024-11-06T03:30:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863855.73 epoch sec; seqNo:6177; 4061837.67 north; 594535.12 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:30:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863853.31 2024-11-06T03:30:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:55Z- waiting for 1730863857.31, time is 1730863855.73 2024-11-06T03:30:56Z- AHRS_M2 msg: 1730863856.12 epoch sec; seqNo:5799 2024-11-06T03:30:56Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 4.20 psi 2024-11-06T03:30:56Z- handleDepth()- Depth_Keller msg: 1730863856.13 epoch sec; seqNo:6178; depth 50.63 2024-11-06T03:30:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863856.14 epoch sec; seqNo:6178; 4061837.28 north; 594533.91 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:30:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863853.31 2024-11-06T03:30:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:56Z- waiting for 1730863857.31, time is 1730863856.14 2024-11-06T03:30:56Z- AHRS_M2 msg: 1730863856.53 epoch sec; seqNo:5800 2024-11-06T03:30:56Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 4.20 psi 2024-11-06T03:30:56Z- handleDepth()- Depth_Keller msg: 1730863856.53 epoch sec; seqNo:6179; depth 50.61 2024-11-06T03:30:56Z- handleDVL() - DVL msg epochMillisec: 1730863856.55 2024-11-06T03:30:56Z- handleDvl() - RDI_Pathfinder msg: 1730863856.55 sec; seqNo:2119; #beams:4 2024-11-06T03:30:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.44 , 33.64 , 36.70 , 37.38 2024-11-06T03:30:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , -0.06 2024-11-06T03:30:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863856.57 epoch sec; seqNo:6179; 4061837.28 north; 594533.91 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:30:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863853.31 2024-11-06T03:30:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:56Z- waiting for 1730863857.31, time is 1730863856.57 2024-11-06T03:30:56Z- AHRS_M2 msg: 1730863856.94 epoch sec; seqNo:5801 2024-11-06T03:30:56Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 4.20 psi 2024-11-06T03:30:56Z- handleDepth()- Depth_Keller msg: 1730863856.94 epoch sec; seqNo:6180; depth 50.62 2024-11-06T03:30:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863856.96 epoch sec; seqNo:6180; 4061836.90 north; 594533.91 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:30:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863853.31 2024-11-06T03:30:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:57Z- waiting for 1730863857.31, time is 1730863856.96 2024-11-06T03:30:57Z- AHRS_M2 msg: 1730863857.34 epoch sec; seqNo:5802 2024-11-06T03:30:57Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 4.20 psi 2024-11-06T03:30:57Z- handleDepth()- Depth_Keller msg: 1730863857.34 epoch sec; seqNo:6181; depth 50.60 2024-11-06T03:30:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863857.35 epoch sec; seqNo:6181; 4061836.90 north; 594533.91 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:30:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863853.31 2024-11-06T03:30:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:57Z- waiting for 1730863857.31, time is 1730863857.35 2024-11-06T03:30:57Z- LcmTrn::updateTrn() >>>> pose time 1730863857.35, meas time 1730863856.55 <<<< 2024-11-06T03:30:57Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863857.35 2024-11-06T03:30:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863856.55 sec, ping # 0; waiting for INS data more recent than 1730863853.31... 2024-11-06T03:30:57Z- TerrainNav::motionUpdate() - currEstimate = (4061837, 594533.91, 50.60) 2024-11-06T03:30:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:30:57Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:30:57Z- TNavPF::Projecting Measurements 2024-11-06T03:30:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:30:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:30:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:30:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:30:57Z- TNavFilter::4 good beams. 2024-11-06T03:30:57Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:30:57Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:30:57Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:30:57Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:30:57Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:30:57Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:30:57Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:30:57Z- TerrainMapOctree: (vehN, vehE ) = (4061837.28, 594534.39). 2024-11-06T03:30:57Z- TerrainMapOctree: Min Distance = 232.61. 2024-11-06T03:30:57Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:30:57Z- Forcing Subcloud Comparison 2024-11-06T03:30:57Z- Weighting particles with subcloud comparison 2024-11-06T03:30:57Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:57Z- meanExpectedMeasDiff: 2.743726 delta_rms_squared: 1.687030 alpha: 0.988284 2024-11-06T03:30:57Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:57Z- meanExpectedMeasDiff: -0.892147 delta_rms_squared: 1.916605 alpha: 0.989673 2024-11-06T03:30:57Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:57Z- meanExpectedMeasDiff: 2.730825 delta_rms_squared: 0.792117 alpha: 0.975373 2024-11-06T03:30:57Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:30:57Z- meanExpectedMeasDiff: -1.772137 delta_rms_squared: 1.322394 alpha: 0.985101 2024-11-06T03:30:57Z- Subcloud NIS: 0.309271 2024-11-06T03:30:57Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:30:57Z- TNavPF:: sumWeights = 0.066715 2024-11-06T03:30:57Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:30:57Z- TNavPF::First Term of Map Covariance Matrix is 0.42 2024-11-06T03:30:57Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.01 2024-11-06T03:30:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863856.55 sec. 2024-11-06T03:30:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:30:57Z- variances([0], [2], and psi_berg) = 0.163396 0.184961 0.000000 2024-11-06T03:30:57Z- publishEstimatesDecoder() - at 1730863857.35 sec 2024-11-06T03:30:57Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:30:57Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061866.18 594518.91 56.89 2024-11-06T03:30:57Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061865.16 594517.85 55.94 lat/lon: 36.70 -121.94 2024-11-06T03:30:57Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.18 0.65 0.00 2024-11-06T03:30:57Z- AHRS_M2 msg: 1730863857.76 epoch sec; seqNo:5803 2024-11-06T03:30:57Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 4.20 psi 2024-11-06T03:30:57Z- handleDepth()- Depth_Keller msg: 1730863857.76 epoch sec; seqNo:6182; depth 50.62 2024-11-06T03:30:57Z- handleDVL() - DVL msg epochMillisec: 1730863857.78 2024-11-06T03:30:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:30:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:30:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:30:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:30:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:30:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:30:57Z- handleDvl() - RDI_Pathfinder msg: 1730863857.78 sec; seqNo:2120; #beams:0 2024-11-06T03:30:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:30:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , -0.06 2024-11-06T03:30:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863857.79 epoch sec; seqNo:6182; 4061836.51 north; 594532.70 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:30:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863857.35 2024-11-06T03:30:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:57Z- waiting for 1730863861.35, time is 1730863857.79 2024-11-06T03:30:58Z- AHRS_M2 msg: 1730863858.15 epoch sec; seqNo:5804 2024-11-06T03:30:58Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.19 psi 2024-11-06T03:30:58Z- handleDepth()- Depth_Keller msg: 1730863858.15 epoch sec; seqNo:6183; depth 50.60 2024-11-06T03:30:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863858.24 epoch sec; seqNo:6183; 4061836.51 north; 594532.70 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:30:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863857.35 2024-11-06T03:30:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:58Z- waiting for 1730863861.35, time is 1730863858.24 2024-11-06T03:30:58Z- AHRS_M2 msg: 1730863858.55 epoch sec; seqNo:5805 2024-11-06T03:30:58Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.19 psi 2024-11-06T03:30:58Z- handleDepth()- Depth_Keller msg: 1730863858.55 epoch sec; seqNo:6184; depth 50.60 2024-11-06T03:30:58Z- handleDVL() - DVL msg epochMillisec: 1730863858.57 2024-11-06T03:30:58Z- handleDvl() - RDI_Pathfinder msg: 1730863858.57 sec; seqNo:2121; #beams:4 2024-11-06T03:30:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.44 , 33.30 , 36.70 , 37.04 2024-11-06T03:30:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.06 , -0.02 2024-11-06T03:30:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863858.59 epoch sec; seqNo:6184; 4061836.13 north; 594532.70 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:30:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:30:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863857.35 2024-11-06T03:30:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:58Z- waiting for 1730863861.35, time is 1730863858.59 2024-11-06T03:30:58Z- AHRS_M2 msg: 1730863858.95 epoch sec; seqNo:5806 2024-11-06T03:30:58Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.19 psi 2024-11-06T03:30:58Z- handleDepth()- Depth_Keller msg: 1730863858.95 epoch sec; seqNo:6185; depth 50.59 2024-11-06T03:30:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863858.99 epoch sec; seqNo:6185; 4061836.13 north; 594532.70 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:30:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863857.35 2024-11-06T03:30:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:59Z- waiting for 1730863861.35, time is 1730863858.99 2024-11-06T03:30:59Z- AHRS_M2 msg: 1730863859.35 epoch sec; seqNo:5807 2024-11-06T03:30:59Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.19 psi 2024-11-06T03:30:59Z- handleDepth()- Depth_Keller msg: 1730863859.36 epoch sec; seqNo:6186; depth 50.59 2024-11-06T03:30:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863859.44 epoch sec; seqNo:6186; 4061835.74 north; 594531.48 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:30:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863857.35 2024-11-06T03:30:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:59Z- waiting for 1730863861.35, time is 1730863859.44 2024-11-06T03:30:59Z- AHRS_M2 msg: 1730863859.76 epoch sec; seqNo:5808 2024-11-06T03:30:59Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 4.20 psi 2024-11-06T03:30:59Z- handleDepth()- Depth_Keller msg: 1730863859.76 epoch sec; seqNo:6187; depth 50.59 2024-11-06T03:30:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863859.77 epoch sec; seqNo:6187; 4061835.74 north; 594531.48 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:30:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:30:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863857.35 2024-11-06T03:30:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:30:59Z- waiting for 1730863861.35, time is 1730863859.77 2024-11-06T03:31:00Z- AHRS_M2 msg: 1730863860.19 epoch sec; seqNo:5809 2024-11-06T03:31:00Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 4.20 psi 2024-11-06T03:31:00Z- handleDepth()- Depth_Keller msg: 1730863860.21 epoch sec; seqNo:6188; depth 50.60 2024-11-06T03:31:00Z- handleDVL() - DVL msg epochMillisec: 1730863860.25 2024-11-06T03:31:00Z- handleDvl() - RDI_Pathfinder msg: 1730863860.25 sec; seqNo:2122; #beams:4 2024-11-06T03:31:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.27 , 33.47 , 36.70 , 36.70 2024-11-06T03:31:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.05 , 0.03 2024-11-06T03:31:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863860.26 epoch sec; seqNo:6188; 4061835.36 north; 594531.49 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:31:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863857.35 2024-11-06T03:31:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:00Z- waiting for 1730863861.35, time is 1730863860.26 2024-11-06T03:31:00Z- AHRS_M2 msg: 1730863860.57 epoch sec; seqNo:5810 2024-11-06T03:31:00Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 4.20 psi 2024-11-06T03:31:00Z- handleDepth()- Depth_Keller msg: 1730863860.57 epoch sec; seqNo:6189; depth 50.60 2024-11-06T03:31:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863860.59 epoch sec; seqNo:6189; 4061835.35 north; 594530.27 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:31:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863857.35 2024-11-06T03:31:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:00Z- waiting for 1730863861.35, time is 1730863860.59 2024-11-06T03:31:00Z- AHRS_M2 msg: 1730863860.97 epoch sec; seqNo:5811 2024-11-06T03:31:00Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 4.20 psi 2024-11-06T03:31:00Z- handleDepth()- Depth_Keller msg: 1730863860.98 epoch sec; seqNo:6190; depth 50.60 2024-11-06T03:31:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863860.99 epoch sec; seqNo:6190; 4061835.35 north; 594530.27 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:31:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863857.35 2024-11-06T03:31:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:01Z- waiting for 1730863861.35, time is 1730863860.99 2024-11-06T03:31:01Z- AHRS_M2 msg: 1730863861.38 epoch sec; seqNo:5812 2024-11-06T03:31:01Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 4.20 psi 2024-11-06T03:31:01Z- handleDepth()- Depth_Keller msg: 1730863861.38 epoch sec; seqNo:6191; depth 50.61 2024-11-06T03:31:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863861.39 epoch sec; seqNo:6191; 4061834.97 north; 594530.27 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:31:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863857.35 2024-11-06T03:31:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:01Z- waiting for 1730863861.35, time is 1730863861.39 2024-11-06T03:31:01Z- LcmTrn::updateTrn() >>>> pose time 1730863861.39, meas time 1730863860.25 <<<< 2024-11-06T03:31:01Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863861.39 2024-11-06T03:31:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863860.25 sec, ping # 0; waiting for INS data more recent than 1730863857.35... 2024-11-06T03:31:01Z- TerrainNav::motionUpdate() - currEstimate = (4061835, 594530.27, 50.61) 2024-11-06T03:31:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:31:01Z- TerrainNav:: SubcloudNIS is 0.31, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:31:01Z- TNavPF::Projecting Measurements 2024-11-06T03:31:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:31:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:31:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:31:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:31:01Z- TNavFilter::4 good beams. 2024-11-06T03:31:01Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:31:01Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:31:01Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:31:01Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:31:01Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:31:01Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:31:01Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:31:01Z- TerrainMapOctree: (vehN, vehE ) = (4061835.52, 594531.30). 2024-11-06T03:31:01Z- TerrainMapOctree: Min Distance = 234.82. 2024-11-06T03:31:01Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:31:01Z- Forcing Subcloud Comparison 2024-11-06T03:31:01Z- Weighting particles with subcloud comparison 2024-11-06T03:31:01Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:01Z- meanExpectedMeasDiff: 3.254800 delta_rms_squared: 1.508805 alpha: 0.986918 2024-11-06T03:31:01Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:01Z- meanExpectedMeasDiff: -0.512879 delta_rms_squared: 1.032701 alpha: 0.981001 2024-11-06T03:31:01Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:01Z- meanExpectedMeasDiff: 2.979259 delta_rms_squared: 1.241559 alpha: 0.984147 2024-11-06T03:31:01Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:01Z- meanExpectedMeasDiff: -1.460247 delta_rms_squared: 1.283586 alpha: 0.984658 2024-11-06T03:31:01Z- Subcloud NIS: 0.326382 2024-11-06T03:31:01Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:31:01Z- TNavPF:: sumWeights = 0.064714 2024-11-06T03:31:01Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:31:01Z- TNavPF::First Term of Map Covariance Matrix is 0.43 2024-11-06T03:31:01Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.01 2024-11-06T03:31:01Z- TerrainNav::Resampling particle filter... 2024-11-06T03:31:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863860.25 sec. 2024-11-06T03:31:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:31:01Z- variances([0], [2], and psi_berg) = 0.173463 0.184649 0.000000 2024-11-06T03:31:01Z- publishEstimatesDecoder() - at 1730863861.39 sec 2024-11-06T03:31:01Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:31:01Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061863.08 594513.53 53.66 2024-11-06T03:31:01Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061863.22 594514.20 55.45 lat/lon: 36.70 -121.94 2024-11-06T03:31:01Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.18 0.66 0.00 2024-11-06T03:31:01Z- handleDepth()- Depth_Keller msg: 1730863861.80 epoch sec; seqNo:6192; depth 50.61 2024-11-06T03:31:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863861.83 epoch sec; seqNo:6192; 4061834.97 north; 594530.27 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:31:01Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:31:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863861.39 2024-11-06T03:31:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:01Z- waiting for 1730863865.39, time is 1730863861.83 2024-11-06T03:31:02Z- AHRS_M2 msg: 1730863862.22 epoch sec; seqNo:5813 2024-11-06T03:31:02Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 4.19 psi 2024-11-06T03:31:02Z- handleDepth()- Depth_Keller msg: 1730863862.23 epoch sec; seqNo:6193; depth 50.61 2024-11-06T03:31:02Z- handleDVL() - DVL msg epochMillisec: 1730863862.25 2024-11-06T03:31:02Z- handleDvl() - RDI_Pathfinder msg: 1730863862.25 sec; seqNo:2123; #beams:4 2024-11-06T03:31:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.11 , 33.13 , 36.87 , 36.70 2024-11-06T03:31:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , 0.02 2024-11-06T03:31:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863862.28 epoch sec; seqNo:6193; 4061834.58 north; 594529.06 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:31:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863861.39 2024-11-06T03:31:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:02Z- waiting for 1730863865.39, time is 1730863862.28 2024-11-06T03:31:02Z- AHRS_M2 msg: 1730863862.59 epoch sec; seqNo:5814 2024-11-06T03:31:02Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.19 psi 2024-11-06T03:31:02Z- handleDepth()- Depth_Keller msg: 1730863862.59 epoch sec; seqNo:6194; depth 50.62 2024-11-06T03:31:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863862.61 epoch sec; seqNo:6194; 4061834.58 north; 594529.06 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:31:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863861.39 2024-11-06T03:31:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:02Z- waiting for 1730863865.39, time is 1730863862.61 2024-11-06T03:31:02Z- AHRS_M2 msg: 1730863862.99 epoch sec; seqNo:5815 2024-11-06T03:31:03Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.18 psi 2024-11-06T03:31:03Z- handleDepth()- Depth_Keller msg: 1730863863.01 epoch sec; seqNo:6195; depth 50.62 2024-11-06T03:31:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863863.01 epoch sec; seqNo:6195; 4061834.20 north; 594529.06 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:31:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863861.39 2024-11-06T03:31:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:03Z- waiting for 1730863865.39, time is 1730863863.01 2024-11-06T03:31:03Z- AHRS_M2 msg: 1730863863.39 epoch sec; seqNo:5816 2024-11-06T03:31:03Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 4.18 psi 2024-11-06T03:31:03Z- handleDepth()- Depth_Keller msg: 1730863863.40 epoch sec; seqNo:6196; depth 50.63 2024-11-06T03:31:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863863.42 epoch sec; seqNo:6196; 4061834.18 north; 594527.84 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:31:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863861.39 2024-11-06T03:31:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:03Z- waiting for 1730863865.39, time is 1730863863.42 2024-11-06T03:31:03Z- AHRS_M2 msg: 1730863863.80 epoch sec; seqNo:5817 2024-11-06T03:31:03Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 4.18 psi 2024-11-06T03:31:03Z- handleDepth()- Depth_Keller msg: 1730863863.80 epoch sec; seqNo:6197; depth 50.62 2024-11-06T03:31:03Z- handleDVL() - DVL msg epochMillisec: 1730863863.81 2024-11-06T03:31:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:31:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:31:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:31:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:31:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:31:03Z- handleDvl() - RDI_Pathfinder msg: 1730863863.81 sec; seqNo:2124; #beams:0 2024-11-06T03:31:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:31:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , 0.02 2024-11-06T03:31:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863863.82 epoch sec; seqNo:6197; 4061833.80 north; 594527.84 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:31:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863861.39 2024-11-06T03:31:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:03Z- waiting for 1730863865.39, time is 1730863863.82 2024-11-06T03:31:04Z- AHRS_M2 msg: 1730863864.23 epoch sec; seqNo:5818 2024-11-06T03:31:04Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 4.19 psi 2024-11-06T03:31:04Z- handleDepth()- Depth_Keller msg: 1730863864.23 epoch sec; seqNo:6198; depth 50.61 2024-11-06T03:31:04Z- handleDVL() - DVL msg epochMillisec: 1730863864.26 2024-11-06T03:31:04Z- handleDvl() - RDI_Pathfinder msg: 1730863864.26 sec; seqNo:2125; #beams:4 2024-11-06T03:31:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.27 , 33.13 , 37.38 , 36.19 2024-11-06T03:31:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , 0.01 2024-11-06T03:31:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863864.28 epoch sec; seqNo:6198; 4061833.80 north; 594527.84 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:31:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863861.39 2024-11-06T03:31:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:04Z- waiting for 1730863865.39, time is 1730863864.28 2024-11-06T03:31:04Z- AHRS_M2 msg: 1730863864.61 epoch sec; seqNo:5819 2024-11-06T03:31:04Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 4.19 psi 2024-11-06T03:31:04Z- handleDepth()- Depth_Keller msg: 1730863864.62 epoch sec; seqNo:6199; depth 50.60 2024-11-06T03:31:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863864.64 epoch sec; seqNo:6199; 4061833.43 north; 594527.85 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:31:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863861.39 2024-11-06T03:31:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:04Z- waiting for 1730863865.39, time is 1730863864.64 2024-11-06T03:31:05Z- AHRS_M2 msg: 1730863865.01 epoch sec; seqNo:5820 2024-11-06T03:31:05Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 4.19 psi 2024-11-06T03:31:05Z- handleDepth()- Depth_Keller msg: 1730863865.01 epoch sec; seqNo:6200; depth 50.60 2024-11-06T03:31:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863865.03 epoch sec; seqNo:6200; 4061833.41 north; 594526.63 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:31:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863861.39 2024-11-06T03:31:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:05Z- waiting for 1730863865.39, time is 1730863865.03 2024-11-06T03:31:05Z- AHRS_M2 msg: 1730863865.41 epoch sec; seqNo:5821 2024-11-06T03:31:05Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 4.20 psi 2024-11-06T03:31:05Z- handleDepth()- Depth_Keller msg: 1730863865.43 epoch sec; seqNo:6201; depth 50.59 2024-11-06T03:31:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863865.44 epoch sec; seqNo:6201; 4061833.03 north; 594526.63 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:31:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863861.39 2024-11-06T03:31:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:05Z- waiting for 1730863865.39, time is 1730863865.44 2024-11-06T03:31:05Z- LcmTrn::updateTrn() >>>> pose time 1730863865.44, meas time 1730863864.26 <<<< 2024-11-06T03:31:05Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863865.44 2024-11-06T03:31:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863864.26 sec, ping # 0; waiting for INS data more recent than 1730863861.39... 2024-11-06T03:31:05Z- TerrainNav::motionUpdate() - currEstimate = (4061833, 594526.63, 50.59) 2024-11-06T03:31:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:31:05Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:31:05Z- TNavPF::Projecting Measurements 2024-11-06T03:31:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:31:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:31:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:31:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:31:05Z- TNavFilter::4 good beams. 2024-11-06T03:31:05Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:31:05Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:31:05Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:31:05Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:31:05Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:31:05Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:31:05Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:31:05Z- TerrainMapOctree: (vehN, vehE ) = (4061833.60, 594527.69). 2024-11-06T03:31:05Z- TerrainMapOctree: Min Distance = 237.51. 2024-11-06T03:31:05Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:31:05Z- Forcing Subcloud Comparison 2024-11-06T03:31:05Z- Weighting particles with subcloud comparison 2024-11-06T03:31:05Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:05Z- meanExpectedMeasDiff: 4.817403 delta_rms_squared: 0.958795 alpha: 0.979567 2024-11-06T03:31:05Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:05Z- meanExpectedMeasDiff: -0.519486 delta_rms_squared: 1.063952 alpha: 0.981549 2024-11-06T03:31:05Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:05Z- meanExpectedMeasDiff: 2.524685 delta_rms_squared: 1.302460 alpha: 0.984877 2024-11-06T03:31:05Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:05Z- meanExpectedMeasDiff: -1.849360 delta_rms_squared: 1.199717 alpha: 0.983603 2024-11-06T03:31:05Z- Subcloud NIS: 0.357029 2024-11-06T03:31:05Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:31:05Z- TNavPF:: sumWeights = 0.018932 2024-11-06T03:31:05Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:31:05Z- TNavPF::First Term of Map Covariance Matrix is 0.34 2024-11-06T03:31:05Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:31:05Z- TerrainNav::Resampling particle filter... 2024-11-06T03:31:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863864.26 sec. 2024-11-06T03:31:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:31:05Z- variances([0], [2], and psi_berg) = 0.181204 0.211468 0.000000 2024-11-06T03:31:05Z- publishEstimatesDecoder() - at 1730863865.44 sec 2024-11-06T03:31:05Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:31:05Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061861.10 594509.91 53.84 2024-11-06T03:31:05Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061861.38 594510.64 55.00 lat/lon: 36.70 -121.94 2024-11-06T03:31:05Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.21 0.70 0.00 2024-11-06T03:31:05Z- AHRS_M2 msg: 1730863865.82 epoch sec; seqNo:5822 2024-11-06T03:31:05Z- AHRS_M2 msg: 0.15 phi; 0.05 theta; 4.20 psi 2024-11-06T03:31:05Z- handleDepth()- Depth_Keller msg: 1730863865.84 epoch sec; seqNo:6202; depth 50.57 2024-11-06T03:31:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863865.85 epoch sec; seqNo:6202; 4061833.03 north; 594526.63 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T03:31:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863865.44 2024-11-06T03:31:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:05Z- waiting for 1730863869.44, time is 1730863865.85 2024-11-06T03:31:06Z- AHRS_M2 msg: 1730863866.23 epoch sec; seqNo:5823 2024-11-06T03:31:06Z- AHRS_M2 msg: 0.16 phi; 0.05 theta; 4.20 psi 2024-11-06T03:31:06Z- handleDepth()- Depth_Keller msg: 1730863866.24 epoch sec; seqNo:6203; depth 50.56 2024-11-06T03:31:06Z- handleDVL() - DVL msg epochMillisec: 1730863866.26 2024-11-06T03:31:06Z- handleDvl() - RDI_Pathfinder msg: 1730863866.26 sec; seqNo:2126; #beams:4 2024-11-06T03:31:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.26 , 33.30 , 37.88 , 36.02 2024-11-06T03:31:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , 0.01 2024-11-06T03:31:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863866.27 epoch sec; seqNo:6203; 4061832.65 north; 594526.64 east; 36.70 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T03:31:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863865.44 2024-11-06T03:31:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:06Z- waiting for 1730863869.44, time is 1730863866.27 2024-11-06T03:31:06Z- AHRS_M2 msg: 1730863866.67 epoch sec; seqNo:5824 2024-11-06T03:31:06Z- AHRS_M2 msg: 0.17 phi; 0.05 theta; 4.20 psi 2024-11-06T03:31:06Z- handleDepth()- Depth_Keller msg: 1730863866.68 epoch sec; seqNo:6204; depth 50.54 2024-11-06T03:31:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863866.71 epoch sec; seqNo:6204; 4061832.64 north; 594525.42 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:31:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863865.44 2024-11-06T03:31:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:06Z- waiting for 1730863869.44, time is 1730863866.71 2024-11-06T03:31:07Z- AHRS_M2 msg: 1730863867.03 epoch sec; seqNo:5825 2024-11-06T03:31:07Z- AHRS_M2 msg: 0.17 phi; 0.05 theta; 4.20 psi 2024-11-06T03:31:07Z- handleDepth()- Depth_Keller msg: 1730863867.04 epoch sec; seqNo:6205; depth 50.51 2024-11-06T03:31:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863867.05 epoch sec; seqNo:6205; 4061832.26 north; 594525.42 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:31:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863865.44 2024-11-06T03:31:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:07Z- waiting for 1730863869.44, time is 1730863867.05 2024-11-06T03:31:07Z- AHRS_M2 msg: 1730863867.44 epoch sec; seqNo:5826 2024-11-06T03:31:07Z- AHRS_M2 msg: 0.16 phi; 0.05 theta; 4.20 psi 2024-11-06T03:31:07Z- handleDepth()- Depth_Keller msg: 1730863867.44 epoch sec; seqNo:6206; depth 50.50 2024-11-06T03:31:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863867.45 epoch sec; seqNo:6206; 4061832.26 north; 594525.42 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:31:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863865.44 2024-11-06T03:31:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:07Z- waiting for 1730863869.44, time is 1730863867.45 2024-11-06T03:31:07Z- AHRS_M2 msg: 1730863867.84 epoch sec; seqNo:5827 2024-11-06T03:31:07Z- AHRS_M2 msg: 0.15 phi; 0.05 theta; 4.20 psi 2024-11-06T03:31:07Z- handleDepth()- Depth_Keller msg: 1730863867.85 epoch sec; seqNo:6207; depth 50.47 2024-11-06T03:31:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863867.87 epoch sec; seqNo:6207; 4061831.88 north; 594525.42 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:31:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863865.44 2024-11-06T03:31:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:08Z- waiting for 1730863869.44, time is 1730863867.87 2024-11-06T03:31:08Z- AHRS_M2 msg: 1730863868.24 epoch sec; seqNo:5828 2024-11-06T03:31:08Z- AHRS_M2 msg: 0.15 phi; 0.05 theta; 4.20 psi 2024-11-06T03:31:08Z- handleDepth()- Depth_Keller msg: 1730863868.25 epoch sec; seqNo:6208; depth 50.46 2024-11-06T03:31:08Z- handleDVL() - DVL msg epochMillisec: 1730863868.28 2024-11-06T03:31:08Z- handleDvl() - RDI_Pathfinder msg: 1730863868.28 sec; seqNo:2127; #beams:4 2024-11-06T03:31:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.94 , 33.81 , 38.06 , 36.02 2024-11-06T03:31:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , 0.00 2024-11-06T03:31:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863868.30 epoch sec; seqNo:6208; 4061831.87 north; 594524.20 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:31:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863865.44 2024-11-06T03:31:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:08Z- waiting for 1730863869.44, time is 1730863868.30 2024-11-06T03:31:08Z- handleDepth()- Depth_Keller msg: 1730863868.67 epoch sec; seqNo:6209; depth 50.43 2024-11-06T03:31:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863868.69 epoch sec; seqNo:6209; 4061831.49 north; 594524.21 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:31:08Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:31:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863865.44 2024-11-06T03:31:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:08Z- waiting for 1730863869.44, time is 1730863868.69 2024-11-06T03:31:09Z- handleDepth()- Depth_Keller msg: 1730863869.07 epoch sec; seqNo:6210; depth 50.41 2024-11-06T03:31:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863869.08 epoch sec; seqNo:6210; 4061831.49 north; 594524.21 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:31:09Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:31:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863865.44 2024-11-06T03:31:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:09Z- waiting for 1730863869.44, time is 1730863869.08 2024-11-06T03:31:09Z- AHRS_M2 msg: 1730863869.46 epoch sec; seqNo:5829 2024-11-06T03:31:09Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 4.21 psi 2024-11-06T03:31:09Z- handleDepth()- Depth_Keller msg: 1730863869.46 epoch sec; seqNo:6211; depth 50.37 2024-11-06T03:31:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863869.47 epoch sec; seqNo:6211; 4061831.11 north; 594524.21 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:31:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863865.44 2024-11-06T03:31:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:09Z- waiting for 1730863869.44, time is 1730863869.47 2024-11-06T03:31:09Z- LcmTrn::updateTrn() >>>> pose time 1730863869.47, meas time 1730863868.28 <<<< 2024-11-06T03:31:09Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863869.47 2024-11-06T03:31:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863868.28 sec, ping # 0; waiting for INS data more recent than 1730863865.44... 2024-11-06T03:31:09Z- TerrainNav::motionUpdate() - currEstimate = (4061831, 594524.21, 50.37) 2024-11-06T03:31:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:31:09Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:31:09Z- TNavPF::Projecting Measurements 2024-11-06T03:31:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:31:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:31:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:31:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:31:09Z- TNavFilter::4 good beams. 2024-11-06T03:31:09Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:31:09Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:31:09Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:31:09Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:31:09Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:31:09Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:31:09Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:31:09Z- TerrainMapOctree: (vehN, vehE ) = (4061831.68, 594524.93). 2024-11-06T03:31:09Z- TerrainMapOctree: Min Distance = 239.43. 2024-11-06T03:31:09Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:31:09Z- Forcing Subcloud Comparison 2024-11-06T03:31:09Z- Weighting particles with subcloud comparison 2024-11-06T03:31:09Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:09Z- meanExpectedMeasDiff: 5.511117 delta_rms_squared: 1.535475 alpha: 0.987142 2024-11-06T03:31:09Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:09Z- meanExpectedMeasDiff: -0.017373 delta_rms_squared: 0.890318 alpha: 0.978030 2024-11-06T03:31:09Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:09Z- meanExpectedMeasDiff: 2.182093 delta_rms_squared: 1.377430 alpha: 0.985688 2024-11-06T03:31:09Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:09Z- meanExpectedMeasDiff: -1.615099 delta_rms_squared: 2.138820 alpha: 0.990736 2024-11-06T03:31:09Z- Subcloud NIS: 0.388171 2024-11-06T03:31:09Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:31:09Z- TNavPF:: sumWeights = 0.014305 2024-11-06T03:31:09Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:31:09Z- TNavPF::First Term of Map Covariance Matrix is 0.31 2024-11-06T03:31:09Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:31:09Z- TerrainNav::Resampling particle filter... 2024-11-06T03:31:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863868.28 sec. 2024-11-06T03:31:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:31:09Z- variances([0], [2], and psi_berg) = 0.194749 0.214812 0.000000 2024-11-06T03:31:09Z- publishEstimatesDecoder() - at 1730863869.47 sec 2024-11-06T03:31:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:31:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061859.16 594507.51 54.14 2024-11-06T03:31:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061859.38 594508.15 54.13 lat/lon: 36.70 -121.94 2024-11-06T03:31:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.19 0.21 0.66 0.00 2024-11-06T03:31:09Z- AHRS_M2 msg: 1730863869.86 epoch sec; seqNo:5830 2024-11-06T03:31:09Z- AHRS_M2 msg: 0.15 phi; 0.05 theta; 4.21 psi 2024-11-06T03:31:09Z- handleDepth()- Depth_Keller msg: 1730863869.88 epoch sec; seqNo:6212; depth 50.36 2024-11-06T03:31:09Z- handleDVL() - DVL msg epochMillisec: 1730863869.88 2024-11-06T03:31:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:31:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:31:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:31:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:31:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:31:09Z- handleDvl() - RDI_Pathfinder msg: 1730863869.88 sec; seqNo:2128; #beams:0 2024-11-06T03:31:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:31:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , 0.00 2024-11-06T03:31:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863869.89 epoch sec; seqNo:6212; 4061831.10 north; 594522.99 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:31:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863869.47 2024-11-06T03:31:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:10Z- waiting for 1730863873.47, time is 1730863869.89 2024-11-06T03:31:10Z- AHRS_M2 msg: 1730863870.26 epoch sec; seqNo:5831 2024-11-06T03:31:10Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 4.21 psi 2024-11-06T03:31:10Z- handleDepth()- Depth_Keller msg: 1730863870.27 epoch sec; seqNo:6213; depth 50.33 2024-11-06T03:31:10Z- handleDVL() - DVL msg epochMillisec: 1730863870.29 2024-11-06T03:31:10Z- handleDvl() - RDI_Pathfinder msg: 1730863870.29 sec; seqNo:2129; #beams:4 2024-11-06T03:31:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.94 , 33.98 , 38.06 , 36.02 2024-11-06T03:31:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.02 2024-11-06T03:31:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863870.32 epoch sec; seqNo:6213; 4061830.72 north; 594523.00 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:31:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863869.47 2024-11-06T03:31:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:10Z- waiting for 1730863873.47, time is 1730863870.32 2024-11-06T03:31:10Z- AHRS_M2 msg: 1730863870.67 epoch sec; seqNo:5832 2024-11-06T03:31:10Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 4.21 psi 2024-11-06T03:31:10Z- handleDepth()- Depth_Keller msg: 1730863870.68 epoch sec; seqNo:6214; depth 50.29 2024-11-06T03:31:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863870.69 epoch sec; seqNo:6214; 4061830.72 north; 594523.00 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:31:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863869.47 2024-11-06T03:31:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:10Z- waiting for 1730863873.47, time is 1730863870.69 2024-11-06T03:31:11Z- AHRS_M2 msg: 1730863871.09 epoch sec; seqNo:5833 2024-11-06T03:31:11Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 4.21 psi 2024-11-06T03:31:11Z- handleDepth()- Depth_Keller msg: 1730863871.09 epoch sec; seqNo:6215; depth 50.27 2024-11-06T03:31:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863871.11 epoch sec; seqNo:6215; 4061830.34 north; 594523.00 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:31:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863869.47 2024-11-06T03:31:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:11Z- waiting for 1730863873.47, time is 1730863871.11 2024-11-06T03:31:11Z- AHRS_M2 msg: 1730863871.47 epoch sec; seqNo:5834 2024-11-06T03:31:11Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 4.21 psi 2024-11-06T03:31:11Z- handleDepth()- Depth_Keller msg: 1730863871.48 epoch sec; seqNo:6216; depth 50.25 2024-11-06T03:31:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863871.49 epoch sec; seqNo:6216; 4061830.33 north; 594521.78 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:31:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863869.47 2024-11-06T03:31:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:11Z- waiting for 1730863873.47, time is 1730863871.49 2024-11-06T03:31:11Z- AHRS_M2 msg: 1730863871.89 epoch sec; seqNo:5835 2024-11-06T03:31:11Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 4.21 psi 2024-11-06T03:31:11Z- handleDepth()- Depth_Keller msg: 1730863871.89 epoch sec; seqNo:6217; depth 50.21 2024-11-06T03:31:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863871.90 epoch sec; seqNo:6217; 4061830.33 north; 594521.78 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:31:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863869.47 2024-11-06T03:31:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:12Z- waiting for 1730863873.47, time is 1730863871.90 2024-11-06T03:31:12Z- AHRS_M2 msg: 1730863872.29 epoch sec; seqNo:5836 2024-11-06T03:31:12Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 4.21 psi 2024-11-06T03:31:12Z- handleDepth()- Depth_Keller msg: 1730863872.30 epoch sec; seqNo:6218; depth 50.18 2024-11-06T03:31:12Z- handleDVL() - DVL msg epochMillisec: 1730863872.32 2024-11-06T03:31:12Z- handleDvl() - RDI_Pathfinder msg: 1730863872.32 sec; seqNo:2130; #beams:4 2024-11-06T03:31:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.60 , 33.98 , 37.88 , 36.36 2024-11-06T03:31:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.02 2024-11-06T03:31:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863872.33 epoch sec; seqNo:6218; 4061829.95 north; 594521.78 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:31:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863869.47 2024-11-06T03:31:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:12Z- waiting for 1730863873.47, time is 1730863872.33 2024-11-06T03:31:12Z- AHRS_M2 msg: 1730863872.69 epoch sec; seqNo:5837 2024-11-06T03:31:12Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 4.21 psi 2024-11-06T03:31:12Z- handleDepth()- Depth_Keller msg: 1730863872.69 epoch sec; seqNo:6219; depth 50.17 2024-11-06T03:31:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863872.72 epoch sec; seqNo:6219; 4061829.94 north; 594520.56 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:31:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863869.47 2024-11-06T03:31:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:12Z- waiting for 1730863873.47, time is 1730863872.72 2024-11-06T03:31:13Z- AHRS_M2 msg: 1730863873.09 epoch sec; seqNo:5838 2024-11-06T03:31:13Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 4.21 psi 2024-11-06T03:31:13Z- handleDepth()- Depth_Keller msg: 1730863873.10 epoch sec; seqNo:6220; depth 50.14 2024-11-06T03:31:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863873.11 epoch sec; seqNo:6220; 4061829.56 north; 594520.57 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:31:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863869.47 2024-11-06T03:31:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:13Z- waiting for 1730863873.47, time is 1730863873.11 2024-11-06T03:31:13Z- AHRS_M2 msg: 1730863873.50 epoch sec; seqNo:5839 2024-11-06T03:31:13Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 4.21 psi 2024-11-06T03:31:13Z- handleDepth()- Depth_Keller msg: 1730863873.51 epoch sec; seqNo:6221; depth 50.11 2024-11-06T03:31:13Z- handleDVL() - DVL msg epochMillisec: 1730863873.51 2024-11-06T03:31:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:31:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:31:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:31:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:31:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:31:13Z- handleDvl() - RDI_Pathfinder msg: 1730863873.51 sec; seqNo:2131; #beams:0 2024-11-06T03:31:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:31:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.02 2024-11-06T03:31:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863873.52 epoch sec; seqNo:6221; 4061829.56 north; 594520.57 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:31:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863869.47 2024-11-06T03:31:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:13Z- waiting for 1730863873.47, time is 1730863873.52 2024-11-06T03:31:13Z- LcmTrn::updateTrn() >>>> pose time 1730863873.52, meas time 1730863873.51 <<<< 2024-11-06T03:31:13Z- LcmTrn::updateTrn() equating pose 1730863873.51 and meas times 1730863873.51 2024-11-06T03:31:13Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863873.51 2024-11-06T03:31:13Z- TerrainNav::motionUpdate() - currEstimate = (4061830, 594520.57, 50.11) 2024-11-06T03:31:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:31:13Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:31:13Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:31:13Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:31:13Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:31:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:31:13Z- variances([0], [2], and psi_berg) = 0.197741 0.216944 0.000000 2024-11-06T03:31:13Z- publishEstimatesDecoder() - at 1730863873.51 sec 2024-11-06T03:31:13Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:31:13Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061857.63 594503.83 53.64 2024-11-06T03:31:13Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061857.82 594504.50 53.86 lat/lon: 36.70 -121.94 2024-11-06T03:31:13Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.20 0.22 0.91 0.00 2024-11-06T03:31:13Z- AHRS_M2 msg: 1730863873.93 epoch sec; seqNo:5840 2024-11-06T03:31:13Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 4.21 psi 2024-11-06T03:31:13Z- handleDepth()- Depth_Keller msg: 1730863873.93 epoch sec; seqNo:6222; depth 50.09 2024-11-06T03:31:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863873.94 epoch sec; seqNo:6222; 4061829.18 north; 594520.57 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:31:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863873.51 2024-11-06T03:31:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:14Z- waiting for 1730863877.51, time is 1730863873.94 2024-11-06T03:31:14Z- AHRS_M2 msg: 1730863874.31 epoch sec; seqNo:5841 2024-11-06T03:31:14Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 4.21 psi 2024-11-06T03:31:14Z- handleDepth()- Depth_Keller msg: 1730863874.31 epoch sec; seqNo:6223; depth 50.07 2024-11-06T03:31:14Z- handleDVL() - DVL msg epochMillisec: 1730863874.34 2024-11-06T03:31:14Z- handleDvl() - RDI_Pathfinder msg: 1730863874.34 sec; seqNo:2132; #beams:4 2024-11-06T03:31:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.94 , 34.15 , 37.88 , 36.53 2024-11-06T03:31:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , -0.01 2024-11-06T03:31:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863874.35 epoch sec; seqNo:6223; 4061829.16 north; 594519.35 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:31:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863873.51 2024-11-06T03:31:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:14Z- waiting for 1730863877.51, time is 1730863874.35 2024-11-06T03:31:14Z- AHRS_M2 msg: 1730863874.71 epoch sec; seqNo:5842 2024-11-06T03:31:14Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 4.21 psi 2024-11-06T03:31:14Z- handleDepth()- Depth_Keller msg: 1730863874.71 epoch sec; seqNo:6224; depth 50.06 2024-11-06T03:31:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863874.73 epoch sec; seqNo:6224; 4061828.79 north; 594519.36 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:31:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863873.51 2024-11-06T03:31:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:14Z- waiting for 1730863877.51, time is 1730863874.73 2024-11-06T03:31:15Z- AHRS_M2 msg: 1730863875.12 epoch sec; seqNo:5843 2024-11-06T03:31:15Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 4.21 psi 2024-11-06T03:31:15Z- handleDepth()- Depth_Keller msg: 1730863875.12 epoch sec; seqNo:6225; depth 50.04 2024-11-06T03:31:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863875.13 epoch sec; seqNo:6225; 4061828.79 north; 594519.36 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:31:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863873.51 2024-11-06T03:31:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:15Z- waiting for 1730863877.51, time is 1730863875.13 2024-11-06T03:31:15Z- handleDepth()- Depth_Keller msg: 1730863875.53 epoch sec; seqNo:6226; depth 50.04 2024-11-06T03:31:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863875.54 epoch sec; seqNo:6226; 4061828.41 north; 594519.36 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:31:15Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:31:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863873.51 2024-11-06T03:31:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:15Z- waiting for 1730863877.51, time is 1730863875.54 2024-11-06T03:31:15Z- AHRS_M2 msg: 1730863875.92 epoch sec; seqNo:5844 2024-11-06T03:31:15Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 4.21 psi 2024-11-06T03:31:15Z- handleDepth()- Depth_Keller msg: 1730863875.93 epoch sec; seqNo:6227; depth 50.03 2024-11-06T03:31:15Z- handleDVL() - DVL msg epochMillisec: 1730863875.94 2024-11-06T03:31:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:31:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:31:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:31:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:31:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:31:15Z- handleDvl() - RDI_Pathfinder msg: 1730863875.94 sec; seqNo:2133; #beams:0 2024-11-06T03:31:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:31:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , -0.01 2024-11-06T03:31:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863875.95 epoch sec; seqNo:6227; 4061828.39 north; 594518.14 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:31:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863873.51 2024-11-06T03:31:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:16Z- waiting for 1730863877.51, time is 1730863875.95 2024-11-06T03:31:16Z- AHRS_M2 msg: 1730863876.33 epoch sec; seqNo:5845 2024-11-06T03:31:16Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 4.21 psi 2024-11-06T03:31:16Z- handleDepth()- Depth_Keller msg: 1730863876.34 epoch sec; seqNo:6228; depth 50.01 2024-11-06T03:31:16Z- handleDVL() - DVL msg epochMillisec: 1730863876.37 2024-11-06T03:31:16Z- handleDvl() - RDI_Pathfinder msg: 1730863876.37 sec; seqNo:2134; #beams:4 2024-11-06T03:31:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.94 , 34.15 , 37.54 , 36.70 2024-11-06T03:31:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.03 , -0.00 2024-11-06T03:31:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863876.38 epoch sec; seqNo:6228; 4061828.01 north; 594518.14 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:31:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863873.51 2024-11-06T03:31:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:16Z- waiting for 1730863877.51, time is 1730863876.38 2024-11-06T03:31:16Z- AHRS_M2 msg: 1730863876.73 epoch sec; seqNo:5846 2024-11-06T03:31:16Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 4.20 psi 2024-11-06T03:31:16Z- handleDepth()- Depth_Keller msg: 1730863876.74 epoch sec; seqNo:6229; depth 49.99 2024-11-06T03:31:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863876.75 epoch sec; seqNo:6229; 4061828.01 north; 594518.14 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:31:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863873.51 2024-11-06T03:31:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:16Z- waiting for 1730863877.51, time is 1730863876.75 2024-11-06T03:31:17Z- AHRS_M2 msg: 1730863877.13 epoch sec; seqNo:5847 2024-11-06T03:31:17Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 4.20 psi 2024-11-06T03:31:17Z- handleDepth()- Depth_Keller msg: 1730863877.14 epoch sec; seqNo:6230; depth 50.00 2024-11-06T03:31:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863877.16 epoch sec; seqNo:6230; 4061828.00 north; 594516.92 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:31:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863873.51 2024-11-06T03:31:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:17Z- waiting for 1730863877.51, time is 1730863877.16 2024-11-06T03:31:17Z- AHRS_M2 msg: 1730863877.54 epoch sec; seqNo:5848 2024-11-06T03:31:17Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 4.20 psi 2024-11-06T03:31:17Z- handleDepth()- Depth_Keller msg: 1730863877.55 epoch sec; seqNo:6231; depth 49.99 2024-11-06T03:31:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863877.56 epoch sec; seqNo:6231; 4061827.62 north; 594516.93 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:31:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863873.51 2024-11-06T03:31:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:17Z- waiting for 1730863877.51, time is 1730863877.56 2024-11-06T03:31:17Z- LcmTrn::updateTrn() >>>> pose time 1730863877.56, meas time 1730863876.37 <<<< 2024-11-06T03:31:17Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863877.56 2024-11-06T03:31:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863876.37 sec, ping # 0; waiting for INS data more recent than 1730863873.51... 2024-11-06T03:31:17Z- TerrainNav::motionUpdate() - currEstimate = (4061828, 594516.93, 49.99) 2024-11-06T03:31:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:31:17Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:31:17Z- TNavPF::Projecting Measurements 2024-11-06T03:31:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:31:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:31:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:31:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:31:17Z- TNavFilter::4 good beams. 2024-11-06T03:31:17Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:31:17Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:31:17Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:31:17Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:31:17Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:31:17Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:31:17Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:31:17Z- TerrainMapOctree: (vehN, vehE ) = (4061828.19, 594518.00). 2024-11-06T03:31:17Z- TerrainMapOctree: Min Distance = 244.82. 2024-11-06T03:31:17Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:31:17Z- Forcing Subcloud Comparison 2024-11-06T03:31:17Z- Weighting particles with subcloud comparison 2024-11-06T03:31:17Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:17Z- meanExpectedMeasDiff: 2.735064 delta_rms_squared: 1.893267 alpha: 0.989547 2024-11-06T03:31:17Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:17Z- meanExpectedMeasDiff: 0.900596 delta_rms_squared: 1.338173 alpha: 0.985274 2024-11-06T03:31:17Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:17Z- meanExpectedMeasDiff: 0.780882 delta_rms_squared: 1.650795 alpha: 0.988030 2024-11-06T03:31:17Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:17Z- meanExpectedMeasDiff: -0.646940 delta_rms_squared: 2.504836 alpha: 0.992079 2024-11-06T03:31:17Z- Subcloud NIS: 0.400114 2024-11-06T03:31:17Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:31:17Z- TNavPF:: sumWeights = 0.249634 2024-11-06T03:31:17Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:31:17Z- TNavPF::First Term of Map Covariance Matrix is 0.82 2024-11-06T03:31:17Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T03:31:17Z- TerrainNav::Resampling particle filter... 2024-11-06T03:31:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863876.37 sec. 2024-11-06T03:31:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:31:17Z- variances([0], [2], and psi_berg) = 0.213868 0.224087 0.000000 2024-11-06T03:31:17Z- publishEstimatesDecoder() - at 1730863877.56 sec 2024-11-06T03:31:17Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:31:17Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061855.62 594500.24 53.80 2024-11-06T03:31:17Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061855.85 594500.83 53.27 lat/lon: 36.70 -121.94 2024-11-06T03:31:17Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.22 0.69 0.00 2024-11-06T03:31:17Z- AHRS_M2 msg: 1730863877.96 epoch sec; seqNo:5849 2024-11-06T03:31:17Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 4.20 psi 2024-11-06T03:31:17Z- handleDepth()- Depth_Keller msg: 1730863877.98 epoch sec; seqNo:6232; depth 49.98 2024-11-06T03:31:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863878.00 epoch sec; seqNo:6232; 4061827.62 north; 594516.93 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:31:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863877.56 2024-11-06T03:31:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:18Z- waiting for 1730863881.56, time is 1730863878.00 2024-11-06T03:31:18Z- AHRS_M2 msg: 1730863878.35 epoch sec; seqNo:5850 2024-11-06T03:31:18Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 4.20 psi 2024-11-06T03:31:18Z- handleDepth()- Depth_Keller msg: 1730863878.36 epoch sec; seqNo:6233; depth 49.97 2024-11-06T03:31:18Z- handleDVL() - DVL msg epochMillisec: 1730863878.38 2024-11-06T03:31:18Z- handleDvl() - RDI_Pathfinder msg: 1730863878.38 sec; seqNo:2135; #beams:4 2024-11-06T03:31:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.60 , 34.32 , 37.72 , 36.70 2024-11-06T03:31:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.04 , 0.03 2024-11-06T03:31:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863878.39 epoch sec; seqNo:6233; 4061827.24 north; 594516.93 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:31:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863877.56 2024-11-06T03:31:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:18Z- waiting for 1730863881.56, time is 1730863878.39 2024-11-06T03:31:18Z- AHRS_M2 msg: 1730863878.75 epoch sec; seqNo:5851 2024-11-06T03:31:18Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 4.20 psi 2024-11-06T03:31:18Z- handleDepth()- Depth_Keller msg: 1730863878.76 epoch sec; seqNo:6234; depth 49.98 2024-11-06T03:31:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863878.78 epoch sec; seqNo:6234; 4061827.23 north; 594515.71 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:31:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863877.56 2024-11-06T03:31:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:18Z- waiting for 1730863881.56, time is 1730863878.78 2024-11-06T03:31:19Z- AHRS_M2 msg: 1730863879.15 epoch sec; seqNo:5852 2024-11-06T03:31:19Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 4.20 psi 2024-11-06T03:31:19Z- handleDepth()- Depth_Keller msg: 1730863879.17 epoch sec; seqNo:6235; depth 49.97 2024-11-06T03:31:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863879.17 epoch sec; seqNo:6235; 4061826.85 north; 594515.72 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:31:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863877.56 2024-11-06T03:31:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:19Z- waiting for 1730863881.56, time is 1730863879.17 2024-11-06T03:31:19Z- AHRS_M2 msg: 1730863879.57 epoch sec; seqNo:5853 2024-11-06T03:31:19Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.20 psi 2024-11-06T03:31:19Z- handleDepth()- Depth_Keller msg: 1730863879.58 epoch sec; seqNo:6236; depth 49.96 2024-11-06T03:31:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863879.59 epoch sec; seqNo:6236; 4061826.85 north; 594515.72 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:31:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863877.56 2024-11-06T03:31:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:19Z- waiting for 1730863881.56, time is 1730863879.59 2024-11-06T03:31:19Z- AHRS_M2 msg: 1730863879.96 epoch sec; seqNo:5854 2024-11-06T03:31:19Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.20 psi 2024-11-06T03:31:19Z- handleDepth()- Depth_Keller msg: 1730863879.97 epoch sec; seqNo:6237; depth 49.96 2024-11-06T03:31:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863879.99 epoch sec; seqNo:6237; 4061826.46 north; 594514.50 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:31:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863877.56 2024-11-06T03:31:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:20Z- waiting for 1730863881.56, time is 1730863879.99 2024-11-06T03:31:20Z- AHRS_M2 msg: 1730863880.37 epoch sec; seqNo:5855 2024-11-06T03:31:20Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.20 psi 2024-11-06T03:31:20Z- handleDepth()- Depth_Keller msg: 1730863880.37 epoch sec; seqNo:6238; depth 49.96 2024-11-06T03:31:20Z- handleDVL() - DVL msg epochMillisec: 1730863880.39 2024-11-06T03:31:20Z- handleDvl() - RDI_Pathfinder msg: 1730863880.39 sec; seqNo:2136; #beams:4 2024-11-06T03:31:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.94 , 34.32 , 37.21 , 37.04 2024-11-06T03:31:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , 0.04 2024-11-06T03:31:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863880.42 epoch sec; seqNo:6238; 4061826.46 north; 594514.50 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:31:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863877.56 2024-11-06T03:31:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:20Z- waiting for 1730863881.56, time is 1730863880.42 2024-11-06T03:31:20Z- AHRS_M2 msg: 1730863880.77 epoch sec; seqNo:5856 2024-11-06T03:31:20Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.20 psi 2024-11-06T03:31:20Z- handleDepth()- Depth_Keller msg: 1730863880.78 epoch sec; seqNo:6239; depth 49.94 2024-11-06T03:31:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863880.80 epoch sec; seqNo:6239; 4061826.08 north; 594514.50 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:31:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863877.56 2024-11-06T03:31:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:20Z- waiting for 1730863881.56, time is 1730863880.80 2024-11-06T03:31:21Z- AHRS_M2 msg: 1730863881.18 epoch sec; seqNo:5857 2024-11-06T03:31:21Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.20 psi 2024-11-06T03:31:21Z- handleDepth()- Depth_Keller msg: 1730863881.19 epoch sec; seqNo:6240; depth 49.95 2024-11-06T03:31:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863881.20 epoch sec; seqNo:6240; 4061826.08 north; 594514.50 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:31:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863877.56 2024-11-06T03:31:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:21Z- waiting for 1730863881.56, time is 1730863881.20 2024-11-06T03:31:21Z- AHRS_M2 msg: 1730863881.58 epoch sec; seqNo:5858 2024-11-06T03:31:21Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.20 psi 2024-11-06T03:31:21Z- handleDepth()- Depth_Keller msg: 1730863881.59 epoch sec; seqNo:6241; depth 49.94 2024-11-06T03:31:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863881.60 epoch sec; seqNo:6241; 4061825.69 north; 594513.29 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:31:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863877.56 2024-11-06T03:31:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:21Z- waiting for 1730863881.56, time is 1730863881.60 2024-11-06T03:31:21Z- LcmTrn::updateTrn() >>>> pose time 1730863881.60, meas time 1730863880.39 <<<< 2024-11-06T03:31:21Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863881.60 2024-11-06T03:31:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863880.39 sec, ping # 0; waiting for INS data more recent than 1730863877.56... 2024-11-06T03:31:21Z- TerrainNav::motionUpdate() - currEstimate = (4061826, 594513.29, 49.94) 2024-11-06T03:31:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:31:21Z- TerrainNav:: SubcloudNIS is 0.40, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:31:21Z- TNavPF::Projecting Measurements 2024-11-06T03:31:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:31:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:31:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:31:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:31:21Z- TNavFilter::4 good beams. 2024-11-06T03:31:21Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:31:21Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:31:21Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:31:21Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:31:21Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:31:21Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:31:21Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:31:21Z- TerrainMapOctree: (vehN, vehE ) = (4061826.26, 594514.37). 2024-11-06T03:31:21Z- TerrainMapOctree: Min Distance = 247.66. 2024-11-06T03:31:21Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:31:21Z- Forcing Subcloud Comparison 2024-11-06T03:31:21Z- Weighting particles with subcloud comparison 2024-11-06T03:31:21Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:21Z- meanExpectedMeasDiff: 2.174142 delta_rms_squared: 1.516215 alpha: 0.986981 2024-11-06T03:31:21Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:21Z- meanExpectedMeasDiff: 0.679169 delta_rms_squared: 0.914202 alpha: 0.978591 2024-11-06T03:31:21Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:21Z- meanExpectedMeasDiff: 0.086956 delta_rms_squared: 1.123064 alpha: 0.982503 2024-11-06T03:31:21Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:21Z- meanExpectedMeasDiff: -0.291814 delta_rms_squared: 1.235755 alpha: 0.984073 2024-11-06T03:31:21Z- Subcloud NIS: 0.395133 2024-11-06T03:31:21Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:31:21Z- TNavPF:: sumWeights = 0.412605 2024-11-06T03:31:21Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:31:21Z- TNavPF::First Term of Map Covariance Matrix is 0.75 2024-11-06T03:31:21Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T03:31:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863880.39 sec. 2024-11-06T03:31:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:31:21Z- variances([0], [2], and psi_berg) = 0.208925 0.224867 0.000000 2024-11-06T03:31:21Z- publishEstimatesDecoder() - at 1730863881.60 sec 2024-11-06T03:31:21Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:31:21Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061854.16 594496.49 52.90 2024-11-06T03:31:21Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061853.89 594497.18 52.92 lat/lon: 36.70 -121.94 2024-11-06T03:31:21Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.22 0.68 0.00 2024-11-06T03:31:21Z- AHRS_M2 msg: 1730863881.98 epoch sec; seqNo:5859 2024-11-06T03:31:21Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.20 psi 2024-11-06T03:31:21Z- handleDepth()- Depth_Keller msg: 1730863881.99 epoch sec; seqNo:6242; depth 49.93 2024-11-06T03:31:21Z- handleDVL() - DVL msg epochMillisec: 1730863882.00 2024-11-06T03:31:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:31:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:31:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:31:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:31:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:31:22Z- handleDvl() - RDI_Pathfinder msg: 1730863882.00 sec; seqNo:2137; #beams:0 2024-11-06T03:31:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:31:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , 0.04 2024-11-06T03:31:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863882.01 epoch sec; seqNo:6242; 4061825.69 north; 594513.29 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:31:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863881.60 2024-11-06T03:31:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:22Z- waiting for 1730863885.60, time is 1730863882.01 2024-11-06T03:31:22Z- AHRS_M2 msg: 1730863882.39 epoch sec; seqNo:5860 2024-11-06T03:31:22Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:22Z- handleDepth()- Depth_Keller msg: 1730863882.40 epoch sec; seqNo:6243; depth 49.93 2024-11-06T03:31:22Z- handleDVL() - DVL msg epochMillisec: 1730863882.42 2024-11-06T03:31:22Z- handleDvl() - RDI_Pathfinder msg: 1730863882.42 sec; seqNo:2138; #beams:4 2024-11-06T03:31:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.11 , 34.49 , 37.04 , 37.04 2024-11-06T03:31:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , -0.00 2024-11-06T03:31:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863882.44 epoch sec; seqNo:6243; 4061825.69 north; 594513.29 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:31:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863881.60 2024-11-06T03:31:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:22Z- waiting for 1730863885.60, time is 1730863882.44 2024-11-06T03:31:22Z- AHRS_M2 msg: 1730863882.79 epoch sec; seqNo:5861 2024-11-06T03:31:22Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:22Z- handleDepth()- Depth_Keller msg: 1730863882.80 epoch sec; seqNo:6244; depth 49.90 2024-11-06T03:31:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863882.81 epoch sec; seqNo:6244; 4061825.31 north; 594513.29 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:31:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863881.60 2024-11-06T03:31:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:22Z- waiting for 1730863885.60, time is 1730863882.81 2024-11-06T03:31:23Z- AHRS_M2 msg: 1730863883.19 epoch sec; seqNo:5862 2024-11-06T03:31:23Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:23Z- handleDepth()- Depth_Keller msg: 1730863883.20 epoch sec; seqNo:6245; depth 49.91 2024-11-06T03:31:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863883.22 epoch sec; seqNo:6245; 4061825.29 north; 594512.07 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:31:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863881.60 2024-11-06T03:31:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:23Z- waiting for 1730863885.60, time is 1730863883.22 2024-11-06T03:31:23Z- AHRS_M2 msg: 1730863883.60 epoch sec; seqNo:5863 2024-11-06T03:31:23Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:23Z- handleDepth()- Depth_Keller msg: 1730863883.62 epoch sec; seqNo:6246; depth 49.90 2024-11-06T03:31:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863883.63 epoch sec; seqNo:6246; 4061824.92 north; 594512.08 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:31:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863881.60 2024-11-06T03:31:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:23Z- waiting for 1730863885.60, time is 1730863883.63 2024-11-06T03:31:24Z- AHRS_M2 msg: 1730863884.01 epoch sec; seqNo:5864 2024-11-06T03:31:24Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:24Z- handleDepth()- Depth_Keller msg: 1730863884.01 epoch sec; seqNo:6247; depth 49.89 2024-11-06T03:31:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863884.02 epoch sec; seqNo:6247; 4061824.92 north; 594512.08 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:31:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863881.60 2024-11-06T03:31:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:24Z- waiting for 1730863885.60, time is 1730863884.02 2024-11-06T03:31:24Z- AHRS_M2 msg: 1730863884.42 epoch sec; seqNo:5865 2024-11-06T03:31:24Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:24Z- handleDepth()- Depth_Keller msg: 1730863884.42 epoch sec; seqNo:6248; depth 49.88 2024-11-06T03:31:24Z- handleDVL() - DVL msg epochMillisec: 1730863884.44 2024-11-06T03:31:24Z- handleDvl() - RDI_Pathfinder msg: 1730863884.44 sec; seqNo:2139; #beams:4 2024-11-06T03:31:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.28 , 34.32 , 36.87 , 37.38 2024-11-06T03:31:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , -0.04 2024-11-06T03:31:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863884.47 epoch sec; seqNo:6248; 4061824.54 north; 594512.08 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:31:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863881.60 2024-11-06T03:31:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:24Z- waiting for 1730863885.60, time is 1730863884.47 2024-11-06T03:31:24Z- AHRS_M2 msg: 1730863884.81 epoch sec; seqNo:5866 2024-11-06T03:31:24Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:24Z- handleDepth()- Depth_Keller msg: 1730863884.82 epoch sec; seqNo:6249; depth 49.87 2024-11-06T03:31:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863884.83 epoch sec; seqNo:6249; 4061824.52 north; 594510.86 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:31:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863881.60 2024-11-06T03:31:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:24Z- waiting for 1730863885.60, time is 1730863884.83 2024-11-06T03:31:25Z- AHRS_M2 msg: 1730863885.21 epoch sec; seqNo:5867 2024-11-06T03:31:25Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:25Z- handleDepth()- Depth_Keller msg: 1730863885.23 epoch sec; seqNo:6250; depth 49.85 2024-11-06T03:31:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863885.24 epoch sec; seqNo:6250; 4061824.14 north; 594510.86 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:31:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863881.60 2024-11-06T03:31:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:25Z- waiting for 1730863885.60, time is 1730863885.24 2024-11-06T03:31:25Z- AHRS_M2 msg: 1730863885.62 epoch sec; seqNo:5868 2024-11-06T03:31:25Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:25Z- handleDepth()- Depth_Keller msg: 1730863885.63 epoch sec; seqNo:6251; depth 49.85 2024-11-06T03:31:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863885.65 epoch sec; seqNo:6251; 4061824.14 north; 594510.86 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:31:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863881.60 2024-11-06T03:31:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:25Z- waiting for 1730863885.60, time is 1730863885.65 2024-11-06T03:31:25Z- LcmTrn::updateTrn() >>>> pose time 1730863885.65, meas time 1730863884.44 <<<< 2024-11-06T03:31:25Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863885.65 2024-11-06T03:31:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863884.44 sec, ping # 0; waiting for INS data more recent than 1730863881.60... 2024-11-06T03:31:25Z- TerrainNav::motionUpdate() - currEstimate = (4061824, 594510.86, 49.85) 2024-11-06T03:31:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:31:25Z- TerrainNav:: SubcloudNIS is 0.40, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:31:25Z- TNavPF::Projecting Measurements 2024-11-06T03:31:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:31:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:31:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:31:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:31:25Z- TNavFilter::4 good beams. 2024-11-06T03:31:25Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:31:25Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:31:25Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:31:25Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:31:25Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:31:25Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:31:25Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:31:25Z- TerrainMapOctree: (vehN, vehE ) = (4061824.60, 594511.59). 2024-11-06T03:31:25Z- TerrainMapOctree: Min Distance = 249.81. 2024-11-06T03:31:25Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:31:25Z- Forcing Subcloud Comparison 2024-11-06T03:31:25Z- Weighting particles with subcloud comparison 2024-11-06T03:31:25Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:25Z- meanExpectedMeasDiff: 1.809329 delta_rms_squared: 1.479003 alpha: 0.986658 2024-11-06T03:31:25Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:25Z- meanExpectedMeasDiff: 0.188412 delta_rms_squared: 1.115630 alpha: 0.982389 2024-11-06T03:31:25Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:25Z- meanExpectedMeasDiff: 0.403517 delta_rms_squared: 1.249718 alpha: 0.984248 2024-11-06T03:31:25Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:25Z- meanExpectedMeasDiff: -0.438957 delta_rms_squared: 1.310068 alpha: 0.984963 2024-11-06T03:31:25Z- Subcloud NIS: 0.395934 2024-11-06T03:31:25Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:31:25Z- TNavPF:: sumWeights = 0.465594 2024-11-06T03:31:25Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:31:25Z- TNavPF::First Term of Map Covariance Matrix is 0.61 2024-11-06T03:31:25Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T03:31:25Z- TerrainNav::Resampling particle filter... 2024-11-06T03:31:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863884.44 sec. 2024-11-06T03:31:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:31:25Z- variances([0], [2], and psi_berg) = 0.208956 0.226203 0.000000 2024-11-06T03:31:25Z- publishEstimatesDecoder() - at 1730863885.65 sec 2024-11-06T03:31:25Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:31:25Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061852.07 594494.11 53.07 2024-11-06T03:31:25Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061852.35 594494.76 52.60 lat/lon: 36.70 -121.94 2024-11-06T03:31:25Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.23 0.65 0.00 2024-11-06T03:31:26Z- AHRS_M2 msg: 1730863886.04 epoch sec; seqNo:5869 2024-11-06T03:31:26Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:26Z- handleDepth()- Depth_Keller msg: 1730863886.05 epoch sec; seqNo:6252; depth 49.82 2024-11-06T03:31:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863886.07 epoch sec; seqNo:6252; 4061823.77 north; 594510.87 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:31:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863885.65 2024-11-06T03:31:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:26Z- waiting for 1730863889.65, time is 1730863886.07 2024-11-06T03:31:26Z- AHRS_M2 msg: 1730863886.43 epoch sec; seqNo:5870 2024-11-06T03:31:26Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:26Z- handleDepth()- Depth_Keller msg: 1730863886.44 epoch sec; seqNo:6253; depth 49.83 2024-11-06T03:31:26Z- handleDVL() - DVL msg epochMillisec: 1730863886.46 2024-11-06T03:31:26Z- handleDvl() - RDI_Pathfinder msg: 1730863886.46 sec; seqNo:2140; #beams:4 2024-11-06T03:31:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.28 , 34.32 , 36.70 , 37.04 2024-11-06T03:31:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.03 , -0.02 2024-11-06T03:31:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863886.50 epoch sec; seqNo:6253; 4061823.75 north; 594509.65 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:31:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863885.65 2024-11-06T03:31:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:26Z- waiting for 1730863889.65, time is 1730863886.50 2024-11-06T03:31:26Z- handleDepth()- Depth_Keller msg: 1730863886.85 epoch sec; seqNo:6254; depth 49.81 2024-11-06T03:31:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863886.86 epoch sec; seqNo:6254; 4061823.37 north; 594509.65 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:31:27Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:31:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863885.65 2024-11-06T03:31:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:27Z- waiting for 1730863889.65, time is 1730863886.86 2024-11-06T03:31:27Z- AHRS_M2 msg: 1730863887.23 epoch sec; seqNo:5871 2024-11-06T03:31:27Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:27Z- handleDepth()- Depth_Keller msg: 1730863887.24 epoch sec; seqNo:6255; depth 49.79 2024-11-06T03:31:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863887.25 epoch sec; seqNo:6255; 4061823.37 north; 594509.65 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:31:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863885.65 2024-11-06T03:31:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:27Z- waiting for 1730863889.65, time is 1730863887.25 2024-11-06T03:31:27Z- AHRS_M2 msg: 1730863887.64 epoch sec; seqNo:5872 2024-11-06T03:31:27Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:27Z- handleDepth()- Depth_Keller msg: 1730863887.65 epoch sec; seqNo:6256; depth 49.79 2024-11-06T03:31:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863887.65 epoch sec; seqNo:6256; 4061822.98 north; 594508.44 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:31:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863885.65 2024-11-06T03:31:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:27Z- waiting for 1730863889.65, time is 1730863887.65 2024-11-06T03:31:28Z- AHRS_M2 msg: 1730863888.05 epoch sec; seqNo:5873 2024-11-06T03:31:28Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:28Z- handleDepth()- Depth_Keller msg: 1730863888.06 epoch sec; seqNo:6257; depth 49.78 2024-11-06T03:31:28Z- handleDVL() - DVL msg epochMillisec: 1730863888.06 2024-11-06T03:31:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:31:28Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:31:28Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:31:28Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:31:28Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:31:28Z- handleDvl() - RDI_Pathfinder msg: 1730863888.06 sec; seqNo:2141; #beams:0 2024-11-06T03:31:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:31:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.03 , -0.02 2024-11-06T03:31:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863888.07 epoch sec; seqNo:6257; 4061822.98 north; 594508.44 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:31:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863885.65 2024-11-06T03:31:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:28Z- waiting for 1730863889.65, time is 1730863888.07 2024-11-06T03:31:28Z- AHRS_M2 msg: 1730863888.45 epoch sec; seqNo:5874 2024-11-06T03:31:28Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:28Z- handleDepth()- Depth_Keller msg: 1730863888.45 epoch sec; seqNo:6258; depth 49.77 2024-11-06T03:31:28Z- handleDVL() - DVL msg epochMillisec: 1730863888.49 2024-11-06T03:31:28Z- handleDvl() - RDI_Pathfinder msg: 1730863888.49 sec; seqNo:2142; #beams:4 2024-11-06T03:31:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.11 , 34.49 , 36.87 , 37.21 2024-11-06T03:31:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.02 , 0.01 2024-11-06T03:31:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863888.52 epoch sec; seqNo:6258; 4061822.60 north; 594508.44 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:31:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863885.65 2024-11-06T03:31:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:28Z- waiting for 1730863889.65, time is 1730863888.52 2024-11-06T03:31:28Z- AHRS_M2 msg: 1730863888.85 epoch sec; seqNo:5875 2024-11-06T03:31:28Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:28Z- handleDepth()- Depth_Keller msg: 1730863888.86 epoch sec; seqNo:6259; depth 49.76 2024-11-06T03:31:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863888.88 epoch sec; seqNo:6259; 4061822.60 north; 594508.44 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:31:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863885.65 2024-11-06T03:31:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:29Z- waiting for 1730863889.65, time is 1730863888.88 2024-11-06T03:31:29Z- AHRS_M2 msg: 1730863889.26 epoch sec; seqNo:5876 2024-11-06T03:31:29Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:29Z- handleDepth()- Depth_Keller msg: 1730863889.27 epoch sec; seqNo:6260; depth 49.75 2024-11-06T03:31:29Z- handleDVL() - DVL msg epochMillisec: 1730863889.27 2024-11-06T03:31:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:31:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:31:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:31:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:31:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:31:29Z- handleDvl() - RDI_Pathfinder msg: 1730863889.27 sec; seqNo:2143; #beams:0 2024-11-06T03:31:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:31:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.02 , 0.01 2024-11-06T03:31:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863889.28 epoch sec; seqNo:6260; 4061822.59 north; 594507.22 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:31:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863885.65 2024-11-06T03:31:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:29Z- waiting for 1730863889.65, time is 1730863889.28 2024-11-06T03:31:29Z- AHRS_M2 msg: 1730863889.66 epoch sec; seqNo:5877 2024-11-06T03:31:29Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.20 psi 2024-11-06T03:31:29Z- handleDepth()- Depth_Keller msg: 1730863889.67 epoch sec; seqNo:6261; depth 49.73 2024-11-06T03:31:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863889.68 epoch sec; seqNo:6261; 4061822.21 north; 594507.22 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:31:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863885.65 2024-11-06T03:31:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:29Z- waiting for 1730863889.65, time is 1730863889.68 2024-11-06T03:31:29Z- LcmTrn::updateTrn() >>>> pose time 1730863889.68, meas time 1730863889.27 <<<< 2024-11-06T03:31:29Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863889.68 2024-11-06T03:31:29Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:31:29Z- TerrainNav::motionUpdate() - currEstimate = (4061822, 594507.22, 49.73) 2024-11-06T03:31:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:31:29Z- TerrainNav:: SubcloudNIS is 0.40, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:31:29Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:31:29Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:31:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:31:29Z- variances([0], [2], and psi_berg) = 0.211133 0.229045 0.000000 2024-11-06T03:31:29Z- publishEstimatesDecoder() - at 1730863889.68 sec 2024-11-06T03:31:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:31:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061850.17 594490.45 52.71 2024-11-06T03:31:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061850.41 594491.12 52.48 lat/lon: 36.70 -121.94 2024-11-06T03:31:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.23 0.90 0.00 2024-11-06T03:31:30Z- AHRS_M2 msg: 1730863890.06 epoch sec; seqNo:5878 2024-11-06T03:31:30Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.20 psi 2024-11-06T03:31:30Z- handleDepth()- Depth_Keller msg: 1730863890.08 epoch sec; seqNo:6262; depth 49.72 2024-11-06T03:31:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863890.09 epoch sec; seqNo:6262; 4061822.21 north; 594507.22 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:31:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863889.68 2024-11-06T03:31:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:30Z- waiting for 1730863893.68, time is 1730863890.09 2024-11-06T03:31:30Z- AHRS_M2 msg: 1730863890.47 epoch sec; seqNo:5879 2024-11-06T03:31:30Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:30Z- handleDepth()- Depth_Keller msg: 1730863890.48 epoch sec; seqNo:6263; depth 49.70 2024-11-06T03:31:30Z- handleDVL() - DVL msg epochMillisec: 1730863890.51 2024-11-06T03:31:30Z- handleDvl() - RDI_Pathfinder msg: 1730863890.51 sec; seqNo:2144; #beams:4 2024-11-06T03:31:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.28 , 34.49 , 36.87 , 37.04 2024-11-06T03:31:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , -0.00 2024-11-06T03:31:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863890.53 epoch sec; seqNo:6263; 4061821.82 north; 594506.01 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:31:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863889.68 2024-11-06T03:31:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:30Z- waiting for 1730863893.68, time is 1730863890.53 2024-11-06T03:31:30Z- AHRS_M2 msg: 1730863890.87 epoch sec; seqNo:5880 2024-11-06T03:31:30Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:30Z- handleDepth()- Depth_Keller msg: 1730863890.88 epoch sec; seqNo:6264; depth 49.70 2024-11-06T03:31:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863890.89 epoch sec; seqNo:6264; 4061821.82 north; 594506.01 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:31:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863889.68 2024-11-06T03:31:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:31Z- waiting for 1730863893.68, time is 1730863890.89 2024-11-06T03:31:31Z- AHRS_M2 msg: 1730863891.28 epoch sec; seqNo:5881 2024-11-06T03:31:31Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:31Z- handleDepth()- Depth_Keller msg: 1730863891.29 epoch sec; seqNo:6265; depth 49.70 2024-11-06T03:31:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863891.29 epoch sec; seqNo:6265; 4061821.44 north; 594506.01 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:31:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863889.68 2024-11-06T03:31:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:31Z- waiting for 1730863893.68, time is 1730863891.29 2024-11-06T03:31:31Z- AHRS_M2 msg: 1730863891.68 epoch sec; seqNo:5882 2024-11-06T03:31:31Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 4.21 psi 2024-11-06T03:31:31Z- handleDepth()- Depth_Keller msg: 1730863891.69 epoch sec; seqNo:6266; depth 49.67 2024-11-06T03:31:31Z- handleDVL() - DVL msg epochMillisec: 1730863891.69 2024-11-06T03:31:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:31:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:31:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:31:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:31:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:31:31Z- handleDvl() - RDI_Pathfinder msg: 1730863891.69 sec; seqNo:2145; #beams:0 2024-11-06T03:31:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:31:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , -0.00 2024-11-06T03:31:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863891.70 epoch sec; seqNo:6266; 4061821.44 north; 594506.01 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:31:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863889.68 2024-11-06T03:31:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:31Z- waiting for 1730863893.68, time is 1730863891.70 2024-11-06T03:31:32Z- AHRS_M2 msg: 1730863892.08 epoch sec; seqNo:5883 2024-11-06T03:31:32Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 4.21 psi 2024-11-06T03:31:32Z- handleDepth()- Depth_Keller msg: 1730863892.09 epoch sec; seqNo:6267; depth 49.67 2024-11-06T03:31:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863892.10 epoch sec; seqNo:6267; 4061821.05 north; 594504.80 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:31:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863889.68 2024-11-06T03:31:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:32Z- waiting for 1730863893.68, time is 1730863892.10 2024-11-06T03:31:32Z- AHRS_M2 msg: 1730863892.49 epoch sec; seqNo:5884 2024-11-06T03:31:32Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 4.21 psi 2024-11-06T03:31:32Z- handleDepth()- Depth_Keller msg: 1730863892.49 epoch sec; seqNo:6268; depth 49.66 2024-11-06T03:31:32Z- handleDVL() - DVL msg epochMillisec: 1730863892.51 2024-11-06T03:31:32Z- handleDvl() - RDI_Pathfinder msg: 1730863892.51 sec; seqNo:2146; #beams:4 2024-11-06T03:31:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.94 , 34.32 , 36.87 , 37.04 2024-11-06T03:31:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.06 , -0.02 2024-11-06T03:31:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863892.55 epoch sec; seqNo:6268; 4061821.05 north; 594504.80 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:31:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863889.68 2024-11-06T03:31:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:32Z- waiting for 1730863893.68, time is 1730863892.55 2024-11-06T03:31:32Z- AHRS_M2 msg: 1730863892.89 epoch sec; seqNo:5885 2024-11-06T03:31:32Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 4.21 psi 2024-11-06T03:31:32Z- handleDepth()- Depth_Keller msg: 1730863892.90 epoch sec; seqNo:6269; depth 49.66 2024-11-06T03:31:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863892.91 epoch sec; seqNo:6269; 4061820.67 north; 594504.80 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:31:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863889.68 2024-11-06T03:31:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:33Z- waiting for 1730863893.68, time is 1730863892.91 2024-11-06T03:31:33Z- AHRS_M2 msg: 1730863893.30 epoch sec; seqNo:5886 2024-11-06T03:31:33Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 4.20 psi 2024-11-06T03:31:33Z- handleDepth()- Depth_Keller msg: 1730863893.31 epoch sec; seqNo:6270; depth 49.65 2024-11-06T03:31:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863893.32 epoch sec; seqNo:6270; 4061820.67 north; 594504.80 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:31:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863889.68 2024-11-06T03:31:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:33Z- waiting for 1730863893.68, time is 1730863893.32 2024-11-06T03:31:33Z- AHRS_M2 msg: 1730863893.70 epoch sec; seqNo:5887 2024-11-06T03:31:33Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 4.20 psi 2024-11-06T03:31:33Z- handleDepth()- Depth_Keller msg: 1730863893.70 epoch sec; seqNo:6271; depth 49.64 2024-11-06T03:31:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863893.72 epoch sec; seqNo:6271; 4061820.65 north; 594503.58 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:31:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863889.68 2024-11-06T03:31:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:33Z- waiting for 1730863893.68, time is 1730863893.72 2024-11-06T03:31:33Z- LcmTrn::updateTrn() >>>> pose time 1730863893.72, meas time 1730863892.51 <<<< 2024-11-06T03:31:33Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863893.72 2024-11-06T03:31:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863892.51 sec, ping # 0; waiting for INS data more recent than 1730863889.68... 2024-11-06T03:31:33Z- TerrainNav::motionUpdate() - currEstimate = (4061821, 594503.58, 49.64) 2024-11-06T03:31:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:31:33Z- TerrainNav:: SubcloudNIS is 0.40, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:31:33Z- TNavPF::Projecting Measurements 2024-11-06T03:31:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:31:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:31:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:31:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:31:33Z- TNavFilter::4 good beams. 2024-11-06T03:31:33Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:31:33Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:31:33Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:31:33Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:31:33Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:31:33Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:31:33Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:31:33Z- TerrainMapOctree: (vehN, vehE ) = (4061821.12, 594504.67). 2024-11-06T03:31:33Z- TerrainMapOctree: Min Distance = 255.43. 2024-11-06T03:31:33Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:31:33Z- Forcing Subcloud Comparison 2024-11-06T03:31:33Z- Weighting particles with subcloud comparison 2024-11-06T03:31:33Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:33Z- meanExpectedMeasDiff: 0.677083 delta_rms_squared: 1.445300 alpha: 0.986351 2024-11-06T03:31:34Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:34Z- meanExpectedMeasDiff: 1.324114 delta_rms_squared: 2.047758 alpha: 0.990328 2024-11-06T03:31:34Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:34Z- meanExpectedMeasDiff: -0.106364 delta_rms_squared: 2.325692 alpha: 0.991474 2024-11-06T03:31:34Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:34Z- meanExpectedMeasDiff: -1.281835 delta_rms_squared: 1.523785 alpha: 0.987045 2024-11-06T03:31:34Z- Subcloud NIS: 0.405725 2024-11-06T03:31:34Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:31:34Z- TNavPF:: sumWeights = 0.349784 2024-11-06T03:31:34Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:31:34Z- TNavPF::First Term of Map Covariance Matrix is 0.27 2024-11-06T03:31:34Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T03:31:34Z- TerrainNav::Resampling particle filter... 2024-11-06T03:31:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863892.51 sec. 2024-11-06T03:31:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:31:34Z- variances([0], [2], and psi_berg) = 0.209482 0.234203 0.000000 2024-11-06T03:31:34Z- publishEstimatesDecoder() - at 1730863893.72 sec 2024-11-06T03:31:34Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:31:34Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061848.62 594486.83 52.16 2024-11-06T03:31:34Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061848.90 594487.51 52.29 lat/lon: 36.70 -121.94 2024-11-06T03:31:34Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.23 0.70 0.00 2024-11-06T03:31:34Z- AHRS_M2 msg: 1730863894.12 epoch sec; seqNo:5888 2024-11-06T03:31:34Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 4.20 psi 2024-11-06T03:31:34Z- handleDepth()- Depth_Keller msg: 1730863894.13 epoch sec; seqNo:6272; depth 49.64 2024-11-06T03:31:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863894.18 epoch sec; seqNo:6272; 4061820.28 north; 594503.58 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:31:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863893.72 2024-11-06T03:31:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:34Z- waiting for 1730863897.72, time is 1730863894.18 2024-11-06T03:31:34Z- AHRS_M2 msg: 1730863894.51 epoch sec; seqNo:5889 2024-11-06T03:31:34Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 4.20 psi 2024-11-06T03:31:34Z- handleDepth()- Depth_Keller msg: 1730863894.53 epoch sec; seqNo:6273; depth 49.65 2024-11-06T03:31:34Z- handleDVL() - DVL msg epochMillisec: 1730863894.55 2024-11-06T03:31:34Z- handleDvl() - RDI_Pathfinder msg: 1730863894.55 sec; seqNo:2147; #beams:4 2024-11-06T03:31:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.11 , 34.32 , 36.70 , 37.21 2024-11-06T03:31:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.07 , 0.01 2024-11-06T03:31:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863894.59 epoch sec; seqNo:6273; 4061820.28 north; 594503.58 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:31:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863893.72 2024-11-06T03:31:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:34Z- waiting for 1730863897.72, time is 1730863894.59 2024-11-06T03:31:34Z- handleDepth()- Depth_Keller msg: 1730863894.93 epoch sec; seqNo:6274; depth 49.64 2024-11-06T03:31:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863894.94 epoch sec; seqNo:6274; 4061819.88 north; 594502.37 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:31:35Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:31:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863893.72 2024-11-06T03:31:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:35Z- waiting for 1730863897.72, time is 1730863894.94 2024-11-06T03:31:35Z- AHRS_M2 msg: 1730863895.32 epoch sec; seqNo:5890 2024-11-06T03:31:35Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 4.20 psi 2024-11-06T03:31:35Z- handleDepth()- Depth_Keller msg: 1730863895.33 epoch sec; seqNo:6275; depth 49.63 2024-11-06T03:31:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863895.35 epoch sec; seqNo:6275; 4061819.88 north; 594502.37 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:31:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863893.72 2024-11-06T03:31:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:35Z- waiting for 1730863897.72, time is 1730863895.35 2024-11-06T03:31:35Z- AHRS_M2 msg: 1730863895.72 epoch sec; seqNo:5891 2024-11-06T03:31:35Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 4.19 psi 2024-11-06T03:31:35Z- handleDepth()- Depth_Keller msg: 1730863895.73 epoch sec; seqNo:6276; depth 49.62 2024-11-06T03:31:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863895.74 epoch sec; seqNo:6276; 4061819.50 north; 594502.37 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:31:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863893.72 2024-11-06T03:31:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:35Z- waiting for 1730863897.72, time is 1730863895.74 2024-11-06T03:31:36Z- AHRS_M2 msg: 1730863896.12 epoch sec; seqNo:5892 2024-11-06T03:31:36Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 4.19 psi 2024-11-06T03:31:36Z- handleDepth()- Depth_Keller msg: 1730863896.13 epoch sec; seqNo:6277; depth 49.63 2024-11-06T03:31:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863896.15 epoch sec; seqNo:6277; 4061819.50 north; 594502.37 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:31:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863893.72 2024-11-06T03:31:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:36Z- waiting for 1730863897.72, time is 1730863896.15 2024-11-06T03:31:36Z- AHRS_M2 msg: 1730863896.53 epoch sec; seqNo:5893 2024-11-06T03:31:36Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 4.19 psi 2024-11-06T03:31:36Z- handleDepth()- Depth_Keller msg: 1730863896.54 epoch sec; seqNo:6278; depth 49.63 2024-11-06T03:31:36Z- handleDVL() - DVL msg epochMillisec: 1730863896.57 2024-11-06T03:31:36Z- handleDvl() - RDI_Pathfinder msg: 1730863896.57 sec; seqNo:2148; #beams:4 2024-11-06T03:31:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.77 , 34.32 , 36.70 , 37.38 2024-11-06T03:31:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.02 , 0.04 2024-11-06T03:31:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863896.59 epoch sec; seqNo:6278; 4061819.49 north; 594501.15 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:31:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863893.72 2024-11-06T03:31:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:36Z- waiting for 1730863897.72, time is 1730863896.59 2024-11-06T03:31:36Z- AHRS_M2 msg: 1730863896.93 epoch sec; seqNo:5894 2024-11-06T03:31:36Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 4.19 psi 2024-11-06T03:31:36Z- handleDepth()- Depth_Keller msg: 1730863896.94 epoch sec; seqNo:6279; depth 49.61 2024-11-06T03:31:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863896.95 epoch sec; seqNo:6279; 4061819.11 north; 594501.16 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:31:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863893.72 2024-11-06T03:31:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:37Z- waiting for 1730863897.72, time is 1730863896.95 2024-11-06T03:31:37Z- AHRS_M2 msg: 1730863897.34 epoch sec; seqNo:5895 2024-11-06T03:31:37Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 4.19 psi 2024-11-06T03:31:37Z- handleDepth()- Depth_Keller msg: 1730863897.35 epoch sec; seqNo:6280; depth 49.61 2024-11-06T03:31:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863897.36 epoch sec; seqNo:6280; 4061819.11 north; 594501.16 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:31:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863893.72 2024-11-06T03:31:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:37Z- waiting for 1730863897.72, time is 1730863897.36 2024-11-06T03:31:37Z- AHRS_M2 msg: 1730863897.74 epoch sec; seqNo:5896 2024-11-06T03:31:37Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 4.19 psi 2024-11-06T03:31:37Z- handleDepth()- Depth_Keller msg: 1730863897.74 epoch sec; seqNo:6281; depth 49.60 2024-11-06T03:31:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863897.75 epoch sec; seqNo:6281; 4061818.73 north; 594501.16 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:31:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863893.72 2024-11-06T03:31:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:37Z- waiting for 1730863897.72, time is 1730863897.75 2024-11-06T03:31:37Z- LcmTrn::updateTrn() >>>> pose time 1730863897.75, meas time 1730863896.57 <<<< 2024-11-06T03:31:37Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863897.75 2024-11-06T03:31:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863896.57 sec, ping # 0; waiting for INS data more recent than 1730863893.72... 2024-11-06T03:31:37Z- TerrainNav::motionUpdate() - currEstimate = (4061819, 594501.16, 49.60) 2024-11-06T03:31:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:31:37Z- TerrainNav:: SubcloudNIS is 0.41, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:31:37Z- TNavPF::Projecting Measurements 2024-11-06T03:31:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:31:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:31:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:31:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:31:37Z- TNavFilter::4 good beams. 2024-11-06T03:31:37Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:31:37Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:31:37Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:31:37Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:31:37Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:31:37Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:31:37Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:31:37Z- TerrainMapOctree: (vehN, vehE ) = (4061819.29, 594501.87). 2024-11-06T03:31:37Z- TerrainMapOctree: Min Distance = 257.63. 2024-11-06T03:31:37Z- TerrainMapOctree: Using tile 5. 2024-11-06T03:31:38Z- Forcing Subcloud Comparison 2024-11-06T03:31:38Z- Weighting particles with subcloud comparison 2024-11-06T03:31:38Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:38Z- meanExpectedMeasDiff: -0.180945 delta_rms_squared: 1.150546 alpha: 0.982914 2024-11-06T03:31:38Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:38Z- meanExpectedMeasDiff: 1.506639 delta_rms_squared: 1.089970 alpha: 0.981981 2024-11-06T03:31:38Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:38Z- meanExpectedMeasDiff: -1.083669 delta_rms_squared: 1.428582 alpha: 0.986193 2024-11-06T03:31:38Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:38Z- meanExpectedMeasDiff: -0.597533 delta_rms_squared: 1.691267 alpha: 0.988313 2024-11-06T03:31:38Z- Subcloud NIS: 0.410204 2024-11-06T03:31:38Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:31:38Z- TNavPF:: sumWeights = 0.391971 2024-11-06T03:31:38Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:31:38Z- TNavPF::First Term of Map Covariance Matrix is 0.22 2024-11-06T03:31:38Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T03:31:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863896.57 sec. 2024-11-06T03:31:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:31:38Z- variances([0], [2], and psi_berg) = 0.200856 0.239582 0.000000 2024-11-06T03:31:38Z- publishEstimatesDecoder() - at 1730863897.75 sec 2024-11-06T03:31:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:31:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061847.21 594485.76 52.22 2024-11-06T03:31:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061846.95 594485.10 52.30 lat/lon: 36.70 -121.94 2024-11-06T03:31:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.20 0.24 0.68 0.00 2024-11-06T03:31:38Z- AHRS_M2 msg: 1730863898.15 epoch sec; seqNo:5897 2024-11-06T03:31:38Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 4.19 psi 2024-11-06T03:31:38Z- handleDepth()- Depth_Keller msg: 1730863898.16 epoch sec; seqNo:6282; depth 49.59 2024-11-06T03:31:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863898.23 epoch sec; seqNo:6282; 4061818.72 north; 594499.94 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:31:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863897.75 2024-11-06T03:31:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:38Z- waiting for 1730863901.75, time is 1730863898.23 2024-11-06T03:31:38Z- AHRS_M2 msg: 1730863898.55 epoch sec; seqNo:5898 2024-11-06T03:31:38Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 4.19 psi 2024-11-06T03:31:38Z- handleDepth()- Depth_Keller msg: 1730863898.55 epoch sec; seqNo:6283; depth 49.59 2024-11-06T03:31:38Z- handleDVL() - DVL msg epochMillisec: 1730863898.58 2024-11-06T03:31:38Z- handleDvl() - RDI_Pathfinder msg: 1730863898.58 sec; seqNo:2149; #beams:4 2024-11-06T03:31:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.60 , 34.66 , 36.53 , 37.54 2024-11-06T03:31:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.01 2024-11-06T03:31:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863898.61 epoch sec; seqNo:6283; 4061818.34 north; 594499.94 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:31:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863897.75 2024-11-06T03:31:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:38Z- waiting for 1730863901.75, time is 1730863898.61 2024-11-06T03:31:38Z- handleDepth()- Depth_Keller msg: 1730863898.97 epoch sec; seqNo:6284; depth 49.59 2024-11-06T03:31:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863898.98 epoch sec; seqNo:6284; 4061818.34 north; 594499.94 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:31:39Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:31:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863897.75 2024-11-06T03:31:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:39Z- waiting for 1730863901.75, time is 1730863898.98 2024-11-06T03:31:39Z- AHRS_M2 msg: 1730863899.36 epoch sec; seqNo:5899 2024-11-06T03:31:39Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 4.19 psi 2024-11-06T03:31:39Z- handleDepth()- Depth_Keller msg: 1730863899.36 epoch sec; seqNo:6285; depth 49.58 2024-11-06T03:31:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863899.38 epoch sec; seqNo:6285; 4061817.96 north; 594499.95 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:31:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863897.75 2024-11-06T03:31:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:39Z- waiting for 1730863901.75, time is 1730863899.38 2024-11-06T03:31:39Z- AHRS_M2 msg: 1730863899.76 epoch sec; seqNo:5900 2024-11-06T03:31:39Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 4.19 psi 2024-11-06T03:31:39Z- handleDepth()- Depth_Keller msg: 1730863899.76 epoch sec; seqNo:6286; depth 49.57 2024-11-06T03:31:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863899.77 epoch sec; seqNo:6286; 4061817.95 north; 594498.73 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:31:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863897.75 2024-11-06T03:31:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:39Z- waiting for 1730863901.75, time is 1730863899.77 2024-11-06T03:31:40Z- AHRS_M2 msg: 1730863900.19 epoch sec; seqNo:5901 2024-11-06T03:31:40Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 4.19 psi 2024-11-06T03:31:40Z- handleDepth()- Depth_Keller msg: 1730863900.21 epoch sec; seqNo:6287; depth 49.56 2024-11-06T03:31:40Z- handleDVL() - DVL msg epochMillisec: 1730863900.25 2024-11-06T03:31:40Z- handleDvl() - RDI_Pathfinder msg: 1730863900.25 sec; seqNo:2150; #beams:4 2024-11-06T03:31:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.60 , 34.49 , 36.19 , 37.72 2024-11-06T03:31:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , -0.04 2024-11-06T03:31:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863900.26 epoch sec; seqNo:6287; 4061817.57 north; 594498.73 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:31:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863897.75 2024-11-06T03:31:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:40Z- waiting for 1730863901.75, time is 1730863900.26 2024-11-06T03:31:40Z- AHRS_M2 msg: 1730863900.57 epoch sec; seqNo:5902 2024-11-06T03:31:40Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.19 psi 2024-11-06T03:31:40Z- handleDepth()- Depth_Keller msg: 1730863900.58 epoch sec; seqNo:6288; depth 49.57 2024-11-06T03:31:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863900.60 epoch sec; seqNo:6288; 4061817.57 north; 594498.73 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:31:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863897.75 2024-11-06T03:31:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:40Z- waiting for 1730863901.75, time is 1730863900.60 2024-11-06T03:31:40Z- AHRS_M2 msg: 1730863900.98 epoch sec; seqNo:5903 2024-11-06T03:31:40Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.19 psi 2024-11-06T03:31:40Z- handleDepth()- Depth_Keller msg: 1730863900.99 epoch sec; seqNo:6289; depth 49.56 2024-11-06T03:31:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863901.00 epoch sec; seqNo:6289; 4061817.18 north; 594497.52 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:31:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863897.75 2024-11-06T03:31:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:41Z- waiting for 1730863901.75, time is 1730863901.00 2024-11-06T03:31:41Z- AHRS_M2 msg: 1730863901.37 epoch sec; seqNo:5904 2024-11-06T03:31:41Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.18 psi 2024-11-06T03:31:41Z- handleDepth()- Depth_Keller msg: 1730863901.38 epoch sec; seqNo:6290; depth 49.55 2024-11-06T03:31:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863901.39 epoch sec; seqNo:6290; 4061817.18 north; 594497.52 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:31:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863897.75 2024-11-06T03:31:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:41Z- waiting for 1730863901.75, time is 1730863901.39 2024-11-06T03:31:41Z- AHRS_M2 msg: 1730863901.78 epoch sec; seqNo:5905 2024-11-06T03:31:41Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.18 psi 2024-11-06T03:31:41Z- handleDepth()- Depth_Keller msg: 1730863901.79 epoch sec; seqNo:6291; depth 49.55 2024-11-06T03:31:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863901.81 epoch sec; seqNo:6291; 4061816.80 north; 594497.52 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:31:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863897.75 2024-11-06T03:31:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:41Z- waiting for 1730863901.75, time is 1730863901.81 2024-11-06T03:31:41Z- LcmTrn::updateTrn() >>>> pose time 1730863901.81, meas time 1730863900.25 <<<< 2024-11-06T03:31:41Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863901.81 2024-11-06T03:31:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863900.25 sec, ping # 0; waiting for INS data more recent than 1730863897.75... 2024-11-06T03:31:41Z- TerrainNav::motionUpdate() - currEstimate = (4061817, 594497.52, 49.55) 2024-11-06T03:31:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:31:41Z- TerrainNav:: SubcloudNIS is 0.41, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:31:41Z- TNavPF::Projecting Measurements 2024-11-06T03:31:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:31:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:31:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:31:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:31:41Z- TNavFilter::4 good beams. 2024-11-06T03:31:41Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:31:41Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:31:41Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:31:41Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:31:41Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:31:41Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:31:41Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:31:41Z- TerrainMapOctree: (vehN, vehE ) = (4061817.54, 594498.92). 2024-11-06T03:31:41Z- TerrainMapOctree: Min Distance = 257.92. 2024-11-06T03:31:41Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:31:42Z- TerrainMapOctree: Switching to tile 4. 2024-11-06T03:31:43Z- TerrainMapOctree::Octree tile load /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile4.bo took 1.246326 seconds. 2024-11-06T03:31:43Z- Forcing Subcloud Comparison 2024-11-06T03:31:43Z- Weighting particles with subcloud comparison 2024-11-06T03:31:43Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:43Z- meanExpectedMeasDiff: -0.506186 delta_rms_squared: 1.463039 alpha: 0.986514 2024-11-06T03:31:43Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:43Z- meanExpectedMeasDiff: 2.562670 delta_rms_squared: 1.446627 alpha: 0.986363 2024-11-06T03:31:43Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:43Z- meanExpectedMeasDiff: -1.358891 delta_rms_squared: 1.218843 alpha: 0.983856 2024-11-06T03:31:43Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:43Z- meanExpectedMeasDiff: -0.349287 delta_rms_squared: 1.253084 alpha: 0.984290 2024-11-06T03:31:43Z- Subcloud NIS: 0.421623 2024-11-06T03:31:43Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:31:43Z- TNavPF:: sumWeights = 0.203518 2024-11-06T03:31:43Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:31:43Z- TNavPF::First Term of Map Covariance Matrix is 0.18 2024-11-06T03:31:43Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T03:31:43Z- TerrainNav::Resampling particle filter... 2024-11-06T03:31:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863900.25 sec. 2024-11-06T03:31:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:31:43Z- variances([0], [2], and psi_berg) = 0.214874 0.258685 0.000000 2024-11-06T03:31:43Z- publishEstimatesDecoder() - at 1730863901.81 sec 2024-11-06T03:31:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:31:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061844.78 594480.72 52.31 2024-11-06T03:31:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061845.07 594481.42 52.17 lat/lon: 36.70 -121.94 2024-11-06T03:31:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.26 0.65 0.00 2024-11-06T03:31:43Z- AHRS_M2 msg: 1730863902.22 epoch sec; seqNo:5906 2024-11-06T03:31:43Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.18 psi 2024-11-06T03:31:43Z- handleDepth()- Depth_Keller msg: 1730863902.23 epoch sec; seqNo:6292; depth 49.55 2024-11-06T03:31:43Z- handleDVL() - DVL msg epochMillisec: 1730863902.25 2024-11-06T03:31:43Z- handleDvl() - RDI_Pathfinder msg: 1730863902.25 sec; seqNo:2151; #beams:4 2024-11-06T03:31:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.60 , 34.15 , 36.02 , 37.72 2024-11-06T03:31:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.04 , -0.03 2024-11-06T03:31:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863902.27 epoch sec; seqNo:6292; 4061816.80 north; 594497.52 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:31:43Z- handleDepth()- Depth_Keller msg: 1730863902.62 epoch sec; seqNo:6293; depth 49.55 2024-11-06T03:31:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863902.64 epoch sec; seqNo:6293; 4061816.41 north; 594496.30 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:31:43Z- AHRS_M2 msg: 1730863902.99 epoch sec; seqNo:5907 2024-11-06T03:31:43Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.18 psi 2024-11-06T03:31:43Z- handleDepth()- Depth_Keller msg: 1730863903.00 epoch sec; seqNo:6294; depth 49.55 2024-11-06T03:31:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863903.01 epoch sec; seqNo:6294; 4061816.41 north; 594496.30 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:31:43Z- handleDepth()- Depth_Keller msg: 1730863903.41 epoch sec; seqNo:6295; depth 49.55 2024-11-06T03:31:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863903.42 epoch sec; seqNo:6295; 4061816.03 north; 594496.31 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:31:43Z- 11 msgs in 200 ms and 12 handle calls 2024-11-06T03:31:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863901.81 2024-11-06T03:31:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:43Z- waiting for 1730863905.81, time is 1730863903.42 2024-11-06T03:31:43Z- handleDepth()- Depth_Keller msg: 1730863903.82 epoch sec; seqNo:6296; depth 49.56 2024-11-06T03:31:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863903.83 epoch sec; seqNo:6296; 4061816.03 north; 594496.31 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:31:43Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:31:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863901.81 2024-11-06T03:31:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:43Z- waiting for 1730863905.81, time is 1730863903.83 2024-11-06T03:31:44Z- AHRS_M2 msg: 1730863904.23 epoch sec; seqNo:5908 2024-11-06T03:31:44Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.17 psi 2024-11-06T03:31:44Z- handleDepth()- Depth_Keller msg: 1730863904.23 epoch sec; seqNo:6297; depth 49.56 2024-11-06T03:31:44Z- handleDVL() - DVL msg epochMillisec: 1730863904.26 2024-11-06T03:31:44Z- handleDvl() - RDI_Pathfinder msg: 1730863904.26 sec; seqNo:2152; #beams:4 2024-11-06T03:31:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.26 , 34.15 , 35.68 , 37.88 2024-11-06T03:31:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.06 , 0.02 2024-11-06T03:31:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863904.28 epoch sec; seqNo:6297; 4061815.64 north; 594495.09 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:31:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863901.81 2024-11-06T03:31:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:44Z- waiting for 1730863905.81, time is 1730863904.28 2024-11-06T03:31:44Z- AHRS_M2 msg: 1730863904.62 epoch sec; seqNo:5909 2024-11-06T03:31:44Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 4.17 psi 2024-11-06T03:31:44Z- handleDepth()- Depth_Keller msg: 1730863904.63 epoch sec; seqNo:6298; depth 49.58 2024-11-06T03:31:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863904.67 epoch sec; seqNo:6298; 4061815.64 north; 594495.09 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:31:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863901.81 2024-11-06T03:31:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:44Z- waiting for 1730863905.81, time is 1730863904.67 2024-11-06T03:31:45Z- AHRS_M2 msg: 1730863905.01 epoch sec; seqNo:5910 2024-11-06T03:31:45Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 4.17 psi 2024-11-06T03:31:45Z- handleDepth()- Depth_Keller msg: 1730863905.02 epoch sec; seqNo:6299; depth 49.59 2024-11-06T03:31:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863905.03 epoch sec; seqNo:6299; 4061815.26 north; 594495.10 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:31:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863901.81 2024-11-06T03:31:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:45Z- waiting for 1730863905.81, time is 1730863905.03 2024-11-06T03:31:45Z- AHRS_M2 msg: 1730863905.41 epoch sec; seqNo:5911 2024-11-06T03:31:45Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 4.16 psi 2024-11-06T03:31:45Z- handleDepth()- Depth_Keller msg: 1730863905.43 epoch sec; seqNo:6300; depth 49.58 2024-11-06T03:31:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863905.45 epoch sec; seqNo:6300; 4061815.24 north; 594493.88 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:31:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863901.81 2024-11-06T03:31:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:45Z- waiting for 1730863905.81, time is 1730863905.45 2024-11-06T03:31:45Z- AHRS_M2 msg: 1730863905.82 epoch sec; seqNo:5912 2024-11-06T03:31:45Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.16 psi 2024-11-06T03:31:45Z- handleDepth()- Depth_Keller msg: 1730863905.82 epoch sec; seqNo:6301; depth 49.60 2024-11-06T03:31:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863905.84 epoch sec; seqNo:6301; 4061814.86 north; 594493.88 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:31:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863901.81 2024-11-06T03:31:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:45Z- waiting for 1730863905.81, time is 1730863905.84 2024-11-06T03:31:45Z- LcmTrn::updateTrn() >>>> pose time 1730863905.84, meas time 1730863904.26 <<<< 2024-11-06T03:31:45Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863905.84 2024-11-06T03:31:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863904.26 sec, ping # 0; waiting for INS data more recent than 1730863901.81... 2024-11-06T03:31:45Z- TerrainNav::motionUpdate() - currEstimate = (4061815, 594493.88, 49.60) 2024-11-06T03:31:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:31:45Z- TerrainNav:: SubcloudNIS is 0.42, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:31:46Z- TNavPF::Projecting Measurements 2024-11-06T03:31:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:31:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:31:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:31:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:31:46Z- TNavFilter::4 good beams. 2024-11-06T03:31:46Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:31:46Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:31:46Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:31:46Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:31:46Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:31:46Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:31:46Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:31:46Z- TerrainMapOctree: (vehN, vehE ) = (4061815.62, 594495.30). 2024-11-06T03:31:46Z- TerrainMapOctree: Min Distance = 253.93. 2024-11-06T03:31:46Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:31:46Z- Forcing Subcloud Comparison 2024-11-06T03:31:46Z- Weighting particles with subcloud comparison 2024-11-06T03:31:46Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:46Z- meanExpectedMeasDiff: -0.876380 delta_rms_squared: 1.455022 alpha: 0.986441 2024-11-06T03:31:46Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:46Z- meanExpectedMeasDiff: 2.314949 delta_rms_squared: 1.054434 alpha: 0.981386 2024-11-06T03:31:46Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:46Z- meanExpectedMeasDiff: -1.692018 delta_rms_squared: 1.198734 alpha: 0.983590 2024-11-06T03:31:46Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:46Z- meanExpectedMeasDiff: -0.489503 delta_rms_squared: 1.264656 alpha: 0.984432 2024-11-06T03:31:46Z- Subcloud NIS: 0.438614 2024-11-06T03:31:46Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:31:46Z- TNavPF:: sumWeights = 0.198940 2024-11-06T03:31:46Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:31:46Z- TNavPF::First Term of Map Covariance Matrix is 0.20 2024-11-06T03:31:46Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T03:31:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863904.26 sec. 2024-11-06T03:31:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:31:46Z- variances([0], [2], and psi_berg) = 0.216985 0.263418 0.000000 2024-11-06T03:31:46Z- publishEstimatesDecoder() - at 1730863905.84 sec 2024-11-06T03:31:46Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:31:46Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061843.17 594477.37 52.82 2024-11-06T03:31:46Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061843.14 594477.78 52.31 lat/lon: 36.70 -121.94 2024-11-06T03:31:46Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.26 0.65 0.00 2024-11-06T03:31:46Z- handleDepth()- Depth_Keller msg: 1730863906.27 epoch sec; seqNo:6302; depth 49.59 2024-11-06T03:31:46Z- handleDVL() - DVL msg epochMillisec: 1730863906.30 2024-11-06T03:31:46Z- handleDvl() - RDI_Pathfinder msg: 1730863906.30 sec; seqNo:2153; #beams:4 2024-11-06T03:31:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.92 , 34.15 , 35.34 , 37.88 2024-11-06T03:31:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.03 2024-11-06T03:31:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863906.31 epoch sec; seqNo:6302; 4061814.86 north; 594493.88 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:31:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863905.84 2024-11-06T03:31:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:46Z- waiting for 1730863909.84, time is 1730863906.31 2024-11-06T03:31:46Z- AHRS_M2 msg: 1730863906.64 epoch sec; seqNo:5913 2024-11-06T03:31:46Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.16 psi 2024-11-06T03:31:46Z- handleDepth()- Depth_Keller msg: 1730863906.64 epoch sec; seqNo:6303; depth 49.59 2024-11-06T03:31:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863906.68 epoch sec; seqNo:6303; 4061814.48 north; 594493.88 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:31:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863905.84 2024-11-06T03:31:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:46Z- waiting for 1730863909.84, time is 1730863906.68 2024-11-06T03:31:47Z- AHRS_M2 msg: 1730863907.03 epoch sec; seqNo:5914 2024-11-06T03:31:47Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.16 psi 2024-11-06T03:31:47Z- handleDepth()- Depth_Keller msg: 1730863907.04 epoch sec; seqNo:6304; depth 49.60 2024-11-06T03:31:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863907.08 epoch sec; seqNo:6304; 4061814.47 north; 594492.66 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:31:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863905.84 2024-11-06T03:31:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:47Z- waiting for 1730863909.84, time is 1730863907.08 2024-11-06T03:31:47Z- AHRS_M2 msg: 1730863907.44 epoch sec; seqNo:5915 2024-11-06T03:31:47Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.16 psi 2024-11-06T03:31:47Z- handleDepth()- Depth_Keller msg: 1730863907.44 epoch sec; seqNo:6305; depth 49.62 2024-11-06T03:31:47Z- handleDVL() - DVL msg epochMillisec: 1730863907.44 2024-11-06T03:31:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:31:47Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:31:47Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:31:47Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:31:47Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:31:47Z- handleDvl() - RDI_Pathfinder msg: 1730863907.44 sec; seqNo:2154; #beams:0 2024-11-06T03:31:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:31:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.03 2024-11-06T03:31:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863907.45 epoch sec; seqNo:6305; 4061814.09 north; 594492.67 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:31:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863905.84 2024-11-06T03:31:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:47Z- waiting for 1730863909.84, time is 1730863907.45 2024-11-06T03:31:47Z- AHRS_M2 msg: 1730863907.84 epoch sec; seqNo:5916 2024-11-06T03:31:47Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.16 psi 2024-11-06T03:31:47Z- handleDepth()- Depth_Keller msg: 1730863907.85 epoch sec; seqNo:6306; depth 49.61 2024-11-06T03:31:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863907.86 epoch sec; seqNo:6306; 4061814.09 north; 594492.67 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:31:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863905.84 2024-11-06T03:31:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:48Z- waiting for 1730863909.84, time is 1730863907.86 2024-11-06T03:31:48Z- handleDepth()- Depth_Keller msg: 1730863908.26 epoch sec; seqNo:6307; depth 49.62 2024-11-06T03:31:48Z- handleDVL() - DVL msg epochMillisec: 1730863908.28 2024-11-06T03:31:48Z- handleDvl() - RDI_Pathfinder msg: 1730863908.28 sec; seqNo:2155; #beams:4 2024-11-06T03:31:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.92 , 34.15 , 35.00 , 37.88 2024-11-06T03:31:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , 0.00 2024-11-06T03:31:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863908.30 epoch sec; seqNo:6307; 4061813.71 north; 594492.67 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:31:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863905.84 2024-11-06T03:31:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:48Z- waiting for 1730863909.84, time is 1730863908.30 2024-11-06T03:31:48Z- AHRS_M2 msg: 1730863908.65 epoch sec; seqNo:5917 2024-11-06T03:31:48Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.16 psi 2024-11-06T03:31:48Z- handleDepth()- Depth_Keller msg: 1730863908.66 epoch sec; seqNo:6308; depth 49.63 2024-11-06T03:31:48Z- handleDVL() - DVL msg epochMillisec: 1730863908.66 2024-11-06T03:31:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:31:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:31:48Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:31:48Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:31:48Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:31:48Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:31:48Z- handleDvl() - RDI_Pathfinder msg: 1730863908.66 sec; seqNo:2156; #beams:0 2024-11-06T03:31:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:31:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , 0.00 2024-11-06T03:31:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863908.67 epoch sec; seqNo:6308; 4061813.70 north; 594491.45 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:31:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863905.84 2024-11-06T03:31:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:48Z- waiting for 1730863909.84, time is 1730863908.67 2024-11-06T03:31:49Z- AHRS_M2 msg: 1730863909.06 epoch sec; seqNo:5918 2024-11-06T03:31:49Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.17 psi 2024-11-06T03:31:49Z- handleDepth()- Depth_Keller msg: 1730863909.08 epoch sec; seqNo:6309; depth 49.65 2024-11-06T03:31:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863909.09 epoch sec; seqNo:6309; 4061813.32 north; 594491.46 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:31:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863905.84 2024-11-06T03:31:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:49Z- waiting for 1730863909.84, time is 1730863909.09 2024-11-06T03:31:49Z- AHRS_M2 msg: 1730863909.45 epoch sec; seqNo:5919 2024-11-06T03:31:49Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.17 psi 2024-11-06T03:31:49Z- handleDepth()- Depth_Keller msg: 1730863909.47 epoch sec; seqNo:6310; depth 49.66 2024-11-06T03:31:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863909.47 epoch sec; seqNo:6310; 4061813.32 north; 594491.46 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:31:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863905.84 2024-11-06T03:31:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:49Z- waiting for 1730863909.84, time is 1730863909.47 2024-11-06T03:31:49Z- AHRS_M2 msg: 1730863909.86 epoch sec; seqNo:5920 2024-11-06T03:31:49Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.17 psi 2024-11-06T03:31:49Z- handleDepth()- Depth_Keller msg: 1730863909.87 epoch sec; seqNo:6311; depth 49.67 2024-11-06T03:31:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863909.87 epoch sec; seqNo:6311; 4061813.32 north; 594491.46 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:31:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863905.84 2024-11-06T03:31:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:50Z- waiting for 1730863909.84, time is 1730863909.87 2024-11-06T03:31:50Z- LcmTrn::updateTrn() >>>> pose time 1730863909.87, meas time 1730863908.66 <<<< 2024-11-06T03:31:50Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863909.87 2024-11-06T03:31:50Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:31:50Z- TerrainNav::motionUpdate() - currEstimate = (4061813, 594491.46, 49.67) 2024-11-06T03:31:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:31:50Z- TerrainNav:: SubcloudNIS is 0.44, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:31:50Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:31:50Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:31:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:31:50Z- variances([0], [2], and psi_berg) = 0.218478 0.264677 0.000000 2024-11-06T03:31:50Z- publishEstimatesDecoder() - at 1730863909.87 sec 2024-11-06T03:31:50Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:31:50Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061841.64 594474.91 52.48 2024-11-06T03:31:50Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061841.59 594475.35 52.38 lat/lon: 36.70 -121.94 2024-11-06T03:31:50Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.26 0.90 0.00 2024-11-06T03:31:50Z- AHRS_M2 msg: 1730863910.26 epoch sec; seqNo:5921 2024-11-06T03:31:50Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.17 psi 2024-11-06T03:31:50Z- handleDepth()- Depth_Keller msg: 1730863910.27 epoch sec; seqNo:6312; depth 49.68 2024-11-06T03:31:50Z- handleDVL() - DVL msg epochMillisec: 1730863910.30 2024-11-06T03:31:50Z- handleDvl() - RDI_Pathfinder msg: 1730863910.30 sec; seqNo:2157; #beams:4 2024-11-06T03:31:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.09 , 33.98 , 34.83 , 38.06 2024-11-06T03:31:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , -0.03 2024-11-06T03:31:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863910.31 epoch sec; seqNo:6312; 4061812.93 north; 594490.24 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:31:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863909.87 2024-11-06T03:31:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:50Z- waiting for 1730863913.87, time is 1730863910.31 2024-11-06T03:31:50Z- AHRS_M2 msg: 1730863910.67 epoch sec; seqNo:5922 2024-11-06T03:31:50Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.17 psi 2024-11-06T03:31:50Z- handleDepth()- Depth_Keller msg: 1730863910.68 epoch sec; seqNo:6313; depth 49.70 2024-11-06T03:31:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863910.71 epoch sec; seqNo:6313; 4061812.93 north; 594490.24 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:31:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863909.87 2024-11-06T03:31:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:50Z- waiting for 1730863913.87, time is 1730863910.71 2024-11-06T03:31:51Z- AHRS_M2 msg: 1730863911.07 epoch sec; seqNo:5923 2024-11-06T03:31:51Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.17 psi 2024-11-06T03:31:51Z- handleDepth()- Depth_Keller msg: 1730863911.08 epoch sec; seqNo:6314; depth 49.70 2024-11-06T03:31:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863911.09 epoch sec; seqNo:6314; 4061812.55 north; 594490.24 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:31:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863909.87 2024-11-06T03:31:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:51Z- waiting for 1730863913.87, time is 1730863911.09 2024-11-06T03:31:51Z- AHRS_M2 msg: 1730863911.48 epoch sec; seqNo:5924 2024-11-06T03:31:51Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.17 psi 2024-11-06T03:31:51Z- handleDepth()- Depth_Keller msg: 1730863911.48 epoch sec; seqNo:6315; depth 49.74 2024-11-06T03:31:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863911.49 epoch sec; seqNo:6315; 4061812.54 north; 594489.02 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:31:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863909.87 2024-11-06T03:31:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:51Z- waiting for 1730863913.87, time is 1730863911.49 2024-11-06T03:31:51Z- AHRS_M2 msg: 1730863911.88 epoch sec; seqNo:5925 2024-11-06T03:31:51Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.16 psi 2024-11-06T03:31:51Z- handleDepth()- Depth_Keller msg: 1730863911.89 epoch sec; seqNo:6316; depth 49.75 2024-11-06T03:31:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863911.90 epoch sec; seqNo:6316; 4061812.16 north; 594489.03 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:31:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863909.87 2024-11-06T03:31:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:52Z- waiting for 1730863913.87, time is 1730863911.90 2024-11-06T03:31:52Z- AHRS_M2 msg: 1730863912.28 epoch sec; seqNo:5926 2024-11-06T03:31:52Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.16 psi 2024-11-06T03:31:52Z- handleDepth()- Depth_Keller msg: 1730863912.29 epoch sec; seqNo:6317; depth 49.77 2024-11-06T03:31:52Z- handleDVL() - DVL msg epochMillisec: 1730863912.32 2024-11-06T03:31:52Z- handleDvl() - RDI_Pathfinder msg: 1730863912.32 sec; seqNo:2158; #beams:4 2024-11-06T03:31:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.09 , 33.64 , 34.15 , 38.06 2024-11-06T03:31:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.05 , 0.00 2024-11-06T03:31:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863912.34 epoch sec; seqNo:6317; 4061812.16 north; 594489.03 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:31:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863909.87 2024-11-06T03:31:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:52Z- waiting for 1730863913.87, time is 1730863912.34 2024-11-06T03:31:52Z- AHRS_M2 msg: 1730863912.69 epoch sec; seqNo:5927 2024-11-06T03:31:52Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.16 psi 2024-11-06T03:31:52Z- handleDepth()- Depth_Keller msg: 1730863912.69 epoch sec; seqNo:6318; depth 49.78 2024-11-06T03:31:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863912.71 epoch sec; seqNo:6318; 4061811.78 north; 594489.03 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:31:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863909.87 2024-11-06T03:31:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:52Z- waiting for 1730863913.87, time is 1730863912.71 2024-11-06T03:31:53Z- AHRS_M2 msg: 1730863913.09 epoch sec; seqNo:5928 2024-11-06T03:31:53Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.16 psi 2024-11-06T03:31:53Z- handleDepth()- Depth_Keller msg: 1730863913.10 epoch sec; seqNo:6319; depth 49.81 2024-11-06T03:31:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863913.11 epoch sec; seqNo:6319; 4061811.77 north; 594487.81 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:31:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863909.87 2024-11-06T03:31:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:53Z- waiting for 1730863913.87, time is 1730863913.11 2024-11-06T03:31:53Z- AHRS_M2 msg: 1730863913.49 epoch sec; seqNo:5929 2024-11-06T03:31:53Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.16 psi 2024-11-06T03:31:53Z- handleDepth()- Depth_Keller msg: 1730863913.50 epoch sec; seqNo:6320; depth 49.82 2024-11-06T03:31:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863913.51 epoch sec; seqNo:6320; 4061811.39 north; 594487.82 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:31:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863909.87 2024-11-06T03:31:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:53Z- waiting for 1730863913.87, time is 1730863913.51 2024-11-06T03:31:53Z- AHRS_M2 msg: 1730863913.90 epoch sec; seqNo:5930 2024-11-06T03:31:53Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.16 psi 2024-11-06T03:31:53Z- handleDepth()- Depth_Keller msg: 1730863913.94 epoch sec; seqNo:6321; depth 49.84 2024-11-06T03:31:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863913.95 epoch sec; seqNo:6321; 4061811.39 north; 594487.82 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:31:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863909.87 2024-11-06T03:31:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:54Z- waiting for 1730863913.87, time is 1730863913.95 2024-11-06T03:31:54Z- LcmTrn::updateTrn() >>>> pose time 1730863913.95, meas time 1730863912.32 <<<< 2024-11-06T03:31:54Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863913.95 2024-11-06T03:31:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863912.32 sec, ping # 0; waiting for INS data more recent than 1730863909.87... 2024-11-06T03:31:54Z- TerrainNav::motionUpdate() - currEstimate = (4061811, 594487.82, 49.84) 2024-11-06T03:31:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:31:54Z- TerrainNav:: SubcloudNIS is 0.44, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:31:54Z- TNavPF::Projecting Measurements 2024-11-06T03:31:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:31:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:31:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:31:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:31:54Z- TNavFilter::4 good beams. 2024-11-06T03:31:54Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:31:54Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:31:54Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:31:54Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:31:54Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:31:54Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:31:54Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:31:54Z- TerrainMapOctree: (vehN, vehE ) = (4061812.16, 594489.27). 2024-11-06T03:31:54Z- TerrainMapOctree: Min Distance = 247.21. 2024-11-06T03:31:54Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:31:54Z- Forcing Subcloud Comparison 2024-11-06T03:31:54Z- Weighting particles with subcloud comparison 2024-11-06T03:31:54Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:54Z- meanExpectedMeasDiff: -1.986286 delta_rms_squared: 1.839941 alpha: 0.989247 2024-11-06T03:31:54Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:54Z- meanExpectedMeasDiff: 2.027797 delta_rms_squared: 1.211257 alpha: 0.983756 2024-11-06T03:31:54Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:54Z- meanExpectedMeasDiff: -2.524789 delta_rms_squared: 1.481059 alpha: 0.986676 2024-11-06T03:31:54Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:54Z- meanExpectedMeasDiff: -0.440521 delta_rms_squared: 1.669751 alpha: 0.988164 2024-11-06T03:31:54Z- Subcloud NIS: 0.460698 2024-11-06T03:31:54Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:31:54Z- TNavPF:: sumWeights = 0.110284 2024-11-06T03:31:54Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:31:54Z- TNavPF::First Term of Map Covariance Matrix is 0.26 2024-11-06T03:31:54Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T03:31:54Z- TerrainNav::Resampling particle filter... 2024-11-06T03:31:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863912.32 sec. 2024-11-06T03:31:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:31:54Z- variances([0], [2], and psi_berg) = 0.246561 0.259337 0.000000 2024-11-06T03:31:54Z- publishEstimatesDecoder() - at 1730863913.95 sec 2024-11-06T03:31:54Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:31:54Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061839.50 594470.92 53.82 2024-11-06T03:31:54Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061839.61 594471.71 52.98 lat/lon: 36.70 -121.94 2024-11-06T03:31:54Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.26 0.71 0.00 2024-11-06T03:31:54Z- AHRS_M2 msg: 1730863914.30 epoch sec; seqNo:5931 2024-11-06T03:31:54Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.15 psi 2024-11-06T03:31:54Z- handleDepth()- Depth_Keller msg: 1730863914.31 epoch sec; seqNo:6322; depth 49.87 2024-11-06T03:31:54Z- handleDVL() - DVL msg epochMillisec: 1730863914.34 2024-11-06T03:31:54Z- handleDvl() - RDI_Pathfinder msg: 1730863914.34 sec; seqNo:2159; #beams:4 2024-11-06T03:31:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.58 , 33.81 , 34.66 , 38.06 2024-11-06T03:31:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , 0.05 2024-11-06T03:31:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863914.35 epoch sec; seqNo:6322; 4061811.01 north; 594487.82 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:31:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863913.95 2024-11-06T03:31:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:54Z- waiting for 1730863917.95, time is 1730863914.35 2024-11-06T03:31:54Z- AHRS_M2 msg: 1730863914.72 epoch sec; seqNo:5932 2024-11-06T03:31:54Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.15 psi 2024-11-06T03:31:54Z- handleDepth()- Depth_Keller msg: 1730863914.73 epoch sec; seqNo:6323; depth 49.90 2024-11-06T03:31:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863914.74 epoch sec; seqNo:6323; 4061810.99 north; 594486.60 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:31:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863913.95 2024-11-06T03:31:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:54Z- waiting for 1730863917.95, time is 1730863914.74 2024-11-06T03:31:55Z- AHRS_M2 msg: 1730863915.12 epoch sec; seqNo:5933 2024-11-06T03:31:55Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.15 psi 2024-11-06T03:31:55Z- handleDepth()- Depth_Keller msg: 1730863915.13 epoch sec; seqNo:6324; depth 49.89 2024-11-06T03:31:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863915.14 epoch sec; seqNo:6324; 4061810.62 north; 594486.60 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:31:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863913.95 2024-11-06T03:31:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:55Z- waiting for 1730863917.95, time is 1730863915.14 2024-11-06T03:31:55Z- AHRS_M2 msg: 1730863915.52 epoch sec; seqNo:5934 2024-11-06T03:31:55Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.15 psi 2024-11-06T03:31:55Z- handleDepth()- Depth_Keller msg: 1730863915.53 epoch sec; seqNo:6325; depth 49.91 2024-11-06T03:31:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863915.54 epoch sec; seqNo:6325; 4061810.62 north; 594486.60 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:31:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863913.95 2024-11-06T03:31:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:55Z- waiting for 1730863917.95, time is 1730863915.54 2024-11-06T03:31:55Z- AHRS_M2 msg: 1730863915.92 epoch sec; seqNo:5935 2024-11-06T03:31:55Z- AHRS_M2 msg: 0.13 phi; -0.05 theta; 4.15 psi 2024-11-06T03:31:55Z- handleDepth()- Depth_Keller msg: 1730863915.94 epoch sec; seqNo:6326; depth 49.93 2024-11-06T03:31:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863915.95 epoch sec; seqNo:6326; 4061810.24 north; 594486.61 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:31:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863913.95 2024-11-06T03:31:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:56Z- waiting for 1730863917.95, time is 1730863915.95 2024-11-06T03:31:56Z- AHRS_M2 msg: 1730863916.33 epoch sec; seqNo:5936 2024-11-06T03:31:56Z- AHRS_M2 msg: 0.13 phi; -0.05 theta; 4.15 psi 2024-11-06T03:31:56Z- handleDepth()- Depth_Keller msg: 1730863916.33 epoch sec; seqNo:6327; depth 49.94 2024-11-06T03:31:56Z- handleDVL() - DVL msg epochMillisec: 1730863916.35 2024-11-06T03:31:56Z- handleDvl() - RDI_Pathfinder msg: 1730863916.35 sec; seqNo:2160; #beams:4 2024-11-06T03:31:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.58 , 33.81 , 34.66 , 37.88 2024-11-06T03:31:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , 0.02 2024-11-06T03:31:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863916.36 epoch sec; seqNo:6327; 4061810.22 north; 594485.39 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:31:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863913.95 2024-11-06T03:31:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:56Z- waiting for 1730863917.95, time is 1730863916.36 2024-11-06T03:31:56Z- AHRS_M2 msg: 1730863916.73 epoch sec; seqNo:5937 2024-11-06T03:31:56Z- AHRS_M2 msg: 0.13 phi; -0.05 theta; 4.16 psi 2024-11-06T03:31:56Z- handleDepth()- Depth_Keller msg: 1730863916.74 epoch sec; seqNo:6328; depth 49.95 2024-11-06T03:31:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863916.77 epoch sec; seqNo:6328; 4061809.84 north; 594485.39 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:31:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863913.95 2024-11-06T03:31:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:56Z- waiting for 1730863917.95, time is 1730863916.77 2024-11-06T03:31:57Z- AHRS_M2 msg: 1730863917.13 epoch sec; seqNo:5938 2024-11-06T03:31:57Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.16 psi 2024-11-06T03:31:57Z- handleDepth()- Depth_Keller msg: 1730863917.14 epoch sec; seqNo:6329; depth 49.96 2024-11-06T03:31:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863917.15 epoch sec; seqNo:6329; 4061809.47 north; 594485.40 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:31:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863913.95 2024-11-06T03:31:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:57Z- waiting for 1730863917.95, time is 1730863917.15 2024-11-06T03:31:57Z- AHRS_M2 msg: 1730863917.54 epoch sec; seqNo:5939 2024-11-06T03:31:57Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.16 psi 2024-11-06T03:31:57Z- handleDepth()- Depth_Keller msg: 1730863917.54 epoch sec; seqNo:6330; depth 49.97 2024-11-06T03:31:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863917.55 epoch sec; seqNo:6330; 4061809.47 north; 594485.40 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:31:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863913.95 2024-11-06T03:31:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:57Z- waiting for 1730863917.95, time is 1730863917.55 2024-11-06T03:31:57Z- AHRS_M2 msg: 1730863917.95 epoch sec; seqNo:5940 2024-11-06T03:31:57Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.16 psi 2024-11-06T03:31:57Z- handleDepth()- Depth_Keller msg: 1730863917.95 epoch sec; seqNo:6331; depth 49.98 2024-11-06T03:31:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863917.96 epoch sec; seqNo:6331; 4061809.07 north; 594484.18 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:31:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863913.95 2024-11-06T03:31:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:58Z- waiting for 1730863917.95, time is 1730863917.96 2024-11-06T03:31:58Z- LcmTrn::updateTrn() >>>> pose time 1730863917.96, meas time 1730863916.35 <<<< 2024-11-06T03:31:58Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863917.96 2024-11-06T03:31:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863916.35 sec, ping # 0; waiting for INS data more recent than 1730863913.95... 2024-11-06T03:31:58Z- TerrainNav::motionUpdate() - currEstimate = (4061809, 594484.18, 49.98) 2024-11-06T03:31:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:31:58Z- TerrainNav:: SubcloudNIS is 0.46, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:31:58Z- TNavPF::Projecting Measurements 2024-11-06T03:31:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:31:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:31:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:31:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:31:58Z- TNavFilter::4 good beams. 2024-11-06T03:31:58Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:31:58Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:31:58Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:31:58Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:31:58Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:31:58Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:31:58Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:31:58Z- TerrainMapOctree: (vehN, vehE ) = (4061810.00, 594485.64). 2024-11-06T03:31:58Z- TerrainMapOctree: Min Distance = 243.16. 2024-11-06T03:31:58Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:31:58Z- Forcing Subcloud Comparison 2024-11-06T03:31:58Z- Weighting particles with subcloud comparison 2024-11-06T03:31:58Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:58Z- meanExpectedMeasDiff: -1.777855 delta_rms_squared: 1.544632 alpha: 0.987217 2024-11-06T03:31:58Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:58Z- meanExpectedMeasDiff: 2.160008 delta_rms_squared: 0.855688 alpha: 0.977161 2024-11-06T03:31:58Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:58Z- meanExpectedMeasDiff: -1.270092 delta_rms_squared: 1.237275 alpha: 0.984093 2024-11-06T03:31:58Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:31:58Z- meanExpectedMeasDiff: 0.123995 delta_rms_squared: 1.545020 alpha: 0.987221 2024-11-06T03:31:58Z- Subcloud NIS: 0.472140 2024-11-06T03:31:58Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:31:58Z- TNavPF:: sumWeights = 0.206271 2024-11-06T03:31:58Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:31:58Z- TNavPF::First Term of Map Covariance Matrix is 0.45 2024-11-06T03:31:58Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T03:31:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863916.35 sec. 2024-11-06T03:31:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:31:58Z- variances([0], [2], and psi_berg) = 0.223043 0.240869 0.000000 2024-11-06T03:31:58Z- publishEstimatesDecoder() - at 1730863917.96 sec 2024-11-06T03:31:58Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:31:58Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061837.37 594468.47 53.16 2024-11-06T03:31:58Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061837.22 594468.14 53.29 lat/lon: 36.70 -121.94 2024-11-06T03:31:58Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.24 0.66 0.00 2024-11-06T03:31:58Z- AHRS_M2 msg: 1730863918.35 epoch sec; seqNo:5941 2024-11-06T03:31:58Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.16 psi 2024-11-06T03:31:58Z- handleDepth()- Depth_Keller msg: 1730863918.35 epoch sec; seqNo:6332; depth 49.99 2024-11-06T03:31:58Z- handleDVL() - DVL msg epochMillisec: 1730863918.39 2024-11-06T03:31:58Z- handleDvl() - RDI_Pathfinder msg: 1730863918.39 sec; seqNo:2161; #beams:4 2024-11-06T03:31:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.58 , 33.64 , 34.83 , 38.06 2024-11-06T03:31:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.04 2024-11-06T03:31:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863918.40 epoch sec; seqNo:6332; 4061809.07 north; 594484.18 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:31:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:31:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863917.96 2024-11-06T03:31:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:58Z- waiting for 1730863921.96, time is 1730863918.40 2024-11-06T03:31:58Z- AHRS_M2 msg: 1730863918.75 epoch sec; seqNo:5942 2024-11-06T03:31:58Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.16 psi 2024-11-06T03:31:58Z- handleDepth()- Depth_Keller msg: 1730863918.76 epoch sec; seqNo:6333; depth 50.02 2024-11-06T03:31:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863918.77 epoch sec; seqNo:6333; 4061808.69 north; 594484.19 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:31:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863917.96 2024-11-06T03:31:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:58Z- waiting for 1730863921.96, time is 1730863918.77 2024-11-06T03:31:59Z- AHRS_M2 msg: 1730863919.16 epoch sec; seqNo:5943 2024-11-06T03:31:59Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.16 psi 2024-11-06T03:31:59Z- handleDepth()- Depth_Keller msg: 1730863919.17 epoch sec; seqNo:6334; depth 50.01 2024-11-06T03:31:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863919.18 epoch sec; seqNo:6334; 4061808.69 north; 594484.19 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:31:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863917.96 2024-11-06T03:31:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:59Z- waiting for 1730863921.96, time is 1730863919.18 2024-11-06T03:31:59Z- AHRS_M2 msg: 1730863919.56 epoch sec; seqNo:5944 2024-11-06T03:31:59Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.16 psi 2024-11-06T03:31:59Z- handleDepth()- Depth_Keller msg: 1730863919.56 epoch sec; seqNo:6335; depth 50.02 2024-11-06T03:31:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863919.58 epoch sec; seqNo:6335; 4061808.30 north; 594482.97 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:31:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:31:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863917.96 2024-11-06T03:31:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:31:59Z- waiting for 1730863921.96, time is 1730863919.58 2024-11-06T03:31:59Z- AHRS_M2 msg: 1730863919.96 epoch sec; seqNo:5945 2024-11-06T03:31:59Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.16 psi 2024-11-06T03:31:59Z- handleDepth()- Depth_Keller msg: 1730863919.97 epoch sec; seqNo:6336; depth 50.03 2024-11-06T03:32:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863919.99 epoch sec; seqNo:6336; 4061808.30 north; 594482.97 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:32:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863917.96 2024-11-06T03:32:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:00Z- waiting for 1730863921.96, time is 1730863919.99 2024-11-06T03:32:00Z- AHRS_M2 msg: 1730863920.37 epoch sec; seqNo:5946 2024-11-06T03:32:00Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.16 psi 2024-11-06T03:32:00Z- handleDepth()- Depth_Keller msg: 1730863920.38 epoch sec; seqNo:6337; depth 50.04 2024-11-06T03:32:00Z- handleDVL() - DVL msg epochMillisec: 1730863920.40 2024-11-06T03:32:00Z- handleDvl() - RDI_Pathfinder msg: 1730863920.40 sec; seqNo:2162; #beams:4 2024-11-06T03:32:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.41 , 33.47 , 34.83 , 37.72 2024-11-06T03:32:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , -0.06 2024-11-06T03:32:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863920.42 epoch sec; seqNo:6337; 4061807.92 north; 594482.97 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:32:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863917.96 2024-11-06T03:32:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:00Z- waiting for 1730863921.96, time is 1730863920.42 2024-11-06T03:32:00Z- handleDepth()- Depth_Keller msg: 1730863920.79 epoch sec; seqNo:6338; depth 50.05 2024-11-06T03:32:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863920.80 epoch sec; seqNo:6338; 4061807.92 north; 594482.97 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:32:00Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:32:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863917.96 2024-11-06T03:32:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:00Z- waiting for 1730863921.96, time is 1730863920.80 2024-11-06T03:32:01Z- AHRS_M2 msg: 1730863921.17 epoch sec; seqNo:5947 2024-11-06T03:32:01Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.16 psi 2024-11-06T03:32:01Z- handleDepth()- Depth_Keller msg: 1730863921.18 epoch sec; seqNo:6339; depth 50.05 2024-11-06T03:32:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863921.20 epoch sec; seqNo:6339; 4061807.53 north; 594481.76 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:32:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863917.96 2024-11-06T03:32:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:01Z- waiting for 1730863921.96, time is 1730863921.20 2024-11-06T03:32:01Z- AHRS_M2 msg: 1730863921.58 epoch sec; seqNo:5948 2024-11-06T03:32:01Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.16 psi 2024-11-06T03:32:01Z- handleDepth()- Depth_Keller msg: 1730863921.59 epoch sec; seqNo:6340; depth 50.08 2024-11-06T03:32:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863921.61 epoch sec; seqNo:6340; 4061807.53 north; 594481.76 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:32:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863917.96 2024-11-06T03:32:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:01Z- waiting for 1730863921.96, time is 1730863921.61 2024-11-06T03:32:01Z- AHRS_M2 msg: 1730863921.98 epoch sec; seqNo:5949 2024-11-06T03:32:01Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.16 psi 2024-11-06T03:32:01Z- handleDepth()- Depth_Keller msg: 1730863921.99 epoch sec; seqNo:6341; depth 50.07 2024-11-06T03:32:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863922.00 epoch sec; seqNo:6341; 4061807.15 north; 594481.76 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:32:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863917.96 2024-11-06T03:32:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:02Z- waiting for 1730863921.96, time is 1730863922.00 2024-11-06T03:32:02Z- LcmTrn::updateTrn() >>>> pose time 1730863922.00, meas time 1730863920.40 <<<< 2024-11-06T03:32:02Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863922.00 2024-11-06T03:32:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863920.40 sec, ping # 0; waiting for INS data more recent than 1730863917.96... 2024-11-06T03:32:02Z- TerrainNav::motionUpdate() - currEstimate = (4061807, 594481.76, 50.07) 2024-11-06T03:32:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:32:02Z- TerrainNav:: SubcloudNIS is 0.47, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:32:02Z- TNavPF::Projecting Measurements 2024-11-06T03:32:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:32:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:32:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:32:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:32:02Z- TNavFilter::4 good beams. 2024-11-06T03:32:02Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:32:02Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:32:02Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:32:02Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:32:02Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:32:02Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:32:02Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:32:02Z- TerrainMapOctree: (vehN, vehE ) = (4061807.91, 594482.72). 2024-11-06T03:32:02Z- TerrainMapOctree: Min Distance = 239.82. 2024-11-06T03:32:02Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:32:02Z- Forcing Subcloud Comparison 2024-11-06T03:32:02Z- Weighting particles with subcloud comparison 2024-11-06T03:32:02Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:02Z- meanExpectedMeasDiff: -1.978313 delta_rms_squared: 1.701755 alpha: 0.988384 2024-11-06T03:32:02Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:02Z- meanExpectedMeasDiff: 1.913976 delta_rms_squared: 1.019157 alpha: 0.980754 2024-11-06T03:32:02Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:02Z- meanExpectedMeasDiff: -0.914994 delta_rms_squared: 1.176228 alpha: 0.983281 2024-11-06T03:32:02Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:02Z- meanExpectedMeasDiff: 0.168689 delta_rms_squared: 1.304537 alpha: 0.984900 2024-11-06T03:32:02Z- Subcloud NIS: 0.474785 2024-11-06T03:32:02Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:32:02Z- TNavPF:: sumWeights = 0.231563 2024-11-06T03:32:02Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:32:02Z- TNavPF::First Term of Map Covariance Matrix is 0.60 2024-11-06T03:32:02Z- TNavPF::Calculated NIS Value : 0.14 numBeams normalized NIS: 0.03 2024-11-06T03:32:02Z- TerrainNav::Resampling particle filter... 2024-11-06T03:32:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863920.40 sec. 2024-11-06T03:32:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:32:02Z- variances([0], [2], and psi_berg) = 0.215151 0.238526 0.000000 2024-11-06T03:32:02Z- publishEstimatesDecoder() - at 1730863922.00 sec 2024-11-06T03:32:02Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:32:02Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061835.10 594464.96 53.65 2024-11-06T03:32:02Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061835.30 594465.72 53.47 lat/lon: 36.70 -121.94 2024-11-06T03:32:02Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.24 0.62 0.00 2024-11-06T03:32:02Z- handleDepth()- Depth_Keller msg: 1730863922.42 epoch sec; seqNo:6342; depth 50.09 2024-11-06T03:32:02Z- handleDVL() - DVL msg epochMillisec: 1730863922.44 2024-11-06T03:32:02Z- handleDvl() - RDI_Pathfinder msg: 1730863922.44 sec; seqNo:2163; #beams:4 2024-11-06T03:32:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.41 , 32.96 , 34.49 , 37.38 2024-11-06T03:32:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.08 , -0.01 2024-11-06T03:32:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863922.46 epoch sec; seqNo:6342; 4061807.14 north; 594480.54 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:32:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863922.00 2024-11-06T03:32:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:02Z- waiting for 1730863926.00, time is 1730863922.46 2024-11-06T03:32:02Z- AHRS_M2 msg: 1730863922.79 epoch sec; seqNo:5950 2024-11-06T03:32:02Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.15 psi 2024-11-06T03:32:02Z- handleDepth()- Depth_Keller msg: 1730863922.80 epoch sec; seqNo:6343; depth 50.10 2024-11-06T03:32:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863922.83 epoch sec; seqNo:6343; 4061806.76 north; 594480.55 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:32:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863922.00 2024-11-06T03:32:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:02Z- waiting for 1730863926.00, time is 1730863922.83 2024-11-06T03:32:03Z- AHRS_M2 msg: 1730863923.20 epoch sec; seqNo:5951 2024-11-06T03:32:03Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.15 psi 2024-11-06T03:32:03Z- handleDepth()- Depth_Keller msg: 1730863923.21 epoch sec; seqNo:6344; depth 50.13 2024-11-06T03:32:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863923.22 epoch sec; seqNo:6344; 4061806.76 north; 594480.55 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:32:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863922.00 2024-11-06T03:32:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:03Z- waiting for 1730863926.00, time is 1730863923.22 2024-11-06T03:32:03Z- AHRS_M2 msg: 1730863923.62 epoch sec; seqNo:5952 2024-11-06T03:32:03Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:32:03Z- handleDepth()- Depth_Keller msg: 1730863923.62 epoch sec; seqNo:6345; depth 50.15 2024-11-06T03:32:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863923.63 epoch sec; seqNo:6345; 4061806.38 north; 594480.55 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:32:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863922.00 2024-11-06T03:32:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:03Z- waiting for 1730863926.00, time is 1730863923.63 2024-11-06T03:32:04Z- AHRS_M2 msg: 1730863924.00 epoch sec; seqNo:5953 2024-11-06T03:32:04Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:32:04Z- handleDepth()- Depth_Keller msg: 1730863924.01 epoch sec; seqNo:6346; depth 50.17 2024-11-06T03:32:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863924.03 epoch sec; seqNo:6346; 4061806.37 north; 594479.33 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:32:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863922.00 2024-11-06T03:32:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:04Z- waiting for 1730863926.00, time is 1730863924.03 2024-11-06T03:32:04Z- AHRS_M2 msg: 1730863924.41 epoch sec; seqNo:5954 2024-11-06T03:32:04Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:32:04Z- handleDepth()- Depth_Keller msg: 1730863924.42 epoch sec; seqNo:6347; depth 50.17 2024-11-06T03:32:04Z- handleDVL() - DVL msg epochMillisec: 1730863924.44 2024-11-06T03:32:04Z- handleDvl() - RDI_Pathfinder msg: 1730863924.44 sec; seqNo:2164; #beams:4 2024-11-06T03:32:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.07 , 32.96 , 34.83 , 37.21 2024-11-06T03:32:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.04 , 0.05 2024-11-06T03:32:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863924.46 epoch sec; seqNo:6347; 4061805.99 north; 594479.33 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:32:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863922.00 2024-11-06T03:32:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:04Z- waiting for 1730863926.00, time is 1730863924.46 2024-11-06T03:32:04Z- AHRS_M2 msg: 1730863924.81 epoch sec; seqNo:5955 2024-11-06T03:32:04Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:32:04Z- handleDepth()- Depth_Keller msg: 1730863924.82 epoch sec; seqNo:6348; depth 50.21 2024-11-06T03:32:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863924.85 epoch sec; seqNo:6348; 4061805.99 north; 594479.33 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:32:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863922.00 2024-11-06T03:32:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:04Z- waiting for 1730863926.00, time is 1730863924.85 2024-11-06T03:32:05Z- AHRS_M2 msg: 1730863925.22 epoch sec; seqNo:5956 2024-11-06T03:32:05Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:32:05Z- handleDepth()- Depth_Keller msg: 1730863925.23 epoch sec; seqNo:6349; depth 50.21 2024-11-06T03:32:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863925.24 epoch sec; seqNo:6349; 4061805.60 north; 594478.12 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:32:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863922.00 2024-11-06T03:32:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:05Z- waiting for 1730863926.00, time is 1730863925.24 2024-11-06T03:32:05Z- AHRS_M2 msg: 1730863925.63 epoch sec; seqNo:5957 2024-11-06T03:32:05Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.16 psi 2024-11-06T03:32:05Z- handleDepth()- Depth_Keller msg: 1730863925.64 epoch sec; seqNo:6350; depth 50.22 2024-11-06T03:32:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863925.65 epoch sec; seqNo:6350; 4061805.60 north; 594478.12 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:32:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863922.00 2024-11-06T03:32:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:05Z- waiting for 1730863926.00, time is 1730863925.65 2024-11-06T03:32:06Z- AHRS_M2 msg: 1730863926.02 epoch sec; seqNo:5958 2024-11-06T03:32:06Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.16 psi 2024-11-06T03:32:06Z- handleDepth()- Depth_Keller msg: 1730863926.04 epoch sec; seqNo:6351; depth 50.24 2024-11-06T03:32:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863926.05 epoch sec; seqNo:6351; 4061805.22 north; 594478.12 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:32:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863922.00 2024-11-06T03:32:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:06Z- waiting for 1730863926.00, time is 1730863926.05 2024-11-06T03:32:06Z- LcmTrn::updateTrn() >>>> pose time 1730863926.05, meas time 1730863924.44 <<<< 2024-11-06T03:32:06Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863926.05 2024-11-06T03:32:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863924.44 sec, ping # 0; waiting for INS data more recent than 1730863922.00... 2024-11-06T03:32:06Z- TerrainNav::motionUpdate() - currEstimate = (4061805, 594478.12, 50.24) 2024-11-06T03:32:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:32:06Z- TerrainNav:: SubcloudNIS is 0.47, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:32:06Z- TNavPF::Projecting Measurements 2024-11-06T03:32:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:32:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:32:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:32:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:32:06Z- TNavFilter::4 good beams. 2024-11-06T03:32:06Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:32:06Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:32:06Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:32:06Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:32:06Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:32:06Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:32:06Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:32:06Z- TerrainMapOctree: (vehN, vehE ) = (4061805.98, 594479.56). 2024-11-06T03:32:06Z- TerrainMapOctree: Min Distance = 236.29. 2024-11-06T03:32:06Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:32:06Z- Forcing Subcloud Comparison 2024-11-06T03:32:06Z- Weighting particles with subcloud comparison 2024-11-06T03:32:06Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:06Z- meanExpectedMeasDiff: -2.896688 delta_rms_squared: 2.825330 alpha: 0.992971 2024-11-06T03:32:06Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:06Z- meanExpectedMeasDiff: 2.001797 delta_rms_squared: 0.779399 alpha: 0.974981 2024-11-06T03:32:06Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:06Z- meanExpectedMeasDiff: -0.942199 delta_rms_squared: 1.133418 alpha: 0.982660 2024-11-06T03:32:06Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:06Z- meanExpectedMeasDiff: 1.203145 delta_rms_squared: 1.846668 alpha: 0.989286 2024-11-06T03:32:06Z- Subcloud NIS: 0.495398 2024-11-06T03:32:06Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:32:06Z- TNavPF:: sumWeights = 0.104337 2024-11-06T03:32:06Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:32:06Z- TNavPF::First Term of Map Covariance Matrix is 0.57 2024-11-06T03:32:06Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T03:32:06Z- TerrainNav::Resampling particle filter... 2024-11-06T03:32:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863924.44 sec. 2024-11-06T03:32:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:32:06Z- variances([0], [2], and psi_berg) = 0.175983 0.246461 0.000000 2024-11-06T03:32:06Z- publishEstimatesDecoder() - at 1730863926.05 sec 2024-11-06T03:32:06Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:32:06Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061833.15 594461.35 53.36 2024-11-06T03:32:06Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061833.42 594462.10 53.87 lat/lon: 36.70 -121.94 2024-11-06T03:32:06Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.25 0.57 0.00 2024-11-06T03:32:06Z- AHRS_M2 msg: 1730863926.43 epoch sec; seqNo:5959 2024-11-06T03:32:06Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.17 psi 2024-11-06T03:32:06Z- handleDepth()- Depth_Keller msg: 1730863926.44 epoch sec; seqNo:6352; depth 50.25 2024-11-06T03:32:06Z- handleDVL() - DVL msg epochMillisec: 1730863926.48 2024-11-06T03:32:06Z- handleDvl() - RDI_Pathfinder msg: 1730863926.48 sec; seqNo:2165; #beams:4 2024-11-06T03:32:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.74 , 32.96 , 35.00 , 36.36 2024-11-06T03:32:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , 0.05 2024-11-06T03:32:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863926.49 epoch sec; seqNo:6352; 4061805.22 north; 594478.12 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:32:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863926.05 2024-11-06T03:32:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:06Z- waiting for 1730863930.05, time is 1730863926.49 2024-11-06T03:32:06Z- AHRS_M2 msg: 1730863926.83 epoch sec; seqNo:5960 2024-11-06T03:32:06Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.17 psi 2024-11-06T03:32:06Z- handleDepth()- Depth_Keller msg: 1730863926.84 epoch sec; seqNo:6353; depth 50.27 2024-11-06T03:32:06Z- handleDVL() - DVL msg epochMillisec: 1730863926.84 2024-11-06T03:32:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:06Z- handleDvl() - RDI_Pathfinder msg: 1730863926.84 sec; seqNo:2166; #beams:0 2024-11-06T03:32:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , 0.05 2024-11-06T03:32:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863926.86 epoch sec; seqNo:6353; 4061804.83 north; 594476.90 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:32:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863926.05 2024-11-06T03:32:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:06Z- waiting for 1730863930.05, time is 1730863926.86 2024-11-06T03:32:07Z- AHRS_M2 msg: 1730863927.24 epoch sec; seqNo:5961 2024-11-06T03:32:07Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.17 psi 2024-11-06T03:32:07Z- handleDepth()- Depth_Keller msg: 1730863927.25 epoch sec; seqNo:6354; depth 50.26 2024-11-06T03:32:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863927.26 epoch sec; seqNo:6354; 4061804.83 north; 594476.90 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:32:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863926.05 2024-11-06T03:32:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:07Z- waiting for 1730863930.05, time is 1730863927.26 2024-11-06T03:32:07Z- AHRS_M2 msg: 1730863927.64 epoch sec; seqNo:5962 2024-11-06T03:32:07Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.18 psi 2024-11-06T03:32:07Z- handleDepth()- Depth_Keller msg: 1730863927.65 epoch sec; seqNo:6355; depth 50.27 2024-11-06T03:32:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863927.66 epoch sec; seqNo:6355; 4061804.45 north; 594476.91 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:32:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863926.05 2024-11-06T03:32:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:07Z- waiting for 1730863930.05, time is 1730863927.66 2024-11-06T03:32:08Z- AHRS_M2 msg: 1730863928.05 epoch sec; seqNo:5963 2024-11-06T03:32:08Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.18 psi 2024-11-06T03:32:08Z- handleDepth()- Depth_Keller msg: 1730863928.06 epoch sec; seqNo:6356; depth 50.27 2024-11-06T03:32:08Z- handleDVL() - DVL msg epochMillisec: 1730863928.06 2024-11-06T03:32:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:08Z- handleDvl() - RDI_Pathfinder msg: 1730863928.06 sec; seqNo:2167; #beams:0 2024-11-06T03:32:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , 0.05 2024-11-06T03:32:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863928.06 epoch sec; seqNo:6356; 4061804.45 north; 594476.91 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:32:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863926.05 2024-11-06T03:32:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:08Z- waiting for 1730863930.05, time is 1730863928.06 2024-11-06T03:32:08Z- AHRS_M2 msg: 1730863928.45 epoch sec; seqNo:5964 2024-11-06T03:32:08Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.18 psi 2024-11-06T03:32:08Z- handleDepth()- Depth_Keller msg: 1730863928.45 epoch sec; seqNo:6357; depth 50.28 2024-11-06T03:32:08Z- handleDVL() - DVL msg epochMillisec: 1730863928.47 2024-11-06T03:32:08Z- handleDvl() - RDI_Pathfinder msg: 1730863928.47 sec; seqNo:2168; #beams:4 2024-11-06T03:32:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.06 , 33.13 , 35.00 , 36.02 2024-11-06T03:32:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.01 2024-11-06T03:32:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863928.50 epoch sec; seqNo:6357; 4061804.05 north; 594475.69 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:32:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863926.05 2024-11-06T03:32:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:08Z- waiting for 1730863930.05, time is 1730863928.50 2024-11-06T03:32:08Z- AHRS_M2 msg: 1730863928.85 epoch sec; seqNo:5965 2024-11-06T03:32:08Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.18 psi 2024-11-06T03:32:08Z- handleDepth()- Depth_Keller msg: 1730863928.87 epoch sec; seqNo:6358; depth 50.29 2024-11-06T03:32:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863928.88 epoch sec; seqNo:6358; 4061804.05 north; 594475.69 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:32:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863926.05 2024-11-06T03:32:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:09Z- waiting for 1730863930.05, time is 1730863928.88 2024-11-06T03:32:09Z- AHRS_M2 msg: 1730863929.25 epoch sec; seqNo:5966 2024-11-06T03:32:09Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.19 psi 2024-11-06T03:32:09Z- handleDepth()- Depth_Keller msg: 1730863929.27 epoch sec; seqNo:6359; depth 50.30 2024-11-06T03:32:09Z- handleDVL() - DVL msg epochMillisec: 1730863929.27 2024-11-06T03:32:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:09Z- handleDvl() - RDI_Pathfinder msg: 1730863929.27 sec; seqNo:2169; #beams:0 2024-11-06T03:32:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.01 2024-11-06T03:32:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863929.29 epoch sec; seqNo:6359; 4061803.68 north; 594475.70 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:32:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863926.05 2024-11-06T03:32:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:09Z- waiting for 1730863930.05, time is 1730863929.29 2024-11-06T03:32:09Z- AHRS_M2 msg: 1730863929.66 epoch sec; seqNo:5967 2024-11-06T03:32:09Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.19 psi 2024-11-06T03:32:09Z- handleDepth()- Depth_Keller msg: 1730863929.66 epoch sec; seqNo:6360; depth 50.32 2024-11-06T03:32:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863929.68 epoch sec; seqNo:6360; 4061803.68 north; 594475.70 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:32:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863926.05 2024-11-06T03:32:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:09Z- waiting for 1730863930.05, time is 1730863929.68 2024-11-06T03:32:10Z- AHRS_M2 msg: 1730863930.07 epoch sec; seqNo:5968 2024-11-06T03:32:10Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.19 psi 2024-11-06T03:32:10Z- handleDepth()- Depth_Keller msg: 1730863930.08 epoch sec; seqNo:6361; depth 50.31 2024-11-06T03:32:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863930.09 epoch sec; seqNo:6361; 4061803.28 north; 594474.48 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:32:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863926.05 2024-11-06T03:32:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:10Z- waiting for 1730863930.05, time is 1730863930.09 2024-11-06T03:32:10Z- LcmTrn::updateTrn() >>>> pose time 1730863930.09, meas time 1730863929.27 <<<< 2024-11-06T03:32:10Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863930.09 2024-11-06T03:32:10Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:32:10Z- TerrainNav::motionUpdate() - currEstimate = (4061803, 594474.48, 50.31) 2024-11-06T03:32:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:32:10Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:32:10Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:32:10Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:32:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:32:10Z- variances([0], [2], and psi_berg) = 0.179055 0.249218 0.000000 2024-11-06T03:32:10Z- publishEstimatesDecoder() - at 1730863930.09 sec 2024-11-06T03:32:10Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:32:10Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061831.18 594457.79 53.08 2024-11-06T03:32:10Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061831.48 594458.46 53.94 lat/lon: 36.70 -121.94 2024-11-06T03:32:10Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.25 0.82 0.00 2024-11-06T03:32:10Z- AHRS_M2 msg: 1730863930.47 epoch sec; seqNo:5969 2024-11-06T03:32:10Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.19 psi 2024-11-06T03:32:10Z- handleDepth()- Depth_Keller msg: 1730863930.48 epoch sec; seqNo:6362; depth 50.31 2024-11-06T03:32:10Z- handleDVL() - DVL msg epochMillisec: 1730863930.51 2024-11-06T03:32:10Z- handleDvl() - RDI_Pathfinder msg: 1730863930.51 sec; seqNo:2170; #beams:4 2024-11-06T03:32:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.53 , 32.79 , 34.83 , 36.02 2024-11-06T03:32:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , -0.02 2024-11-06T03:32:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863930.52 epoch sec; seqNo:6362; 4061803.28 north; 594474.48 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:32:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863930.09 2024-11-06T03:32:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:10Z- waiting for 1730863934.09, time is 1730863930.52 2024-11-06T03:32:10Z- AHRS_M2 msg: 1730863930.87 epoch sec; seqNo:5970 2024-11-06T03:32:10Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.19 psi 2024-11-06T03:32:10Z- handleDepth()- Depth_Keller msg: 1730863930.88 epoch sec; seqNo:6363; depth 50.31 2024-11-06T03:32:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863930.89 epoch sec; seqNo:6363; 4061802.90 north; 594474.49 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:32:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863930.09 2024-11-06T03:32:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:11Z- waiting for 1730863934.09, time is 1730863930.89 2024-11-06T03:32:11Z- handleDepth()- Depth_Keller msg: 1730863931.29 epoch sec; seqNo:6364; depth 50.33 2024-11-06T03:32:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863931.30 epoch sec; seqNo:6364; 4061802.90 north; 594474.49 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:32:11Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:32:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863930.09 2024-11-06T03:32:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:11Z- waiting for 1730863934.09, time is 1730863931.30 2024-11-06T03:32:11Z- AHRS_M2 msg: 1730863931.68 epoch sec; seqNo:5971 2024-11-06T03:32:11Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.19 psi 2024-11-06T03:32:11Z- handleDepth()- Depth_Keller msg: 1730863931.68 epoch sec; seqNo:6365; depth 50.32 2024-11-06T03:32:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863931.70 epoch sec; seqNo:6365; 4061802.51 north; 594473.27 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:32:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863930.09 2024-11-06T03:32:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:11Z- waiting for 1730863934.09, time is 1730863931.70 2024-11-06T03:32:12Z- AHRS_M2 msg: 1730863932.08 epoch sec; seqNo:5972 2024-11-06T03:32:12Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.19 psi 2024-11-06T03:32:12Z- handleDepth()- Depth_Keller msg: 1730863932.09 epoch sec; seqNo:6366; depth 50.33 2024-11-06T03:32:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863932.11 epoch sec; seqNo:6366; 4061802.51 north; 594473.27 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:32:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863930.09 2024-11-06T03:32:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:12Z- waiting for 1730863934.09, time is 1730863932.11 2024-11-06T03:32:12Z- AHRS_M2 msg: 1730863932.49 epoch sec; seqNo:5973 2024-11-06T03:32:12Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.19 psi 2024-11-06T03:32:12Z- handleDepth()- Depth_Keller msg: 1730863932.50 epoch sec; seqNo:6367; depth 50.32 2024-11-06T03:32:12Z- handleDVL() - DVL msg epochMillisec: 1730863932.52 2024-11-06T03:32:12Z- handleDvl() - RDI_Pathfinder msg: 1730863932.52 sec; seqNo:2171; #beams:4 2024-11-06T03:32:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.02 , 32.62 , 34.83 , 36.02 2024-11-06T03:32:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , 0.00 2024-11-06T03:32:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863932.55 epoch sec; seqNo:6367; 4061802.13 north; 594473.27 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:32:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863930.09 2024-11-06T03:32:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:12Z- waiting for 1730863934.09, time is 1730863932.55 2024-11-06T03:32:12Z- AHRS_M2 msg: 1730863932.89 epoch sec; seqNo:5974 2024-11-06T03:32:12Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.19 psi 2024-11-06T03:32:12Z- handleDepth()- Depth_Keller msg: 1730863932.90 epoch sec; seqNo:6368; depth 50.33 2024-11-06T03:32:12Z- handleDVL() - DVL msg epochMillisec: 1730863932.90 2024-11-06T03:32:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:12Z- handleDvl() - RDI_Pathfinder msg: 1730863932.90 sec; seqNo:2172; #beams:0 2024-11-06T03:32:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , 0.00 2024-11-06T03:32:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863932.92 epoch sec; seqNo:6368; 4061802.12 north; 594472.05 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:32:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863930.09 2024-11-06T03:32:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:13Z- waiting for 1730863934.09, time is 1730863932.92 2024-11-06T03:32:13Z- AHRS_M2 msg: 1730863933.29 epoch sec; seqNo:5975 2024-11-06T03:32:13Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.19 psi 2024-11-06T03:32:13Z- handleDepth()- Depth_Keller msg: 1730863933.30 epoch sec; seqNo:6369; depth 50.34 2024-11-06T03:32:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863933.32 epoch sec; seqNo:6369; 4061801.74 north; 594472.06 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:32:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863930.09 2024-11-06T03:32:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:13Z- waiting for 1730863934.09, time is 1730863933.32 2024-11-06T03:32:13Z- AHRS_M2 msg: 1730863933.70 epoch sec; seqNo:5976 2024-11-06T03:32:13Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.19 psi 2024-11-06T03:32:13Z- handleDepth()- Depth_Keller msg: 1730863933.70 epoch sec; seqNo:6370; depth 50.33 2024-11-06T03:32:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863933.71 epoch sec; seqNo:6370; 4061801.74 north; 594472.06 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:32:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863930.09 2024-11-06T03:32:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:13Z- waiting for 1730863934.09, time is 1730863933.71 2024-11-06T03:32:14Z- AHRS_M2 msg: 1730863934.10 epoch sec; seqNo:5977 2024-11-06T03:32:14Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.19 psi 2024-11-06T03:32:14Z- handleDepth()- Depth_Keller msg: 1730863934.11 epoch sec; seqNo:6371; depth 50.34 2024-11-06T03:32:14Z- handleDVL() - DVL msg epochMillisec: 1730863934.12 2024-11-06T03:32:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:14Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:14Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:14Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:14Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:14Z- handleDvl() - RDI_Pathfinder msg: 1730863934.12 sec; seqNo:2173; #beams:0 2024-11-06T03:32:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , 0.00 2024-11-06T03:32:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863934.12 epoch sec; seqNo:6371; 4061801.36 north; 594472.06 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:32:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863930.09 2024-11-06T03:32:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:14Z- waiting for 1730863934.09, time is 1730863934.12 2024-11-06T03:32:14Z- LcmTrn::updateTrn() >>>> pose time 1730863934.12, meas time 1730863934.12 <<<< 2024-11-06T03:32:14Z- LcmTrn::updateTrn() equating pose 1730863934.12 and meas times 1730863934.12 2024-11-06T03:32:14Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863934.12 2024-11-06T03:32:14Z- TerrainNav::motionUpdate() - currEstimate = (4061801, 594472.06, 50.34) 2024-11-06T03:32:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:32:14Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:32:14Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:32:14Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:32:14Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:32:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:32:14Z- variances([0], [2], and psi_berg) = 0.179988 0.251259 0.000000 2024-11-06T03:32:14Z- publishEstimatesDecoder() - at 1730863934.12 sec 2024-11-06T03:32:14Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:32:14Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061829.30 594455.35 53.59 2024-11-06T03:32:14Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061829.56 594456.04 53.98 lat/lon: 36.70 -121.94 2024-11-06T03:32:14Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.25 1.08 0.00 2024-11-06T03:32:14Z- AHRS_M2 msg: 1730863934.51 epoch sec; seqNo:5978 2024-11-06T03:32:14Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.19 psi 2024-11-06T03:32:14Z- handleDepth()- Depth_Keller msg: 1730863934.51 epoch sec; seqNo:6372; depth 50.35 2024-11-06T03:32:14Z- handleDVL() - DVL msg epochMillisec: 1730863934.53 2024-11-06T03:32:14Z- handleDvl() - RDI_Pathfinder msg: 1730863934.53 sec; seqNo:2174; #beams:4 2024-11-06T03:32:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.02 , 32.79 , 34.83 , 36.19 2024-11-06T03:32:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , 0.03 2024-11-06T03:32:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863934.56 epoch sec; seqNo:6372; 4061801.35 north; 594470.84 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:32:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863934.12 2024-11-06T03:32:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:14Z- waiting for 1730863938.12, time is 1730863934.56 2024-11-06T03:32:14Z- AHRS_M2 msg: 1730863934.92 epoch sec; seqNo:5979 2024-11-06T03:32:14Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.19 psi 2024-11-06T03:32:14Z- handleDepth()- Depth_Keller msg: 1730863934.92 epoch sec; seqNo:6373; depth 50.35 2024-11-06T03:32:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863934.94 epoch sec; seqNo:6373; 4061800.97 north; 594470.85 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:32:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863934.12 2024-11-06T03:32:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:15Z- waiting for 1730863938.12, time is 1730863934.94 2024-11-06T03:32:15Z- AHRS_M2 msg: 1730863935.32 epoch sec; seqNo:5980 2024-11-06T03:32:15Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.19 psi 2024-11-06T03:32:15Z- handleDepth()- Depth_Keller msg: 1730863935.32 epoch sec; seqNo:6374; depth 50.35 2024-11-06T03:32:15Z- handleDVL() - DVL msg epochMillisec: 1730863935.33 2024-11-06T03:32:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:15Z- handleDvl() - RDI_Pathfinder msg: 1730863935.33 sec; seqNo:2175; #beams:0 2024-11-06T03:32:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , 0.03 2024-11-06T03:32:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863935.34 epoch sec; seqNo:6374; 4061800.97 north; 594470.85 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:32:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863934.12 2024-11-06T03:32:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:15Z- waiting for 1730863938.12, time is 1730863935.34 2024-11-06T03:32:15Z- AHRS_M2 msg: 1730863935.72 epoch sec; seqNo:5981 2024-11-06T03:32:15Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.19 psi 2024-11-06T03:32:15Z- handleDepth()- Depth_Keller msg: 1730863935.72 epoch sec; seqNo:6375; depth 50.34 2024-11-06T03:32:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863935.73 epoch sec; seqNo:6375; 4061800.97 north; 594470.85 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:32:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863934.12 2024-11-06T03:32:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:15Z- waiting for 1730863938.12, time is 1730863935.73 2024-11-06T03:32:16Z- AHRS_M2 msg: 1730863936.12 epoch sec; seqNo:5982 2024-11-06T03:32:16Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.19 psi 2024-11-06T03:32:16Z- handleDepth()- Depth_Keller msg: 1730863936.13 epoch sec; seqNo:6376; depth 50.36 2024-11-06T03:32:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863936.14 epoch sec; seqNo:6376; 4061800.58 north; 594469.63 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:32:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863934.12 2024-11-06T03:32:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:16Z- waiting for 1730863938.12, time is 1730863936.14 2024-11-06T03:32:16Z- handleDepth()- Depth_Keller msg: 1730863936.54 epoch sec; seqNo:6377; depth 50.36 2024-11-06T03:32:16Z- handleDVL() - DVL msg epochMillisec: 1730863936.56 2024-11-06T03:32:16Z- handleDvl() - RDI_Pathfinder msg: 1730863936.56 sec; seqNo:2176; #beams:4 2024-11-06T03:32:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.19 , 32.79 , 34.66 , 35.85 2024-11-06T03:32:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.01 2024-11-06T03:32:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863936.58 epoch sec; seqNo:6377; 4061800.58 north; 594469.63 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:32:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863934.12 2024-11-06T03:32:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:16Z- waiting for 1730863938.12, time is 1730863936.58 2024-11-06T03:32:16Z- AHRS_M2 msg: 1730863936.93 epoch sec; seqNo:5983 2024-11-06T03:32:16Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.20 psi 2024-11-06T03:32:16Z- handleDepth()- Depth_Keller msg: 1730863936.94 epoch sec; seqNo:6378; depth 50.34 2024-11-06T03:32:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863936.97 epoch sec; seqNo:6378; 4061800.20 north; 594469.63 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:32:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863934.12 2024-11-06T03:32:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:17Z- waiting for 1730863938.12, time is 1730863936.97 2024-11-06T03:32:17Z- AHRS_M2 msg: 1730863937.33 epoch sec; seqNo:5984 2024-11-06T03:32:17Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.20 psi 2024-11-06T03:32:17Z- handleDepth()- Depth_Keller msg: 1730863937.35 epoch sec; seqNo:6379; depth 50.35 2024-11-06T03:32:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863937.36 epoch sec; seqNo:6379; 4061800.20 north; 594469.63 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:32:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863934.12 2024-11-06T03:32:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:17Z- waiting for 1730863938.12, time is 1730863937.36 2024-11-06T03:32:17Z- AHRS_M2 msg: 1730863937.74 epoch sec; seqNo:5985 2024-11-06T03:32:17Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.20 psi 2024-11-06T03:32:17Z- handleDepth()- Depth_Keller msg: 1730863937.75 epoch sec; seqNo:6380; depth 50.36 2024-11-06T03:32:17Z- handleDVL() - DVL msg epochMillisec: 1730863937.75 2024-11-06T03:32:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:17Z- handleDvl() - RDI_Pathfinder msg: 1730863937.75 sec; seqNo:2177; #beams:0 2024-11-06T03:32:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.01 2024-11-06T03:32:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863937.76 epoch sec; seqNo:6380; 4061799.81 north; 594468.42 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:32:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863934.12 2024-11-06T03:32:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:17Z- waiting for 1730863938.12, time is 1730863937.76 2024-11-06T03:32:18Z- AHRS_M2 msg: 1730863938.15 epoch sec; seqNo:5986 2024-11-06T03:32:18Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.20 psi 2024-11-06T03:32:18Z- handleDepth()- Depth_Keller msg: 1730863938.17 epoch sec; seqNo:6381; depth 50.35 2024-11-06T03:32:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863938.23 epoch sec; seqNo:6381; 4061799.81 north; 594468.42 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:32:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863934.12 2024-11-06T03:32:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:18Z- waiting for 1730863938.12, time is 1730863938.23 2024-11-06T03:32:18Z- LcmTrn::updateTrn() >>>> pose time 1730863938.23, meas time 1730863937.75 <<<< 2024-11-06T03:32:18Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863938.23 2024-11-06T03:32:18Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:32:18Z- TerrainNav::motionUpdate() - currEstimate = (4061800, 594468.42, 50.35) 2024-11-06T03:32:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:32:18Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:32:18Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:32:18Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:32:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:32:18Z- variances([0], [2], and psi_berg) = 0.182445 0.254444 0.000000 2024-11-06T03:32:18Z- publishEstimatesDecoder() - at 1730863938.23 sec 2024-11-06T03:32:18Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:32:18Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061827.72 594451.71 53.89 2024-11-06T03:32:18Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061828.01 594452.39 54.00 lat/lon: 36.70 -121.94 2024-11-06T03:32:18Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.25 1.33 0.00 2024-11-06T03:32:18Z- AHRS_M2 msg: 1730863938.55 epoch sec; seqNo:5987 2024-11-06T03:32:18Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.20 psi 2024-11-06T03:32:18Z- handleDepth()- Depth_Keller msg: 1730863938.56 epoch sec; seqNo:6382; depth 50.35 2024-11-06T03:32:18Z- handleDVL() - DVL msg epochMillisec: 1730863938.58 2024-11-06T03:32:18Z- handleDvl() - RDI_Pathfinder msg: 1730863938.58 sec; seqNo:2178; #beams:4 2024-11-06T03:32:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.36 , 32.62 , 34.83 , 35.85 2024-11-06T03:32:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.00 2024-11-06T03:32:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863938.60 epoch sec; seqNo:6382; 4061799.43 north; 594468.42 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:32:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863938.23 2024-11-06T03:32:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:18Z- waiting for 1730863942.23, time is 1730863938.60 2024-11-06T03:32:18Z- AHRS_M2 msg: 1730863938.96 epoch sec; seqNo:5988 2024-11-06T03:32:18Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.21 psi 2024-11-06T03:32:18Z- handleDepth()- Depth_Keller msg: 1730863938.97 epoch sec; seqNo:6383; depth 50.35 2024-11-06T03:32:18Z- handleDVL() - DVL msg epochMillisec: 1730863938.97 2024-11-06T03:32:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:18Z- handleDvl() - RDI_Pathfinder msg: 1730863938.97 sec; seqNo:2179; #beams:0 2024-11-06T03:32:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.00 2024-11-06T03:32:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863939.00 epoch sec; seqNo:6383; 4061799.41 north; 594467.20 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:32:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863938.23 2024-11-06T03:32:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:19Z- waiting for 1730863942.23, time is 1730863939.00 2024-11-06T03:32:19Z- AHRS_M2 msg: 1730863939.36 epoch sec; seqNo:5989 2024-11-06T03:32:19Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.21 psi 2024-11-06T03:32:19Z- handleDepth()- Depth_Keller msg: 1730863939.37 epoch sec; seqNo:6384; depth 50.35 2024-11-06T03:32:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863939.38 epoch sec; seqNo:6384; 4061799.03 north; 594467.21 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:32:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863938.23 2024-11-06T03:32:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:19Z- waiting for 1730863942.23, time is 1730863939.38 2024-11-06T03:32:19Z- AHRS_M2 msg: 1730863939.76 epoch sec; seqNo:5990 2024-11-06T03:32:19Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.21 psi 2024-11-06T03:32:19Z- handleDepth()- Depth_Keller msg: 1730863939.76 epoch sec; seqNo:6385; depth 50.35 2024-11-06T03:32:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863939.79 epoch sec; seqNo:6385; 4061799.03 north; 594467.21 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:32:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863938.23 2024-11-06T03:32:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:19Z- waiting for 1730863942.23, time is 1730863939.79 2024-11-06T03:32:20Z- AHRS_M2 msg: 1730863940.22 epoch sec; seqNo:5991 2024-11-06T03:32:20Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 4.21 psi 2024-11-06T03:32:20Z- handleDepth()- Depth_Keller msg: 1730863940.23 epoch sec; seqNo:6386; depth 50.34 2024-11-06T03:32:20Z- handleDVL() - DVL msg epochMillisec: 1730863940.27 2024-11-06T03:32:20Z- handleDvl() - RDI_Pathfinder msg: 1730863940.27 sec; seqNo:2180; #beams:4 2024-11-06T03:32:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.70 , 32.45 , 35.17 , 35.51 2024-11-06T03:32:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , 0.00 2024-11-06T03:32:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863940.29 epoch sec; seqNo:6386; 4061798.66 north; 594467.21 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:32:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863938.23 2024-11-06T03:32:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:20Z- waiting for 1730863942.23, time is 1730863940.29 2024-11-06T03:32:20Z- AHRS_M2 msg: 1730863940.57 epoch sec; seqNo:5992 2024-11-06T03:32:20Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 4.21 psi 2024-11-06T03:32:20Z- handleDepth()- Depth_Keller msg: 1730863940.58 epoch sec; seqNo:6387; depth 50.34 2024-11-06T03:32:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863940.60 epoch sec; seqNo:6387; 4061798.64 north; 594465.99 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:32:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863938.23 2024-11-06T03:32:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:20Z- waiting for 1730863942.23, time is 1730863940.60 2024-11-06T03:32:20Z- AHRS_M2 msg: 1730863940.97 epoch sec; seqNo:5993 2024-11-06T03:32:20Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 4.21 psi 2024-11-06T03:32:20Z- handleDepth()- Depth_Keller msg: 1730863940.99 epoch sec; seqNo:6388; depth 50.33 2024-11-06T03:32:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863940.99 epoch sec; seqNo:6388; 4061798.64 north; 594465.99 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:32:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863938.23 2024-11-06T03:32:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:21Z- waiting for 1730863942.23, time is 1730863940.99 2024-11-06T03:32:21Z- AHRS_M2 msg: 1730863941.38 epoch sec; seqNo:5994 2024-11-06T03:32:21Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 4.21 psi 2024-11-06T03:32:21Z- handleDepth()- Depth_Keller msg: 1730863941.39 epoch sec; seqNo:6389; depth 50.33 2024-11-06T03:32:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863941.41 epoch sec; seqNo:6389; 4061798.26 north; 594465.99 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:32:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863938.23 2024-11-06T03:32:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:21Z- waiting for 1730863942.23, time is 1730863941.41 2024-11-06T03:32:21Z- AHRS_M2 msg: 1730863941.78 epoch sec; seqNo:5995 2024-11-06T03:32:21Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 4.21 psi 2024-11-06T03:32:21Z- handleDepth()- Depth_Keller msg: 1730863941.80 epoch sec; seqNo:6390; depth 50.34 2024-11-06T03:32:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863941.88 epoch sec; seqNo:6390; 4061798.25 north; 594464.77 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:32:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863938.23 2024-11-06T03:32:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:21Z- waiting for 1730863942.23, time is 1730863941.88 2024-11-06T03:32:22Z- AHRS_M2 msg: 1730863942.24 epoch sec; seqNo:5996 2024-11-06T03:32:22Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 4.21 psi 2024-11-06T03:32:22Z- handleDepth()- Depth_Keller msg: 1730863942.24 epoch sec; seqNo:6391; depth 50.34 2024-11-06T03:32:22Z- handleDVL() - DVL msg epochMillisec: 1730863942.27 2024-11-06T03:32:22Z- handleDvl() - RDI_Pathfinder msg: 1730863942.27 sec; seqNo:2181; #beams:4 2024-11-06T03:32:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.54 , 32.28 , 34.83 , 35.34 2024-11-06T03:32:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.03 , 0.01 2024-11-06T03:32:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863942.31 epoch sec; seqNo:6391; 4061797.87 north; 594464.78 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:32:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863938.23 2024-11-06T03:32:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:22Z- waiting for 1730863942.23, time is 1730863942.31 2024-11-06T03:32:22Z- LcmTrn::updateTrn() >>>> pose time 1730863942.31, meas time 1730863942.27 <<<< 2024-11-06T03:32:22Z- LcmTrn::updateTrn() equating pose 1730863942.27 and meas times 1730863942.27 2024-11-06T03:32:22Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863942.27 2024-11-06T03:32:22Z- TerrainNav::motionUpdate() - currEstimate = (4061798, 594464.78, 50.34) 2024-11-06T03:32:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:32:22Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:32:22Z- TNavPF::Projecting Measurements 2024-11-06T03:32:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:32:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:32:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:32:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:32:22Z- TNavFilter::4 good beams. 2024-11-06T03:32:22Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:32:22Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:32:22Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:32:22Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:32:22Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:32:22Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:32:22Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:32:22Z- TerrainMapOctree: (vehN, vehE ) = (4061797.87, 594464.78). 2024-11-06T03:32:22Z- TerrainMapOctree: Min Distance = 220.05. 2024-11-06T03:32:22Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:32:22Z- Forcing Subcloud Comparison 2024-11-06T03:32:22Z- Weighting particles with subcloud comparison 2024-11-06T03:32:22Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:22Z- meanExpectedMeasDiff: -2.767269 delta_rms_squared: 1.723793 alpha: 0.988531 2024-11-06T03:32:22Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:22Z- meanExpectedMeasDiff: 0.397586 delta_rms_squared: 1.800679 alpha: 0.989015 2024-11-06T03:32:22Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:22Z- meanExpectedMeasDiff: -1.055551 delta_rms_squared: 2.174534 alpha: 0.990886 2024-11-06T03:32:22Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:22Z- meanExpectedMeasDiff: -0.465122 delta_rms_squared: 2.127058 alpha: 0.990685 2024-11-06T03:32:22Z- Subcloud NIS: 0.503593 2024-11-06T03:32:22Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:32:22Z- TNavPF:: sumWeights = 0.227752 2024-11-06T03:32:22Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:32:22Z- TNavPF::First Term of Map Covariance Matrix is 0.83 2024-11-06T03:32:22Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T03:32:22Z- TerrainNav::Resampling particle filter... 2024-11-06T03:32:22Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863942.27 sec, ping # 0. 2024-11-06T03:32:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:32:22Z- variances([0], [2], and psi_berg) = 0.194899 0.251289 0.000000 2024-11-06T03:32:22Z- publishEstimatesDecoder() - at 1730863942.27 sec 2024-11-06T03:32:22Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:32:22Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061826.80 594447.73 53.49 2024-11-06T03:32:22Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061826.03 594448.75 54.55 lat/lon: 36.70 -121.94 2024-11-06T03:32:22Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.19 0.25 0.54 0.00 2024-11-06T03:32:22Z- AHRS_M2 msg: 1730863942.65 epoch sec; seqNo:5997 2024-11-06T03:32:22Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 4.21 psi 2024-11-06T03:32:22Z- handleDepth()- Depth_Keller msg: 1730863942.66 epoch sec; seqNo:6392; depth 50.34 2024-11-06T03:32:22Z- handleDVL() - DVL msg epochMillisec: 1730863942.66 2024-11-06T03:32:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:22Z- handleDvl() - RDI_Pathfinder msg: 1730863942.66 sec; seqNo:2182; #beams:0 2024-11-06T03:32:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.03 , 0.01 2024-11-06T03:32:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863942.70 epoch sec; seqNo:6392; 4061797.87 north; 594464.78 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:32:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863942.27 2024-11-06T03:32:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:22Z- waiting for 1730863946.27, time is 1730863942.70 2024-11-06T03:32:23Z- AHRS_M2 msg: 1730863943.02 epoch sec; seqNo:5998 2024-11-06T03:32:23Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 4.21 psi 2024-11-06T03:32:23Z- handleDepth()- Depth_Keller msg: 1730863943.03 epoch sec; seqNo:6393; depth 50.34 2024-11-06T03:32:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863943.04 epoch sec; seqNo:6393; 4061797.49 north; 594464.78 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:32:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863942.27 2024-11-06T03:32:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:23Z- waiting for 1730863946.27, time is 1730863943.04 2024-11-06T03:32:23Z- AHRS_M2 msg: 1730863943.42 epoch sec; seqNo:5999 2024-11-06T03:32:23Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 4.21 psi 2024-11-06T03:32:23Z- handleDepth()- Depth_Keller msg: 1730863943.43 epoch sec; seqNo:6394; depth 50.34 2024-11-06T03:32:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863943.45 epoch sec; seqNo:6394; 4061797.48 north; 594463.56 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:32:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863942.27 2024-11-06T03:32:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:23Z- waiting for 1730863946.27, time is 1730863943.45 2024-11-06T03:32:23Z- AHRS_M2 msg: 1730863943.83 epoch sec; seqNo:6000 2024-11-06T03:32:23Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.21 psi 2024-11-06T03:32:23Z- handleDepth()- Depth_Keller msg: 1730863943.84 epoch sec; seqNo:6395; depth 50.34 2024-11-06T03:32:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863943.85 epoch sec; seqNo:6395; 4061797.10 north; 594463.57 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:32:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863942.27 2024-11-06T03:32:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:23Z- waiting for 1730863946.27, time is 1730863943.85 2024-11-06T03:32:24Z- AHRS_M2 msg: 1730863944.24 epoch sec; seqNo:6001 2024-11-06T03:32:24Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.21 psi 2024-11-06T03:32:24Z- handleDepth()- Depth_Keller msg: 1730863944.24 epoch sec; seqNo:6396; depth 50.34 2024-11-06T03:32:24Z- handleDVL() - DVL msg epochMillisec: 1730863944.27 2024-11-06T03:32:24Z- handleDvl() - RDI_Pathfinder msg: 1730863944.27 sec; seqNo:2183; #beams:4 2024-11-06T03:32:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.72 , 31.26 , 35.00 , 35.17 2024-11-06T03:32:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , 0.01 2024-11-06T03:32:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863944.30 epoch sec; seqNo:6396; 4061797.10 north; 594463.57 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:32:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863942.27 2024-11-06T03:32:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:24Z- waiting for 1730863946.27, time is 1730863944.30 2024-11-06T03:32:24Z- AHRS_M2 msg: 1730863944.64 epoch sec; seqNo:6002 2024-11-06T03:32:24Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.21 psi 2024-11-06T03:32:24Z- handleDepth()- Depth_Keller msg: 1730863944.64 epoch sec; seqNo:6397; depth 50.33 2024-11-06T03:32:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863944.66 epoch sec; seqNo:6397; 4061796.72 north; 594463.57 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:32:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863942.27 2024-11-06T03:32:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:24Z- waiting for 1730863946.27, time is 1730863944.66 2024-11-06T03:32:25Z- AHRS_M2 msg: 1730863945.04 epoch sec; seqNo:6003 2024-11-06T03:32:25Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.21 psi 2024-11-06T03:32:25Z- handleDepth()- Depth_Keller msg: 1730863945.04 epoch sec; seqNo:6398; depth 50.34 2024-11-06T03:32:25Z- handleDVL() - DVL msg epochMillisec: 1730863945.05 2024-11-06T03:32:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:25Z- handleDvl() - RDI_Pathfinder msg: 1730863945.05 sec; seqNo:2184; #beams:0 2024-11-06T03:32:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , 0.01 2024-11-06T03:32:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863945.07 epoch sec; seqNo:6398; 4061796.71 north; 594462.35 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:32:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863942.27 2024-11-06T03:32:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:25Z- waiting for 1730863946.27, time is 1730863945.07 2024-11-06T03:32:25Z- AHRS_M2 msg: 1730863945.45 epoch sec; seqNo:6004 2024-11-06T03:32:25Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.21 psi 2024-11-06T03:32:25Z- handleDepth()- Depth_Keller msg: 1730863945.45 epoch sec; seqNo:6399; depth 50.34 2024-11-06T03:32:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863945.46 epoch sec; seqNo:6399; 4061796.33 north; 594462.35 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:32:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863942.27 2024-11-06T03:32:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:25Z- waiting for 1730863946.27, time is 1730863945.46 2024-11-06T03:32:25Z- AHRS_M2 msg: 1730863945.85 epoch sec; seqNo:6005 2024-11-06T03:32:25Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.21 psi 2024-11-06T03:32:25Z- handleDepth()- Depth_Keller msg: 1730863945.85 epoch sec; seqNo:6400; depth 50.33 2024-11-06T03:32:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863945.86 epoch sec; seqNo:6400; 4061796.33 north; 594462.35 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:32:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863942.27 2024-11-06T03:32:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:26Z- waiting for 1730863946.27, time is 1730863945.86 2024-11-06T03:32:26Z- AHRS_M2 msg: 1730863946.25 epoch sec; seqNo:6006 2024-11-06T03:32:26Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.21 psi 2024-11-06T03:32:26Z- handleDepth()- Depth_Keller msg: 1730863946.26 epoch sec; seqNo:6401; depth 50.33 2024-11-06T03:32:26Z- handleDVL() - DVL msg epochMillisec: 1730863946.28 2024-11-06T03:32:26Z- handleDvl() - RDI_Pathfinder msg: 1730863946.28 sec; seqNo:2185; #beams:4 2024-11-06T03:32:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.21 , 31.26 , 34.83 , 35.00 2024-11-06T03:32:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , 0.01 2024-11-06T03:32:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863946.30 epoch sec; seqNo:6401; 4061795.94 north; 594461.14 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:32:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863942.27 2024-11-06T03:32:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:26Z- waiting for 1730863946.27, time is 1730863946.30 2024-11-06T03:32:26Z- LcmTrn::updateTrn() >>>> pose time 1730863946.30, meas time 1730863946.28 <<<< 2024-11-06T03:32:26Z- LcmTrn::updateTrn() equating pose 1730863946.28 and meas times 1730863946.28 2024-11-06T03:32:26Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863946.28 2024-11-06T03:32:26Z- TerrainNav::motionUpdate() - currEstimate = (4061796, 594461.14, 50.33) 2024-11-06T03:32:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:32:26Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:32:26Z- TNavPF::Projecting Measurements 2024-11-06T03:32:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:32:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:32:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:32:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:32:26Z- TNavFilter::4 good beams. 2024-11-06T03:32:26Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:32:26Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:32:26Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:32:26Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:32:26Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:32:26Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:32:26Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:32:26Z- TerrainMapOctree: (vehN, vehE ) = (4061795.94, 594461.14). 2024-11-06T03:32:26Z- TerrainMapOctree: Min Distance = 216.08. 2024-11-06T03:32:26Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:32:26Z- Forcing Subcloud Comparison 2024-11-06T03:32:26Z- Weighting particles with subcloud comparison 2024-11-06T03:32:26Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:26Z- meanExpectedMeasDiff: -2.248871 delta_rms_squared: 1.261473 alpha: 0.984393 2024-11-06T03:32:26Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:26Z- meanExpectedMeasDiff: -0.186755 delta_rms_squared: 0.916312 alpha: 0.978640 2024-11-06T03:32:26Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:26Z- meanExpectedMeasDiff: -0.591091 delta_rms_squared: 1.132758 alpha: 0.982650 2024-11-06T03:32:26Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:26Z- meanExpectedMeasDiff: 0.143655 delta_rms_squared: 1.105471 alpha: 0.982230 2024-11-06T03:32:26Z- Subcloud NIS: 0.490227 2024-11-06T03:32:26Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:32:26Z- TNavPF:: sumWeights = 0.398166 2024-11-06T03:32:26Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:32:26Z- TNavPF::First Term of Map Covariance Matrix is 0.71 2024-11-06T03:32:26Z- TNavPF::Calculated NIS Value : 0.21 numBeams normalized NIS: 0.05 2024-11-06T03:32:26Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863946.28 sec, ping # 0. 2024-11-06T03:32:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:32:26Z- variances([0], [2], and psi_berg) = 0.188150 0.250964 0.000000 2024-11-06T03:32:26Z- publishEstimatesDecoder() - at 1730863946.28 sec 2024-11-06T03:32:26Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:32:26Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061822.90 594445.65 55.38 2024-11-06T03:32:26Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061824.11 594445.12 54.89 lat/lon: 36.70 -121.94 2024-11-06T03:32:26Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.19 0.25 0.36 0.00 2024-11-06T03:32:26Z- AHRS_M2 msg: 1730863946.65 epoch sec; seqNo:6007 2024-11-06T03:32:26Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:32:26Z- handleDepth()- Depth_Keller msg: 1730863946.67 epoch sec; seqNo:6402; depth 50.32 2024-11-06T03:32:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863946.69 epoch sec; seqNo:6402; 4061795.94 north; 594461.14 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:32:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863946.28 2024-11-06T03:32:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:26Z- waiting for 1730863950.28, time is 1730863946.69 2024-11-06T03:32:27Z- AHRS_M2 msg: 1730863947.06 epoch sec; seqNo:6008 2024-11-06T03:32:27Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:32:27Z- handleDepth()- Depth_Keller msg: 1730863947.07 epoch sec; seqNo:6403; depth 50.32 2024-11-06T03:32:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863947.08 epoch sec; seqNo:6403; 4061795.94 north; 594461.14 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:32:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863946.28 2024-11-06T03:32:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:27Z- waiting for 1730863950.28, time is 1730863947.08 2024-11-06T03:32:27Z- AHRS_M2 msg: 1730863947.48 epoch sec; seqNo:6009 2024-11-06T03:32:27Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:32:27Z- handleDepth()- Depth_Keller msg: 1730863947.48 epoch sec; seqNo:6404; depth 50.32 2024-11-06T03:32:27Z- handleDVL() - DVL msg epochMillisec: 1730863947.49 2024-11-06T03:32:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:27Z- handleDvl() - RDI_Pathfinder msg: 1730863947.49 sec; seqNo:2186; #beams:0 2024-11-06T03:32:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , 0.01 2024-11-06T03:32:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863947.49 epoch sec; seqNo:6404; 4061795.56 north; 594461.14 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:32:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863946.28 2024-11-06T03:32:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:27Z- waiting for 1730863950.28, time is 1730863947.49 2024-11-06T03:32:27Z- AHRS_M2 msg: 1730863947.87 epoch sec; seqNo:6010 2024-11-06T03:32:27Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:32:27Z- handleDepth()- Depth_Keller msg: 1730863947.87 epoch sec; seqNo:6405; depth 50.31 2024-11-06T03:32:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863947.89 epoch sec; seqNo:6405; 4061795.54 north; 594459.92 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:32:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863946.28 2024-11-06T03:32:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:28Z- waiting for 1730863950.28, time is 1730863947.89 2024-11-06T03:32:28Z- AHRS_M2 msg: 1730863948.28 epoch sec; seqNo:6011 2024-11-06T03:32:28Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:32:28Z- handleDepth()- Depth_Keller msg: 1730863948.28 epoch sec; seqNo:6406; depth 50.30 2024-11-06T03:32:28Z- handleDVL() - DVL msg epochMillisec: 1730863948.33 2024-11-06T03:32:28Z- handleDvl() - RDI_Pathfinder msg: 1730863948.33 sec; seqNo:2187; #beams:4 2024-11-06T03:32:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.54 , 31.60 , 34.83 , 34.66 2024-11-06T03:32:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , 0.01 2024-11-06T03:32:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863948.35 epoch sec; seqNo:6406; 4061795.17 north; 594459.93 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:32:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863946.28 2024-11-06T03:32:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:28Z- waiting for 1730863950.28, time is 1730863948.35 2024-11-06T03:32:28Z- AHRS_M2 msg: 1730863948.67 epoch sec; seqNo:6012 2024-11-06T03:32:28Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:32:28Z- handleDepth()- Depth_Keller msg: 1730863948.68 epoch sec; seqNo:6407; depth 50.29 2024-11-06T03:32:28Z- handleDVL() - DVL msg epochMillisec: 1730863948.68 2024-11-06T03:32:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:28Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:28Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:28Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:28Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:28Z- handleDvl() - RDI_Pathfinder msg: 1730863948.68 sec; seqNo:2188; #beams:0 2024-11-06T03:32:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , 0.01 2024-11-06T03:32:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863948.69 epoch sec; seqNo:6407; 4061795.17 north; 594459.93 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:32:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863946.28 2024-11-06T03:32:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:28Z- waiting for 1730863950.28, time is 1730863948.69 2024-11-06T03:32:29Z- AHRS_M2 msg: 1730863949.08 epoch sec; seqNo:6013 2024-11-06T03:32:29Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 4.21 psi 2024-11-06T03:32:29Z- handleDepth()- Depth_Keller msg: 1730863949.09 epoch sec; seqNo:6408; depth 50.28 2024-11-06T03:32:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863949.10 epoch sec; seqNo:6408; 4061794.79 north; 594459.93 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:32:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863946.28 2024-11-06T03:32:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:29Z- waiting for 1730863950.28, time is 1730863949.10 2024-11-06T03:32:29Z- AHRS_M2 msg: 1730863949.48 epoch sec; seqNo:6014 2024-11-06T03:32:29Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 4.21 psi 2024-11-06T03:32:29Z- handleDepth()- Depth_Keller msg: 1730863949.51 epoch sec; seqNo:6409; depth 50.28 2024-11-06T03:32:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863949.52 epoch sec; seqNo:6409; 4061794.77 north; 594458.71 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:32:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863946.28 2024-11-06T03:32:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:29Z- waiting for 1730863950.28, time is 1730863949.52 2024-11-06T03:32:29Z- AHRS_M2 msg: 1730863949.89 epoch sec; seqNo:6015 2024-11-06T03:32:29Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 4.21 psi 2024-11-06T03:32:29Z- handleDepth()- Depth_Keller msg: 1730863949.89 epoch sec; seqNo:6410; depth 50.25 2024-11-06T03:32:29Z- handleDVL() - DVL msg epochMillisec: 1730863949.89 2024-11-06T03:32:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:29Z- handleDvl() - RDI_Pathfinder msg: 1730863949.89 sec; seqNo:2189; #beams:0 2024-11-06T03:32:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , 0.01 2024-11-06T03:32:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863949.90 epoch sec; seqNo:6410; 4061794.39 north; 594458.71 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:32:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863946.28 2024-11-06T03:32:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:30Z- waiting for 1730863950.28, time is 1730863949.90 2024-11-06T03:32:30Z- AHRS_M2 msg: 1730863950.29 epoch sec; seqNo:6016 2024-11-06T03:32:30Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 4.21 psi 2024-11-06T03:32:30Z- handleDepth()- Depth_Keller msg: 1730863950.30 epoch sec; seqNo:6411; depth 50.24 2024-11-06T03:32:30Z- handleDVL() - DVL msg epochMillisec: 1730863950.32 2024-11-06T03:32:30Z- handleDvl() - RDI_Pathfinder msg: 1730863950.32 sec; seqNo:2190; #beams:4 2024-11-06T03:32:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.04 , 32.96 , 34.15 , 34.32 2024-11-06T03:32:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.02 2024-11-06T03:32:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863950.35 epoch sec; seqNo:6411; 4061794.39 north; 594458.71 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:32:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863946.28 2024-11-06T03:32:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:30Z- waiting for 1730863950.28, time is 1730863950.35 2024-11-06T03:32:30Z- LcmTrn::updateTrn() >>>> pose time 1730863950.35, meas time 1730863950.32 <<<< 2024-11-06T03:32:30Z- LcmTrn::updateTrn() equating pose 1730863950.32 and meas times 1730863950.32 2024-11-06T03:32:30Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863950.32 2024-11-06T03:32:30Z- TerrainNav::motionUpdate() - currEstimate = (4061794, 594458.71, 50.24) 2024-11-06T03:32:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:32:30Z- TerrainNav:: SubcloudNIS is 0.49, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:32:30Z- TNavPF::Projecting Measurements 2024-11-06T03:32:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:32:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:32:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:32:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:32:30Z- TNavFilter::4 good beams. 2024-11-06T03:32:30Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:32:30Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:32:30Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:32:30Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:32:30Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:32:30Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:32:30Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:32:30Z- TerrainMapOctree: (vehN, vehE ) = (4061794.39, 594458.71). 2024-11-06T03:32:30Z- TerrainMapOctree: Min Distance = 213.38. 2024-11-06T03:32:30Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:32:30Z- Forcing Subcloud Comparison 2024-11-06T03:32:30Z- Weighting particles with subcloud comparison 2024-11-06T03:32:30Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:30Z- meanExpectedMeasDiff: -1.798565 delta_rms_squared: 0.951558 alpha: 0.979415 2024-11-06T03:32:30Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:30Z- meanExpectedMeasDiff: 1.649032 delta_rms_squared: 0.704559 alpha: 0.972397 2024-11-06T03:32:30Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:30Z- meanExpectedMeasDiff: -1.098215 delta_rms_squared: 0.835975 alpha: 0.976635 2024-11-06T03:32:30Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:30Z- meanExpectedMeasDiff: -0.034280 delta_rms_squared: 0.793562 alpha: 0.975417 2024-11-06T03:32:30Z- Subcloud NIS: 0.469849 2024-11-06T03:32:30Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:32:30Z- TNavPF:: sumWeights = 0.290586 2024-11-06T03:32:30Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:32:30Z- TNavPF::First Term of Map Covariance Matrix is 0.53 2024-11-06T03:32:30Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T03:32:30Z- TerrainNav::Resampling particle filter... 2024-11-06T03:32:30Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863950.32 sec, ping # 0. 2024-11-06T03:32:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:32:30Z- variances([0], [2], and psi_berg) = 0.188278 0.250393 0.000000 2024-11-06T03:32:30Z- publishEstimatesDecoder() - at 1730863950.32 sec 2024-11-06T03:32:30Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:32:30Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061822.80 594441.66 54.16 2024-11-06T03:32:30Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061822.57 594442.70 54.94 lat/lon: 36.70 -121.94 2024-11-06T03:32:30Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.19 0.25 0.33 0.00 2024-11-06T03:32:30Z- AHRS_M2 msg: 1730863950.69 epoch sec; seqNo:6017 2024-11-06T03:32:30Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 4.21 psi 2024-11-06T03:32:30Z- handleDepth()- Depth_Keller msg: 1730863950.71 epoch sec; seqNo:6412; depth 50.23 2024-11-06T03:32:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863950.72 epoch sec; seqNo:6412; 4061794.02 north; 594458.72 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:32:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863950.32 2024-11-06T03:32:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:30Z- waiting for 1730863954.32, time is 1730863950.72 2024-11-06T03:32:31Z- AHRS_M2 msg: 1730863951.10 epoch sec; seqNo:6018 2024-11-06T03:32:31Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 4.21 psi 2024-11-06T03:32:31Z- handleDepth()- Depth_Keller msg: 1730863951.10 epoch sec; seqNo:6413; depth 50.22 2024-11-06T03:32:31Z- handleDVL() - DVL msg epochMillisec: 1730863951.11 2024-11-06T03:32:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:31Z- handleDvl() - RDI_Pathfinder msg: 1730863951.11 sec; seqNo:2191; #beams:0 2024-11-06T03:32:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.02 2024-11-06T03:32:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863951.12 epoch sec; seqNo:6413; 4061794.00 north; 594457.50 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:32:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863950.32 2024-11-06T03:32:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:31Z- waiting for 1730863954.32, time is 1730863951.12 2024-11-06T03:32:31Z- AHRS_M2 msg: 1730863951.50 epoch sec; seqNo:6019 2024-11-06T03:32:31Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.21 psi 2024-11-06T03:32:31Z- handleDepth()- Depth_Keller msg: 1730863951.51 epoch sec; seqNo:6414; depth 50.20 2024-11-06T03:32:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863951.53 epoch sec; seqNo:6414; 4061793.62 north; 594457.50 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:32:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863950.32 2024-11-06T03:32:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:31Z- waiting for 1730863954.32, time is 1730863951.53 2024-11-06T03:32:31Z- AHRS_M2 msg: 1730863951.91 epoch sec; seqNo:6020 2024-11-06T03:32:31Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.21 psi 2024-11-06T03:32:31Z- handleDepth()- Depth_Keller msg: 1730863951.91 epoch sec; seqNo:6415; depth 50.20 2024-11-06T03:32:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863951.92 epoch sec; seqNo:6415; 4061793.62 north; 594457.50 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:32:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863950.32 2024-11-06T03:32:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:32Z- waiting for 1730863954.32, time is 1730863951.92 2024-11-06T03:32:32Z- handleDepth()- Depth_Keller msg: 1730863952.33 epoch sec; seqNo:6416; depth 50.17 2024-11-06T03:32:32Z- handleDVL() - DVL msg epochMillisec: 1730863952.35 2024-11-06T03:32:32Z- handleDvl() - RDI_Pathfinder msg: 1730863952.35 sec; seqNo:2192; #beams:4 2024-11-06T03:32:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.53 , 32.96 , 34.15 , 34.83 2024-11-06T03:32:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , -0.04 2024-11-06T03:32:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863952.37 epoch sec; seqNo:6416; 4061793.62 north; 594457.50 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:32:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863950.32 2024-11-06T03:32:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:32Z- waiting for 1730863954.32, time is 1730863952.37 2024-11-06T03:32:32Z- AHRS_M2 msg: 1730863952.71 epoch sec; seqNo:6021 2024-11-06T03:32:32Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.20 psi 2024-11-06T03:32:32Z- handleDepth()- Depth_Keller msg: 1730863952.72 epoch sec; seqNo:6417; depth 50.16 2024-11-06T03:32:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863952.74 epoch sec; seqNo:6417; 4061793.23 north; 594456.29 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:32:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863950.32 2024-11-06T03:32:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:32Z- waiting for 1730863954.32, time is 1730863952.74 2024-11-06T03:32:33Z- handleDepth()- Depth_Keller msg: 1730863953.14 epoch sec; seqNo:6418; depth 50.15 2024-11-06T03:32:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863953.15 epoch sec; seqNo:6418; 4061793.23 north; 594456.29 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:32:33Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:32:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863950.32 2024-11-06T03:32:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:33Z- waiting for 1730863954.32, time is 1730863953.15 2024-11-06T03:32:33Z- AHRS_M2 msg: 1730863953.53 epoch sec; seqNo:6022 2024-11-06T03:32:33Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.20 psi 2024-11-06T03:32:33Z- handleDepth()- Depth_Keller msg: 1730863953.54 epoch sec; seqNo:6419; depth 50.13 2024-11-06T03:32:33Z- handleDVL() - DVL msg epochMillisec: 1730863953.54 2024-11-06T03:32:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:33Z- handleDvl() - RDI_Pathfinder msg: 1730863953.54 sec; seqNo:2193; #beams:0 2024-11-06T03:32:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , -0.04 2024-11-06T03:32:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863953.55 epoch sec; seqNo:6419; 4061792.85 north; 594456.29 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:32:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863950.32 2024-11-06T03:32:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:33Z- waiting for 1730863954.32, time is 1730863953.55 2024-11-06T03:32:33Z- AHRS_M2 msg: 1730863953.93 epoch sec; seqNo:6023 2024-11-06T03:32:33Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.20 psi 2024-11-06T03:32:33Z- handleDepth()- Depth_Keller msg: 1730863953.93 epoch sec; seqNo:6420; depth 50.11 2024-11-06T03:32:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863953.99 epoch sec; seqNo:6420; 4061792.84 north; 594455.07 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:32:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863950.32 2024-11-06T03:32:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:34Z- waiting for 1730863954.32, time is 1730863953.99 2024-11-06T03:32:34Z- AHRS_M2 msg: 1730863954.33 epoch sec; seqNo:6024 2024-11-06T03:32:34Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.20 psi 2024-11-06T03:32:34Z- handleDepth()- Depth_Keller msg: 1730863954.35 epoch sec; seqNo:6421; depth 50.10 2024-11-06T03:32:34Z- handleDVL() - DVL msg epochMillisec: 1730863954.37 2024-11-06T03:32:34Z- handleDvl() - RDI_Pathfinder msg: 1730863954.37 sec; seqNo:2194; #beams:4 2024-11-06T03:32:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.87 , 32.79 , 33.81 , 34.83 2024-11-06T03:32:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , -0.03 2024-11-06T03:32:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863954.38 epoch sec; seqNo:6421; 4061792.46 north; 594455.07 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:32:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863950.32 2024-11-06T03:32:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:34Z- waiting for 1730863954.32, time is 1730863954.38 2024-11-06T03:32:34Z- LcmTrn::updateTrn() >>>> pose time 1730863954.38, meas time 1730863954.37 <<<< 2024-11-06T03:32:34Z- LcmTrn::updateTrn() equating pose 1730863954.37 and meas times 1730863954.37 2024-11-06T03:32:34Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863954.37 2024-11-06T03:32:34Z- TerrainNav::motionUpdate() - currEstimate = (4061792, 594455.07, 50.10) 2024-11-06T03:32:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:32:34Z- TerrainNav:: SubcloudNIS is 0.47, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:32:34Z- TNavPF::Projecting Measurements 2024-11-06T03:32:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:32:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:32:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:32:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:32:34Z- TNavFilter::4 good beams. 2024-11-06T03:32:34Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:32:34Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:32:34Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:32:34Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:32:34Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:32:34Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:32:34Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:32:34Z- TerrainMapOctree: (vehN, vehE ) = (4061792.46, 594455.07). 2024-11-06T03:32:34Z- TerrainMapOctree: Min Distance = 209.42. 2024-11-06T03:32:34Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:32:34Z- Forcing Subcloud Comparison 2024-11-06T03:32:34Z- Weighting particles with subcloud comparison 2024-11-06T03:32:34Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:34Z- meanExpectedMeasDiff: -2.055156 delta_rms_squared: 0.912589 alpha: 0.978554 2024-11-06T03:32:34Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:34Z- meanExpectedMeasDiff: 1.513742 delta_rms_squared: 0.641239 alpha: 0.969754 2024-11-06T03:32:34Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:34Z- meanExpectedMeasDiff: -1.664681 delta_rms_squared: 0.806007 alpha: 0.975787 2024-11-06T03:32:34Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:34Z- meanExpectedMeasDiff: 0.623046 delta_rms_squared: 0.750975 alpha: 0.974059 2024-11-06T03:32:34Z- Subcloud NIS: 0.469087 2024-11-06T03:32:34Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:32:34Z- TNavPF:: sumWeights = 0.215194 2024-11-06T03:32:34Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:32:34Z- TNavPF::First Term of Map Covariance Matrix is 0.55 2024-11-06T03:32:34Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T03:32:34Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863954.37 sec, ping # 0. 2024-11-06T03:32:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:32:34Z- variances([0], [2], and psi_berg) = 0.182812 0.253284 0.000000 2024-11-06T03:32:34Z- publishEstimatesDecoder() - at 1730863954.37 sec 2024-11-06T03:32:34Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:32:34Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061821.73 594437.89 54.70 2024-11-06T03:32:34Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061820.63 594439.06 54.96 lat/lon: 36.70 -121.94 2024-11-06T03:32:34Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.25 0.32 0.00 2024-11-06T03:32:34Z- AHRS_M2 msg: 1730863954.73 epoch sec; seqNo:6025 2024-11-06T03:32:34Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.20 psi 2024-11-06T03:32:34Z- handleDepth()- Depth_Keller msg: 1730863954.74 epoch sec; seqNo:6422; depth 50.08 2024-11-06T03:32:34Z- handleDVL() - DVL msg epochMillisec: 1730863954.74 2024-11-06T03:32:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:34Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:34Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:34Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:34Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:34Z- handleDvl() - RDI_Pathfinder msg: 1730863954.74 sec; seqNo:2195; #beams:0 2024-11-06T03:32:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , -0.03 2024-11-06T03:32:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863954.77 epoch sec; seqNo:6422; 4061792.46 north; 594455.07 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:32:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863954.37 2024-11-06T03:32:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:34Z- waiting for 1730863958.37, time is 1730863954.77 2024-11-06T03:32:35Z- AHRS_M2 msg: 1730863955.14 epoch sec; seqNo:6026 2024-11-06T03:32:35Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.20 psi 2024-11-06T03:32:35Z- handleDepth()- Depth_Keller msg: 1730863955.14 epoch sec; seqNo:6423; depth 50.08 2024-11-06T03:32:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863955.16 epoch sec; seqNo:6423; 4061792.08 north; 594455.08 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:32:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863954.37 2024-11-06T03:32:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:35Z- waiting for 1730863958.37, time is 1730863955.16 2024-11-06T03:32:35Z- handleDepth()- Depth_Keller msg: 1730863955.58 epoch sec; seqNo:6424; depth 50.06 2024-11-06T03:32:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863955.59 epoch sec; seqNo:6424; 4061792.07 north; 594453.86 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:32:35Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:32:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863954.37 2024-11-06T03:32:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:35Z- waiting for 1730863958.37, time is 1730863955.59 2024-11-06T03:32:35Z- AHRS_M2 msg: 1730863955.95 epoch sec; seqNo:6027 2024-11-06T03:32:35Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.19 psi 2024-11-06T03:32:35Z- handleDepth()- Depth_Keller msg: 1730863955.95 epoch sec; seqNo:6425; depth 50.05 2024-11-06T03:32:35Z- handleDVL() - DVL msg epochMillisec: 1730863955.96 2024-11-06T03:32:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:35Z- handleDvl() - RDI_Pathfinder msg: 1730863955.96 sec; seqNo:2196; #beams:0 2024-11-06T03:32:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , -0.03 2024-11-06T03:32:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863955.97 epoch sec; seqNo:6425; 4061791.69 north; 594453.86 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:32:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863954.37 2024-11-06T03:32:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:36Z- waiting for 1730863958.37, time is 1730863955.97 2024-11-06T03:32:36Z- AHRS_M2 msg: 1730863956.36 epoch sec; seqNo:6028 2024-11-06T03:32:36Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.19 psi 2024-11-06T03:32:36Z- handleDepth()- Depth_Keller msg: 1730863956.36 epoch sec; seqNo:6426; depth 50.04 2024-11-06T03:32:36Z- handleDVL() - DVL msg epochMillisec: 1730863956.39 2024-11-06T03:32:36Z- handleDvl() - RDI_Pathfinder msg: 1730863956.39 sec; seqNo:2197; #beams:4 2024-11-06T03:32:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.21 , 32.45 , 34.83 , 34.83 2024-11-06T03:32:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.04 , 0.01 2024-11-06T03:32:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863956.40 epoch sec; seqNo:6426; 4061791.69 north; 594453.86 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:32:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863954.37 2024-11-06T03:32:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:36Z- waiting for 1730863958.37, time is 1730863956.40 2024-11-06T03:32:36Z- AHRS_M2 msg: 1730863956.76 epoch sec; seqNo:6029 2024-11-06T03:32:36Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.19 psi 2024-11-06T03:32:36Z- handleDepth()- Depth_Keller msg: 1730863956.77 epoch sec; seqNo:6427; depth 50.03 2024-11-06T03:32:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863956.78 epoch sec; seqNo:6427; 4061791.69 north; 594453.86 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:32:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863954.37 2024-11-06T03:32:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:36Z- waiting for 1730863958.37, time is 1730863956.78 2024-11-06T03:32:37Z- AHRS_M2 msg: 1730863957.16 epoch sec; seqNo:6030 2024-11-06T03:32:37Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.18 psi 2024-11-06T03:32:37Z- handleDepth()- Depth_Keller msg: 1730863957.16 epoch sec; seqNo:6428; depth 50.03 2024-11-06T03:32:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863957.18 epoch sec; seqNo:6428; 4061791.30 north; 594452.65 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:32:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863954.37 2024-11-06T03:32:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:37Z- waiting for 1730863958.37, time is 1730863957.18 2024-11-06T03:32:37Z- handleDepth()- Depth_Keller msg: 1730863957.60 epoch sec; seqNo:6429; depth 50.03 2024-11-06T03:32:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863957.61 epoch sec; seqNo:6429; 4061791.30 north; 594452.65 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:32:37Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:32:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863954.37 2024-11-06T03:32:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:37Z- waiting for 1730863958.37, time is 1730863957.61 2024-11-06T03:32:37Z- AHRS_M2 msg: 1730863957.97 epoch sec; seqNo:6031 2024-11-06T03:32:37Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.18 psi 2024-11-06T03:32:37Z- handleDepth()- Depth_Keller msg: 1730863957.97 epoch sec; seqNo:6430; depth 50.02 2024-11-06T03:32:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863957.98 epoch sec; seqNo:6430; 4061790.92 north; 594452.65 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:32:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863954.37 2024-11-06T03:32:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:38Z- waiting for 1730863958.37, time is 1730863957.98 2024-11-06T03:32:38Z- AHRS_M2 msg: 1730863958.37 epoch sec; seqNo:6032 2024-11-06T03:32:38Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.18 psi 2024-11-06T03:32:38Z- handleDepth()- Depth_Keller msg: 1730863958.38 epoch sec; seqNo:6431; depth 50.01 2024-11-06T03:32:38Z- handleDVL() - DVL msg epochMillisec: 1730863958.40 2024-11-06T03:32:38Z- handleDvl() - RDI_Pathfinder msg: 1730863958.40 sec; seqNo:2198; #beams:4 2024-11-06T03:32:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.04 , 32.45 , 34.49 , 34.66 2024-11-06T03:32:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , 0.03 2024-11-06T03:32:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863958.41 epoch sec; seqNo:6431; 4061790.90 north; 594451.43 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:32:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863954.37 2024-11-06T03:32:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:38Z- waiting for 1730863958.37, time is 1730863958.41 2024-11-06T03:32:38Z- LcmTrn::updateTrn() >>>> pose time 1730863958.41, meas time 1730863958.40 <<<< 2024-11-06T03:32:38Z- LcmTrn::updateTrn() equating pose 1730863958.40 and meas times 1730863958.40 2024-11-06T03:32:38Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863958.40 2024-11-06T03:32:38Z- TerrainNav::motionUpdate() - currEstimate = (4061791, 594451.43, 50.01) 2024-11-06T03:32:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:32:38Z- TerrainNav:: SubcloudNIS is 0.47, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:32:38Z- TNavPF::Projecting Measurements 2024-11-06T03:32:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:32:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:32:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:32:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:32:38Z- TNavFilter::4 good beams. 2024-11-06T03:32:38Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:32:38Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:32:38Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:32:38Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:32:38Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:32:38Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:32:38Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:32:38Z- TerrainMapOctree: (vehN, vehE ) = (4061790.90, 594451.43). 2024-11-06T03:32:38Z- TerrainMapOctree: Min Distance = 205.54. 2024-11-06T03:32:38Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:32:38Z- Forcing Subcloud Comparison 2024-11-06T03:32:38Z- Weighting particles with subcloud comparison 2024-11-06T03:32:38Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:38Z- meanExpectedMeasDiff: -1.725803 delta_rms_squared: 0.888025 alpha: 0.977974 2024-11-06T03:32:38Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:38Z- meanExpectedMeasDiff: 1.839166 delta_rms_squared: 0.610320 alpha: 0.968270 2024-11-06T03:32:38Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:38Z- meanExpectedMeasDiff: -0.020741 delta_rms_squared: 0.706872 alpha: 0.972485 2024-11-06T03:32:38Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:38Z- meanExpectedMeasDiff: 0.595122 delta_rms_squared: 0.730820 alpha: 0.973362 2024-11-06T03:32:38Z- Subcloud NIS: 0.454423 2024-11-06T03:32:38Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:32:38Z- TNavPF:: sumWeights = 0.307054 2024-11-06T03:32:38Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:32:38Z- TNavPF::First Term of Map Covariance Matrix is 0.62 2024-11-06T03:32:38Z- TNavPF::Calculated NIS Value : 0.21 numBeams normalized NIS: 0.05 2024-11-06T03:32:38Z- TerrainNav::Resampling particle filter... 2024-11-06T03:32:38Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863958.40 sec, ping # 0. 2024-11-06T03:32:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:32:38Z- variances([0], [2], and psi_berg) = 0.201486 0.260289 0.000000 2024-11-06T03:32:38Z- publishEstimatesDecoder() - at 1730863958.40 sec 2024-11-06T03:32:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:32:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061819.22 594434.48 54.26 2024-11-06T03:32:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061819.03 594435.48 54.86 lat/lon: 36.70 -121.94 2024-11-06T03:32:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.20 0.26 0.35 0.00 2024-11-06T03:32:38Z- AHRS_M2 msg: 1730863958.77 epoch sec; seqNo:6033 2024-11-06T03:32:38Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.18 psi 2024-11-06T03:32:38Z- handleDepth()- Depth_Keller msg: 1730863958.79 epoch sec; seqNo:6432; depth 50.01 2024-11-06T03:32:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863958.81 epoch sec; seqNo:6432; 4061790.53 north; 594451.43 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:32:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863958.40 2024-11-06T03:32:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:38Z- waiting for 1730863962.40, time is 1730863958.81 2024-11-06T03:32:39Z- AHRS_M2 msg: 1730863959.18 epoch sec; seqNo:6034 2024-11-06T03:32:39Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.18 psi 2024-11-06T03:32:39Z- handleDepth()- Depth_Keller msg: 1730863959.19 epoch sec; seqNo:6433; depth 49.99 2024-11-06T03:32:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863959.20 epoch sec; seqNo:6433; 4061790.53 north; 594451.43 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:32:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863958.40 2024-11-06T03:32:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:39Z- waiting for 1730863962.40, time is 1730863959.20 2024-11-06T03:32:39Z- AHRS_M2 msg: 1730863959.60 epoch sec; seqNo:6035 2024-11-06T03:32:39Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.17 psi 2024-11-06T03:32:39Z- handleDepth()- Depth_Keller msg: 1730863959.60 epoch sec; seqNo:6434; depth 50.01 2024-11-06T03:32:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863959.62 epoch sec; seqNo:6434; 4061790.15 north; 594451.44 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:32:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863958.40 2024-11-06T03:32:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:39Z- waiting for 1730863962.40, time is 1730863959.62 2024-11-06T03:32:39Z- AHRS_M2 msg: 1730863959.99 epoch sec; seqNo:6036 2024-11-06T03:32:39Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.17 psi 2024-11-06T03:32:39Z- handleDepth()- Depth_Keller msg: 1730863959.99 epoch sec; seqNo:6435; depth 50.00 2024-11-06T03:32:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863960.01 epoch sec; seqNo:6435; 4061790.13 north; 594450.22 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:32:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863958.40 2024-11-06T03:32:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:40Z- waiting for 1730863962.40, time is 1730863960.01 2024-11-06T03:32:40Z- AHRS_M2 msg: 1730863960.39 epoch sec; seqNo:6037 2024-11-06T03:32:40Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.17 psi 2024-11-06T03:32:40Z- handleDepth()- Depth_Keller msg: 1730863960.39 epoch sec; seqNo:6436; depth 49.99 2024-11-06T03:32:40Z- handleDVL() - DVL msg epochMillisec: 1730863960.42 2024-11-06T03:32:40Z- handleDvl() - RDI_Pathfinder msg: 1730863960.42 sec; seqNo:2199; #beams:4 2024-11-06T03:32:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.87 , 32.11 , 34.83 , 34.83 2024-11-06T03:32:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , 0.02 2024-11-06T03:32:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863960.43 epoch sec; seqNo:6436; 4061789.75 north; 594450.22 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:32:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863958.40 2024-11-06T03:32:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:40Z- waiting for 1730863962.40, time is 1730863960.43 2024-11-06T03:32:40Z- AHRS_M2 msg: 1730863960.79 epoch sec; seqNo:6038 2024-11-06T03:32:40Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.17 psi 2024-11-06T03:32:40Z- handleDepth()- Depth_Keller msg: 1730863960.80 epoch sec; seqNo:6437; depth 49.98 2024-11-06T03:32:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863960.81 epoch sec; seqNo:6437; 4061789.75 north; 594450.22 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:32:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863958.40 2024-11-06T03:32:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:40Z- waiting for 1730863962.40, time is 1730863960.81 2024-11-06T03:32:41Z- AHRS_M2 msg: 1730863961.20 epoch sec; seqNo:6039 2024-11-06T03:32:41Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.17 psi 2024-11-06T03:32:41Z- handleDepth()- Depth_Keller msg: 1730863961.21 epoch sec; seqNo:6438; depth 49.97 2024-11-06T03:32:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863961.22 epoch sec; seqNo:6438; 4061789.38 north; 594450.23 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:32:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863958.40 2024-11-06T03:32:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:41Z- waiting for 1730863962.40, time is 1730863961.22 2024-11-06T03:32:41Z- AHRS_M2 msg: 1730863961.61 epoch sec; seqNo:6040 2024-11-06T03:32:41Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.17 psi 2024-11-06T03:32:41Z- handleDepth()- Depth_Keller msg: 1730863961.62 epoch sec; seqNo:6439; depth 49.95 2024-11-06T03:32:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863961.63 epoch sec; seqNo:6439; 4061789.36 north; 594449.00 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:32:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863958.40 2024-11-06T03:32:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:41Z- waiting for 1730863962.40, time is 1730863961.63 2024-11-06T03:32:42Z- AHRS_M2 msg: 1730863962.01 epoch sec; seqNo:6041 2024-11-06T03:32:42Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.17 psi 2024-11-06T03:32:42Z- handleDepth()- Depth_Keller msg: 1730863962.01 epoch sec; seqNo:6440; depth 49.95 2024-11-06T03:32:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863962.02 epoch sec; seqNo:6440; 4061788.98 north; 594449.01 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:32:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863958.40 2024-11-06T03:32:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:42Z- waiting for 1730863962.40, time is 1730863962.02 2024-11-06T03:32:42Z- AHRS_M2 msg: 1730863962.41 epoch sec; seqNo:6042 2024-11-06T03:32:42Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.17 psi 2024-11-06T03:32:42Z- handleDepth()- Depth_Keller msg: 1730863962.42 epoch sec; seqNo:6441; depth 49.93 2024-11-06T03:32:42Z- handleDVL() - DVL msg epochMillisec: 1730863962.44 2024-11-06T03:32:42Z- handleDvl() - RDI_Pathfinder msg: 1730863962.44 sec; seqNo:2200; #beams:4 2024-11-06T03:32:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.21 , 31.77 , 34.83 , 34.83 2024-11-06T03:32:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , -0.00 2024-11-06T03:32:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863962.45 epoch sec; seqNo:6441; 4061788.98 north; 594449.01 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:32:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863958.40 2024-11-06T03:32:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:42Z- waiting for 1730863962.40, time is 1730863962.45 2024-11-06T03:32:42Z- LcmTrn::updateTrn() >>>> pose time 1730863962.45, meas time 1730863962.44 <<<< 2024-11-06T03:32:42Z- LcmTrn::updateTrn() equating pose 1730863962.44 and meas times 1730863962.44 2024-11-06T03:32:42Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863962.44 2024-11-06T03:32:42Z- TerrainNav::motionUpdate() - currEstimate = (4061789, 594449.01, 49.93) 2024-11-06T03:32:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:32:42Z- TerrainNav:: SubcloudNIS is 0.45, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:32:42Z- TNavPF::Projecting Measurements 2024-11-06T03:32:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:32:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:32:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:32:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:32:42Z- TNavFilter::4 good beams. 2024-11-06T03:32:42Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:32:42Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:32:42Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:32:42Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:32:42Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:32:42Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:32:42Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:32:42Z- TerrainMapOctree: (vehN, vehE ) = (4061788.98, 594449.01). 2024-11-06T03:32:42Z- TerrainMapOctree: Min Distance = 202.79. 2024-11-06T03:32:42Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:32:42Z- Forcing Subcloud Comparison 2024-11-06T03:32:42Z- Weighting particles with subcloud comparison 2024-11-06T03:32:42Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:42Z- meanExpectedMeasDiff: -1.711086 delta_rms_squared: 0.931229 alpha: 0.978975 2024-11-06T03:32:42Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:42Z- meanExpectedMeasDiff: 0.577527 delta_rms_squared: 0.577534 alpha: 0.966529 2024-11-06T03:32:42Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:42Z- meanExpectedMeasDiff: 0.506937 delta_rms_squared: 0.826434 alpha: 0.976371 2024-11-06T03:32:42Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:42Z- meanExpectedMeasDiff: 0.653459 delta_rms_squared: 0.765188 alpha: 0.974528 2024-11-06T03:32:42Z- Subcloud NIS: 0.424871 2024-11-06T03:32:42Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:32:42Z- TNavPF:: sumWeights = 0.447334 2024-11-06T03:32:42Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:32:42Z- TNavPF::First Term of Map Covariance Matrix is 0.63 2024-11-06T03:32:42Z- TNavPF::Calculated NIS Value : 0.30 numBeams normalized NIS: 0.07 2024-11-06T03:32:42Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863962.44 sec, ping # 0. 2024-11-06T03:32:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:32:42Z- variances([0], [2], and psi_berg) = 0.199122 0.262013 0.000000 2024-11-06T03:32:42Z- publishEstimatesDecoder() - at 1730863962.44 sec 2024-11-06T03:32:42Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:32:42Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061817.05 594433.44 54.81 2024-11-06T03:32:42Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061817.10 594433.06 54.79 lat/lon: 36.70 -121.94 2024-11-06T03:32:42Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.20 0.26 0.33 0.00 2024-11-06T03:32:42Z- AHRS_M2 msg: 1730863962.82 epoch sec; seqNo:6043 2024-11-06T03:32:42Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.17 psi 2024-11-06T03:32:42Z- handleDepth()- Depth_Keller msg: 1730863962.82 epoch sec; seqNo:6442; depth 49.93 2024-11-06T03:32:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863962.84 epoch sec; seqNo:6442; 4061788.60 north; 594449.01 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:32:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863962.44 2024-11-06T03:32:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:42Z- waiting for 1730863966.44, time is 1730863962.84 2024-11-06T03:32:43Z- AHRS_M2 msg: 1730863963.22 epoch sec; seqNo:6044 2024-11-06T03:32:43Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.17 psi 2024-11-06T03:32:43Z- handleDepth()- Depth_Keller msg: 1730863963.23 epoch sec; seqNo:6443; depth 49.90 2024-11-06T03:32:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863963.24 epoch sec; seqNo:6443; 4061788.59 north; 594447.79 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:32:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863962.44 2024-11-06T03:32:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:43Z- waiting for 1730863966.44, time is 1730863963.24 2024-11-06T03:32:43Z- AHRS_M2 msg: 1730863963.63 epoch sec; seqNo:6045 2024-11-06T03:32:43Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.17 psi 2024-11-06T03:32:43Z- handleDepth()- Depth_Keller msg: 1730863963.64 epoch sec; seqNo:6444; depth 49.88 2024-11-06T03:32:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863963.66 epoch sec; seqNo:6444; 4061788.21 north; 594447.80 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:32:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863962.44 2024-11-06T03:32:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:43Z- waiting for 1730863966.44, time is 1730863963.66 2024-11-06T03:32:44Z- AHRS_M2 msg: 1730863964.03 epoch sec; seqNo:6046 2024-11-06T03:32:44Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.17 psi 2024-11-06T03:32:44Z- handleDepth()- Depth_Keller msg: 1730863964.03 epoch sec; seqNo:6445; depth 49.87 2024-11-06T03:32:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863964.04 epoch sec; seqNo:6445; 4061788.21 north; 594447.80 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:32:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863962.44 2024-11-06T03:32:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:44Z- waiting for 1730863966.44, time is 1730863964.04 2024-11-06T03:32:44Z- AHRS_M2 msg: 1730863964.43 epoch sec; seqNo:6047 2024-11-06T03:32:44Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.17 psi 2024-11-06T03:32:44Z- handleDepth()- Depth_Keller msg: 1730863964.44 epoch sec; seqNo:6446; depth 49.85 2024-11-06T03:32:44Z- handleDVL() - DVL msg epochMillisec: 1730863964.46 2024-11-06T03:32:44Z- handleDvl() - RDI_Pathfinder msg: 1730863964.46 sec; seqNo:2201; #beams:4 2024-11-06T03:32:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.55 , 30.92 , 34.66 , 35.00 2024-11-06T03:32:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , -0.02 2024-11-06T03:32:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863964.47 epoch sec; seqNo:6446; 4061787.83 north; 594447.80 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:32:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863962.44 2024-11-06T03:32:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:44Z- waiting for 1730863966.44, time is 1730863964.47 2024-11-06T03:32:44Z- AHRS_M2 msg: 1730863964.84 epoch sec; seqNo:6048 2024-11-06T03:32:44Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.17 psi 2024-11-06T03:32:44Z- handleDepth()- Depth_Keller msg: 1730863964.84 epoch sec; seqNo:6447; depth 49.83 2024-11-06T03:32:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863964.86 epoch sec; seqNo:6447; 4061787.82 north; 594446.58 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:32:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863962.44 2024-11-06T03:32:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:44Z- waiting for 1730863966.44, time is 1730863964.86 2024-11-06T03:32:45Z- AHRS_M2 msg: 1730863965.25 epoch sec; seqNo:6049 2024-11-06T03:32:45Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.17 psi 2024-11-06T03:32:45Z- handleDepth()- Depth_Keller msg: 1730863965.26 epoch sec; seqNo:6448; depth 49.81 2024-11-06T03:32:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863965.27 epoch sec; seqNo:6448; 4061787.44 north; 594446.59 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:32:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863962.44 2024-11-06T03:32:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:45Z- waiting for 1730863966.44, time is 1730863965.27 2024-11-06T03:32:45Z- AHRS_M2 msg: 1730863965.64 epoch sec; seqNo:6050 2024-11-06T03:32:45Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.17 psi 2024-11-06T03:32:45Z- handleDepth()- Depth_Keller msg: 1730863965.66 epoch sec; seqNo:6449; depth 49.79 2024-11-06T03:32:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863965.68 epoch sec; seqNo:6449; 4061787.44 north; 594446.59 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:32:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863962.44 2024-11-06T03:32:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:45Z- waiting for 1730863966.44, time is 1730863965.68 2024-11-06T03:32:46Z- AHRS_M2 msg: 1730863966.05 epoch sec; seqNo:6051 2024-11-06T03:32:46Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.16 psi 2024-11-06T03:32:46Z- handleDepth()- Depth_Keller msg: 1730863966.05 epoch sec; seqNo:6450; depth 49.76 2024-11-06T03:32:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863966.06 epoch sec; seqNo:6450; 4061787.06 north; 594446.59 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:32:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863962.44 2024-11-06T03:32:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:46Z- waiting for 1730863966.44, time is 1730863966.06 2024-11-06T03:32:46Z- AHRS_M2 msg: 1730863966.45 epoch sec; seqNo:6052 2024-11-06T03:32:46Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.16 psi 2024-11-06T03:32:46Z- handleDepth()- Depth_Keller msg: 1730863966.46 epoch sec; seqNo:6451; depth 49.75 2024-11-06T03:32:46Z- handleDVL() - DVL msg epochMillisec: 1730863966.49 2024-11-06T03:32:46Z- handleDvl() - RDI_Pathfinder msg: 1730863966.49 sec; seqNo:2202; #beams:4 2024-11-06T03:32:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.72 , 30.07 , 34.15 , 35.17 2024-11-06T03:32:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , -0.04 2024-11-06T03:32:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863966.50 epoch sec; seqNo:6451; 4061787.05 north; 594445.37 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:32:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863962.44 2024-11-06T03:32:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:46Z- waiting for 1730863966.44, time is 1730863966.50 2024-11-06T03:32:46Z- LcmTrn::updateTrn() >>>> pose time 1730863966.50, meas time 1730863966.49 <<<< 2024-11-06T03:32:46Z- LcmTrn::updateTrn() equating pose 1730863966.49 and meas times 1730863966.49 2024-11-06T03:32:46Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863966.49 2024-11-06T03:32:46Z- TerrainNav::motionUpdate() - currEstimate = (4061787, 594445.37, 49.75) 2024-11-06T03:32:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:32:46Z- TerrainNav:: SubcloudNIS is 0.42, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:32:46Z- TNavPF::Projecting Measurements 2024-11-06T03:32:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:32:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:32:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:32:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:32:46Z- TNavFilter::4 good beams. 2024-11-06T03:32:46Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:32:46Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:32:46Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:32:46Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:32:46Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:32:46Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:32:46Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:32:46Z- TerrainMapOctree: (vehN, vehE ) = (4061787.05, 594445.37). 2024-11-06T03:32:46Z- TerrainMapOctree: Min Distance = 198.85. 2024-11-06T03:32:46Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:32:46Z- Forcing Subcloud Comparison 2024-11-06T03:32:46Z- Weighting particles with subcloud comparison 2024-11-06T03:32:46Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:46Z- meanExpectedMeasDiff: -1.519871 delta_rms_squared: 0.906845 alpha: 0.978421 2024-11-06T03:32:46Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:46Z- meanExpectedMeasDiff: -1.244343 delta_rms_squared: 0.674132 alpha: 0.971187 2024-11-06T03:32:46Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:46Z- meanExpectedMeasDiff: -0.373425 delta_rms_squared: 0.800421 alpha: 0.975622 2024-11-06T03:32:46Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:46Z- meanExpectedMeasDiff: 1.842589 delta_rms_squared: 0.919555 alpha: 0.978713 2024-11-06T03:32:46Z- Subcloud NIS: 0.410011 2024-11-06T03:32:46Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:32:46Z- TNavPF:: sumWeights = 0.276062 2024-11-06T03:32:46Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:32:46Z- TNavPF::First Term of Map Covariance Matrix is 0.41 2024-11-06T03:32:46Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.05 2024-11-06T03:32:46Z- TerrainNav::Resampling particle filter... 2024-11-06T03:32:46Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863966.49 sec, ping # 0. 2024-11-06T03:32:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:32:46Z- variances([0], [2], and psi_berg) = 0.203733 0.287295 0.000000 2024-11-06T03:32:46Z- publishEstimatesDecoder() - at 1730863966.49 sec 2024-11-06T03:32:46Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:32:46Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061815.42 594428.28 53.91 2024-11-06T03:32:46Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061815.16 594429.39 54.73 lat/lon: 36.70 -121.94 2024-11-06T03:32:46Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.20 0.29 0.32 0.00 2024-11-06T03:32:46Z- AHRS_M2 msg: 1730863966.86 epoch sec; seqNo:6053 2024-11-06T03:32:46Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.16 psi 2024-11-06T03:32:46Z- handleDepth()- Depth_Keller msg: 1730863966.86 epoch sec; seqNo:6452; depth 49.72 2024-11-06T03:32:46Z- handleDVL() - DVL msg epochMillisec: 1730863966.87 2024-11-06T03:32:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:46Z- handleDvl() - RDI_Pathfinder msg: 1730863966.87 sec; seqNo:2203; #beams:0 2024-11-06T03:32:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , -0.04 2024-11-06T03:32:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863966.88 epoch sec; seqNo:6452; 4061786.67 north; 594445.37 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:32:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863966.49 2024-11-06T03:32:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:47Z- waiting for 1730863970.49, time is 1730863966.88 2024-11-06T03:32:47Z- AHRS_M2 msg: 1730863967.26 epoch sec; seqNo:6054 2024-11-06T03:32:47Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.16 psi 2024-11-06T03:32:47Z- handleDepth()- Depth_Keller msg: 1730863967.27 epoch sec; seqNo:6453; depth 49.71 2024-11-06T03:32:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863967.28 epoch sec; seqNo:6453; 4061786.67 north; 594445.37 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:32:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863966.49 2024-11-06T03:32:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:47Z- waiting for 1730863970.49, time is 1730863967.28 2024-11-06T03:32:47Z- AHRS_M2 msg: 1730863967.66 epoch sec; seqNo:6055 2024-11-06T03:32:47Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.16 psi 2024-11-06T03:32:47Z- handleDepth()- Depth_Keller msg: 1730863967.67 epoch sec; seqNo:6454; depth 49.69 2024-11-06T03:32:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863967.69 epoch sec; seqNo:6454; 4061786.29 north; 594445.38 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:32:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863966.49 2024-11-06T03:32:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:47Z- waiting for 1730863970.49, time is 1730863967.69 2024-11-06T03:32:48Z- AHRS_M2 msg: 1730863968.07 epoch sec; seqNo:6056 2024-11-06T03:32:48Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.16 psi 2024-11-06T03:32:48Z- handleDepth()- Depth_Keller msg: 1730863968.07 epoch sec; seqNo:6455; depth 49.68 2024-11-06T03:32:48Z- handleDVL() - DVL msg epochMillisec: 1730863968.08 2024-11-06T03:32:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:48Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:48Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:48Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:48Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:48Z- handleDvl() - RDI_Pathfinder msg: 1730863968.08 sec; seqNo:2204; #beams:0 2024-11-06T03:32:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , -0.04 2024-11-06T03:32:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863968.09 epoch sec; seqNo:6455; 4061786.28 north; 594444.16 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:32:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863966.49 2024-11-06T03:32:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:48Z- waiting for 1730863970.49, time is 1730863968.09 2024-11-06T03:32:48Z- AHRS_M2 msg: 1730863968.47 epoch sec; seqNo:6057 2024-11-06T03:32:48Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 4.16 psi 2024-11-06T03:32:48Z- handleDepth()- Depth_Keller msg: 1730863968.48 epoch sec; seqNo:6456; depth 49.66 2024-11-06T03:32:48Z- handleDVL() - DVL msg epochMillisec: 1730863968.50 2024-11-06T03:32:48Z- handleDvl() - RDI_Pathfinder msg: 1730863968.50 sec; seqNo:2205; #beams:4 2024-11-06T03:32:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.46 , 31.19 , 33.95 , 34.87 2024-11-06T03:32:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.04 , -0.02 2024-11-06T03:32:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863968.52 epoch sec; seqNo:6456; 4061785.90 north; 594444.16 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:32:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863966.49 2024-11-06T03:32:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:48Z- waiting for 1730863970.49, time is 1730863968.52 2024-11-06T03:32:48Z- AHRS_M2 msg: 1730863968.88 epoch sec; seqNo:6058 2024-11-06T03:32:48Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 4.15 psi 2024-11-06T03:32:48Z- handleDepth()- Depth_Keller msg: 1730863968.88 epoch sec; seqNo:6457; depth 49.65 2024-11-06T03:32:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863968.89 epoch sec; seqNo:6457; 4061785.90 north; 594444.16 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:32:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863966.49 2024-11-06T03:32:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:49Z- waiting for 1730863970.49, time is 1730863968.89 2024-11-06T03:32:49Z- AHRS_M2 msg: 1730863969.28 epoch sec; seqNo:6059 2024-11-06T03:32:49Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.15 psi 2024-11-06T03:32:49Z- handleDepth()- Depth_Keller msg: 1730863969.29 epoch sec; seqNo:6458; depth 49.62 2024-11-06T03:32:49Z- handleDVL() - DVL msg epochMillisec: 1730863969.29 2024-11-06T03:32:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:49Z- handleDvl() - RDI_Pathfinder msg: 1730863969.29 sec; seqNo:2206; #beams:0 2024-11-06T03:32:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.04 , -0.02 2024-11-06T03:32:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863969.30 epoch sec; seqNo:6458; 4061785.52 north; 594444.17 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:32:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863966.49 2024-11-06T03:32:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:49Z- waiting for 1730863970.49, time is 1730863969.30 2024-11-06T03:32:49Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:32:49Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:32:49Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:32:49Z- AHRS_M2 msg: 1730863969.67 epoch sec; seqNo:6060 2024-11-06T03:32:49Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.15 psi 2024-11-06T03:32:49Z- handleDepth()- Depth_Keller msg: 1730863969.70 epoch sec; seqNo:6459; depth 49.62 2024-11-06T03:32:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863969.72 epoch sec; seqNo:6459; 4061785.51 north; 594442.95 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:32:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863966.49 2024-11-06T03:32:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:49Z- waiting for 1730863970.49, time is 1730863969.72 2024-11-06T03:32:50Z- AHRS_M2 msg: 1730863970.09 epoch sec; seqNo:6061 2024-11-06T03:32:50Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 4.15 psi 2024-11-06T03:32:50Z- handleDepth()- Depth_Keller msg: 1730863970.10 epoch sec; seqNo:6460; depth 49.61 2024-11-06T03:32:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863970.10 epoch sec; seqNo:6460; 4061785.13 north; 594442.95 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:32:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863966.49 2024-11-06T03:32:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:50Z- waiting for 1730863970.49, time is 1730863970.10 2024-11-06T03:32:50Z- AHRS_M2 msg: 1730863970.49 epoch sec; seqNo:6062 2024-11-06T03:32:50Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 4.15 psi 2024-11-06T03:32:50Z- handleDepth()- Depth_Keller msg: 1730863970.50 epoch sec; seqNo:6461; depth 49.61 2024-11-06T03:32:50Z- handleDVL() - DVL msg epochMillisec: 1730863970.53 2024-11-06T03:32:50Z- handleDvl() - RDI_Pathfinder msg: 1730863970.53 sec; seqNo:2207; #beams:4 2024-11-06T03:32:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.77 , 31.35 , 33.79 , 35.17 2024-11-06T03:32:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.03 , 0.02 2024-11-06T03:32:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863970.54 epoch sec; seqNo:6461; 4061785.13 north; 594442.95 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:32:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863966.49 2024-11-06T03:32:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:50Z- waiting for 1730863970.49, time is 1730863970.54 2024-11-06T03:32:50Z- LcmTrn::updateTrn() >>>> pose time 1730863970.54, meas time 1730863970.53 <<<< 2024-11-06T03:32:50Z- LcmTrn::updateTrn() equating pose 1730863970.53 and meas times 1730863970.53 2024-11-06T03:32:50Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863970.53 2024-11-06T03:32:50Z- TerrainNav::motionUpdate() - currEstimate = (4061785, 594442.95, 49.61) 2024-11-06T03:32:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:32:50Z- TerrainNav:: SubcloudNIS is 0.41, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:32:50Z- TNavPF::Projecting Measurements 2024-11-06T03:32:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:32:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:32:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:32:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:32:50Z- TNavFilter::4 good beams. 2024-11-06T03:32:50Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:32:50Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:32:50Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:32:50Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:32:50Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:32:50Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:32:50Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:32:50Z- TerrainMapOctree: (vehN, vehE ) = (4061785.13, 594442.95). 2024-11-06T03:32:50Z- TerrainMapOctree: Min Distance = 196.12. 2024-11-06T03:32:50Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:32:50Z- Forcing Subcloud Comparison 2024-11-06T03:32:50Z- Weighting particles with subcloud comparison 2024-11-06T03:32:50Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:50Z- meanExpectedMeasDiff: -1.931353 delta_rms_squared: 0.926824 alpha: 0.978877 2024-11-06T03:32:50Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:50Z- meanExpectedMeasDiff: 1.118730 delta_rms_squared: 0.647702 alpha: 0.970047 2024-11-06T03:32:50Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:50Z- meanExpectedMeasDiff: 0.034418 delta_rms_squared: 0.585232 alpha: 0.966955 2024-11-06T03:32:50Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:50Z- meanExpectedMeasDiff: 1.434379 delta_rms_squared: 0.764403 alpha: 0.974503 2024-11-06T03:32:50Z- Subcloud NIS: 0.384703 2024-11-06T03:32:50Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:32:50Z- TNavPF:: sumWeights = 0.300210 2024-11-06T03:32:50Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:32:50Z- TNavPF::First Term of Map Covariance Matrix is 0.76 2024-11-06T03:32:50Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T03:32:50Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863970.53 sec, ping # 0. 2024-11-06T03:32:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:32:50Z- variances([0], [2], and psi_berg) = 0.200102 0.294047 0.000000 2024-11-06T03:32:50Z- publishEstimatesDecoder() - at 1730863970.53 sec 2024-11-06T03:32:50Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:32:50Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061813.19 594428.29 54.80 2024-11-06T03:32:50Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061813.24 594426.97 54.52 lat/lon: 36.70 -121.94 2024-11-06T03:32:50Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.20 0.29 0.34 0.00 2024-11-06T03:32:50Z- AHRS_M2 msg: 1730863970.89 epoch sec; seqNo:6063 2024-11-06T03:32:50Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 4.15 psi 2024-11-06T03:32:50Z- handleDepth()- Depth_Keller msg: 1730863970.91 epoch sec; seqNo:6462; depth 49.60 2024-11-06T03:32:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863970.93 epoch sec; seqNo:6462; 4061784.73 north; 594441.73 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:32:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863970.53 2024-11-06T03:32:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:51Z- waiting for 1730863974.53, time is 1730863970.93 2024-11-06T03:32:51Z- AHRS_M2 msg: 1730863971.30 epoch sec; seqNo:6064 2024-11-06T03:32:51Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 4.15 psi 2024-11-06T03:32:51Z- handleDepth()- Depth_Keller msg: 1730863971.30 epoch sec; seqNo:6463; depth 49.60 2024-11-06T03:32:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863971.31 epoch sec; seqNo:6463; 4061784.73 north; 594441.73 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:32:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863970.53 2024-11-06T03:32:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:51Z- waiting for 1730863974.53, time is 1730863971.31 2024-11-06T03:32:51Z- AHRS_M2 msg: 1730863971.71 epoch sec; seqNo:6065 2024-11-06T03:32:51Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 4.15 psi 2024-11-06T03:32:51Z- handleDepth()- Depth_Keller msg: 1730863971.71 epoch sec; seqNo:6464; depth 49.61 2024-11-06T03:32:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863971.72 epoch sec; seqNo:6464; 4061784.36 north; 594441.74 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:32:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863970.53 2024-11-06T03:32:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:51Z- waiting for 1730863974.53, time is 1730863971.72 2024-11-06T03:32:52Z- AHRS_M2 msg: 1730863972.11 epoch sec; seqNo:6066 2024-11-06T03:32:52Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 4.15 psi 2024-11-06T03:32:52Z- handleDepth()- Depth_Keller msg: 1730863972.13 epoch sec; seqNo:6465; depth 49.60 2024-11-06T03:32:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863972.14 epoch sec; seqNo:6465; 4061784.36 north; 594441.74 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:32:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863970.53 2024-11-06T03:32:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:52Z- waiting for 1730863974.53, time is 1730863972.14 2024-11-06T03:32:52Z- AHRS_M2 msg: 1730863972.51 epoch sec; seqNo:6067 2024-11-06T03:32:52Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 4.15 psi 2024-11-06T03:32:52Z- handleDepth()- Depth_Keller msg: 1730863972.52 epoch sec; seqNo:6466; depth 49.61 2024-11-06T03:32:52Z- handleDVL() - DVL msg epochMillisec: 1730863972.55 2024-11-06T03:32:52Z- handleDvl() - RDI_Pathfinder msg: 1730863972.55 sec; seqNo:2208; #beams:4 2024-11-06T03:32:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.89 , 31.78 , 33.47 , 35.51 2024-11-06T03:32:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.00 , 0.04 2024-11-06T03:32:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863972.56 epoch sec; seqNo:6466; 4061783.96 north; 594440.52 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:32:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863970.53 2024-11-06T03:32:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:52Z- waiting for 1730863974.53, time is 1730863972.56 2024-11-06T03:32:52Z- AHRS_M2 msg: 1730863972.92 epoch sec; seqNo:6068 2024-11-06T03:32:52Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 4.14 psi 2024-11-06T03:32:52Z- handleDepth()- Depth_Keller msg: 1730863972.92 epoch sec; seqNo:6467; depth 49.60 2024-11-06T03:32:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863973.01 epoch sec; seqNo:6467; 4061783.58 north; 594440.53 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:32:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863970.53 2024-11-06T03:32:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:53Z- waiting for 1730863974.53, time is 1730863973.01 2024-11-06T03:32:53Z- AHRS_M2 msg: 1730863973.33 epoch sec; seqNo:6069 2024-11-06T03:32:53Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 4.14 psi 2024-11-06T03:32:53Z- handleDepth()- Depth_Keller msg: 1730863973.33 epoch sec; seqNo:6468; depth 49.61 2024-11-06T03:32:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863973.34 epoch sec; seqNo:6468; 4061783.58 north; 594440.53 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:32:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863970.53 2024-11-06T03:32:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:53Z- waiting for 1730863974.53, time is 1730863973.34 2024-11-06T03:32:53Z- AHRS_M2 msg: 1730863973.73 epoch sec; seqNo:6070 2024-11-06T03:32:53Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 4.14 psi 2024-11-06T03:32:53Z- handleDepth()- Depth_Keller msg: 1730863973.74 epoch sec; seqNo:6469; depth 49.62 2024-11-06T03:32:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863973.75 epoch sec; seqNo:6469; 4061783.21 north; 594440.53 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:32:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863970.53 2024-11-06T03:32:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:53Z- waiting for 1730863974.53, time is 1730863973.75 2024-11-06T03:32:54Z- AHRS_M2 msg: 1730863974.13 epoch sec; seqNo:6071 2024-11-06T03:32:54Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 4.14 psi 2024-11-06T03:32:54Z- handleDepth()- Depth_Keller msg: 1730863974.13 epoch sec; seqNo:6470; depth 49.61 2024-11-06T03:32:54Z- handleDVL() - DVL msg epochMillisec: 1730863974.13 2024-11-06T03:32:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:32:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:32:54Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:32:54Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:32:54Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:32:54Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:32:54Z- handleDvl() - RDI_Pathfinder msg: 1730863974.13 sec; seqNo:2209; #beams:0 2024-11-06T03:32:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:32:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.00 , 0.04 2024-11-06T03:32:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863974.17 epoch sec; seqNo:6470; 4061783.19 north; 594439.31 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:32:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863970.53 2024-11-06T03:32:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:54Z- waiting for 1730863974.53, time is 1730863974.17 2024-11-06T03:32:54Z- AHRS_M2 msg: 1730863974.53 epoch sec; seqNo:6072 2024-11-06T03:32:54Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 4.14 psi 2024-11-06T03:32:54Z- handleDepth()- Depth_Keller msg: 1730863974.54 epoch sec; seqNo:6471; depth 49.62 2024-11-06T03:32:54Z- handleDVL() - DVL msg epochMillisec: 1730863974.56 2024-11-06T03:32:54Z- handleDvl() - RDI_Pathfinder msg: 1730863974.56 sec; seqNo:2210; #beams:4 2024-11-06T03:32:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.06 , 31.94 , 32.96 , 35.85 2024-11-06T03:32:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , 0.02 2024-11-06T03:32:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863974.58 epoch sec; seqNo:6471; 4061782.81 north; 594439.31 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:32:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863970.53 2024-11-06T03:32:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:54Z- waiting for 1730863974.53, time is 1730863974.58 2024-11-06T03:32:54Z- LcmTrn::updateTrn() >>>> pose time 1730863974.58, meas time 1730863974.56 <<<< 2024-11-06T03:32:54Z- LcmTrn::updateTrn() equating pose 1730863974.56 and meas times 1730863974.56 2024-11-06T03:32:54Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863974.56 2024-11-06T03:32:54Z- TerrainNav::motionUpdate() - currEstimate = (4061783, 594439.31, 49.62) 2024-11-06T03:32:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:32:54Z- TerrainNav:: SubcloudNIS is 0.38, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:32:54Z- TNavPF::Projecting Measurements 2024-11-06T03:32:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:32:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:32:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:32:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:32:54Z- TNavFilter::4 good beams. 2024-11-06T03:32:54Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:32:54Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:32:54Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:32:54Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:32:54Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:32:54Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:32:54Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:32:54Z- TerrainMapOctree: (vehN, vehE ) = (4061782.81, 594439.31). 2024-11-06T03:32:54Z- TerrainMapOctree: Min Distance = 192.14. 2024-11-06T03:32:54Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:32:54Z- Forcing Subcloud Comparison 2024-11-06T03:32:54Z- Weighting particles with subcloud comparison 2024-11-06T03:32:54Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:54Z- meanExpectedMeasDiff: -1.461781 delta_rms_squared: 0.922998 alpha: 0.978791 2024-11-06T03:32:54Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:54Z- meanExpectedMeasDiff: 1.571003 delta_rms_squared: 0.664679 alpha: 0.970789 2024-11-06T03:32:54Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:54Z- meanExpectedMeasDiff: 0.165436 delta_rms_squared: 0.657885 alpha: 0.970496 2024-11-06T03:32:54Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:54Z- meanExpectedMeasDiff: 0.752782 delta_rms_squared: 0.747290 alpha: 0.973934 2024-11-06T03:32:54Z- Subcloud NIS: 0.363328 2024-11-06T03:32:54Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:32:54Z- TNavPF:: sumWeights = 0.376303 2024-11-06T03:32:54Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:32:54Z- TNavPF::First Term of Map Covariance Matrix is 0.57 2024-11-06T03:32:54Z- TNavPF::Calculated NIS Value : 0.26 numBeams normalized NIS: 0.07 2024-11-06T03:32:54Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730863974.56 sec, ping # 0. 2024-11-06T03:32:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:32:54Z- variances([0], [2], and psi_berg) = 0.195433 0.291160 0.000000 2024-11-06T03:32:54Z- publishEstimatesDecoder() - at 1730863974.56 sec 2024-11-06T03:32:54Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:32:54Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061810.86 594424.60 54.42 2024-11-06T03:32:54Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061810.91 594423.33 54.44 lat/lon: 36.70 -121.94 2024-11-06T03:32:54Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.20 0.29 0.33 0.00 2024-11-06T03:32:54Z- AHRS_M2 msg: 1730863974.95 epoch sec; seqNo:6073 2024-11-06T03:32:54Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 4.14 psi 2024-11-06T03:32:54Z- handleDepth()- Depth_Keller msg: 1730863974.95 epoch sec; seqNo:6472; depth 49.63 2024-11-06T03:32:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863974.97 epoch sec; seqNo:6472; 4061782.81 north; 594439.31 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:32:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863974.56 2024-11-06T03:32:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:55Z- waiting for 1730863978.56, time is 1730863974.97 2024-11-06T03:32:55Z- AHRS_M2 msg: 1730863975.34 epoch sec; seqNo:6074 2024-11-06T03:32:55Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 4.14 psi 2024-11-06T03:32:55Z- handleDepth()- Depth_Keller msg: 1730863975.35 epoch sec; seqNo:6473; depth 49.62 2024-11-06T03:32:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863975.36 epoch sec; seqNo:6473; 4061782.43 north; 594439.32 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:32:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863974.56 2024-11-06T03:32:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:55Z- waiting for 1730863978.56, time is 1730863975.36 2024-11-06T03:32:55Z- AHRS_M2 msg: 1730863975.74 epoch sec; seqNo:6075 2024-11-06T03:32:55Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 4.14 psi 2024-11-06T03:32:55Z- handleDepth()- Depth_Keller msg: 1730863975.75 epoch sec; seqNo:6474; depth 49.62 2024-11-06T03:32:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863975.76 epoch sec; seqNo:6474; 4061782.42 north; 594438.10 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:32:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863974.56 2024-11-06T03:32:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:55Z- waiting for 1730863978.56, time is 1730863975.76 2024-11-06T03:32:56Z- AHRS_M2 msg: 1730863976.15 epoch sec; seqNo:6076 2024-11-06T03:32:56Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 4.15 psi 2024-11-06T03:32:56Z- handleDepth()- Depth_Keller msg: 1730863976.16 epoch sec; seqNo:6475; depth 49.62 2024-11-06T03:32:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863976.23 epoch sec; seqNo:6475; 4061782.04 north; 594438.10 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:32:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863974.56 2024-11-06T03:32:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:56Z- waiting for 1730863978.56, time is 1730863976.23 2024-11-06T03:32:56Z- AHRS_M2 msg: 1730863976.55 epoch sec; seqNo:6077 2024-11-06T03:32:56Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.15 psi 2024-11-06T03:32:56Z- handleDepth()- Depth_Keller msg: 1730863976.56 epoch sec; seqNo:6476; depth 49.63 2024-11-06T03:32:56Z- handleDVL() - DVL msg epochMillisec: 1730863976.58 2024-11-06T03:32:56Z- handleDvl() - RDI_Pathfinder msg: 1730863976.58 sec; seqNo:2211; #beams:4 2024-11-06T03:32:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.72 , 32.12 , 32.62 , 36.19 2024-11-06T03:32:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , -0.00 2024-11-06T03:32:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863976.59 epoch sec; seqNo:6476; 4061782.04 north; 594438.10 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:32:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863974.56 2024-11-06T03:32:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:56Z- waiting for 1730863978.56, time is 1730863976.59 2024-11-06T03:32:56Z- AHRS_M2 msg: 1730863976.95 epoch sec; seqNo:6078 2024-11-06T03:32:56Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.15 psi 2024-11-06T03:32:56Z- handleDepth()- Depth_Keller msg: 1730863976.97 epoch sec; seqNo:6477; depth 49.64 2024-11-06T03:32:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863976.99 epoch sec; seqNo:6477; 4061781.66 north; 594438.11 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:32:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863974.56 2024-11-06T03:32:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:57Z- waiting for 1730863978.56, time is 1730863976.99 2024-11-06T03:32:57Z- AHRS_M2 msg: 1730863977.36 epoch sec; seqNo:6079 2024-11-06T03:32:57Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.15 psi 2024-11-06T03:32:57Z- handleDepth()- Depth_Keller msg: 1730863977.37 epoch sec; seqNo:6478; depth 49.63 2024-11-06T03:32:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863977.38 epoch sec; seqNo:6478; 4061781.65 north; 594436.89 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:32:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863974.56 2024-11-06T03:32:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:57Z- waiting for 1730863978.56, time is 1730863977.38 2024-11-06T03:32:57Z- AHRS_M2 msg: 1730863977.77 epoch sec; seqNo:6080 2024-11-06T03:32:57Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.16 psi 2024-11-06T03:32:57Z- handleDepth()- Depth_Keller msg: 1730863977.77 epoch sec; seqNo:6479; depth 49.63 2024-11-06T03:32:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863977.78 epoch sec; seqNo:6479; 4061781.27 north; 594436.89 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:32:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863974.56 2024-11-06T03:32:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:57Z- waiting for 1730863978.56, time is 1730863977.78 2024-11-06T03:32:58Z- AHRS_M2 msg: 1730863978.22 epoch sec; seqNo:6081 2024-11-06T03:32:58Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.16 psi 2024-11-06T03:32:58Z- handleDepth()- Depth_Keller msg: 1730863978.23 epoch sec; seqNo:6480; depth 49.63 2024-11-06T03:32:58Z- handleDVL() - DVL msg epochMillisec: 1730863978.25 2024-11-06T03:32:58Z- handleDvl() - RDI_Pathfinder msg: 1730863978.25 sec; seqNo:2212; #beams:4 2024-11-06T03:32:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.40 , 31.27 , 32.62 , 36.19 2024-11-06T03:32:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.02 2024-11-06T03:32:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863978.27 epoch sec; seqNo:6480; 4061781.27 north; 594436.89 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:32:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:32:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863974.56 2024-11-06T03:32:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:58Z- waiting for 1730863978.56, time is 1730863978.27 2024-11-06T03:32:58Z- AHRS_M2 msg: 1730863978.57 epoch sec; seqNo:6082 2024-11-06T03:32:58Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.16 psi 2024-11-06T03:32:58Z- handleDepth()- Depth_Keller msg: 1730863978.58 epoch sec; seqNo:6481; depth 49.63 2024-11-06T03:32:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863978.61 epoch sec; seqNo:6481; 4061780.89 north; 594436.89 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:32:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863974.56 2024-11-06T03:32:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:58Z- waiting for 1730863978.56, time is 1730863978.61 2024-11-06T03:32:58Z- LcmTrn::updateTrn() >>>> pose time 1730863978.61, meas time 1730863978.25 <<<< 2024-11-06T03:32:58Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863978.61 2024-11-06T03:32:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863978.25 sec, ping # 0; waiting for INS data more recent than 1730863974.56... 2024-11-06T03:32:58Z- TerrainNav::motionUpdate() - currEstimate = (4061781, 594436.89, 49.63) 2024-11-06T03:32:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:32:58Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:32:58Z- TNavPF::Projecting Measurements 2024-11-06T03:32:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:32:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:32:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:32:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:32:58Z- TNavFilter::4 good beams. 2024-11-06T03:32:58Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:32:58Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:32:58Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:32:58Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:32:58Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:32:58Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:32:58Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:32:58Z- TerrainMapOctree: (vehN, vehE ) = (4061781.06, 594437.11). 2024-11-06T03:32:58Z- TerrainMapOctree: Min Distance = 189.67. 2024-11-06T03:32:58Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:32:58Z- Forcing Subcloud Comparison 2024-11-06T03:32:58Z- Weighting particles with subcloud comparison 2024-11-06T03:32:58Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:58Z- meanExpectedMeasDiff: -1.784178 delta_rms_squared: 0.941862 alpha: 0.979207 2024-11-06T03:32:58Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:58Z- meanExpectedMeasDiff: 1.024250 delta_rms_squared: 0.646853 alpha: 0.970008 2024-11-06T03:32:58Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:58Z- meanExpectedMeasDiff: 0.048622 delta_rms_squared: 0.756572 alpha: 0.974246 2024-11-06T03:32:58Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:32:58Z- meanExpectedMeasDiff: 0.378441 delta_rms_squared: 0.796882 alpha: 0.975517 2024-11-06T03:32:58Z- Subcloud NIS: 0.360730 2024-11-06T03:32:58Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:32:58Z- TNavPF:: sumWeights = 0.423593 2024-11-06T03:32:58Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:32:58Z- TNavPF::First Term of Map Covariance Matrix is 0.65 2024-11-06T03:32:58Z- TNavPF::Calculated NIS Value : 0.29 numBeams normalized NIS: 0.07 2024-11-06T03:32:58Z- TerrainNav::Resampling particle filter... 2024-11-06T03:32:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863978.25 sec. 2024-11-06T03:32:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:32:58Z- variances([0], [2], and psi_berg) = 0.198014 0.292722 0.000000 2024-11-06T03:32:58Z- publishEstimatesDecoder() - at 1730863978.61 sec 2024-11-06T03:32:58Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:32:58Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061809.41 594419.64 54.71 2024-11-06T03:32:58Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061808.98 594420.91 54.49 lat/lon: 36.70 -121.94 2024-11-06T03:32:58Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.20 0.29 0.56 0.00 2024-11-06T03:32:59Z- AHRS_M2 msg: 1730863979.01 epoch sec; seqNo:6083 2024-11-06T03:32:59Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:32:59Z- handleDepth()- Depth_Keller msg: 1730863979.01 epoch sec; seqNo:6482; depth 49.64 2024-11-06T03:32:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863979.03 epoch sec; seqNo:6482; 4061780.88 north; 594435.67 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:32:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863978.61 2024-11-06T03:32:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:59Z- waiting for 1730863982.61, time is 1730863979.03 2024-11-06T03:32:59Z- AHRS_M2 msg: 1730863979.38 epoch sec; seqNo:6084 2024-11-06T03:32:59Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:32:59Z- handleDepth()- Depth_Keller msg: 1730863979.39 epoch sec; seqNo:6483; depth 49.64 2024-11-06T03:32:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863979.40 epoch sec; seqNo:6483; 4061780.50 north; 594435.68 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:32:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863978.61 2024-11-06T03:32:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:59Z- waiting for 1730863982.61, time is 1730863979.40 2024-11-06T03:32:59Z- AHRS_M2 msg: 1730863979.78 epoch sec; seqNo:6085 2024-11-06T03:32:59Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:32:59Z- handleDepth()- Depth_Keller msg: 1730863979.79 epoch sec; seqNo:6484; depth 49.64 2024-11-06T03:32:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863979.80 epoch sec; seqNo:6484; 4061780.50 north; 594435.68 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:32:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:32:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863978.61 2024-11-06T03:32:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:32:59Z- waiting for 1730863982.61, time is 1730863979.80 2024-11-06T03:33:00Z- AHRS_M2 msg: 1730863980.22 epoch sec; seqNo:6086 2024-11-06T03:33:00Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:33:00Z- handleDepth()- Depth_Keller msg: 1730863980.23 epoch sec; seqNo:6485; depth 49.64 2024-11-06T03:33:00Z- handleDVL() - DVL msg epochMillisec: 1730863980.26 2024-11-06T03:33:00Z- handleDvl() - RDI_Pathfinder msg: 1730863980.26 sec; seqNo:2213; #beams:4 2024-11-06T03:33:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.89 , 31.44 , 31.94 , 36.36 2024-11-06T03:33:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.03 , -0.02 2024-11-06T03:33:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863980.29 epoch sec; seqNo:6485; 4061780.11 north; 594434.46 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:33:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863978.61 2024-11-06T03:33:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:00Z- waiting for 1730863982.61, time is 1730863980.29 2024-11-06T03:33:00Z- AHRS_M2 msg: 1730863980.59 epoch sec; seqNo:6087 2024-11-06T03:33:00Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:33:00Z- handleDepth()- Depth_Keller msg: 1730863980.60 epoch sec; seqNo:6486; depth 49.65 2024-11-06T03:33:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863980.62 epoch sec; seqNo:6486; 4061780.11 north; 594434.46 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:33:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863978.61 2024-11-06T03:33:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:00Z- waiting for 1730863982.61, time is 1730863980.62 2024-11-06T03:33:01Z- AHRS_M2 msg: 1730863981.00 epoch sec; seqNo:6088 2024-11-06T03:33:01Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:33:01Z- handleDepth()- Depth_Keller msg: 1730863981.01 epoch sec; seqNo:6487; depth 49.66 2024-11-06T03:33:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863981.02 epoch sec; seqNo:6487; 4061779.73 north; 594434.47 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:33:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863978.61 2024-11-06T03:33:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:01Z- waiting for 1730863982.61, time is 1730863981.02 2024-11-06T03:33:01Z- AHRS_M2 msg: 1730863981.40 epoch sec; seqNo:6089 2024-11-06T03:33:01Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.17 psi 2024-11-06T03:33:01Z- handleDepth()- Depth_Keller msg: 1730863981.40 epoch sec; seqNo:6488; depth 49.65 2024-11-06T03:33:01Z- handleDVL() - DVL msg epochMillisec: 1730863981.41 2024-11-06T03:33:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:33:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:33:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:33:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:33:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:33:01Z- handleDvl() - RDI_Pathfinder msg: 1730863981.41 sec; seqNo:2214; #beams:0 2024-11-06T03:33:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:33:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.03 , -0.02 2024-11-06T03:33:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863981.43 epoch sec; seqNo:6488; 4061779.73 north; 594434.47 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:33:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863978.61 2024-11-06T03:33:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:01Z- waiting for 1730863982.61, time is 1730863981.43 2024-11-06T03:33:01Z- AHRS_M2 msg: 1730863981.81 epoch sec; seqNo:6090 2024-11-06T03:33:01Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.17 psi 2024-11-06T03:33:01Z- handleDepth()- Depth_Keller msg: 1730863981.82 epoch sec; seqNo:6489; depth 49.68 2024-11-06T03:33:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863981.83 epoch sec; seqNo:6489; 4061779.72 north; 594433.25 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:33:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863978.61 2024-11-06T03:33:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:01Z- waiting for 1730863982.61, time is 1730863981.83 2024-11-06T03:33:02Z- AHRS_M2 msg: 1730863982.23 epoch sec; seqNo:6091 2024-11-06T03:33:02Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.17 psi 2024-11-06T03:33:02Z- handleDepth()- Depth_Keller msg: 1730863982.23 epoch sec; seqNo:6490; depth 49.69 2024-11-06T03:33:02Z- handleDVL() - DVL msg epochMillisec: 1730863982.26 2024-11-06T03:33:02Z- handleDvl() - RDI_Pathfinder msg: 1730863982.26 sec; seqNo:2215; #beams:4 2024-11-06T03:33:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.93 , 31.66 , 31.20 , 36.40 2024-11-06T03:33:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.02 , 0.00 2024-11-06T03:33:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863982.28 epoch sec; seqNo:6490; 4061779.34 north; 594433.25 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:33:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863978.61 2024-11-06T03:33:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:02Z- waiting for 1730863982.61, time is 1730863982.28 2024-11-06T03:33:02Z- AHRS_M2 msg: 1730863982.62 epoch sec; seqNo:6092 2024-11-06T03:33:02Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:33:02Z- handleDepth()- Depth_Keller msg: 1730863982.63 epoch sec; seqNo:6491; depth 49.69 2024-11-06T03:33:02Z- handleDVL() - DVL msg epochMillisec: 1730863982.63 2024-11-06T03:33:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:33:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:33:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:33:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:33:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:33:02Z- handleDvl() - RDI_Pathfinder msg: 1730863982.63 sec; seqNo:2216; #beams:0 2024-11-06T03:33:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:33:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.02 , 0.00 2024-11-06T03:33:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863982.65 epoch sec; seqNo:6491; 4061778.96 north; 594433.25 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:33:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863978.61 2024-11-06T03:33:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:02Z- waiting for 1730863982.61, time is 1730863982.65 2024-11-06T03:33:02Z- LcmTrn::updateTrn() >>>> pose time 1730863982.65, meas time 1730863982.63 <<<< 2024-11-06T03:33:02Z- LcmTrn::updateTrn() equating pose 1730863982.63 and meas times 1730863982.63 2024-11-06T03:33:02Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863982.63 2024-11-06T03:33:02Z- TerrainNav::motionUpdate() - currEstimate = (4061779, 594433.25, 49.69) 2024-11-06T03:33:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:33:02Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:33:02Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:33:02Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:33:02Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:33:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:33:02Z- variances([0], [2], and psi_berg) = 0.200833 0.296519 0.000000 2024-11-06T03:33:02Z- publishEstimatesDecoder() - at 1730863982.63 sec 2024-11-06T03:33:02Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:33:02Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061807.47 594416.03 55.14 2024-11-06T03:33:02Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061807.05 594417.27 54.55 lat/lon: 36.70 -121.94 2024-11-06T03:33:02Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.20 0.30 0.81 0.00 2024-11-06T03:33:03Z- AHRS_M2 msg: 1730863983.01 epoch sec; seqNo:6093 2024-11-06T03:33:03Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:33:03Z- handleDepth()- Depth_Keller msg: 1730863983.03 epoch sec; seqNo:6492; depth 49.69 2024-11-06T03:33:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863983.04 epoch sec; seqNo:6492; 4061778.96 north; 594433.25 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:33:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863982.63 2024-11-06T03:33:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:03Z- waiting for 1730863986.63, time is 1730863983.04 2024-11-06T03:33:03Z- AHRS_M2 msg: 1730863983.42 epoch sec; seqNo:6094 2024-11-06T03:33:03Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.17 psi 2024-11-06T03:33:03Z- handleDepth()- Depth_Keller msg: 1730863983.43 epoch sec; seqNo:6493; depth 49.72 2024-11-06T03:33:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863983.44 epoch sec; seqNo:6493; 4061778.57 north; 594432.04 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:33:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863982.63 2024-11-06T03:33:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:03Z- waiting for 1730863986.63, time is 1730863983.44 2024-11-06T03:33:03Z- AHRS_M2 msg: 1730863983.83 epoch sec; seqNo:6095 2024-11-06T03:33:03Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.17 psi 2024-11-06T03:33:03Z- handleDepth()- Depth_Keller msg: 1730863983.83 epoch sec; seqNo:6494; depth 49.71 2024-11-06T03:33:03Z- handleDVL() - DVL msg epochMillisec: 1730863983.84 2024-11-06T03:33:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:33:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:33:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:33:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:33:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:33:03Z- handleDvl() - RDI_Pathfinder msg: 1730863983.84 sec; seqNo:2217; #beams:0 2024-11-06T03:33:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:33:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.02 , 0.00 2024-11-06T03:33:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863983.84 epoch sec; seqNo:6494; 4061778.57 north; 594432.04 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:33:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863982.63 2024-11-06T03:33:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:03Z- waiting for 1730863986.63, time is 1730863983.84 2024-11-06T03:33:04Z- AHRS_M2 msg: 1730863984.23 epoch sec; seqNo:6096 2024-11-06T03:33:04Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.17 psi 2024-11-06T03:33:04Z- handleDepth()- Depth_Keller msg: 1730863984.25 epoch sec; seqNo:6495; depth 49.74 2024-11-06T03:33:04Z- handleDVL() - DVL msg epochMillisec: 1730863984.27 2024-11-06T03:33:04Z- handleDvl() - RDI_Pathfinder msg: 1730863984.27 sec; seqNo:2218; #beams:4 2024-11-06T03:33:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.93 , 31.20 , 30.74 , 36.09 2024-11-06T03:33:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , 0.01 2024-11-06T03:33:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863984.28 epoch sec; seqNo:6495; 4061778.19 north; 594432.04 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:33:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863982.63 2024-11-06T03:33:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:04Z- waiting for 1730863986.63, time is 1730863984.28 2024-11-06T03:33:04Z- AHRS_M2 msg: 1730863984.63 epoch sec; seqNo:6097 2024-11-06T03:33:04Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.17 psi 2024-11-06T03:33:04Z- handleDepth()- Depth_Keller msg: 1730863984.64 epoch sec; seqNo:6496; depth 49.76 2024-11-06T03:33:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863984.66 epoch sec; seqNo:6496; 4061778.17 north; 594430.82 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:33:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863982.63 2024-11-06T03:33:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:04Z- waiting for 1730863986.63, time is 1730863984.66 2024-11-06T03:33:05Z- AHRS_M2 msg: 1730863985.04 epoch sec; seqNo:6098 2024-11-06T03:33:05Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.17 psi 2024-11-06T03:33:05Z- handleDepth()- Depth_Keller msg: 1730863985.05 epoch sec; seqNo:6497; depth 49.76 2024-11-06T03:33:05Z- handleDVL() - DVL msg epochMillisec: 1730863985.05 2024-11-06T03:33:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:33:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:33:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:33:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:33:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:33:05Z- handleDvl() - RDI_Pathfinder msg: 1730863985.05 sec; seqNo:2219; #beams:0 2024-11-06T03:33:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:33:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , 0.01 2024-11-06T03:33:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863985.06 epoch sec; seqNo:6497; 4061777.79 north; 594430.83 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T03:33:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863982.63 2024-11-06T03:33:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:05Z- waiting for 1730863986.63, time is 1730863985.06 2024-11-06T03:33:05Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:33:05Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:33:05Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:33:05Z- AHRS_M2 msg: 1730863985.43 epoch sec; seqNo:6099 2024-11-06T03:33:05Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.17 psi 2024-11-06T03:33:05Z- handleDepth()- Depth_Keller msg: 1730863985.46 epoch sec; seqNo:6498; depth 49.78 2024-11-06T03:33:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863985.52 epoch sec; seqNo:6498; 4061777.79 north; 594430.83 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:33:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863982.63 2024-11-06T03:33:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:05Z- waiting for 1730863986.63, time is 1730863985.52 2024-11-06T03:33:05Z- AHRS_M2 msg: 1730863985.84 epoch sec; seqNo:6100 2024-11-06T03:33:05Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.18 psi 2024-11-06T03:33:05Z- handleDepth()- Depth_Keller msg: 1730863985.85 epoch sec; seqNo:6499; depth 49.78 2024-11-06T03:33:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863985.86 epoch sec; seqNo:6499; 4061777.42 north; 594430.83 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:33:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863982.63 2024-11-06T03:33:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:06Z- waiting for 1730863986.63, time is 1730863985.86 2024-11-06T03:33:06Z- AHRS_M2 msg: 1730863986.25 epoch sec; seqNo:6101 2024-11-06T03:33:06Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.18 psi 2024-11-06T03:33:06Z- handleDepth()- Depth_Keller msg: 1730863986.25 epoch sec; seqNo:6500; depth 49.79 2024-11-06T03:33:06Z- handleDVL() - DVL msg epochMillisec: 1730863986.27 2024-11-06T03:33:06Z- handleDvl() - RDI_Pathfinder msg: 1730863986.27 sec; seqNo:2220; #beams:4 2024-11-06T03:33:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.31 , 31.35 , 30.43 , 36.09 2024-11-06T03:33:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.01 2024-11-06T03:33:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863986.30 epoch sec; seqNo:6500; 4061777.40 north; 594429.61 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:33:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863982.63 2024-11-06T03:33:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:06Z- waiting for 1730863986.63, time is 1730863986.30 2024-11-06T03:33:06Z- AHRS_M2 msg: 1730863986.66 epoch sec; seqNo:6102 2024-11-06T03:33:06Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.18 psi 2024-11-06T03:33:06Z- handleDepth()- Depth_Keller msg: 1730863986.67 epoch sec; seqNo:6501; depth 49.81 2024-11-06T03:33:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863986.68 epoch sec; seqNo:6501; 4061777.02 north; 594429.61 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:33:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863982.63 2024-11-06T03:33:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:06Z- waiting for 1730863986.63, time is 1730863986.68 2024-11-06T03:33:06Z- LcmTrn::updateTrn() >>>> pose time 1730863986.68, meas time 1730863986.27 <<<< 2024-11-06T03:33:06Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863986.68 2024-11-06T03:33:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863986.27 sec, ping # 0; waiting for INS data more recent than 1730863982.63... 2024-11-06T03:33:06Z- TerrainNav::motionUpdate() - currEstimate = (4061777, 594429.61, 49.81) 2024-11-06T03:33:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:33:06Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:33:06Z- TNavPF::Projecting Measurements 2024-11-06T03:33:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:33:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:33:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:33:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:33:06Z- TNavFilter::4 good beams. 2024-11-06T03:33:06Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:33:06Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:33:06Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:33:06Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:33:06Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:33:06Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:33:06Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:33:06Z- TerrainMapOctree: (vehN, vehE ) = (4061777.22, 594429.98). 2024-11-06T03:33:06Z- TerrainMapOctree: Min Distance = 182.03. 2024-11-06T03:33:06Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:33:06Z- Forcing Subcloud Comparison 2024-11-06T03:33:06Z- Weighting particles with subcloud comparison 2024-11-06T03:33:06Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:06Z- meanExpectedMeasDiff: -2.230427 delta_rms_squared: 1.393350 alpha: 0.985849 2024-11-06T03:33:06Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:06Z- meanExpectedMeasDiff: 2.341338 delta_rms_squared: 1.082975 alpha: 0.981867 2024-11-06T03:33:06Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:06Z- meanExpectedMeasDiff: -1.649933 delta_rms_squared: 1.397597 alpha: 0.985892 2024-11-06T03:33:06Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:06Z- meanExpectedMeasDiff: 0.206447 delta_rms_squared: 1.525413 alpha: 0.987058 2024-11-06T03:33:06Z- Subcloud NIS: 0.385264 2024-11-06T03:33:06Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:33:06Z- TNavPF:: sumWeights = 0.103827 2024-11-06T03:33:06Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:33:06Z- TNavPF::First Term of Map Covariance Matrix is 0.38 2024-11-06T03:33:06Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T03:33:06Z- TerrainNav::Resampling particle filter... 2024-11-06T03:33:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863986.27 sec. 2024-11-06T03:33:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:33:06Z- variances([0], [2], and psi_berg) = 0.202119 0.314423 0.000000 2024-11-06T03:33:06Z- publishEstimatesDecoder() - at 1730863986.68 sec 2024-11-06T03:33:06Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:33:06Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061805.42 594412.43 54.96 2024-11-06T03:33:06Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061805.10 594413.75 54.82 lat/lon: 36.70 -121.94 2024-11-06T03:33:06Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.20 0.31 0.72 0.00 2024-11-06T03:33:07Z- AHRS_M2 msg: 1730863987.05 epoch sec; seqNo:6103 2024-11-06T03:33:07Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.18 psi 2024-11-06T03:33:07Z- handleDepth()- Depth_Keller msg: 1730863987.07 epoch sec; seqNo:6502; depth 49.83 2024-11-06T03:33:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863987.09 epoch sec; seqNo:6502; 4061777.02 north; 594429.61 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T03:33:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863986.68 2024-11-06T03:33:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:07Z- waiting for 1730863990.68, time is 1730863987.09 2024-11-06T03:33:07Z- AHRS_M2 msg: 1730863987.46 epoch sec; seqNo:6104 2024-11-06T03:33:07Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.18 psi 2024-11-06T03:33:07Z- handleDepth()- Depth_Keller msg: 1730863987.47 epoch sec; seqNo:6503; depth 49.85 2024-11-06T03:33:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863987.49 epoch sec; seqNo:6503; 4061776.64 north; 594429.62 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:33:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863986.68 2024-11-06T03:33:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:07Z- waiting for 1730863990.68, time is 1730863987.49 2024-11-06T03:33:07Z- AHRS_M2 msg: 1730863987.87 epoch sec; seqNo:6105 2024-11-06T03:33:07Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.18 psi 2024-11-06T03:33:07Z- handleDepth()- Depth_Keller msg: 1730863987.87 epoch sec; seqNo:6504; depth 49.85 2024-11-06T03:33:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863987.88 epoch sec; seqNo:6504; 4061776.63 north; 594428.40 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:33:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863986.68 2024-11-06T03:33:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:08Z- waiting for 1730863990.68, time is 1730863987.88 2024-11-06T03:33:08Z- AHRS_M2 msg: 1730863988.27 epoch sec; seqNo:6106 2024-11-06T03:33:08Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.19 psi 2024-11-06T03:33:08Z- handleDepth()- Depth_Keller msg: 1730863988.27 epoch sec; seqNo:6505; depth 49.85 2024-11-06T03:33:08Z- handleDVL() - DVL msg epochMillisec: 1730863988.30 2024-11-06T03:33:08Z- handleDvl() - RDI_Pathfinder msg: 1730863988.30 sec; seqNo:2221; #beams:4 2024-11-06T03:33:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.47 , 31.20 , 29.36 , 36.09 2024-11-06T03:33:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , -0.00 2024-11-06T03:33:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863988.31 epoch sec; seqNo:6505; 4061776.25 north; 594428.40 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:33:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863986.68 2024-11-06T03:33:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:08Z- waiting for 1730863990.68, time is 1730863988.31 2024-11-06T03:33:08Z- AHRS_M2 msg: 1730863988.67 epoch sec; seqNo:6107 2024-11-06T03:33:08Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.19 psi 2024-11-06T03:33:08Z- handleDepth()- Depth_Keller msg: 1730863988.68 epoch sec; seqNo:6506; depth 49.87 2024-11-06T03:33:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863988.69 epoch sec; seqNo:6506; 4061776.25 north; 594428.40 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:33:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863986.68 2024-11-06T03:33:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:08Z- waiting for 1730863990.68, time is 1730863988.69 2024-11-06T03:33:09Z- AHRS_M2 msg: 1730863989.12 epoch sec; seqNo:6108 2024-11-06T03:33:09Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.19 psi 2024-11-06T03:33:09Z- handleDepth()- Depth_Keller msg: 1730863989.13 epoch sec; seqNo:6507; depth 49.89 2024-11-06T03:33:09Z- handleDVL() - DVL msg epochMillisec: 1730863989.15 2024-11-06T03:33:09Z- handleDvl() - RDI_Pathfinder msg: 1730863989.15 sec; seqNo:2222; #beams:4 2024-11-06T03:33:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.16 , 31.20 , 28.90 , 36.09 2024-11-06T03:33:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.00 2024-11-06T03:33:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863989.19 epoch sec; seqNo:6507; 4061775.86 north; 594427.19 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T03:33:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863986.68 2024-11-06T03:33:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:09Z- waiting for 1730863990.68, time is 1730863989.19 2024-11-06T03:33:09Z- handleDepth()- Depth_Keller msg: 1730863989.49 epoch sec; seqNo:6508; depth 49.90 2024-11-06T03:33:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863989.52 epoch sec; seqNo:6508; 4061775.86 north; 594427.19 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:33:09Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:33:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863986.68 2024-11-06T03:33:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:09Z- waiting for 1730863990.68, time is 1730863989.52 2024-11-06T03:33:09Z- AHRS_M2 msg: 1730863989.88 epoch sec; seqNo:6109 2024-11-06T03:33:09Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.19 psi 2024-11-06T03:33:09Z- handleDepth()- Depth_Keller msg: 1730863989.89 epoch sec; seqNo:6509; depth 49.92 2024-11-06T03:33:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863989.91 epoch sec; seqNo:6509; 4061775.86 north; 594427.19 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:33:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863986.68 2024-11-06T03:33:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:10Z- waiting for 1730863990.68, time is 1730863989.91 2024-11-06T03:33:10Z- AHRS_M2 msg: 1730863990.29 epoch sec; seqNo:6110 2024-11-06T03:33:10Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.19 psi 2024-11-06T03:33:10Z- handleDepth()- Depth_Keller msg: 1730863990.30 epoch sec; seqNo:6510; depth 49.93 2024-11-06T03:33:10Z- handleDVL() - DVL msg epochMillisec: 1730863990.32 2024-11-06T03:33:10Z- handleDvl() - RDI_Pathfinder msg: 1730863990.32 sec; seqNo:2223; #beams:4 2024-11-06T03:33:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.01 , 31.35 , 28.14 , 35.79 2024-11-06T03:33:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , -0.01 2024-11-06T03:33:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863990.34 epoch sec; seqNo:6510; 4061775.48 north; 594427.19 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:33:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863986.68 2024-11-06T03:33:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:10Z- waiting for 1730863990.68, time is 1730863990.34 2024-11-06T03:33:10Z- AHRS_M2 msg: 1730863990.70 epoch sec; seqNo:6111 2024-11-06T03:33:10Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.19 psi 2024-11-06T03:33:10Z- handleDepth()- Depth_Keller msg: 1730863990.70 epoch sec; seqNo:6511; depth 49.94 2024-11-06T03:33:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863990.72 epoch sec; seqNo:6511; 4061775.47 north; 594425.97 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:33:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863986.68 2024-11-06T03:33:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:10Z- waiting for 1730863990.68, time is 1730863990.72 2024-11-06T03:33:10Z- LcmTrn::updateTrn() >>>> pose time 1730863990.72, meas time 1730863990.32 <<<< 2024-11-06T03:33:10Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863990.72 2024-11-06T03:33:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863990.32 sec, ping # 0; waiting for INS data more recent than 1730863986.68... 2024-11-06T03:33:10Z- TerrainNav::motionUpdate() - currEstimate = (4061775, 594425.97, 49.94) 2024-11-06T03:33:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:33:10Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:33:10Z- TNavPF::Projecting Measurements 2024-11-06T03:33:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:33:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:33:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:33:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:33:10Z- TNavFilter::4 good beams. 2024-11-06T03:33:10Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:33:10Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:33:10Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:33:10Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:33:10Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:33:10Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:33:10Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:33:10Z- TerrainMapOctree: (vehN, vehE ) = (4061775.62, 594426.34). 2024-11-06T03:33:10Z- TerrainMapOctree: Min Distance = 178.19. 2024-11-06T03:33:10Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:33:10Z- Forcing Subcloud Comparison 2024-11-06T03:33:10Z- Weighting particles with subcloud comparison 2024-11-06T03:33:10Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:10Z- meanExpectedMeasDiff: -2.247014 delta_rms_squared: 1.562315 alpha: 0.987360 2024-11-06T03:33:10Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:10Z- meanExpectedMeasDiff: 2.577162 delta_rms_squared: 1.096549 alpha: 0.982088 2024-11-06T03:33:10Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:10Z- meanExpectedMeasDiff: -2.397990 delta_rms_squared: 1.114907 alpha: 0.982377 2024-11-06T03:33:10Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:10Z- meanExpectedMeasDiff: 0.142255 delta_rms_squared: 1.374939 alpha: 0.985662 2024-11-06T03:33:10Z- Subcloud NIS: 0.406357 2024-11-06T03:33:10Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:33:10Z- TNavPF:: sumWeights = 0.054328 2024-11-06T03:33:10Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:33:11Z- TNavPF::First Term of Map Covariance Matrix is 0.21 2024-11-06T03:33:11Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.01 2024-11-06T03:33:11Z- TerrainNav::Resampling particle filter... 2024-11-06T03:33:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863990.32 sec. 2024-11-06T03:33:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:33:11Z- variances([0], [2], and psi_berg) = 0.209244 0.292497 0.000000 2024-11-06T03:33:11Z- publishEstimatesDecoder() - at 1730863990.72 sec 2024-11-06T03:33:11Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:33:11Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061803.87 594408.81 55.30 2024-11-06T03:33:11Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061803.54 594410.05 55.23 lat/lon: 36.70 -121.94 2024-11-06T03:33:11Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.29 0.63 0.00 2024-11-06T03:33:11Z- AHRS_M2 msg: 1730863991.17 epoch sec; seqNo:6112 2024-11-06T03:33:11Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.19 psi 2024-11-06T03:33:11Z- handleDepth()- Depth_Keller msg: 1730863991.18 epoch sec; seqNo:6512; depth 49.96 2024-11-06T03:33:11Z- handleDVL() - DVL msg epochMillisec: 1730863991.23 2024-11-06T03:33:11Z- handleDvl() - RDI_Pathfinder msg: 1730863991.23 sec; seqNo:2224; #beams:4 2024-11-06T03:33:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.01 , 31.05 , 27.83 , 35.79 2024-11-06T03:33:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , 0.00 2024-11-06T03:33:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863991.24 epoch sec; seqNo:6512; 4061775.09 north; 594425.97 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:33:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863990.72 2024-11-06T03:33:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:11Z- waiting for 1730863994.72, time is 1730863991.24 2024-11-06T03:33:11Z- AHRS_M2 msg: 1730863991.50 epoch sec; seqNo:6113 2024-11-06T03:33:11Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.19 psi 2024-11-06T03:33:11Z- handleDepth()- Depth_Keller msg: 1730863991.51 epoch sec; seqNo:6513; depth 50.00 2024-11-06T03:33:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863991.52 epoch sec; seqNo:6513; 4061775.09 north; 594425.97 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:33:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863990.72 2024-11-06T03:33:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:11Z- waiting for 1730863994.72, time is 1730863991.52 2024-11-06T03:33:11Z- AHRS_M2 msg: 1730863991.91 epoch sec; seqNo:6114 2024-11-06T03:33:11Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.20 psi 2024-11-06T03:33:11Z- handleDepth()- Depth_Keller msg: 1730863991.92 epoch sec; seqNo:6514; depth 49.99 2024-11-06T03:33:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863991.93 epoch sec; seqNo:6514; 4061774.71 north; 594425.98 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:33:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863990.72 2024-11-06T03:33:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:12Z- waiting for 1730863994.72, time is 1730863991.93 2024-11-06T03:33:12Z- AHRS_M2 msg: 1730863992.31 epoch sec; seqNo:6115 2024-11-06T03:33:12Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.20 psi 2024-11-06T03:33:12Z- handleDepth()- Depth_Keller msg: 1730863992.32 epoch sec; seqNo:6515; depth 50.00 2024-11-06T03:33:12Z- handleDVL() - DVL msg epochMillisec: 1730863992.35 2024-11-06T03:33:12Z- handleDvl() - RDI_Pathfinder msg: 1730863992.35 sec; seqNo:2225; #beams:4 2024-11-06T03:33:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.86 , 30.74 , 27.53 , 35.48 2024-11-06T03:33:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , 0.00 2024-11-06T03:33:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863992.36 epoch sec; seqNo:6515; 4061774.70 north; 594424.76 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:33:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863990.72 2024-11-06T03:33:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:12Z- waiting for 1730863994.72, time is 1730863992.36 2024-11-06T03:33:12Z- AHRS_M2 msg: 1730863992.71 epoch sec; seqNo:6116 2024-11-06T03:33:12Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.20 psi 2024-11-06T03:33:12Z- handleDepth()- Depth_Keller msg: 1730863992.72 epoch sec; seqNo:6516; depth 50.03 2024-11-06T03:33:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863992.74 epoch sec; seqNo:6516; 4061774.32 north; 594424.76 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:33:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863990.72 2024-11-06T03:33:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:12Z- waiting for 1730863994.72, time is 1730863992.74 2024-11-06T03:33:13Z- AHRS_M2 msg: 1730863993.16 epoch sec; seqNo:6117 2024-11-06T03:33:13Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.20 psi 2024-11-06T03:33:13Z- handleDepth()- Depth_Keller msg: 1730863993.17 epoch sec; seqNo:6517; depth 50.04 2024-11-06T03:33:13Z- handleDVL() - DVL msg epochMillisec: 1730863993.19 2024-11-06T03:33:13Z- handleDvl() - RDI_Pathfinder msg: 1730863993.19 sec; seqNo:2226; #beams:4 2024-11-06T03:33:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.16 , 30.74 , 27.83 , 35.63 2024-11-06T03:33:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , 0.01 2024-11-06T03:33:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863993.22 epoch sec; seqNo:6517; 4061774.32 north; 594424.76 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:33:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863990.72 2024-11-06T03:33:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:13Z- waiting for 1730863994.72, time is 1730863993.22 2024-11-06T03:33:13Z- AHRS_M2 msg: 1730863993.52 epoch sec; seqNo:6118 2024-11-06T03:33:13Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.20 psi 2024-11-06T03:33:13Z- handleDepth()- Depth_Keller msg: 1730863993.53 epoch sec; seqNo:6518; depth 50.05 2024-11-06T03:33:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863993.54 epoch sec; seqNo:6518; 4061774.32 north; 594424.76 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:33:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863990.72 2024-11-06T03:33:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:13Z- waiting for 1730863994.72, time is 1730863993.54 2024-11-06T03:33:13Z- handleDepth()- Depth_Keller msg: 1730863993.95 epoch sec; seqNo:6519; depth 50.07 2024-11-06T03:33:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863993.96 epoch sec; seqNo:6519; 4061773.93 north; 594423.55 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:33:14Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:33:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863990.72 2024-11-06T03:33:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:14Z- waiting for 1730863994.72, time is 1730863993.96 2024-11-06T03:33:14Z- AHRS_M2 msg: 1730863994.33 epoch sec; seqNo:6119 2024-11-06T03:33:14Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.20 psi 2024-11-06T03:33:14Z- handleDepth()- Depth_Keller msg: 1730863994.34 epoch sec; seqNo:6520; depth 50.08 2024-11-06T03:33:14Z- handleDVL() - DVL msg epochMillisec: 1730863994.36 2024-11-06T03:33:14Z- handleDvl() - RDI_Pathfinder msg: 1730863994.36 sec; seqNo:2227; #beams:4 2024-11-06T03:33:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.72 , 30.74 , 27.99 , 35.63 2024-11-06T03:33:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , 0.01 2024-11-06T03:33:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863994.38 epoch sec; seqNo:6520; 4061773.93 north; 594423.55 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:33:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863990.72 2024-11-06T03:33:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:14Z- waiting for 1730863994.72, time is 1730863994.38 2024-11-06T03:33:14Z- AHRS_M2 msg: 1730863994.74 epoch sec; seqNo:6120 2024-11-06T03:33:14Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.20 psi 2024-11-06T03:33:14Z- handleDepth()- Depth_Keller msg: 1730863994.74 epoch sec; seqNo:6521; depth 50.11 2024-11-06T03:33:14Z- handleDVL() - DVL msg epochMillisec: 1730863994.74 2024-11-06T03:33:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:33:14Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:33:14Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:33:14Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:33:14Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:33:14Z- handleDvl() - RDI_Pathfinder msg: 1730863994.74 sec; seqNo:2228; #beams:0 2024-11-06T03:33:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:33:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , 0.01 2024-11-06T03:33:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863994.76 epoch sec; seqNo:6521; 4061773.55 north; 594423.55 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:33:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863990.72 2024-11-06T03:33:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:14Z- waiting for 1730863994.72, time is 1730863994.76 2024-11-06T03:33:14Z- LcmTrn::updateTrn() >>>> pose time 1730863994.76, meas time 1730863994.74 <<<< 2024-11-06T03:33:14Z- LcmTrn::updateTrn() equating pose 1730863994.74 and meas times 1730863994.74 2024-11-06T03:33:14Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730863994.74 2024-11-06T03:33:14Z- TerrainNav::motionUpdate() - currEstimate = (4061774, 594423.55, 50.11) 2024-11-06T03:33:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:33:14Z- TerrainNav:: SubcloudNIS is 0.41, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:33:14Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:33:14Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:33:14Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:33:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:33:14Z- variances([0], [2], and psi_berg) = 0.210421 0.293437 0.000000 2024-11-06T03:33:14Z- publishEstimatesDecoder() - at 1730863994.74 sec 2024-11-06T03:33:14Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:33:14Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061801.87 594406.38 55.37 2024-11-06T03:33:14Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061801.62 594407.63 55.40 lat/lon: 36.70 -121.94 2024-11-06T03:33:14Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.29 0.88 0.00 2024-11-06T03:33:15Z- AHRS_M2 msg: 1730863995.16 epoch sec; seqNo:6121 2024-11-06T03:33:15Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.20 psi 2024-11-06T03:33:15Z- handleDepth()- Depth_Keller msg: 1730863995.18 epoch sec; seqNo:6522; depth 50.12 2024-11-06T03:33:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863995.21 epoch sec; seqNo:6522; 4061773.53 north; 594422.33 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:33:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863994.74 2024-11-06T03:33:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:15Z- waiting for 1730863998.74, time is 1730863995.21 2024-11-06T03:33:15Z- AHRS_M2 msg: 1730863995.54 epoch sec; seqNo:6122 2024-11-06T03:33:15Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.20 psi 2024-11-06T03:33:15Z- handleDepth()- Depth_Keller msg: 1730863995.55 epoch sec; seqNo:6523; depth 50.13 2024-11-06T03:33:15Z- handleDVL() - DVL msg epochMillisec: 1730863995.57 2024-11-06T03:33:15Z- handleDvl() - RDI_Pathfinder msg: 1730863995.57 sec; seqNo:2229; #beams:4 2024-11-06T03:33:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.09 , 31.35 , 29.82 , 34.87 2024-11-06T03:33:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , 0.02 2024-11-06T03:33:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863995.59 epoch sec; seqNo:6523; 4061773.15 north; 594422.33 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:33:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863994.74 2024-11-06T03:33:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:15Z- waiting for 1730863998.74, time is 1730863995.59 2024-11-06T03:33:15Z- AHRS_M2 msg: 1730863995.95 epoch sec; seqNo:6123 2024-11-06T03:33:15Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.20 psi 2024-11-06T03:33:15Z- handleDepth()- Depth_Keller msg: 1730863995.95 epoch sec; seqNo:6524; depth 50.15 2024-11-06T03:33:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863995.97 epoch sec; seqNo:6524; 4061773.15 north; 594422.33 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:33:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863994.74 2024-11-06T03:33:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:16Z- waiting for 1730863998.74, time is 1730863995.97 2024-11-06T03:33:16Z- AHRS_M2 msg: 1730863996.35 epoch sec; seqNo:6124 2024-11-06T03:33:16Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.20 psi 2024-11-06T03:33:16Z- handleDepth()- Depth_Keller msg: 1730863996.36 epoch sec; seqNo:6525; depth 50.16 2024-11-06T03:33:16Z- handleDVL() - DVL msg epochMillisec: 1730863996.39 2024-11-06T03:33:16Z- handleDvl() - RDI_Pathfinder msg: 1730863996.39 sec; seqNo:2230; #beams:4 2024-11-06T03:33:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.79 , 31.20 , 29.52 , 34.41 2024-11-06T03:33:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , 0.02 2024-11-06T03:33:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863996.40 epoch sec; seqNo:6525; 4061772.78 north; 594422.34 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:33:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863994.74 2024-11-06T03:33:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:16Z- waiting for 1730863998.74, time is 1730863996.40 2024-11-06T03:33:16Z- AHRS_M2 msg: 1730863996.76 epoch sec; seqNo:6125 2024-11-06T03:33:16Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.20 psi 2024-11-06T03:33:16Z- handleDepth()- Depth_Keller msg: 1730863996.76 epoch sec; seqNo:6526; depth 50.18 2024-11-06T03:33:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863996.77 epoch sec; seqNo:6526; 4061772.76 north; 594421.12 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:33:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863994.74 2024-11-06T03:33:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:16Z- waiting for 1730863998.74, time is 1730863996.77 2024-11-06T03:33:17Z- AHRS_M2 msg: 1730863997.18 epoch sec; seqNo:6126 2024-11-06T03:33:17Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.20 psi 2024-11-06T03:33:17Z- handleDepth()- Depth_Keller msg: 1730863997.19 epoch sec; seqNo:6527; depth 50.20 2024-11-06T03:33:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863997.24 epoch sec; seqNo:6527; 4061772.38 north; 594421.12 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:33:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863994.74 2024-11-06T03:33:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:17Z- waiting for 1730863998.74, time is 1730863997.24 2024-11-06T03:33:17Z- AHRS_M2 msg: 1730863997.56 epoch sec; seqNo:6127 2024-11-06T03:33:17Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.20 psi 2024-11-06T03:33:17Z- handleDepth()- Depth_Keller msg: 1730863997.57 epoch sec; seqNo:6528; depth 50.19 2024-11-06T03:33:17Z- handleDVL() - DVL msg epochMillisec: 1730863997.59 2024-11-06T03:33:17Z- handleDvl() - RDI_Pathfinder msg: 1730863997.59 sec; seqNo:2231; #beams:4 2024-11-06T03:33:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.95 , 30.59 , 29.82 , 33.49 2024-11-06T03:33:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , 0.01 2024-11-06T03:33:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863997.61 epoch sec; seqNo:6528; 4061772.38 north; 594421.12 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:33:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863994.74 2024-11-06T03:33:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:17Z- waiting for 1730863998.74, time is 1730863997.61 2024-11-06T03:33:17Z- AHRS_M2 msg: 1730863997.97 epoch sec; seqNo:6128 2024-11-06T03:33:17Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.20 psi 2024-11-06T03:33:17Z- handleDepth()- Depth_Keller msg: 1730863997.97 epoch sec; seqNo:6529; depth 50.22 2024-11-06T03:33:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863998.00 epoch sec; seqNo:6529; 4061772.00 north; 594421.13 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:33:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863994.74 2024-11-06T03:33:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:18Z- waiting for 1730863998.74, time is 1730863998.00 2024-11-06T03:33:18Z- AHRS_M2 msg: 1730863998.37 epoch sec; seqNo:6129 2024-11-06T03:33:18Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.21 psi 2024-11-06T03:33:18Z- handleDepth()- Depth_Keller msg: 1730863998.38 epoch sec; seqNo:6530; depth 50.21 2024-11-06T03:33:18Z- handleDVL() - DVL msg epochMillisec: 1730863998.40 2024-11-06T03:33:18Z- handleDvl() - RDI_Pathfinder msg: 1730863998.40 sec; seqNo:2232; #beams:4 2024-11-06T03:33:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.49 , 30.74 , 29.97 , 33.95 2024-11-06T03:33:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , -0.01 2024-11-06T03:33:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863998.43 epoch sec; seqNo:6530; 4061771.99 north; 594419.91 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:33:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863994.74 2024-11-06T03:33:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:18Z- waiting for 1730863998.74, time is 1730863998.43 2024-11-06T03:33:18Z- AHRS_M2 msg: 1730863998.78 epoch sec; seqNo:6130 2024-11-06T03:33:18Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.21 psi 2024-11-06T03:33:18Z- handleDepth()- Depth_Keller msg: 1730863998.79 epoch sec; seqNo:6531; depth 50.21 2024-11-06T03:33:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863998.80 epoch sec; seqNo:6531; 4061771.61 north; 594419.91 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:33:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863994.74 2024-11-06T03:33:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:18Z- waiting for 1730863998.74, time is 1730863998.80 2024-11-06T03:33:18Z- LcmTrn::updateTrn() >>>> pose time 1730863998.80, meas time 1730863998.40 <<<< 2024-11-06T03:33:18Z- LcmTrn::updateTrn() measUpdate first last update time: 1730863998.80 2024-11-06T03:33:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730863998.40 sec, ping # 0; waiting for INS data more recent than 1730863994.74... 2024-11-06T03:33:18Z- TerrainNav::motionUpdate() - currEstimate = (4061772, 594419.91, 50.21) 2024-11-06T03:33:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:33:18Z- TerrainNav:: SubcloudNIS is 0.41, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:33:18Z- TNavPF::Projecting Measurements 2024-11-06T03:33:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:33:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:33:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:33:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:33:18Z- TNavFilter::4 good beams. 2024-11-06T03:33:18Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:33:18Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:33:18Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:33:18Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:33:18Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:33:18Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:33:18Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:33:18Z- TerrainMapOctree: (vehN, vehE ) = (4061771.80, 594420.27). 2024-11-06T03:33:18Z- TerrainMapOctree: Min Distance = 171.66. 2024-11-06T03:33:18Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:33:19Z- Forcing Subcloud Comparison 2024-11-06T03:33:19Z- Weighting particles with subcloud comparison 2024-11-06T03:33:19Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:19Z- meanExpectedMeasDiff: -4.715596 delta_rms_squared: 1.784617 alpha: 0.988917 2024-11-06T03:33:19Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:19Z- meanExpectedMeasDiff: 2.425668 delta_rms_squared: 1.290945 alpha: 0.984744 2024-11-06T03:33:19Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:19Z- meanExpectedMeasDiff: 1.198675 delta_rms_squared: 1.420596 alpha: 0.986117 2024-11-06T03:33:19Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:19Z- meanExpectedMeasDiff: -0.173652 delta_rms_squared: 2.790240 alpha: 0.992883 2024-11-06T03:33:19Z- Subcloud NIS: 0.441595 2024-11-06T03:33:19Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:33:19Z- TNavPF:: sumWeights = 0.008269 2024-11-06T03:33:19Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:33:19Z- TNavPF::First Term of Map Covariance Matrix is 0.17 2024-11-06T03:33:19Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:33:19Z- TerrainNav::Resampling particle filter... 2024-11-06T03:33:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730863998.40 sec. 2024-11-06T03:33:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:33:19Z- variances([0], [2], and psi_berg) = 0.214122 0.304488 0.000000 2024-11-06T03:33:19Z- publishEstimatesDecoder() - at 1730863998.80 sec 2024-11-06T03:33:19Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:33:19Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061799.89 594402.84 54.80 2024-11-06T03:33:19Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061799.69 594403.98 55.69 lat/lon: 36.70 -121.94 2024-11-06T03:33:19Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.30 0.61 0.00 2024-11-06T03:33:19Z- handleDepth()- Depth_Keller msg: 1730863999.22 epoch sec; seqNo:6532; depth 50.23 2024-11-06T03:33:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863999.26 epoch sec; seqNo:6532; 4061771.61 north; 594419.91 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:33:19Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:33:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863998.80 2024-11-06T03:33:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:19Z- waiting for 1730864002.80, time is 1730863999.26 2024-11-06T03:33:19Z- AHRS_M2 msg: 1730863999.58 epoch sec; seqNo:6131 2024-11-06T03:33:19Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.21 psi 2024-11-06T03:33:19Z- handleDepth()- Depth_Keller msg: 1730863999.59 epoch sec; seqNo:6533; depth 50.22 2024-11-06T03:33:19Z- handleDVL() - DVL msg epochMillisec: 1730863999.61 2024-11-06T03:33:19Z- handleDvl() - RDI_Pathfinder msg: 1730863999.61 sec; seqNo:2233; #beams:4 2024-11-06T03:33:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.17 , 30.28 , 29.52 , 34.26 2024-11-06T03:33:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , -0.03 2024-11-06T03:33:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730863999.64 epoch sec; seqNo:6533; 4061771.23 north; 594419.91 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:33:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863998.80 2024-11-06T03:33:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:19Z- waiting for 1730864002.80, time is 1730863999.64 2024-11-06T03:33:19Z- AHRS_M2 msg: 1730863999.99 epoch sec; seqNo:6132 2024-11-06T03:33:19Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.21 psi 2024-11-06T03:33:19Z- handleDepth()- Depth_Keller msg: 1730863999.99 epoch sec; seqNo:6534; depth 50.21 2024-11-06T03:33:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864000.01 epoch sec; seqNo:6534; 4061771.22 north; 594418.69 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:33:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863998.80 2024-11-06T03:33:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:20Z- waiting for 1730864002.80, time is 1730864000.01 2024-11-06T03:33:20Z- handleDepth()- Depth_Keller msg: 1730864000.41 epoch sec; seqNo:6535; depth 50.24 2024-11-06T03:33:20Z- handleDVL() - DVL msg epochMillisec: 1730864000.43 2024-11-06T03:33:20Z- handleDvl() - RDI_Pathfinder msg: 1730864000.43 sec; seqNo:2234; #beams:4 2024-11-06T03:33:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.56 , 30.13 , 29.36 , 34.56 2024-11-06T03:33:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.04 2024-11-06T03:33:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864000.45 epoch sec; seqNo:6535; 4061770.84 north; 594418.70 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:33:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863998.80 2024-11-06T03:33:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:20Z- waiting for 1730864002.80, time is 1730864000.45 2024-11-06T03:33:20Z- AHRS_M2 msg: 1730864000.79 epoch sec; seqNo:6133 2024-11-06T03:33:20Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 4.21 psi 2024-11-06T03:33:20Z- handleDepth()- Depth_Keller msg: 1730864000.80 epoch sec; seqNo:6536; depth 50.24 2024-11-06T03:33:20Z- handleDVL() - DVL msg epochMillisec: 1730864000.80 2024-11-06T03:33:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:33:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:33:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:33:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:33:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:33:20Z- handleDvl() - RDI_Pathfinder msg: 1730864000.80 sec; seqNo:2235; #beams:0 2024-11-06T03:33:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:33:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.04 2024-11-06T03:33:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864000.82 epoch sec; seqNo:6536; 4061770.84 north; 594418.70 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:33:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863998.80 2024-11-06T03:33:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:20Z- waiting for 1730864002.80, time is 1730864000.82 2024-11-06T03:33:21Z- AHRS_M2 msg: 1730864001.20 epoch sec; seqNo:6134 2024-11-06T03:33:21Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 4.21 psi 2024-11-06T03:33:21Z- handleDepth()- Depth_Keller msg: 1730864001.21 epoch sec; seqNo:6537; depth 50.24 2024-11-06T03:33:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864001.22 epoch sec; seqNo:6537; 4061770.46 north; 594418.70 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:33:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863998.80 2024-11-06T03:33:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:21Z- waiting for 1730864002.80, time is 1730864001.22 2024-11-06T03:33:21Z- AHRS_M2 msg: 1730864001.60 epoch sec; seqNo:6135 2024-11-06T03:33:21Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 4.21 psi 2024-11-06T03:33:21Z- handleDepth()- Depth_Keller msg: 1730864001.61 epoch sec; seqNo:6538; depth 50.25 2024-11-06T03:33:21Z- handleDVL() - DVL msg epochMillisec: 1730864001.65 2024-11-06T03:33:21Z- handleDvl() - RDI_Pathfinder msg: 1730864001.65 sec; seqNo:2236; #beams:4 2024-11-06T03:33:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.87 , 29.82 , 29.21 , 33.80 2024-11-06T03:33:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , -0.03 2024-11-06T03:33:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864001.66 epoch sec; seqNo:6538; 4061770.45 north; 594417.48 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:33:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863998.80 2024-11-06T03:33:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:21Z- waiting for 1730864002.80, time is 1730864001.66 2024-11-06T03:33:22Z- AHRS_M2 msg: 1730864002.01 epoch sec; seqNo:6136 2024-11-06T03:33:22Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 4.21 psi 2024-11-06T03:33:22Z- handleDepth()- Depth_Keller msg: 1730864002.01 epoch sec; seqNo:6539; depth 50.26 2024-11-06T03:33:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864002.03 epoch sec; seqNo:6539; 4061770.45 north; 594417.48 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:33:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863998.80 2024-11-06T03:33:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:22Z- waiting for 1730864002.80, time is 1730864002.03 2024-11-06T03:33:22Z- AHRS_M2 msg: 1730864002.41 epoch sec; seqNo:6137 2024-11-06T03:33:22Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 4.21 psi 2024-11-06T03:33:22Z- handleDepth()- Depth_Keller msg: 1730864002.43 epoch sec; seqNo:6540; depth 50.26 2024-11-06T03:33:22Z- handleDVL() - DVL msg epochMillisec: 1730864002.46 2024-11-06T03:33:22Z- handleDvl() - RDI_Pathfinder msg: 1730864002.46 sec; seqNo:2237; #beams:4 2024-11-06T03:33:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.87 , 28.44 , 28.90 , 33.95 2024-11-06T03:33:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.08 , -0.02 2024-11-06T03:33:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864002.49 epoch sec; seqNo:6540; 4061770.07 north; 594417.49 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:33:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863998.80 2024-11-06T03:33:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:22Z- waiting for 1730864002.80, time is 1730864002.49 2024-11-06T03:33:22Z- AHRS_M2 msg: 1730864002.81 epoch sec; seqNo:6138 2024-11-06T03:33:22Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.21 psi 2024-11-06T03:33:22Z- handleDepth()- Depth_Keller msg: 1730864002.83 epoch sec; seqNo:6541; depth 50.27 2024-11-06T03:33:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864002.85 epoch sec; seqNo:6541; 4061770.06 north; 594416.27 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:33:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730863998.80 2024-11-06T03:33:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:22Z- waiting for 1730864002.80, time is 1730864002.85 2024-11-06T03:33:22Z- LcmTrn::updateTrn() >>>> pose time 1730864002.85, meas time 1730864002.46 <<<< 2024-11-06T03:33:22Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864002.85 2024-11-06T03:33:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864002.46 sec, ping # 0; waiting for INS data more recent than 1730863998.80... 2024-11-06T03:33:22Z- TerrainNav::motionUpdate() - currEstimate = (4061770, 594416.27, 50.27) 2024-11-06T03:33:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:33:22Z- TerrainNav:: SubcloudNIS is 0.44, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:33:23Z- TNavPF::Projecting Measurements 2024-11-06T03:33:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:33:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:33:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:33:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:33:23Z- TNavFilter::4 good beams. 2024-11-06T03:33:23Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:33:23Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:33:23Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:33:23Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:33:23Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:33:23Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:33:23Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:33:23Z- TerrainMapOctree: (vehN, vehE ) = (4061770.20, 594416.61). 2024-11-06T03:33:23Z- TerrainMapOctree: Min Distance = 167.83. 2024-11-06T03:33:23Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:33:23Z- Forcing Subcloud Comparison 2024-11-06T03:33:23Z- Weighting particles with subcloud comparison 2024-11-06T03:33:23Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:23Z- meanExpectedMeasDiff: -2.840401 delta_rms_squared: 2.380286 alpha: 0.991668 2024-11-06T03:33:23Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:23Z- meanExpectedMeasDiff: 0.551128 delta_rms_squared: 0.932538 alpha: 0.979003 2024-11-06T03:33:23Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:23Z- meanExpectedMeasDiff: 0.459636 delta_rms_squared: 0.969506 alpha: 0.979788 2024-11-06T03:33:23Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:23Z- meanExpectedMeasDiff: 0.734091 delta_rms_squared: 1.206680 alpha: 0.983696 2024-11-06T03:33:23Z- Subcloud NIS: 0.449043 2024-11-06T03:33:23Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:33:23Z- TNavPF:: sumWeights = 0.184754 2024-11-06T03:33:23Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:33:23Z- TNavPF::First Term of Map Covariance Matrix is 0.94 2024-11-06T03:33:23Z- TNavPF::Calculated NIS Value : 0.09 numBeams normalized NIS: 0.02 2024-11-06T03:33:23Z- TerrainNav::Resampling particle filter... 2024-11-06T03:33:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864002.46 sec. 2024-11-06T03:33:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:33:23Z- variances([0], [2], and psi_berg) = 0.223083 0.337793 0.000000 2024-11-06T03:33:23Z- publishEstimatesDecoder() - at 1730864002.85 sec 2024-11-06T03:33:23Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:33:23Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061798.49 594398.99 55.03 2024-11-06T03:33:23Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061798.17 594400.26 56.00 lat/lon: 36.70 -121.94 2024-11-06T03:33:23Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.34 0.61 0.00 2024-11-06T03:33:23Z- AHRS_M2 msg: 1730864003.22 epoch sec; seqNo:6139 2024-11-06T03:33:23Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.21 psi 2024-11-06T03:33:23Z- handleDepth()- Depth_Keller msg: 1730864003.24 epoch sec; seqNo:6542; depth 50.29 2024-11-06T03:33:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864003.25 epoch sec; seqNo:6542; 4061769.68 north; 594416.27 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:33:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864002.85 2024-11-06T03:33:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:23Z- waiting for 1730864006.85, time is 1730864003.25 2024-11-06T03:33:23Z- AHRS_M2 msg: 1730864003.63 epoch sec; seqNo:6140 2024-11-06T03:33:23Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 4.21 psi 2024-11-06T03:33:23Z- handleDepth()- Depth_Keller msg: 1730864003.64 epoch sec; seqNo:6543; depth 50.30 2024-11-06T03:33:23Z- handleDVL() - DVL msg epochMillisec: 1730864003.67 2024-11-06T03:33:23Z- handleDvl() - RDI_Pathfinder msg: 1730864003.67 sec; seqNo:2238; #beams:4 2024-11-06T03:33:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.56 , 27.53 , 28.90 , 34.26 2024-11-06T03:33:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.05 , -0.01 2024-11-06T03:33:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864003.68 epoch sec; seqNo:6543; 4061769.68 north; 594416.27 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:33:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864002.85 2024-11-06T03:33:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:23Z- waiting for 1730864006.85, time is 1730864003.68 2024-11-06T03:33:24Z- AHRS_M2 msg: 1730864004.03 epoch sec; seqNo:6141 2024-11-06T03:33:24Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 4.20 psi 2024-11-06T03:33:24Z- handleDepth()- Depth_Keller msg: 1730864004.03 epoch sec; seqNo:6544; depth 50.30 2024-11-06T03:33:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864004.05 epoch sec; seqNo:6544; 4061769.30 north; 594416.27 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:33:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864002.85 2024-11-06T03:33:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:24Z- waiting for 1730864006.85, time is 1730864004.05 2024-11-06T03:33:24Z- AHRS_M2 msg: 1730864004.44 epoch sec; seqNo:6142 2024-11-06T03:33:24Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.20 psi 2024-11-06T03:33:24Z- handleDepth()- Depth_Keller msg: 1730864004.44 epoch sec; seqNo:6545; depth 50.32 2024-11-06T03:33:24Z- handleDVL() - DVL msg epochMillisec: 1730864004.46 2024-11-06T03:33:24Z- handleDvl() - RDI_Pathfinder msg: 1730864004.46 sec; seqNo:2239; #beams:4 2024-11-06T03:33:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.65 , 27.53 , 29.21 , 33.95 2024-11-06T03:33:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , 0.03 2024-11-06T03:33:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864004.48 epoch sec; seqNo:6545; 4061769.29 north; 594415.05 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:33:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864002.85 2024-11-06T03:33:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:24Z- waiting for 1730864006.85, time is 1730864004.48 2024-11-06T03:33:24Z- AHRS_M2 msg: 1730864004.84 epoch sec; seqNo:6143 2024-11-06T03:33:24Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.20 psi 2024-11-06T03:33:24Z- handleDepth()- Depth_Keller msg: 1730864004.84 epoch sec; seqNo:6546; depth 50.32 2024-11-06T03:33:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864004.86 epoch sec; seqNo:6546; 4061769.29 north; 594415.05 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:33:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864002.85 2024-11-06T03:33:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:24Z- waiting for 1730864006.85, time is 1730864004.86 2024-11-06T03:33:25Z- AHRS_M2 msg: 1730864005.24 epoch sec; seqNo:6144 2024-11-06T03:33:25Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.20 psi 2024-11-06T03:33:25Z- handleDepth()- Depth_Keller msg: 1730864005.25 epoch sec; seqNo:6547; depth 50.33 2024-11-06T03:33:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864005.26 epoch sec; seqNo:6547; 4061768.91 north; 594415.06 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:33:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864002.85 2024-11-06T03:33:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:25Z- waiting for 1730864006.85, time is 1730864005.26 2024-11-06T03:33:25Z- handleDepth()- Depth_Keller msg: 1730864005.67 epoch sec; seqNo:6548; depth 50.37 2024-11-06T03:33:25Z- handleDVL() - DVL msg epochMillisec: 1730864005.69 2024-11-06T03:33:25Z- handleDvl() - RDI_Pathfinder msg: 1730864005.69 sec; seqNo:2240; #beams:4 2024-11-06T03:33:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.80 , 27.53 , 29.06 , 34.11 2024-11-06T03:33:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.08 , 0.03 2024-11-06T03:33:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864005.70 epoch sec; seqNo:6548; 4061768.89 north; 594413.84 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:33:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864002.85 2024-11-06T03:33:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:25Z- waiting for 1730864006.85, time is 1730864005.70 2024-11-06T03:33:26Z- AHRS_M2 msg: 1730864006.05 epoch sec; seqNo:6145 2024-11-06T03:33:26Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.20 psi 2024-11-06T03:33:26Z- handleDepth()- Depth_Keller msg: 1730864006.05 epoch sec; seqNo:6549; depth 50.38 2024-11-06T03:33:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864006.09 epoch sec; seqNo:6549; 4061768.51 north; 594413.84 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:33:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864002.85 2024-11-06T03:33:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:26Z- waiting for 1730864006.85, time is 1730864006.09 2024-11-06T03:33:26Z- AHRS_M2 msg: 1730864006.46 epoch sec; seqNo:6146 2024-11-06T03:33:26Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 4.20 psi 2024-11-06T03:33:26Z- handleDepth()- Depth_Keller msg: 1730864006.47 epoch sec; seqNo:6550; depth 50.39 2024-11-06T03:33:26Z- handleDVL() - DVL msg epochMillisec: 1730864006.50 2024-11-06T03:33:26Z- handleDvl() - RDI_Pathfinder msg: 1730864006.50 sec; seqNo:2241; #beams:4 2024-11-06T03:33:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.12 , 27.84 , 29.21 , 33.80 2024-11-06T03:33:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.03 , 0.03 2024-11-06T03:33:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864006.51 epoch sec; seqNo:6550; 4061768.51 north; 594413.84 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:33:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864002.85 2024-11-06T03:33:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:26Z- waiting for 1730864006.85, time is 1730864006.51 2024-11-06T03:33:26Z- AHRS_M2 msg: 1730864006.86 epoch sec; seqNo:6147 2024-11-06T03:33:26Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 4.19 psi 2024-11-06T03:33:26Z- handleDepth()- Depth_Keller msg: 1730864006.86 epoch sec; seqNo:6551; depth 50.40 2024-11-06T03:33:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864006.88 epoch sec; seqNo:6551; 4061768.14 north; 594413.85 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:33:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864002.85 2024-11-06T03:33:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:27Z- waiting for 1730864006.85, time is 1730864006.88 2024-11-06T03:33:27Z- LcmTrn::updateTrn() >>>> pose time 1730864006.88, meas time 1730864006.50 <<<< 2024-11-06T03:33:27Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864006.88 2024-11-06T03:33:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864006.50 sec, ping # 0; waiting for INS data more recent than 1730864002.85... 2024-11-06T03:33:27Z- TerrainNav::motionUpdate() - currEstimate = (4061768, 594413.85, 50.40) 2024-11-06T03:33:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:33:27Z- TerrainNav:: SubcloudNIS is 0.45, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:33:27Z- TNavPF::Projecting Measurements 2024-11-06T03:33:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:33:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:33:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:33:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:33:27Z- TNavFilter::4 good beams. 2024-11-06T03:33:27Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:33:27Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:33:27Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:33:27Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:33:27Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:33:27Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:33:27Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:33:27Z- TerrainMapOctree: (vehN, vehE ) = (4061768.32, 594414.08). 2024-11-06T03:33:27Z- TerrainMapOctree: Min Distance = 165.10. 2024-11-06T03:33:27Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:33:27Z- Forcing Subcloud Comparison 2024-11-06T03:33:27Z- Weighting particles with subcloud comparison 2024-11-06T03:33:27Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:27Z- meanExpectedMeasDiff: -3.951204 delta_rms_squared: 1.433802 alpha: 0.986243 2024-11-06T03:33:27Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:27Z- meanExpectedMeasDiff: 1.195145 delta_rms_squared: 0.876998 alpha: 0.977703 2024-11-06T03:33:27Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:27Z- meanExpectedMeasDiff: 1.317337 delta_rms_squared: 2.047288 alpha: 0.990325 2024-11-06T03:33:27Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:27Z- meanExpectedMeasDiff: 1.448550 delta_rms_squared: 1.270135 alpha: 0.984498 2024-11-06T03:33:27Z- Subcloud NIS: 0.473135 2024-11-06T03:33:27Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:33:27Z- TNavPF:: sumWeights = 0.028449 2024-11-06T03:33:27Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:33:27Z- TNavPF::First Term of Map Covariance Matrix is 0.43 2024-11-06T03:33:27Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:33:27Z- TerrainNav::Resampling particle filter... 2024-11-06T03:33:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864006.50 sec. 2024-11-06T03:33:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:33:27Z- variances([0], [2], and psi_berg) = 0.199206 0.341676 0.000000 2024-11-06T03:33:27Z- publishEstimatesDecoder() - at 1730864006.88 sec 2024-11-06T03:33:27Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:33:27Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061796.54 594396.58 54.95 2024-11-06T03:33:27Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061796.18 594397.85 56.14 lat/lon: 36.70 -121.94 2024-11-06T03:33:27Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.20 0.34 0.57 0.00 2024-11-06T03:33:27Z- AHRS_M2 msg: 1730864007.26 epoch sec; seqNo:6148 2024-11-06T03:33:27Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 4.19 psi 2024-11-06T03:33:27Z- handleDepth()- Depth_Keller msg: 1730864007.28 epoch sec; seqNo:6552; depth 50.41 2024-11-06T03:33:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864007.30 epoch sec; seqNo:6552; 4061768.12 north; 594412.63 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:33:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864006.88 2024-11-06T03:33:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:27Z- waiting for 1730864010.88, time is 1730864007.30 2024-11-06T03:33:27Z- AHRS_M2 msg: 1730864007.67 epoch sec; seqNo:6149 2024-11-06T03:33:27Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 4.19 psi 2024-11-06T03:33:27Z- handleDepth()- Depth_Keller msg: 1730864007.67 epoch sec; seqNo:6553; depth 50.44 2024-11-06T03:33:27Z- handleDVL() - DVL msg epochMillisec: 1730864007.69 2024-11-06T03:33:27Z- handleDvl() - RDI_Pathfinder msg: 1730864007.69 sec; seqNo:2242; #beams:4 2024-11-06T03:33:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.35 , 27.87 , 29.23 , 33.17 2024-11-06T03:33:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , 0.03 2024-11-06T03:33:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864007.72 epoch sec; seqNo:6553; 4061767.74 north; 594412.63 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:33:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864006.88 2024-11-06T03:33:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:27Z- waiting for 1730864010.88, time is 1730864007.72 2024-11-06T03:33:28Z- AHRS_M2 msg: 1730864008.08 epoch sec; seqNo:6150 2024-11-06T03:33:28Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 4.19 psi 2024-11-06T03:33:28Z- handleDepth()- Depth_Keller msg: 1730864008.09 epoch sec; seqNo:6554; depth 50.44 2024-11-06T03:33:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864008.10 epoch sec; seqNo:6554; 4061767.74 north; 594412.63 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:33:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864006.88 2024-11-06T03:33:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:28Z- waiting for 1730864010.88, time is 1730864008.10 2024-11-06T03:33:28Z- AHRS_M2 msg: 1730864008.47 epoch sec; seqNo:6151 2024-11-06T03:33:28Z- AHRS_M2 msg: 0.12 phi; -0.01 theta; 4.19 psi 2024-11-06T03:33:28Z- handleDepth()- Depth_Keller msg: 1730864008.50 epoch sec; seqNo:6555; depth 50.47 2024-11-06T03:33:28Z- handleDVL() - DVL msg epochMillisec: 1730864008.52 2024-11-06T03:33:28Z- handleDvl() - RDI_Pathfinder msg: 1730864008.52 sec; seqNo:2243; #beams:4 2024-11-06T03:33:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.49 , 27.59 , 29.23 , 32.76 2024-11-06T03:33:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , 0.01 2024-11-06T03:33:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864008.55 epoch sec; seqNo:6555; 4061767.36 north; 594412.63 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:33:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864006.88 2024-11-06T03:33:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:28Z- waiting for 1730864010.88, time is 1730864008.55 2024-11-06T03:33:28Z- AHRS_M2 msg: 1730864008.88 epoch sec; seqNo:6152 2024-11-06T03:33:28Z- AHRS_M2 msg: 0.12 phi; -0.00 theta; 4.19 psi 2024-11-06T03:33:28Z- handleDepth()- Depth_Keller msg: 1730864008.89 epoch sec; seqNo:6556; depth 50.46 2024-11-06T03:33:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864008.91 epoch sec; seqNo:6556; 4061767.35 north; 594411.41 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:33:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864006.88 2024-11-06T03:33:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:29Z- waiting for 1730864010.88, time is 1730864008.91 2024-11-06T03:33:29Z- AHRS_M2 msg: 1730864009.28 epoch sec; seqNo:6153 2024-11-06T03:33:29Z- AHRS_M2 msg: 0.12 phi; -0.00 theta; 4.19 psi 2024-11-06T03:33:29Z- handleDepth()- Depth_Keller msg: 1730864009.29 epoch sec; seqNo:6557; depth 50.47 2024-11-06T03:33:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864009.30 epoch sec; seqNo:6557; 4061767.35 north; 594411.41 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:33:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864006.88 2024-11-06T03:33:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:29Z- waiting for 1730864010.88, time is 1730864009.30 2024-11-06T03:33:29Z- AHRS_M2 msg: 1730864009.68 epoch sec; seqNo:6154 2024-11-06T03:33:29Z- AHRS_M2 msg: 0.12 phi; -0.00 theta; 4.19 psi 2024-11-06T03:33:29Z- handleDepth()- Depth_Keller msg: 1730864009.69 epoch sec; seqNo:6558; depth 50.47 2024-11-06T03:33:29Z- handleDVL() - DVL msg epochMillisec: 1730864009.72 2024-11-06T03:33:29Z- handleDvl() - RDI_Pathfinder msg: 1730864009.72 sec; seqNo:2244; #beams:4 2024-11-06T03:33:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.76 , 27.33 , 29.50 , 33.04 2024-11-06T03:33:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.00 2024-11-06T03:33:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864009.73 epoch sec; seqNo:6558; 4061766.97 north; 594411.42 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:33:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864006.88 2024-11-06T03:33:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:29Z- waiting for 1730864010.88, time is 1730864009.73 2024-11-06T03:33:30Z- AHRS_M2 msg: 1730864010.09 epoch sec; seqNo:6155 2024-11-06T03:33:30Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 4.19 psi 2024-11-06T03:33:30Z- handleDepth()- Depth_Keller msg: 1730864010.09 epoch sec; seqNo:6559; depth 50.49 2024-11-06T03:33:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864010.11 epoch sec; seqNo:6559; 4061766.59 north; 594411.42 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:33:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864006.88 2024-11-06T03:33:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:30Z- waiting for 1730864010.88, time is 1730864010.11 2024-11-06T03:33:30Z- AHRS_M2 msg: 1730864010.49 epoch sec; seqNo:6156 2024-11-06T03:33:30Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 4.19 psi 2024-11-06T03:33:30Z- handleDepth()- Depth_Keller msg: 1730864010.51 epoch sec; seqNo:6560; depth 50.49 2024-11-06T03:33:30Z- handleDVL() - DVL msg epochMillisec: 1730864010.56 2024-11-06T03:33:30Z- handleDvl() - RDI_Pathfinder msg: 1730864010.56 sec; seqNo:2245; #beams:4 2024-11-06T03:33:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.04 , 27.19 , 29.64 , 32.76 2024-11-06T03:33:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , -0.02 2024-11-06T03:33:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864010.57 epoch sec; seqNo:6560; 4061766.58 north; 594410.20 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:33:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864006.88 2024-11-06T03:33:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:30Z- waiting for 1730864010.88, time is 1730864010.57 2024-11-06T03:33:30Z- AHRS_M2 msg: 1730864010.89 epoch sec; seqNo:6157 2024-11-06T03:33:30Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 4.19 psi 2024-11-06T03:33:30Z- handleDepth()- Depth_Keller msg: 1730864010.91 epoch sec; seqNo:6561; depth 50.49 2024-11-06T03:33:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864010.93 epoch sec; seqNo:6561; 4061766.20 north; 594410.21 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:33:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864006.88 2024-11-06T03:33:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:31Z- waiting for 1730864010.88, time is 1730864010.93 2024-11-06T03:33:31Z- LcmTrn::updateTrn() >>>> pose time 1730864010.93, meas time 1730864010.56 <<<< 2024-11-06T03:33:31Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864010.93 2024-11-06T03:33:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864010.56 sec, ping # 0; waiting for INS data more recent than 1730864006.88... 2024-11-06T03:33:31Z- TerrainNav::motionUpdate() - currEstimate = (4061766, 594410.21, 50.49) 2024-11-06T03:33:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:33:31Z- TerrainNav:: SubcloudNIS is 0.47, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:33:31Z- TNavPF::Projecting Measurements 2024-11-06T03:33:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:33:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:33:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:33:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:33:31Z- TNavFilter::4 good beams. 2024-11-06T03:33:31Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:33:31Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:33:31Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:33:31Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:33:31Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:33:31Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:33:31Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:33:31Z- TerrainMapOctree: (vehN, vehE ) = (4061766.38, 594410.54). 2024-11-06T03:33:31Z- TerrainMapOctree: Min Distance = 161.38. 2024-11-06T03:33:31Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:33:31Z- Forcing Subcloud Comparison 2024-11-06T03:33:31Z- Weighting particles with subcloud comparison 2024-11-06T03:33:31Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:31Z- meanExpectedMeasDiff: -2.438888 delta_rms_squared: 1.950419 alpha: 0.989850 2024-11-06T03:33:31Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:31Z- meanExpectedMeasDiff: 1.691497 delta_rms_squared: 0.958936 alpha: 0.979570 2024-11-06T03:33:31Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:31Z- meanExpectedMeasDiff: 1.136307 delta_rms_squared: 1.117495 alpha: 0.982418 2024-11-06T03:33:31Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:31Z- meanExpectedMeasDiff: 1.983209 delta_rms_squared: 1.218840 alpha: 0.983856 2024-11-06T03:33:31Z- Subcloud NIS: 0.481304 2024-11-06T03:33:31Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:33:31Z- TNavPF:: sumWeights = 0.080621 2024-11-06T03:33:31Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:33:31Z- TNavPF::First Term of Map Covariance Matrix is 0.61 2024-11-06T03:33:31Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T03:33:31Z- TerrainNav::Resampling particle filter... 2024-11-06T03:33:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864010.56 sec. 2024-11-06T03:33:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:33:31Z- variances([0], [2], and psi_berg) = 0.209877 0.381200 0.000000 2024-11-06T03:33:31Z- publishEstimatesDecoder() - at 1730864010.93 sec 2024-11-06T03:33:31Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:33:31Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061794.41 594392.64 57.28 2024-11-06T03:33:31Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061794.26 594394.17 55.96 lat/lon: 36.70 -121.94 2024-11-06T03:33:31Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.38 0.60 0.00 2024-11-06T03:33:31Z- AHRS_M2 msg: 1730864011.30 epoch sec; seqNo:6158 2024-11-06T03:33:31Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 4.19 psi 2024-11-06T03:33:31Z- handleDepth()- Depth_Keller msg: 1730864011.31 epoch sec; seqNo:6562; depth 50.48 2024-11-06T03:33:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864011.34 epoch sec; seqNo:6562; 4061766.20 north; 594410.21 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:33:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864010.93 2024-11-06T03:33:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:31Z- waiting for 1730864014.93, time is 1730864011.34 2024-11-06T03:33:31Z- AHRS_M2 msg: 1730864011.70 epoch sec; seqNo:6159 2024-11-06T03:33:31Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 4.19 psi 2024-11-06T03:33:31Z- handleDepth()- Depth_Keller msg: 1730864011.76 epoch sec; seqNo:6563; depth 50.50 2024-11-06T03:33:31Z- handleDVL() - DVL msg epochMillisec: 1730864011.83 2024-11-06T03:33:31Z- handleDvl() - RDI_Pathfinder msg: 1730864011.83 sec; seqNo:2246; #beams:4 2024-11-06T03:33:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.17 , 27.05 , 29.50 , 32.49 2024-11-06T03:33:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , -0.02 2024-11-06T03:33:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864011.84 epoch sec; seqNo:6563; 4061765.81 north; 594408.99 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:33:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864010.93 2024-11-06T03:33:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:31Z- waiting for 1730864014.93, time is 1730864011.84 2024-11-06T03:33:32Z- AHRS_M2 msg: 1730864012.11 epoch sec; seqNo:6160 2024-11-06T03:33:32Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 4.19 psi 2024-11-06T03:33:32Z- handleDepth()- Depth_Keller msg: 1730864012.11 epoch sec; seqNo:6564; depth 50.49 2024-11-06T03:33:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864012.13 epoch sec; seqNo:6564; 4061765.81 north; 594408.99 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T03:33:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864010.93 2024-11-06T03:33:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:32Z- waiting for 1730864014.93, time is 1730864012.13 2024-11-06T03:33:32Z- AHRS_M2 msg: 1730864012.51 epoch sec; seqNo:6161 2024-11-06T03:33:32Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 4.19 psi 2024-11-06T03:33:32Z- handleDepth()- Depth_Keller msg: 1730864012.52 epoch sec; seqNo:6565; depth 50.47 2024-11-06T03:33:32Z- handleDVL() - DVL msg epochMillisec: 1730864012.57 2024-11-06T03:33:32Z- handleDvl() - RDI_Pathfinder msg: 1730864012.57 sec; seqNo:2247; #beams:4 2024-11-06T03:33:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.90 , 26.78 , 30.04 , 32.35 2024-11-06T03:33:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , -0.02 2024-11-06T03:33:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864012.60 epoch sec; seqNo:6565; 4061765.43 north; 594408.99 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T03:33:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864010.93 2024-11-06T03:33:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:32Z- waiting for 1730864014.93, time is 1730864012.60 2024-11-06T03:33:32Z- AHRS_M2 msg: 1730864012.91 epoch sec; seqNo:6162 2024-11-06T03:33:32Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 4.19 psi 2024-11-06T03:33:32Z- handleDepth()- Depth_Keller msg: 1730864012.92 epoch sec; seqNo:6566; depth 50.48 2024-11-06T03:33:32Z- handleDVL() - DVL msg epochMillisec: 1730864012.92 2024-11-06T03:33:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:33:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:33:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:33:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:33:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:33:32Z- handleDvl() - RDI_Pathfinder msg: 1730864012.92 sec; seqNo:2248; #beams:0 2024-11-06T03:33:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:33:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , -0.02 2024-11-06T03:33:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864012.94 epoch sec; seqNo:6566; 4061765.43 north; 594408.99 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:33:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864010.93 2024-11-06T03:33:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:33Z- waiting for 1730864014.93, time is 1730864012.94 2024-11-06T03:33:33Z- AHRS_M2 msg: 1730864013.32 epoch sec; seqNo:6163 2024-11-06T03:33:33Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 4.19 psi 2024-11-06T03:33:33Z- handleDepth()- Depth_Keller msg: 1730864013.33 epoch sec; seqNo:6567; depth 50.45 2024-11-06T03:33:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864013.34 epoch sec; seqNo:6567; 4061765.05 north; 594409.00 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T03:33:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864010.93 2024-11-06T03:33:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:33Z- waiting for 1730864014.93, time is 1730864013.34 2024-11-06T03:33:33Z- AHRS_M2 msg: 1730864013.73 epoch sec; seqNo:6164 2024-11-06T03:33:33Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 4.19 psi 2024-11-06T03:33:33Z- handleDepth()- Depth_Keller msg: 1730864013.74 epoch sec; seqNo:6568; depth 50.46 2024-11-06T03:33:33Z- handleDVL() - DVL msg epochMillisec: 1730864013.76 2024-11-06T03:33:33Z- handleDvl() - RDI_Pathfinder msg: 1730864013.76 sec; seqNo:2249; #beams:4 2024-11-06T03:33:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.76 , 27.05 , 29.91 , 32.49 2024-11-06T03:33:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , -0.02 2024-11-06T03:33:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864013.77 epoch sec; seqNo:6568; 4061765.04 north; 594407.78 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:33:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864010.93 2024-11-06T03:33:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:33Z- waiting for 1730864014.93, time is 1730864013.77 2024-11-06T03:33:34Z- AHRS_M2 msg: 1730864014.13 epoch sec; seqNo:6165 2024-11-06T03:33:34Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 4.19 psi 2024-11-06T03:33:34Z- handleDepth()- Depth_Keller msg: 1730864014.13 epoch sec; seqNo:6569; depth 50.44 2024-11-06T03:33:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864014.15 epoch sec; seqNo:6569; 4061765.04 north; 594407.78 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:33:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864010.93 2024-11-06T03:33:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:34Z- waiting for 1730864014.93, time is 1730864014.15 2024-11-06T03:33:34Z- AHRS_M2 msg: 1730864014.53 epoch sec; seqNo:6166 2024-11-06T03:33:34Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 4.19 psi 2024-11-06T03:33:34Z- handleDepth()- Depth_Keller msg: 1730864014.54 epoch sec; seqNo:6570; depth 50.43 2024-11-06T03:33:34Z- handleDVL() - DVL msg epochMillisec: 1730864014.56 2024-11-06T03:33:34Z- handleDvl() - RDI_Pathfinder msg: 1730864014.56 sec; seqNo:2250; #beams:4 2024-11-06T03:33:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.36 , 27.19 , 29.77 , 32.36 2024-11-06T03:33:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , -0.03 2024-11-06T03:33:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864014.59 epoch sec; seqNo:6570; 4061764.66 north; 594407.78 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T03:33:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864010.93 2024-11-06T03:33:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:34Z- waiting for 1730864014.93, time is 1730864014.59 2024-11-06T03:33:34Z- AHRS_M2 msg: 1730864014.93 epoch sec; seqNo:6167 2024-11-06T03:33:34Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 4.19 psi 2024-11-06T03:33:34Z- handleDepth()- Depth_Keller msg: 1730864014.94 epoch sec; seqNo:6571; depth 50.40 2024-11-06T03:33:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864014.96 epoch sec; seqNo:6571; 4061764.66 north; 594407.78 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:33:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864010.93 2024-11-06T03:33:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:35Z- waiting for 1730864014.93, time is 1730864014.96 2024-11-06T03:33:35Z- LcmTrn::updateTrn() >>>> pose time 1730864014.96, meas time 1730864014.56 <<<< 2024-11-06T03:33:35Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864014.96 2024-11-06T03:33:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864014.56 sec, ping # 0; waiting for INS data more recent than 1730864010.93... 2024-11-06T03:33:35Z- TerrainNav::motionUpdate() - currEstimate = (4061765, 594407.78, 50.40) 2024-11-06T03:33:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:33:35Z- TerrainNav:: SubcloudNIS is 0.48, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:33:35Z- TNavPF::Projecting Measurements 2024-11-06T03:33:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:33:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:33:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:33:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:33:35Z- TNavFilter::4 good beams. 2024-11-06T03:33:35Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:33:35Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:33:35Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:33:35Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:33:35Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:33:35Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:33:35Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:33:35Z- TerrainMapOctree: (vehN, vehE ) = (4061764.81, 594408.02). 2024-11-06T03:33:35Z- TerrainMapOctree: Min Distance = 158.71. 2024-11-06T03:33:35Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:33:35Z- Forcing Subcloud Comparison 2024-11-06T03:33:35Z- Weighting particles with subcloud comparison 2024-11-06T03:33:35Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:35Z- meanExpectedMeasDiff: -1.809539 delta_rms_squared: 1.740200 alpha: 0.988638 2024-11-06T03:33:35Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:35Z- meanExpectedMeasDiff: 1.956179 delta_rms_squared: 1.028261 alpha: 0.980921 2024-11-06T03:33:35Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:35Z- meanExpectedMeasDiff: 0.648562 delta_rms_squared: 1.229382 alpha: 0.983992 2024-11-06T03:33:35Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:35Z- meanExpectedMeasDiff: 1.454112 delta_rms_squared: 1.129540 alpha: 0.982602 2024-11-06T03:33:35Z- Subcloud NIS: 0.482864 2024-11-06T03:33:35Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:33:35Z- TNavPF:: sumWeights = 0.153171 2024-11-06T03:33:35Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:33:35Z- TNavPF::First Term of Map Covariance Matrix is 0.57 2024-11-06T03:33:35Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T03:33:35Z- TerrainNav::Resampling particle filter... 2024-11-06T03:33:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864014.56 sec. 2024-11-06T03:33:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:33:35Z- variances([0], [2], and psi_berg) = 0.207456 0.372584 0.000000 2024-11-06T03:33:35Z- publishEstimatesDecoder() - at 1730864014.96 sec 2024-11-06T03:33:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:33:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061792.90 594390.71 56.77 2024-11-06T03:33:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061792.70 594391.72 55.63 lat/lon: 36.70 -121.94 2024-11-06T03:33:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.37 0.60 0.00 2024-11-06T03:33:35Z- AHRS_M2 msg: 1730864015.35 epoch sec; seqNo:6168 2024-11-06T03:33:35Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 4.19 psi 2024-11-06T03:33:35Z- handleDepth()- Depth_Keller msg: 1730864015.37 epoch sec; seqNo:6572; depth 50.39 2024-11-06T03:33:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864015.37 epoch sec; seqNo:6572; 4061764.27 north; 594406.57 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:33:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864014.96 2024-11-06T03:33:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:35Z- waiting for 1730864018.96, time is 1730864015.37 2024-11-06T03:33:35Z- AHRS_M2 msg: 1730864015.74 epoch sec; seqNo:6169 2024-11-06T03:33:35Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 4.19 psi 2024-11-06T03:33:35Z- handleDepth()- Depth_Keller msg: 1730864015.75 epoch sec; seqNo:6573; depth 50.38 2024-11-06T03:33:35Z- handleDVL() - DVL msg epochMillisec: 1730864015.78 2024-11-06T03:33:35Z- handleDvl() - RDI_Pathfinder msg: 1730864015.78 sec; seqNo:2251; #beams:4 2024-11-06T03:33:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.35 , 26.92 , 30.18 , 31.67 2024-11-06T03:33:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , -0.04 2024-11-06T03:33:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864015.79 epoch sec; seqNo:6573; 4061764.27 north; 594406.57 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:33:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864014.96 2024-11-06T03:33:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:35Z- waiting for 1730864018.96, time is 1730864015.79 2024-11-06T03:33:36Z- handleDepth()- Depth_Keller msg: 1730864016.17 epoch sec; seqNo:6574; depth 50.37 2024-11-06T03:33:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864016.18 epoch sec; seqNo:6574; 4061763.89 north; 594406.57 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:33:36Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:33:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864014.96 2024-11-06T03:33:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:36Z- waiting for 1730864018.96, time is 1730864016.18 2024-11-06T03:33:36Z- AHRS_M2 msg: 1730864016.56 epoch sec; seqNo:6170 2024-11-06T03:33:36Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 4.19 psi 2024-11-06T03:33:36Z- handleDepth()- Depth_Keller msg: 1730864016.57 epoch sec; seqNo:6575; depth 50.35 2024-11-06T03:33:36Z- handleDVL() - DVL msg epochMillisec: 1730864016.60 2024-11-06T03:33:36Z- handleDvl() - RDI_Pathfinder msg: 1730864016.60 sec; seqNo:2252; #beams:4 2024-11-06T03:33:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.90 , 27.05 , 29.91 , 32.35 2024-11-06T03:33:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , -0.04 2024-11-06T03:33:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864016.65 epoch sec; seqNo:6575; 4061763.87 north; 594405.35 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:33:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864014.96 2024-11-06T03:33:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:36Z- waiting for 1730864018.96, time is 1730864016.65 2024-11-06T03:33:36Z- AHRS_M2 msg: 1730864016.95 epoch sec; seqNo:6171 2024-11-06T03:33:36Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 4.19 psi 2024-11-06T03:33:36Z- handleDepth()- Depth_Keller msg: 1730864016.96 epoch sec; seqNo:6576; depth 50.34 2024-11-06T03:33:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864016.97 epoch sec; seqNo:6576; 4061763.49 north; 594405.35 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:33:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864014.96 2024-11-06T03:33:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:37Z- waiting for 1730864018.96, time is 1730864016.97 2024-11-06T03:33:37Z- AHRS_M2 msg: 1730864017.36 epoch sec; seqNo:6172 2024-11-06T03:33:37Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 4.19 psi 2024-11-06T03:33:37Z- handleDepth()- Depth_Keller msg: 1730864017.36 epoch sec; seqNo:6577; depth 50.32 2024-11-06T03:33:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864017.37 epoch sec; seqNo:6577; 4061763.49 north; 594405.35 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:33:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864014.96 2024-11-06T03:33:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:37Z- waiting for 1730864018.96, time is 1730864017.37 2024-11-06T03:33:37Z- handleDepth()- Depth_Keller msg: 1730864017.78 epoch sec; seqNo:6578; depth 50.31 2024-11-06T03:33:37Z- handleDVL() - DVL msg epochMillisec: 1730864017.80 2024-11-06T03:33:37Z- handleDvl() - RDI_Pathfinder msg: 1730864017.80 sec; seqNo:2253; #beams:4 2024-11-06T03:33:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.49 , 27.05 , 30.04 , 30.86 2024-11-06T03:33:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , -0.03 2024-11-06T03:33:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864017.83 epoch sec; seqNo:6578; 4061763.49 north; 594405.35 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:33:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864014.96 2024-11-06T03:33:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:37Z- waiting for 1730864018.96, time is 1730864017.83 2024-11-06T03:33:38Z- AHRS_M2 msg: 1730864018.17 epoch sec; seqNo:6173 2024-11-06T03:33:38Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.19 psi 2024-11-06T03:33:38Z- handleDepth()- Depth_Keller msg: 1730864018.18 epoch sec; seqNo:6579; depth 50.30 2024-11-06T03:33:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864018.20 epoch sec; seqNo:6579; 4061763.10 north; 594404.14 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:33:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864014.96 2024-11-06T03:33:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:38Z- waiting for 1730864018.96, time is 1730864018.20 2024-11-06T03:33:38Z- AHRS_M2 msg: 1730864018.57 epoch sec; seqNo:6174 2024-11-06T03:33:38Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.19 psi 2024-11-06T03:33:38Z- handleDepth()- Depth_Keller msg: 1730864018.57 epoch sec; seqNo:6580; depth 50.31 2024-11-06T03:33:38Z- handleDVL() - DVL msg epochMillisec: 1730864018.62 2024-11-06T03:33:38Z- handleDvl() - RDI_Pathfinder msg: 1730864018.62 sec; seqNo:2254; #beams:4 2024-11-06T03:33:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.76 , 26.92 , 30.31 , 30.45 2024-11-06T03:33:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , -0.02 2024-11-06T03:33:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864018.64 epoch sec; seqNo:6580; 4061763.10 north; 594404.14 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:33:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864014.96 2024-11-06T03:33:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:38Z- waiting for 1730864018.96, time is 1730864018.64 2024-11-06T03:33:39Z- handleDepth()- Depth_Keller msg: 1730864018.99 epoch sec; seqNo:6581; depth 50.27 2024-11-06T03:33:39Z- handleDVL() - DVL msg epochMillisec: 1730864019.01 2024-11-06T03:33:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:33:39Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:33:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:33:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:33:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:33:39Z- handleDvl() - RDI_Pathfinder msg: 1730864019.01 sec; seqNo:2255; #beams:0 2024-11-06T03:33:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:33:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , -0.02 2024-11-06T03:33:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864019.04 epoch sec; seqNo:6581; 4061762.72 north; 594404.14 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:33:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864014.96 2024-11-06T03:33:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:39Z- waiting for 1730864018.96, time is 1730864019.04 2024-11-06T03:33:39Z- LcmTrn::updateTrn() >>>> pose time 1730864019.04, meas time 1730864019.01 <<<< 2024-11-06T03:33:39Z- LcmTrn::updateTrn() equating pose 1730864019.01 and meas times 1730864019.01 2024-11-06T03:33:39Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730864019.01 2024-11-06T03:33:39Z- TerrainNav::motionUpdate() - currEstimate = (4061763, 594404.14, 50.27) 2024-11-06T03:33:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:33:39Z- TerrainNav:: SubcloudNIS is 0.48, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:33:39Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:33:39Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:33:39Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:33:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:33:39Z- variances([0], [2], and psi_berg) = 0.210051 0.375830 0.000000 2024-11-06T03:33:39Z- publishEstimatesDecoder() - at 1730864019.01 sec 2024-11-06T03:33:39Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:33:39Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061790.97 594386.96 57.11 2024-11-06T03:33:39Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061790.76 594388.08 55.50 lat/lon: 36.70 -121.94 2024-11-06T03:33:39Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.38 0.87 0.00 2024-11-06T03:33:39Z- AHRS_M2 msg: 1730864019.38 epoch sec; seqNo:6175 2024-11-06T03:33:39Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.19 psi 2024-11-06T03:33:39Z- handleDepth()- Depth_Keller msg: 1730864019.39 epoch sec; seqNo:6582; depth 50.28 2024-11-06T03:33:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864019.40 epoch sec; seqNo:6582; 4061762.71 north; 594402.92 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:33:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864019.01 2024-11-06T03:33:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:39Z- waiting for 1730864023.01, time is 1730864019.40 2024-11-06T03:33:39Z- AHRS_M2 msg: 1730864019.78 epoch sec; seqNo:6176 2024-11-06T03:33:39Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.18 psi 2024-11-06T03:33:39Z- handleDepth()- Depth_Keller msg: 1730864019.80 epoch sec; seqNo:6583; depth 50.27 2024-11-06T03:33:39Z- handleDVL() - DVL msg epochMillisec: 1730864019.82 2024-11-06T03:33:39Z- handleDvl() - RDI_Pathfinder msg: 1730864019.82 sec; seqNo:2256; #beams:4 2024-11-06T03:33:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.36 , 27.05 , 30.18 , 30.05 2024-11-06T03:33:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.11 , 0.00 2024-11-06T03:33:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864019.83 epoch sec; seqNo:6583; 4061762.33 north; 594402.93 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:33:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864019.01 2024-11-06T03:33:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:39Z- waiting for 1730864023.01, time is 1730864019.83 2024-11-06T03:33:40Z- AHRS_M2 msg: 1730864020.19 epoch sec; seqNo:6177 2024-11-06T03:33:40Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.18 psi 2024-11-06T03:33:40Z- handleDepth()- Depth_Keller msg: 1730864020.19 epoch sec; seqNo:6584; depth 50.27 2024-11-06T03:33:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864020.21 epoch sec; seqNo:6584; 4061762.33 north; 594402.93 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:33:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864019.01 2024-11-06T03:33:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:40Z- waiting for 1730864023.01, time is 1730864020.21 2024-11-06T03:33:40Z- AHRS_M2 msg: 1730864020.60 epoch sec; seqNo:6178 2024-11-06T03:33:40Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.18 psi 2024-11-06T03:33:40Z- handleDepth()- Depth_Keller msg: 1730864020.61 epoch sec; seqNo:6585; depth 50.27 2024-11-06T03:33:40Z- handleDVL() - DVL msg epochMillisec: 1730864020.65 2024-11-06T03:33:40Z- handleDvl() - RDI_Pathfinder msg: 1730864020.65 sec; seqNo:2257; #beams:4 2024-11-06T03:33:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.27 , 27.05 , 30.86 , 27.33 2024-11-06T03:33:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.08 , 0.03 2024-11-06T03:33:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864020.66 epoch sec; seqNo:6585; 4061762.33 north; 594402.93 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:33:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864019.01 2024-11-06T03:33:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:40Z- waiting for 1730864023.01, time is 1730864020.66 2024-11-06T03:33:41Z- AHRS_M2 msg: 1730864021.00 epoch sec; seqNo:6179 2024-11-06T03:33:41Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.18 psi 2024-11-06T03:33:41Z- handleDepth()- Depth_Keller msg: 1730864021.02 epoch sec; seqNo:6586; depth 50.26 2024-11-06T03:33:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864021.03 epoch sec; seqNo:6586; 4061761.94 north; 594401.71 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:33:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864019.01 2024-11-06T03:33:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:41Z- waiting for 1730864023.01, time is 1730864021.03 2024-11-06T03:33:41Z- AHRS_M2 msg: 1730864021.41 epoch sec; seqNo:6180 2024-11-06T03:33:41Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 4.17 psi 2024-11-06T03:33:41Z- handleDepth()- Depth_Keller msg: 1730864021.41 epoch sec; seqNo:6587; depth 50.26 2024-11-06T03:33:41Z- handleDVL() - DVL msg epochMillisec: 1730864021.42 2024-11-06T03:33:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:33:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:33:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:33:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:33:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:33:41Z- handleDvl() - RDI_Pathfinder msg: 1730864021.42 sec; seqNo:2258; #beams:0 2024-11-06T03:33:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:33:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.08 , 0.03 2024-11-06T03:33:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864021.43 epoch sec; seqNo:6587; 4061761.94 north; 594401.71 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:33:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864019.01 2024-11-06T03:33:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:41Z- waiting for 1730864023.01, time is 1730864021.43 2024-11-06T03:33:41Z- AHRS_M2 msg: 1730864021.81 epoch sec; seqNo:6181 2024-11-06T03:33:41Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 4.17 psi 2024-11-06T03:33:41Z- handleDepth()- Depth_Keller msg: 1730864021.82 epoch sec; seqNo:6588; depth 50.25 2024-11-06T03:33:41Z- handleDVL() - DVL msg epochMillisec: 1730864021.85 2024-11-06T03:33:41Z- handleDvl() - RDI_Pathfinder msg: 1730864021.85 sec; seqNo:2259; #beams:4 2024-11-06T03:33:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.41 , 27.33 , 17.13 , 27.87 2024-11-06T03:33:41Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.05 , 0.02 2024-11-06T03:33:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864021.86 epoch sec; seqNo:6588; 4061761.56 north; 594401.71 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:33:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864019.01 2024-11-06T03:33:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:41Z- waiting for 1730864023.01, time is 1730864021.86 2024-11-06T03:33:42Z- AHRS_M2 msg: 1730864022.21 epoch sec; seqNo:6182 2024-11-06T03:33:42Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 4.17 psi 2024-11-06T03:33:42Z- handleDepth()- Depth_Keller msg: 1730864022.22 epoch sec; seqNo:6589; depth 50.25 2024-11-06T03:33:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864022.23 epoch sec; seqNo:6589; 4061761.55 north; 594400.49 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:33:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864019.01 2024-11-06T03:33:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:42Z- waiting for 1730864023.01, time is 1730864022.23 2024-11-06T03:33:42Z- AHRS_M2 msg: 1730864022.63 epoch sec; seqNo:6183 2024-11-06T03:33:42Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 4.17 psi 2024-11-06T03:33:42Z- handleDepth()- Depth_Keller msg: 1730864022.64 epoch sec; seqNo:6590; depth 50.24 2024-11-06T03:33:42Z- handleDVL() - DVL msg epochMillisec: 1730864022.68 2024-11-06T03:33:42Z- handleDvl() - RDI_Pathfinder msg: 1730864022.68 sec; seqNo:2260; #beams:4 2024-11-06T03:33:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.64 , 27.48 , 31.29 , 27.72 2024-11-06T03:33:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , 0.05 2024-11-06T03:33:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864022.69 epoch sec; seqNo:6590; 4061761.17 north; 594400.50 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:33:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864019.01 2024-11-06T03:33:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:42Z- waiting for 1730864023.01, time is 1730864022.69 2024-11-06T03:33:43Z- AHRS_M2 msg: 1730864023.02 epoch sec; seqNo:6184 2024-11-06T03:33:43Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 4.17 psi 2024-11-06T03:33:43Z- handleDepth()- Depth_Keller msg: 1730864023.03 epoch sec; seqNo:6591; depth 50.22 2024-11-06T03:33:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864023.05 epoch sec; seqNo:6591; 4061761.17 north; 594400.50 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:33:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864019.01 2024-11-06T03:33:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:43Z- waiting for 1730864023.01, time is 1730864023.05 2024-11-06T03:33:43Z- LcmTrn::updateTrn() >>>> pose time 1730864023.05, meas time 1730864022.68 <<<< 2024-11-06T03:33:43Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864023.05 2024-11-06T03:33:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864022.68 sec, ping # 0; waiting for INS data more recent than 1730864019.01... 2024-11-06T03:33:43Z- TerrainNav::motionUpdate() - currEstimate = (4061761, 594400.50, 50.22) 2024-11-06T03:33:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:33:43Z- TerrainNav:: SubcloudNIS is 0.48, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:33:43Z- TNavPF::Projecting Measurements 2024-11-06T03:33:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:33:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:33:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:33:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:33:43Z- TNavFilter::4 good beams. 2024-11-06T03:33:43Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:33:43Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:33:43Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:33:43Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:33:43Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:33:43Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:33:43Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:33:43Z- TerrainMapOctree: (vehN, vehE ) = (4061761.31, 594400.83). 2024-11-06T03:33:43Z- TerrainMapOctree: Min Distance = 151.26. 2024-11-06T03:33:43Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:33:43Z- Forcing Subcloud Comparison 2024-11-06T03:33:43Z- Weighting particles with subcloud comparison 2024-11-06T03:33:43Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:43Z- meanExpectedMeasDiff: -0.174547 delta_rms_squared: 2.530747 alpha: 0.992159 2024-11-06T03:33:43Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:43Z- meanExpectedMeasDiff: 2.106053 delta_rms_squared: 1.297772 alpha: 0.984823 2024-11-06T03:33:43Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:43Z- meanExpectedMeasDiff: 1.280848 delta_rms_squared: 1.545255 alpha: 0.987223 2024-11-06T03:33:43Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:43Z- meanExpectedMeasDiff: -1.981566 delta_rms_squared: 1.831952 alpha: 0.989201 2024-11-06T03:33:43Z- Subcloud NIS: 0.493921 2024-11-06T03:33:43Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:33:43Z- TNavPF:: sumWeights = 0.106658 2024-11-06T03:33:43Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:33:43Z- TNavPF::First Term of Map Covariance Matrix is 0.10 2024-11-06T03:33:43Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T03:33:43Z- TerrainNav::Resampling particle filter... 2024-11-06T03:33:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864022.68 sec. 2024-11-06T03:33:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:33:43Z- variances([0], [2], and psi_berg) = 0.253803 0.373119 0.000000 2024-11-06T03:33:43Z- publishEstimatesDecoder() - at 1730864023.05 sec 2024-11-06T03:33:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:33:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061789.41 594383.41 54.91 2024-11-06T03:33:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061789.13 594384.33 55.31 lat/lon: 36.70 -121.94 2024-11-06T03:33:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.37 0.62 0.00 2024-11-06T03:33:43Z- AHRS_M2 msg: 1730864023.43 epoch sec; seqNo:6185 2024-11-06T03:33:43Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 4.17 psi 2024-11-06T03:33:43Z- handleDepth()- Depth_Keller msg: 1730864023.43 epoch sec; seqNo:6592; depth 50.22 2024-11-06T03:33:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864023.45 epoch sec; seqNo:6592; 4061760.79 north; 594400.50 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:33:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864023.05 2024-11-06T03:33:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:43Z- waiting for 1730864027.05, time is 1730864023.45 2024-11-06T03:33:43Z- AHRS_M2 msg: 1730864023.83 epoch sec; seqNo:6186 2024-11-06T03:33:43Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 4.17 psi 2024-11-06T03:33:43Z- handleDepth()- Depth_Keller msg: 1730864023.84 epoch sec; seqNo:6593; depth 50.21 2024-11-06T03:33:43Z- handleDVL() - DVL msg epochMillisec: 1730864023.88 2024-11-06T03:33:43Z- handleDvl() - RDI_Pathfinder msg: 1730864023.88 sec; seqNo:2261; #beams:4 2024-11-06T03:33:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.81 , 27.46 , 31.41 , 28.14 2024-11-06T03:33:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.04 2024-11-06T03:33:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864023.89 epoch sec; seqNo:6593; 4061760.78 north; 594399.28 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:33:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864023.05 2024-11-06T03:33:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:43Z- waiting for 1730864027.05, time is 1730864023.89 2024-11-06T03:33:44Z- AHRS_M2 msg: 1730864024.24 epoch sec; seqNo:6187 2024-11-06T03:33:44Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 4.16 psi 2024-11-06T03:33:44Z- handleDepth()- Depth_Keller msg: 1730864024.25 epoch sec; seqNo:6594; depth 50.20 2024-11-06T03:33:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864024.26 epoch sec; seqNo:6594; 4061760.40 north; 594399.29 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:33:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864023.05 2024-11-06T03:33:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:44Z- waiting for 1730864027.05, time is 1730864024.26 2024-11-06T03:33:44Z- AHRS_M2 msg: 1730864024.65 epoch sec; seqNo:6188 2024-11-06T03:33:44Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 4.16 psi 2024-11-06T03:33:44Z- handleDepth()- Depth_Keller msg: 1730864024.67 epoch sec; seqNo:6595; depth 50.20 2024-11-06T03:33:44Z- handleDVL() - DVL msg epochMillisec: 1730864024.69 2024-11-06T03:33:44Z- handleDvl() - RDI_Pathfinder msg: 1730864024.69 sec; seqNo:2262; #beams:4 2024-11-06T03:33:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.68 , 27.46 , 32.63 , 27.87 2024-11-06T03:33:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.02 2024-11-06T03:33:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864024.71 epoch sec; seqNo:6595; 4061760.02 north; 594399.29 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:33:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864023.05 2024-11-06T03:33:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:44Z- waiting for 1730864027.05, time is 1730864024.71 2024-11-06T03:33:45Z- handleDepth()- Depth_Keller msg: 1730864025.06 epoch sec; seqNo:6596; depth 50.17 2024-11-06T03:33:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864025.08 epoch sec; seqNo:6596; 4061760.02 north; 594399.29 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:33:45Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:33:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864023.05 2024-11-06T03:33:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:45Z- waiting for 1730864027.05, time is 1730864025.08 2024-11-06T03:33:45Z- AHRS_M2 msg: 1730864025.45 epoch sec; seqNo:6189 2024-11-06T03:33:45Z- AHRS_M2 msg: 0.11 phi; 0.04 theta; 4.16 psi 2024-11-06T03:33:45Z- handleDepth()- Depth_Keller msg: 1730864025.46 epoch sec; seqNo:6597; depth 50.16 2024-11-06T03:33:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864025.47 epoch sec; seqNo:6597; 4061759.63 north; 594398.07 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:33:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864023.05 2024-11-06T03:33:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:45Z- waiting for 1730864027.05, time is 1730864025.47 2024-11-06T03:33:45Z- AHRS_M2 msg: 1730864025.85 epoch sec; seqNo:6190 2024-11-06T03:33:45Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 4.17 psi 2024-11-06T03:33:45Z- handleDepth()- Depth_Keller msg: 1730864025.87 epoch sec; seqNo:6598; depth 50.14 2024-11-06T03:33:45Z- handleDVL() - DVL msg epochMillisec: 1730864025.88 2024-11-06T03:33:45Z- handleDvl() - RDI_Pathfinder msg: 1730864025.88 sec; seqNo:2263; #beams:4 2024-11-06T03:33:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.54 , 27.59 , 33.71 , 28.27 2024-11-06T03:33:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.07 , 0.00 2024-11-06T03:33:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864025.90 epoch sec; seqNo:6598; 4061759.63 north; 594398.07 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:33:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864023.05 2024-11-06T03:33:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:46Z- waiting for 1730864027.05, time is 1730864025.90 2024-11-06T03:33:46Z- AHRS_M2 msg: 1730864026.26 epoch sec; seqNo:6191 2024-11-06T03:33:46Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 4.17 psi 2024-11-06T03:33:46Z- handleDepth()- Depth_Keller msg: 1730864026.26 epoch sec; seqNo:6599; depth 50.11 2024-11-06T03:33:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864026.28 epoch sec; seqNo:6599; 4061759.25 north; 594398.08 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:33:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864023.05 2024-11-06T03:33:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:46Z- waiting for 1730864027.05, time is 1730864026.28 2024-11-06T03:33:46Z- handleDepth()- Depth_Keller msg: 1730864026.70 epoch sec; seqNo:6600; depth 50.10 2024-11-06T03:33:46Z- handleDVL() - DVL msg epochMillisec: 1730864026.72 2024-11-06T03:33:46Z- handleDvl() - RDI_Pathfinder msg: 1730864026.72 sec; seqNo:2264; #beams:4 2024-11-06T03:33:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.54 , 27.32 , 34.66 , 28.14 2024-11-06T03:33:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , -0.03 2024-11-06T03:33:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864026.73 epoch sec; seqNo:6600; 4061759.25 north; 594398.08 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:33:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864023.05 2024-11-06T03:33:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:46Z- waiting for 1730864027.05, time is 1730864026.73 2024-11-06T03:33:47Z- AHRS_M2 msg: 1730864027.06 epoch sec; seqNo:6192 2024-11-06T03:33:47Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 4.16 psi 2024-11-06T03:33:47Z- handleDepth()- Depth_Keller msg: 1730864027.07 epoch sec; seqNo:6601; depth 50.06 2024-11-06T03:33:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864027.08 epoch sec; seqNo:6601; 4061758.87 north; 594398.08 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:33:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864023.05 2024-11-06T03:33:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:47Z- waiting for 1730864027.05, time is 1730864027.08 2024-11-06T03:33:47Z- LcmTrn::updateTrn() >>>> pose time 1730864027.08, meas time 1730864026.72 <<<< 2024-11-06T03:33:47Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864027.08 2024-11-06T03:33:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864026.72 sec, ping # 0; waiting for INS data more recent than 1730864023.05... 2024-11-06T03:33:47Z- TerrainNav::motionUpdate() - currEstimate = (4061759, 594398.08, 50.06) 2024-11-06T03:33:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:33:47Z- TerrainNav:: SubcloudNIS is 0.49, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:33:47Z- TNavPF::Projecting Measurements 2024-11-06T03:33:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:33:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:33:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:33:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:33:47Z- TNavFilter::4 good beams. 2024-11-06T03:33:47Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:33:47Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:33:47Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:33:47Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:33:47Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:33:47Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:33:47Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:33:47Z- TerrainMapOctree: (vehN, vehE ) = (4061759.08, 594398.30). 2024-11-06T03:33:47Z- TerrainMapOctree: Min Distance = 148.58. 2024-11-06T03:33:47Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:33:47Z- Forcing Subcloud Comparison 2024-11-06T03:33:47Z- Weighting particles with subcloud comparison 2024-11-06T03:33:47Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:47Z- meanExpectedMeasDiff: -0.500465 delta_rms_squared: 1.977222 alpha: 0.989986 2024-11-06T03:33:47Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:47Z- meanExpectedMeasDiff: 0.416270 delta_rms_squared: 1.145425 alpha: 0.982839 2024-11-06T03:33:47Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:47Z- meanExpectedMeasDiff: 4.727950 delta_rms_squared: 1.404244 alpha: 0.985957 2024-11-06T03:33:47Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:47Z- meanExpectedMeasDiff: -0.903208 delta_rms_squared: 1.098301 alpha: 0.982116 2024-11-06T03:33:47Z- Subcloud NIS: 0.505292 2024-11-06T03:33:47Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:33:47Z- TNavPF:: sumWeights = 0.032497 2024-11-06T03:33:47Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:33:47Z- TNavPF::First Term of Map Covariance Matrix is 0.05 2024-11-06T03:33:47Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:33:47Z- TerrainNav::Resampling particle filter... 2024-11-06T03:33:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864026.72 sec. 2024-11-06T03:33:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:33:47Z- variances([0], [2], and psi_berg) = 0.226814 0.359112 0.000000 2024-11-06T03:33:47Z- publishEstimatesDecoder() - at 1730864027.08 sec 2024-11-06T03:33:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:33:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061787.10 594381.00 54.94 2024-11-06T03:33:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061787.00 594381.76 54.79 lat/lon: 36.70 -121.94 2024-11-06T03:33:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.36 0.59 0.00 2024-11-06T03:33:47Z- AHRS_M2 msg: 1730864027.47 epoch sec; seqNo:6193 2024-11-06T03:33:47Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 4.16 psi 2024-11-06T03:33:47Z- handleDepth()- Depth_Keller msg: 1730864027.47 epoch sec; seqNo:6602; depth 50.04 2024-11-06T03:33:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864027.48 epoch sec; seqNo:6602; 4061758.85 north; 594396.86 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:33:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864027.08 2024-11-06T03:33:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:47Z- waiting for 1730864031.08, time is 1730864027.48 2024-11-06T03:33:47Z- AHRS_M2 msg: 1730864027.87 epoch sec; seqNo:6194 2024-11-06T03:33:47Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 4.16 psi 2024-11-06T03:33:47Z- handleDepth()- Depth_Keller msg: 1730864027.88 epoch sec; seqNo:6603; depth 50.02 2024-11-06T03:33:47Z- handleDVL() - DVL msg epochMillisec: 1730864027.91 2024-11-06T03:33:47Z- handleDvl() - RDI_Pathfinder msg: 1730864027.91 sec; seqNo:2265; #beams:4 2024-11-06T03:33:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.81 , 27.73 , 34.12 , 28.55 2024-11-06T03:33:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , -0.04 2024-11-06T03:33:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864027.92 epoch sec; seqNo:6603; 4061758.48 north; 594396.87 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:33:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864027.08 2024-11-06T03:33:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:48Z- waiting for 1730864031.08, time is 1730864027.92 2024-11-06T03:33:48Z- AHRS_M2 msg: 1730864028.28 epoch sec; seqNo:6195 2024-11-06T03:33:48Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 4.16 psi 2024-11-06T03:33:48Z- handleDepth()- Depth_Keller msg: 1730864028.28 epoch sec; seqNo:6604; depth 50.00 2024-11-06T03:33:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864028.29 epoch sec; seqNo:6604; 4061758.48 north; 594396.87 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:33:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864027.08 2024-11-06T03:33:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:48Z- waiting for 1730864031.08, time is 1730864028.29 2024-11-06T03:33:48Z- AHRS_M2 msg: 1730864028.71 epoch sec; seqNo:6196 2024-11-06T03:33:48Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 4.16 psi 2024-11-06T03:33:48Z- handleDepth()- Depth_Keller msg: 1730864028.72 epoch sec; seqNo:6605; depth 49.95 2024-11-06T03:33:48Z- handleDVL() - DVL msg epochMillisec: 1730864028.74 2024-11-06T03:33:48Z- handleDvl() - RDI_Pathfinder msg: 1730864028.74 sec; seqNo:2266; #beams:4 2024-11-06T03:33:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.22 , 27.46 , 33.85 , 29.09 2024-11-06T03:33:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , -0.04 2024-11-06T03:33:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864028.76 epoch sec; seqNo:6605; 4061758.10 north; 594396.87 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:33:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864027.08 2024-11-06T03:33:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:48Z- waiting for 1730864031.08, time is 1730864028.76 2024-11-06T03:33:49Z- AHRS_M2 msg: 1730864029.08 epoch sec; seqNo:6197 2024-11-06T03:33:49Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.16 psi 2024-11-06T03:33:49Z- handleDepth()- Depth_Keller msg: 1730864029.09 epoch sec; seqNo:6606; depth 49.94 2024-11-06T03:33:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864029.11 epoch sec; seqNo:6606; 4061758.08 north; 594395.65 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:33:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864027.08 2024-11-06T03:33:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:49Z- waiting for 1730864031.08, time is 1730864029.11 2024-11-06T03:33:49Z- AHRS_M2 msg: 1730864029.49 epoch sec; seqNo:6198 2024-11-06T03:33:49Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.16 psi 2024-11-06T03:33:49Z- handleDepth()- Depth_Keller msg: 1730864029.50 epoch sec; seqNo:6607; depth 49.90 2024-11-06T03:33:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864029.51 epoch sec; seqNo:6607; 4061757.70 north; 594395.65 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:33:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864027.08 2024-11-06T03:33:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:49Z- waiting for 1730864031.08, time is 1730864029.51 2024-11-06T03:33:49Z- AHRS_M2 msg: 1730864029.89 epoch sec; seqNo:6199 2024-11-06T03:33:49Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.16 psi 2024-11-06T03:33:49Z- handleDepth()- Depth_Keller msg: 1730864029.90 epoch sec; seqNo:6608; depth 49.88 2024-11-06T03:33:49Z- handleDVL() - DVL msg epochMillisec: 1730864029.92 2024-11-06T03:33:49Z- handleDvl() - RDI_Pathfinder msg: 1730864029.92 sec; seqNo:2267; #beams:4 2024-11-06T03:33:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.36 , 29.50 , 34.40 , 29.37 2024-11-06T03:33:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , -0.03 2024-11-06T03:33:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864029.94 epoch sec; seqNo:6608; 4061757.70 north; 594395.65 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:33:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864027.08 2024-11-06T03:33:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:50Z- waiting for 1730864031.08, time is 1730864029.94 2024-11-06T03:33:50Z- AHRS_M2 msg: 1730864030.30 epoch sec; seqNo:6200 2024-11-06T03:33:50Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.16 psi 2024-11-06T03:33:50Z- handleDepth()- Depth_Keller msg: 1730864030.30 epoch sec; seqNo:6609; depth 49.85 2024-11-06T03:33:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864030.31 epoch sec; seqNo:6609; 4061757.33 north; 594395.66 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:33:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864027.08 2024-11-06T03:33:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:50Z- waiting for 1730864031.08, time is 1730864030.31 2024-11-06T03:33:50Z- AHRS_M2 msg: 1730864030.73 epoch sec; seqNo:6201 2024-11-06T03:33:50Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.16 psi 2024-11-06T03:33:50Z- handleDepth()- Depth_Keller msg: 1730864030.74 epoch sec; seqNo:6610; depth 49.82 2024-11-06T03:33:50Z- handleDVL() - DVL msg epochMillisec: 1730864030.77 2024-11-06T03:33:50Z- handleDvl() - RDI_Pathfinder msg: 1730864030.77 sec; seqNo:2268; #beams:4 2024-11-06T03:33:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.42 , 28.91 , 34.26 , 29.98 2024-11-06T03:33:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.06 , -0.02 2024-11-06T03:33:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864030.78 epoch sec; seqNo:6610; 4061757.31 north; 594394.44 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:33:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864027.08 2024-11-06T03:33:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:50Z- waiting for 1730864031.08, time is 1730864030.78 2024-11-06T03:33:51Z- AHRS_M2 msg: 1730864031.10 epoch sec; seqNo:6202 2024-11-06T03:33:51Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.16 psi 2024-11-06T03:33:51Z- handleDepth()- Depth_Keller msg: 1730864031.11 epoch sec; seqNo:6611; depth 49.79 2024-11-06T03:33:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864031.14 epoch sec; seqNo:6611; 4061757.31 north; 594394.44 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:33:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864027.08 2024-11-06T03:33:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:51Z- waiting for 1730864031.08, time is 1730864031.14 2024-11-06T03:33:51Z- LcmTrn::updateTrn() >>>> pose time 1730864031.14, meas time 1730864030.77 <<<< 2024-11-06T03:33:51Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864031.14 2024-11-06T03:33:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864030.77 sec, ping # 0; waiting for INS data more recent than 1730864027.08... 2024-11-06T03:33:51Z- TerrainNav::motionUpdate() - currEstimate = (4061757, 594394.44, 49.79) 2024-11-06T03:33:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:33:51Z- TerrainNav:: SubcloudNIS is 0.51, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:33:51Z- TNavPF::Projecting Measurements 2024-11-06T03:33:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:33:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:33:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:33:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:33:51Z- TNavFilter::4 good beams. 2024-11-06T03:33:51Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:33:51Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:33:51Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:33:51Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:33:51Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:33:51Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:33:51Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:33:51Z- TerrainMapOctree: (vehN, vehE ) = (4061757.45, 594394.77). 2024-11-06T03:33:51Z- TerrainMapOctree: Min Distance = 144.96. 2024-11-06T03:33:51Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:33:51Z- Forcing Subcloud Comparison 2024-11-06T03:33:51Z- Weighting particles with subcloud comparison 2024-11-06T03:33:51Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:51Z- meanExpectedMeasDiff: -2.241272 delta_rms_squared: 2.724112 alpha: 0.992712 2024-11-06T03:33:51Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:51Z- meanExpectedMeasDiff: 0.285873 delta_rms_squared: 1.129435 alpha: 0.982600 2024-11-06T03:33:51Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:51Z- meanExpectedMeasDiff: 3.218916 delta_rms_squared: 1.133286 alpha: 0.982658 2024-11-06T03:33:51Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:51Z- meanExpectedMeasDiff: 0.157072 delta_rms_squared: 1.119478 alpha: 0.982448 2024-11-06T03:33:51Z- Subcloud NIS: 0.512160 2024-11-06T03:33:51Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:33:51Z- TNavPF:: sumWeights = 0.073675 2024-11-06T03:33:51Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:33:51Z- TNavPF::First Term of Map Covariance Matrix is 0.34 2024-11-06T03:33:51Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T03:33:51Z- TerrainNav::Resampling particle filter... 2024-11-06T03:33:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864030.77 sec. 2024-11-06T03:33:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:33:51Z- variances([0], [2], and psi_berg) = 0.198867 0.409388 0.000000 2024-11-06T03:33:51Z- publishEstimatesDecoder() - at 1730864031.14 sec 2024-11-06T03:33:51Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:33:51Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061785.56 594377.32 53.56 2024-11-06T03:33:51Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061785.57 594378.14 54.50 lat/lon: 36.70 -121.94 2024-11-06T03:33:51Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.20 0.41 0.57 0.00 2024-11-06T03:33:51Z- AHRS_M2 msg: 1730864031.51 epoch sec; seqNo:6203 2024-11-06T03:33:51Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.16 psi 2024-11-06T03:33:51Z- handleDepth()- Depth_Keller msg: 1730864031.52 epoch sec; seqNo:6612; depth 49.77 2024-11-06T03:33:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864031.53 epoch sec; seqNo:6612; 4061756.93 north; 594394.44 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:33:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864031.14 2024-11-06T03:33:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:51Z- waiting for 1730864035.14, time is 1730864031.53 2024-11-06T03:33:51Z- AHRS_M2 msg: 1730864031.91 epoch sec; seqNo:6204 2024-11-06T03:33:51Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.15 psi 2024-11-06T03:33:51Z- handleDepth()- Depth_Keller msg: 1730864031.93 epoch sec; seqNo:6613; depth 49.75 2024-11-06T03:33:51Z- handleDVL() - DVL msg epochMillisec: 1730864031.94 2024-11-06T03:33:51Z- handleDvl() - RDI_Pathfinder msg: 1730864031.94 sec; seqNo:2269; #beams:4 2024-11-06T03:33:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.73 , 28.75 , 35.18 , 29.52 2024-11-06T03:33:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.07 , -0.01 2024-11-06T03:33:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864031.96 epoch sec; seqNo:6613; 4061756.92 north; 594393.22 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:33:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864031.14 2024-11-06T03:33:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:52Z- waiting for 1730864035.14, time is 1730864031.96 2024-11-06T03:33:52Z- handleDepth()- Depth_Keller msg: 1730864032.33 epoch sec; seqNo:6614; depth 49.73 2024-11-06T03:33:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864032.35 epoch sec; seqNo:6614; 4061756.54 north; 594393.23 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T03:33:52Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:33:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864031.14 2024-11-06T03:33:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:52Z- waiting for 1730864035.14, time is 1730864032.35 2024-11-06T03:33:52Z- handleDepth()- Depth_Keller msg: 1730864032.77 epoch sec; seqNo:6615; depth 49.70 2024-11-06T03:33:52Z- handleDVL() - DVL msg epochMillisec: 1730864032.80 2024-11-06T03:33:52Z- handleDvl() - RDI_Pathfinder msg: 1730864032.80 sec; seqNo:2270; #beams:4 2024-11-06T03:33:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.73 , 30.13 , 34.72 , 29.52 2024-11-06T03:33:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.07 , 0.01 2024-11-06T03:33:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864032.82 epoch sec; seqNo:6615; 4061756.54 north; 594393.23 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:33:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864031.14 2024-11-06T03:33:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:52Z- waiting for 1730864035.14, time is 1730864032.82 2024-11-06T03:33:53Z- AHRS_M2 msg: 1730864033.13 epoch sec; seqNo:6205 2024-11-06T03:33:53Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.15 psi 2024-11-06T03:33:53Z- handleDepth()- Depth_Keller msg: 1730864033.13 epoch sec; seqNo:6616; depth 49.70 2024-11-06T03:33:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864033.15 epoch sec; seqNo:6616; 4061756.16 north; 594393.23 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:33:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864031.14 2024-11-06T03:33:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:53Z- waiting for 1730864035.14, time is 1730864033.15 2024-11-06T03:33:53Z- AHRS_M2 msg: 1730864033.53 epoch sec; seqNo:6206 2024-11-06T03:33:53Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.14 psi 2024-11-06T03:33:53Z- handleDepth()- Depth_Keller msg: 1730864033.53 epoch sec; seqNo:6617; depth 49.68 2024-11-06T03:33:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864033.54 epoch sec; seqNo:6617; 4061756.15 north; 594392.01 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:33:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864031.14 2024-11-06T03:33:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:53Z- waiting for 1730864035.14, time is 1730864033.54 2024-11-06T03:33:53Z- AHRS_M2 msg: 1730864033.93 epoch sec; seqNo:6207 2024-11-06T03:33:53Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.14 psi 2024-11-06T03:33:53Z- handleDepth()- Depth_Keller msg: 1730864033.95 epoch sec; seqNo:6618; depth 49.66 2024-11-06T03:33:53Z- handleDVL() - DVL msg epochMillisec: 1730864033.97 2024-11-06T03:33:53Z- handleDvl() - RDI_Pathfinder msg: 1730864033.97 sec; seqNo:2271; #beams:4 2024-11-06T03:33:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.73 , 30.28 , 35.64 , 29.67 2024-11-06T03:33:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.06 , 0.02 2024-11-06T03:33:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864033.98 epoch sec; seqNo:6618; 4061755.77 north; 594392.01 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:33:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864031.14 2024-11-06T03:33:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:54Z- waiting for 1730864035.14, time is 1730864033.98 2024-11-06T03:33:54Z- AHRS_M2 msg: 1730864034.34 epoch sec; seqNo:6208 2024-11-06T03:33:54Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.14 psi 2024-11-06T03:33:54Z- handleDepth()- Depth_Keller msg: 1730864034.34 epoch sec; seqNo:6619; depth 49.65 2024-11-06T03:33:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864034.35 epoch sec; seqNo:6619; 4061755.77 north; 594392.01 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:33:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864031.14 2024-11-06T03:33:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:54Z- waiting for 1730864035.14, time is 1730864034.35 2024-11-06T03:33:54Z- AHRS_M2 msg: 1730864034.78 epoch sec; seqNo:6209 2024-11-06T03:33:54Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.14 psi 2024-11-06T03:33:54Z- handleDepth()- Depth_Keller msg: 1730864034.79 epoch sec; seqNo:6620; depth 49.64 2024-11-06T03:33:54Z- handleDVL() - DVL msg epochMillisec: 1730864034.81 2024-11-06T03:33:54Z- handleDvl() - RDI_Pathfinder msg: 1730864034.81 sec; seqNo:2272; #beams:4 2024-11-06T03:33:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.88 , 30.90 , 17.28 , 29.37 2024-11-06T03:33:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.65 , 0.05 , 0.11 2024-11-06T03:33:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864034.83 epoch sec; seqNo:6620; 4061755.39 north; 594392.02 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:33:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864031.14 2024-11-06T03:33:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:54Z- waiting for 1730864035.14, time is 1730864034.83 2024-11-06T03:33:55Z- AHRS_M2 msg: 1730864035.15 epoch sec; seqNo:6210 2024-11-06T03:33:55Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.14 psi 2024-11-06T03:33:55Z- handleDepth()- Depth_Keller msg: 1730864035.15 epoch sec; seqNo:6621; depth 49.63 2024-11-06T03:33:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864035.17 epoch sec; seqNo:6621; 4061755.38 north; 594390.80 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:33:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864031.14 2024-11-06T03:33:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:55Z- waiting for 1730864035.14, time is 1730864035.17 2024-11-06T03:33:55Z- LcmTrn::updateTrn() >>>> pose time 1730864035.17, meas time 1730864034.81 <<<< 2024-11-06T03:33:55Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864035.17 2024-11-06T03:33:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864034.81 sec, ping # 0; waiting for INS data more recent than 1730864031.14... 2024-11-06T03:33:55Z- TerrainNav::motionUpdate() - currEstimate = (4061755, 594390.80, 49.63) 2024-11-06T03:33:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:33:55Z- TerrainNav:: SubcloudNIS is 0.51, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:33:55Z- TNavPF::Projecting Measurements 2024-11-06T03:33:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:33:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:33:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:33:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:33:55Z- TNavFilter::4 good beams. 2024-11-06T03:33:55Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:33:55Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:33:55Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:33:55Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:33:55Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:33:55Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:33:55Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:33:55Z- TerrainMapOctree: (vehN, vehE ) = (4061755.55, 594391.13). 2024-11-06T03:33:55Z- TerrainMapOctree: Min Distance = 141.24. 2024-11-06T03:33:55Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:33:55Z- Forcing Subcloud Comparison 2024-11-06T03:33:55Z- Weighting particles with subcloud comparison 2024-11-06T03:33:55Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:55Z- meanExpectedMeasDiff: -1.626815 delta_rms_squared: 1.873680 alpha: 0.989439 2024-11-06T03:33:55Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:55Z- meanExpectedMeasDiff: 1.290244 delta_rms_squared: 0.928935 alpha: 0.978924 2024-11-06T03:33:55Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:55Z- meanExpectedMeasDiff: -13.197898 delta_rms_squared: 0.769856 alpha: 0.974679 2024-11-06T03:33:55Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:55Z- meanExpectedMeasDiff: 0.292209 delta_rms_squared: 1.144206 alpha: 0.982821 2024-11-06T03:33:55Z- Subcloud NIS: 0.563432 2024-11-06T03:33:55Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:33:55Z- TNavPF:: sumWeights = 0.000000 2024-11-06T03:33:55Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:33:55Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:33:55Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:33:55Z- TerrainNav::Resampling particle filter... 2024-11-06T03:33:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864034.81 sec. 2024-11-06T03:33:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:33:55Z- variances([0], [2], and psi_berg) = 0.154546 0.394799 0.000000 2024-11-06T03:33:55Z- publishEstimatesDecoder() - at 1730864035.17 sec 2024-11-06T03:33:55Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:33:55Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061783.46 594373.69 54.65 2024-11-06T03:33:55Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061782.85 594374.15 55.94 lat/lon: 36.70 -121.94 2024-11-06T03:33:55Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.39 0.53 0.00 2024-11-06T03:33:55Z- AHRS_M2 msg: 1730864035.55 epoch sec; seqNo:6211 2024-11-06T03:33:55Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.13 psi 2024-11-06T03:33:55Z- handleDepth()- Depth_Keller msg: 1730864035.56 epoch sec; seqNo:6622; depth 49.63 2024-11-06T03:33:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864035.57 epoch sec; seqNo:6622; 4061755.38 north; 594390.80 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:33:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864035.17 2024-11-06T03:33:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:55Z- waiting for 1730864039.17, time is 1730864035.57 2024-11-06T03:33:55Z- AHRS_M2 msg: 1730864035.96 epoch sec; seqNo:6212 2024-11-06T03:33:55Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 4.13 psi 2024-11-06T03:33:55Z- handleDepth()- Depth_Keller msg: 1730864035.96 epoch sec; seqNo:6623; depth 49.61 2024-11-06T03:33:56Z- handleDVL() - DVL msg epochMillisec: 1730864036.00 2024-11-06T03:33:56Z- handleDvl() - RDI_Pathfinder msg: 1730864036.00 sec; seqNo:2273; #beams:4 2024-11-06T03:33:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.44 , 31.00 , 36.57 , 29.77 2024-11-06T03:33:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , 0.05 2024-11-06T03:33:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864036.01 epoch sec; seqNo:6623; 4061755.00 north; 594390.80 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:33:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864035.17 2024-11-06T03:33:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:56Z- waiting for 1730864039.17, time is 1730864036.01 2024-11-06T03:33:56Z- AHRS_M2 msg: 1730864036.36 epoch sec; seqNo:6213 2024-11-06T03:33:56Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 4.13 psi 2024-11-06T03:33:56Z- handleDepth()- Depth_Keller msg: 1730864036.36 epoch sec; seqNo:6624; depth 49.61 2024-11-06T03:33:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864036.38 epoch sec; seqNo:6624; 4061755.00 north; 594390.80 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:33:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864035.17 2024-11-06T03:33:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:56Z- waiting for 1730864039.17, time is 1730864036.38 2024-11-06T03:33:56Z- AHRS_M2 msg: 1730864036.80 epoch sec; seqNo:6214 2024-11-06T03:33:56Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 4.13 psi 2024-11-06T03:33:56Z- handleDepth()- Depth_Keller msg: 1730864036.81 epoch sec; seqNo:6625; depth 49.60 2024-11-06T03:33:56Z- handleDVL() - DVL msg epochMillisec: 1730864036.83 2024-11-06T03:33:56Z- handleDvl() - RDI_Pathfinder msg: 1730864036.83 sec; seqNo:2274; #beams:4 2024-11-06T03:33:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.04 , 30.90 , 37.47 , 29.21 2024-11-06T03:33:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , 0.05 2024-11-06T03:33:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864036.85 epoch sec; seqNo:6625; 4061754.61 north; 594389.59 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:33:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864035.17 2024-11-06T03:33:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:56Z- waiting for 1730864039.17, time is 1730864036.85 2024-11-06T03:33:57Z- handleDepth()- Depth_Keller msg: 1730864037.18 epoch sec; seqNo:6626; depth 49.60 2024-11-06T03:33:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864037.20 epoch sec; seqNo:6626; 4061754.61 north; 594389.59 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:33:57Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:33:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864035.17 2024-11-06T03:33:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:57Z- waiting for 1730864039.17, time is 1730864037.20 2024-11-06T03:33:57Z- AHRS_M2 msg: 1730864037.57 epoch sec; seqNo:6215 2024-11-06T03:33:57Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 4.12 psi 2024-11-06T03:33:57Z- handleDepth()- Depth_Keller msg: 1730864037.58 epoch sec; seqNo:6627; depth 49.59 2024-11-06T03:33:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864037.59 epoch sec; seqNo:6627; 4061754.23 north; 594389.59 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:33:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864035.17 2024-11-06T03:33:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:57Z- waiting for 1730864039.17, time is 1730864037.59 2024-11-06T03:33:57Z- AHRS_M2 msg: 1730864037.97 epoch sec; seqNo:6216 2024-11-06T03:33:57Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 4.12 psi 2024-11-06T03:33:57Z- handleDepth()- Depth_Keller msg: 1730864037.98 epoch sec; seqNo:6628; depth 49.58 2024-11-06T03:33:58Z- handleDVL() - DVL msg epochMillisec: 1730864038.01 2024-11-06T03:33:58Z- handleDvl() - RDI_Pathfinder msg: 1730864038.01 sec; seqNo:2275; #beams:4 2024-11-06T03:33:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.04 , 31.05 , 37.62 , 29.67 2024-11-06T03:33:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.05 2024-11-06T03:33:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864038.02 epoch sec; seqNo:6628; 4061754.23 north; 594389.59 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:33:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864035.17 2024-11-06T03:33:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:58Z- waiting for 1730864039.17, time is 1730864038.02 2024-11-06T03:33:58Z- AHRS_M2 msg: 1730864038.38 epoch sec; seqNo:6217 2024-11-06T03:33:58Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 4.12 psi 2024-11-06T03:33:58Z- handleDepth()- Depth_Keller msg: 1730864038.38 epoch sec; seqNo:6629; depth 49.57 2024-11-06T03:33:58Z- handleDVL() - DVL msg epochMillisec: 1730864038.38 2024-11-06T03:33:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:33:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:33:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:33:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:33:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:33:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:33:58Z- handleDvl() - RDI_Pathfinder msg: 1730864038.38 sec; seqNo:2276; #beams:0 2024-11-06T03:33:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:33:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.05 2024-11-06T03:33:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864038.40 epoch sec; seqNo:6629; 4061753.84 north; 594388.37 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:33:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864035.17 2024-11-06T03:33:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:58Z- waiting for 1730864039.17, time is 1730864038.40 2024-11-06T03:33:58Z- AHRS_M2 msg: 1730864038.85 epoch sec; seqNo:6218 2024-11-06T03:33:58Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 4.13 psi 2024-11-06T03:33:58Z- handleDepth()- Depth_Keller msg: 1730864038.86 epoch sec; seqNo:6630; depth 49.56 2024-11-06T03:33:58Z- handleDVL() - DVL msg epochMillisec: 1730864038.88 2024-11-06T03:33:58Z- handleDvl() - RDI_Pathfinder msg: 1730864038.88 sec; seqNo:2277; #beams:4 2024-11-06T03:33:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.04 , 31.51 , 37.62 , 29.98 2024-11-06T03:33:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.04 2024-11-06T03:33:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864038.91 epoch sec; seqNo:6630; 4061753.46 north; 594388.38 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:33:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:33:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864035.17 2024-11-06T03:33:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:59Z- waiting for 1730864039.17, time is 1730864038.91 2024-11-06T03:33:59Z- handleDepth()- Depth_Keller msg: 1730864039.20 epoch sec; seqNo:6631; depth 49.56 2024-11-06T03:33:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864039.22 epoch sec; seqNo:6631; 4061753.46 north; 594388.38 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:33:59Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:33:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864035.17 2024-11-06T03:33:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:59Z- waiting for 1730864039.17, time is 1730864039.22 2024-11-06T03:33:59Z- LcmTrn::updateTrn() >>>> pose time 1730864039.22, meas time 1730864038.88 <<<< 2024-11-06T03:33:59Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864039.22 2024-11-06T03:33:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864038.88 sec, ping # 0; waiting for INS data more recent than 1730864035.17... 2024-11-06T03:33:59Z- TerrainNav::motionUpdate() - currEstimate = (4061753, 594388.38, 49.56) 2024-11-06T03:33:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:33:59Z- TerrainNav:: SubcloudNIS is 0.56, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:33:59Z- TNavPF::Projecting Measurements 2024-11-06T03:33:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:33:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:33:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:33:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:33:59Z- TNavFilter::4 good beams. 2024-11-06T03:33:59Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:33:59Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:33:59Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:33:59Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:33:59Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:33:59Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:33:59Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:33:59Z- TerrainMapOctree: (vehN, vehE ) = (4061753.62, 594388.58). 2024-11-06T03:33:59Z- TerrainMapOctree: Min Distance = 138.63. 2024-11-06T03:33:59Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:33:59Z- Forcing Subcloud Comparison 2024-11-06T03:33:59Z- Weighting particles with subcloud comparison 2024-11-06T03:33:59Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:59Z- meanExpectedMeasDiff: 0.661177 delta_rms_squared: 2.172084 alpha: 0.990876 2024-11-06T03:33:59Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:59Z- meanExpectedMeasDiff: 3.600685 delta_rms_squared: 0.918073 alpha: 0.978680 2024-11-06T03:33:59Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:59Z- meanExpectedMeasDiff: 9.696879 delta_rms_squared: 1.064695 alpha: 0.981562 2024-11-06T03:33:59Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:33:59Z- meanExpectedMeasDiff: 1.252173 delta_rms_squared: 1.047884 alpha: 0.981271 2024-11-06T03:33:59Z- Subcloud NIS: 0.629988 2024-11-06T03:33:59Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:33:59Z- TNavPF:: sumWeights = 0.000028 2024-11-06T03:33:59Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:33:59Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:33:59Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:33:59Z- TerrainNav::Resampling particle filter... 2024-11-06T03:33:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864038.88 sec. 2024-11-06T03:33:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:33:59Z- variances([0], [2], and psi_berg) = 0.194852 0.258493 0.000000 2024-11-06T03:33:59Z- publishEstimatesDecoder() - at 1730864039.22 sec 2024-11-06T03:33:59Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:33:59Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061782.04 594371.12 54.54 2024-11-06T03:33:59Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061781.07 594371.56 54.26 lat/lon: 36.70 -121.94 2024-11-06T03:33:59Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.19 0.26 0.60 0.00 2024-11-06T03:33:59Z- AHRS_M2 msg: 1730864039.59 epoch sec; seqNo:6219 2024-11-06T03:33:59Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 4.13 psi 2024-11-06T03:33:59Z- handleDepth()- Depth_Keller msg: 1730864039.61 epoch sec; seqNo:6632; depth 49.53 2024-11-06T03:33:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864039.64 epoch sec; seqNo:6632; 4061753.08 north; 594388.38 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T03:33:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:33:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864039.22 2024-11-06T03:33:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:33:59Z- waiting for 1730864043.22, time is 1730864039.64 2024-11-06T03:34:00Z- AHRS_M2 msg: 1730864040.02 epoch sec; seqNo:6220 2024-11-06T03:34:00Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.13 psi 2024-11-06T03:34:00Z- handleDepth()- Depth_Keller msg: 1730864040.02 epoch sec; seqNo:6633; depth 49.53 2024-11-06T03:34:00Z- handleDVL() - DVL msg epochMillisec: 1730864040.05 2024-11-06T03:34:00Z- handleDvl() - RDI_Pathfinder msg: 1730864040.05 sec; seqNo:2278; #beams:4 2024-11-06T03:34:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.19 , 31.66 , 37.62 , 30.13 2024-11-06T03:34:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , 0.01 2024-11-06T03:34:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864040.07 epoch sec; seqNo:6633; 4061753.08 north; 594388.38 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T03:34:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864039.22 2024-11-06T03:34:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:00Z- waiting for 1730864043.22, time is 1730864040.07 2024-11-06T03:34:00Z- AHRS_M2 msg: 1730864040.39 epoch sec; seqNo:6221 2024-11-06T03:34:00Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.13 psi 2024-11-06T03:34:00Z- handleDepth()- Depth_Keller msg: 1730864040.40 epoch sec; seqNo:6634; depth 49.52 2024-11-06T03:34:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864040.41 epoch sec; seqNo:6634; 4061752.69 north; 594387.17 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T03:34:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864039.22 2024-11-06T03:34:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:00Z- waiting for 1730864043.22, time is 1730864040.41 2024-11-06T03:34:00Z- AHRS_M2 msg: 1730864040.80 epoch sec; seqNo:6222 2024-11-06T03:34:00Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 4.13 psi 2024-11-06T03:34:00Z- handleDepth()- Depth_Keller msg: 1730864040.81 epoch sec; seqNo:6635; depth 49.51 2024-11-06T03:34:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864040.82 epoch sec; seqNo:6635; 4061752.69 north; 594387.17 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T03:34:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864039.22 2024-11-06T03:34:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:00Z- waiting for 1730864043.22, time is 1730864040.82 2024-11-06T03:34:01Z- AHRS_M2 msg: 1730864041.20 epoch sec; seqNo:6223 2024-11-06T03:34:01Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 4.14 psi 2024-11-06T03:34:01Z- handleDepth()- Depth_Keller msg: 1730864041.21 epoch sec; seqNo:6636; depth 49.50 2024-11-06T03:34:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864041.22 epoch sec; seqNo:6636; 4061752.31 north; 594387.17 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:34:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864039.22 2024-11-06T03:34:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:01Z- waiting for 1730864043.22, time is 1730864041.22 2024-11-06T03:34:01Z- AHRS_M2 msg: 1730864041.62 epoch sec; seqNo:6224 2024-11-06T03:34:01Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 4.14 psi 2024-11-06T03:34:01Z- handleDepth()- Depth_Keller msg: 1730864041.63 epoch sec; seqNo:6637; depth 49.48 2024-11-06T03:34:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864041.64 epoch sec; seqNo:6637; 4061752.31 north; 594387.17 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:34:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864039.22 2024-11-06T03:34:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:01Z- waiting for 1730864043.22, time is 1730864041.64 2024-11-06T03:34:02Z- AHRS_M2 msg: 1730864042.07 epoch sec; seqNo:6225 2024-11-06T03:34:02Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 4.14 psi 2024-11-06T03:34:02Z- handleDepth()- Depth_Keller msg: 1730864042.15 epoch sec; seqNo:6638; depth 49.46 2024-11-06T03:34:02Z- handleDVL() - DVL msg epochMillisec: 1730864042.18 2024-11-06T03:34:02Z- handleDvl() - RDI_Pathfinder msg: 1730864042.18 sec; seqNo:2279; #beams:4 2024-11-06T03:34:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.95 , 31.51 , 37.78 , 30.28 2024-11-06T03:34:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.03 2024-11-06T03:34:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864042.20 epoch sec; seqNo:6638; 4061751.91 north; 594385.95 east; 36.70 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T03:34:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864039.22 2024-11-06T03:34:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:02Z- waiting for 1730864043.22, time is 1730864042.20 2024-11-06T03:34:02Z- AHRS_M2 msg: 1730864042.41 epoch sec; seqNo:6226 2024-11-06T03:34:02Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.15 psi 2024-11-06T03:34:02Z- handleDepth()- Depth_Keller msg: 1730864042.42 epoch sec; seqNo:6639; depth 49.44 2024-11-06T03:34:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864042.44 epoch sec; seqNo:6639; 4061751.91 north; 594385.95 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:34:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864039.22 2024-11-06T03:34:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:02Z- waiting for 1730864043.22, time is 1730864042.44 2024-11-06T03:34:02Z- AHRS_M2 msg: 1730864042.82 epoch sec; seqNo:6227 2024-11-06T03:34:02Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.15 psi 2024-11-06T03:34:02Z- handleDepth()- Depth_Keller msg: 1730864042.83 epoch sec; seqNo:6640; depth 49.44 2024-11-06T03:34:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864042.85 epoch sec; seqNo:6640; 4061751.54 north; 594385.96 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T03:34:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864039.22 2024-11-06T03:34:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:02Z- waiting for 1730864043.22, time is 1730864042.85 2024-11-06T03:34:03Z- AHRS_M2 msg: 1730864043.22 epoch sec; seqNo:6228 2024-11-06T03:34:03Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.15 psi 2024-11-06T03:34:03Z- handleDepth()- Depth_Keller msg: 1730864043.23 epoch sec; seqNo:6641; depth 49.45 2024-11-06T03:34:03Z- handleDVL() - DVL msg epochMillisec: 1730864043.23 2024-11-06T03:34:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:03Z- handleDvl() - RDI_Pathfinder msg: 1730864043.23 sec; seqNo:2280; #beams:0 2024-11-06T03:34:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.03 2024-11-06T03:34:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864043.24 epoch sec; seqNo:6641; 4061751.54 north; 594385.96 east; 36.70 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:34:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864039.22 2024-11-06T03:34:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:03Z- waiting for 1730864043.22, time is 1730864043.24 2024-11-06T03:34:03Z- LcmTrn::updateTrn() >>>> pose time 1730864043.24, meas time 1730864043.23 <<<< 2024-11-06T03:34:03Z- LcmTrn::updateTrn() equating pose 1730864043.23 and meas times 1730864043.23 2024-11-06T03:34:03Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730864043.23 2024-11-06T03:34:03Z- TerrainNav::motionUpdate() - currEstimate = (4061752, 594385.96, 49.45) 2024-11-06T03:34:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:34:03Z- TerrainNav:: SubcloudNIS is 0.63, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:34:03Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:34:03Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:34:03Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:34:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:34:03Z- variances([0], [2], and psi_berg) = 0.196410 0.259861 0.000000 2024-11-06T03:34:03Z- publishEstimatesDecoder() - at 1730864043.23 sec 2024-11-06T03:34:03Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:34:03Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061780.08 594368.71 54.09 2024-11-06T03:34:03Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061779.15 594369.14 54.15 lat/lon: 36.70 -121.94 2024-11-06T03:34:03Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.20 0.26 0.86 0.00 2024-11-06T03:34:03Z- AHRS_M2 msg: 1730864043.64 epoch sec; seqNo:6229 2024-11-06T03:34:03Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.15 psi 2024-11-06T03:34:03Z- handleDepth()- Depth_Keller msg: 1730864043.65 epoch sec; seqNo:6642; depth 49.43 2024-11-06T03:34:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864043.65 epoch sec; seqNo:6642; 4061751.14 north; 594384.74 east; 36.70 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T03:34:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864043.23 2024-11-06T03:34:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:03Z- waiting for 1730864047.23, time is 1730864043.65 2024-11-06T03:34:04Z- AHRS_M2 msg: 1730864044.03 epoch sec; seqNo:6230 2024-11-06T03:34:04Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.15 psi 2024-11-06T03:34:04Z- handleDepth()- Depth_Keller msg: 1730864044.06 epoch sec; seqNo:6643; depth 49.42 2024-11-06T03:34:04Z- handleDVL() - DVL msg epochMillisec: 1730864044.11 2024-11-06T03:34:04Z- handleDvl() - RDI_Pathfinder msg: 1730864044.11 sec; seqNo:2281; #beams:4 2024-11-06T03:34:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.41 , 31.66 , 37.62 , 30.13 2024-11-06T03:34:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.07 , -0.02 2024-11-06T03:34:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864044.12 epoch sec; seqNo:6643; 4061751.14 north; 594384.74 east; 36.70 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T03:34:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864043.23 2024-11-06T03:34:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:04Z- waiting for 1730864047.23, time is 1730864044.12 2024-11-06T03:34:04Z- AHRS_M2 msg: 1730864044.43 epoch sec; seqNo:6231 2024-11-06T03:34:04Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.15 psi 2024-11-06T03:34:04Z- handleDepth()- Depth_Keller msg: 1730864044.44 epoch sec; seqNo:6644; depth 49.40 2024-11-06T03:34:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864044.45 epoch sec; seqNo:6644; 4061750.76 north; 594384.75 east; 36.70 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T03:34:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864043.23 2024-11-06T03:34:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:04Z- waiting for 1730864047.23, time is 1730864044.45 2024-11-06T03:34:04Z- AHRS_M2 msg: 1730864044.84 epoch sec; seqNo:6232 2024-11-06T03:34:04Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.15 psi 2024-11-06T03:34:04Z- handleDepth()- Depth_Keller msg: 1730864044.85 epoch sec; seqNo:6645; depth 49.40 2024-11-06T03:34:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864044.86 epoch sec; seqNo:6645; 4061750.76 north; 594384.75 east; 36.70 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T03:34:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864043.23 2024-11-06T03:34:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:05Z- waiting for 1730864047.23, time is 1730864044.86 2024-11-06T03:34:05Z- AHRS_M2 msg: 1730864045.24 epoch sec; seqNo:6233 2024-11-06T03:34:05Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.15 psi 2024-11-06T03:34:05Z- handleDepth()- Depth_Keller msg: 1730864045.25 epoch sec; seqNo:6646; depth 49.39 2024-11-06T03:34:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864045.26 epoch sec; seqNo:6646; 4061750.37 north; 594383.53 east; 36.70 lat; -121.94 lon; 49.39 depth; utmZone: 10 2024-11-06T03:34:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864043.23 2024-11-06T03:34:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:05Z- waiting for 1730864047.23, time is 1730864045.26 2024-11-06T03:34:05Z- AHRS_M2 msg: 1730864045.65 epoch sec; seqNo:6234 2024-11-06T03:34:05Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.15 psi 2024-11-06T03:34:05Z- handleDepth()- Depth_Keller msg: 1730864045.65 epoch sec; seqNo:6647; depth 49.40 2024-11-06T03:34:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864045.66 epoch sec; seqNo:6647; 4061750.37 north; 594383.53 east; 36.70 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T03:34:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864043.23 2024-11-06T03:34:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:05Z- waiting for 1730864047.23, time is 1730864045.66 2024-11-06T03:34:06Z- AHRS_M2 msg: 1730864046.05 epoch sec; seqNo:6235 2024-11-06T03:34:06Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.14 psi 2024-11-06T03:34:06Z- handleDepth()- Depth_Keller msg: 1730864046.06 epoch sec; seqNo:6648; depth 49.39 2024-11-06T03:34:06Z- handleDVL() - DVL msg epochMillisec: 1730864046.09 2024-11-06T03:34:06Z- handleDvl() - RDI_Pathfinder msg: 1730864046.09 sec; seqNo:2282; #beams:4 2024-11-06T03:34:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.95 , 32.12 , 37.78 , 30.28 2024-11-06T03:34:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.08 , 0.04 2024-11-06T03:34:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864046.10 epoch sec; seqNo:6648; 4061749.99 north; 594383.53 east; 36.70 lat; -121.94 lon; 49.39 depth; utmZone: 10 2024-11-06T03:34:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864043.23 2024-11-06T03:34:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:06Z- waiting for 1730864047.23, time is 1730864046.10 2024-11-06T03:34:06Z- AHRS_M2 msg: 1730864046.45 epoch sec; seqNo:6236 2024-11-06T03:34:06Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.14 psi 2024-11-06T03:34:06Z- handleDepth()- Depth_Keller msg: 1730864046.47 epoch sec; seqNo:6649; depth 49.39 2024-11-06T03:34:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864046.48 epoch sec; seqNo:6649; 4061749.99 north; 594383.53 east; 36.70 lat; -121.94 lon; 49.39 depth; utmZone: 10 2024-11-06T03:34:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864043.23 2024-11-06T03:34:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:06Z- waiting for 1730864047.23, time is 1730864046.48 2024-11-06T03:34:06Z- AHRS_M2 msg: 1730864046.86 epoch sec; seqNo:6237 2024-11-06T03:34:06Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.14 psi 2024-11-06T03:34:06Z- handleDepth()- Depth_Keller msg: 1730864046.88 epoch sec; seqNo:6650; depth 49.40 2024-11-06T03:34:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864046.89 epoch sec; seqNo:6650; 4061749.60 north; 594382.32 east; 36.70 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T03:34:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864043.23 2024-11-06T03:34:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:07Z- waiting for 1730864047.23, time is 1730864046.89 2024-11-06T03:34:07Z- AHRS_M2 msg: 1730864047.26 epoch sec; seqNo:6238 2024-11-06T03:34:07Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.14 psi 2024-11-06T03:34:07Z- handleDepth()- Depth_Keller msg: 1730864047.27 epoch sec; seqNo:6651; depth 49.41 2024-11-06T03:34:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864047.28 epoch sec; seqNo:6651; 4061749.60 north; 594382.32 east; 36.70 lat; -121.94 lon; 49.41 depth; utmZone: 10 2024-11-06T03:34:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864043.23 2024-11-06T03:34:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:07Z- waiting for 1730864047.23, time is 1730864047.28 2024-11-06T03:34:07Z- LcmTrn::updateTrn() >>>> pose time 1730864047.28, meas time 1730864046.09 <<<< 2024-11-06T03:34:07Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864047.28 2024-11-06T03:34:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864046.09 sec, ping # 0; waiting for INS data more recent than 1730864043.23... 2024-11-06T03:34:07Z- TerrainNav::motionUpdate() - currEstimate = (4061750, 594382.32, 49.41) 2024-11-06T03:34:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:34:07Z- TerrainNav:: SubcloudNIS is 0.63, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:34:07Z- TNavPF::Projecting Measurements 2024-11-06T03:34:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:34:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:34:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:34:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:34:07Z- TNavFilter::4 good beams. 2024-11-06T03:34:07Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:34:07Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:34:07Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:34:07Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:34:07Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:34:07Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:34:07Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:34:07Z- TerrainMapOctree: (vehN, vehE ) = (4061750.17, 594383.39). 2024-11-06T03:34:07Z- TerrainMapOctree: Min Distance = 133.39. 2024-11-06T03:34:07Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:34:07Z- Forcing Subcloud Comparison 2024-11-06T03:34:07Z- Weighting particles with subcloud comparison 2024-11-06T03:34:07Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:07Z- meanExpectedMeasDiff: -1.400217 delta_rms_squared: 1.868554 alpha: 0.989410 2024-11-06T03:34:07Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:07Z- meanExpectedMeasDiff: 2.542061 delta_rms_squared: 1.279376 alpha: 0.984608 2024-11-06T03:34:07Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:07Z- meanExpectedMeasDiff: 8.159002 delta_rms_squared: 1.956855 alpha: 0.989883 2024-11-06T03:34:07Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:07Z- meanExpectedMeasDiff: -1.149438 delta_rms_squared: 1.557935 alpha: 0.987325 2024-11-06T03:34:07Z- Subcloud NIS: 0.638376 2024-11-06T03:34:07Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:34:07Z- TNavPF:: sumWeights = 0.000173 2024-11-06T03:34:07Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:34:07Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:34:07Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:34:07Z- TerrainNav::Resampling particle filter... 2024-11-06T03:34:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864046.09 sec. 2024-11-06T03:34:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:34:07Z- variances([0], [2], and psi_berg) = 0.292336 0.196425 0.000000 2024-11-06T03:34:07Z- publishEstimatesDecoder() - at 1730864047.28 sec 2024-11-06T03:34:07Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:34:07Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061778.19 594365.08 52.76 2024-11-06T03:34:07Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061777.38 594365.27 52.98 lat/lon: 36.70 -121.94 2024-11-06T03:34:07Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.29 0.20 0.65 0.00 2024-11-06T03:34:07Z- AHRS_M2 msg: 1730864047.67 epoch sec; seqNo:6239 2024-11-06T03:34:07Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.14 psi 2024-11-06T03:34:07Z- handleDepth()- Depth_Keller msg: 1730864047.68 epoch sec; seqNo:6652; depth 49.42 2024-11-06T03:34:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864047.70 epoch sec; seqNo:6652; 4061749.22 north; 594382.32 east; 36.70 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T03:34:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864047.28 2024-11-06T03:34:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:07Z- waiting for 1730864051.28, time is 1730864047.70 2024-11-06T03:34:08Z- AHRS_M2 msg: 1730864048.07 epoch sec; seqNo:6240 2024-11-06T03:34:08Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.14 psi 2024-11-06T03:34:08Z- handleDepth()- Depth_Keller msg: 1730864048.09 epoch sec; seqNo:6653; depth 49.41 2024-11-06T03:34:08Z- handleDVL() - DVL msg epochMillisec: 1730864048.11 2024-11-06T03:34:08Z- handleDvl() - RDI_Pathfinder msg: 1730864048.11 sec; seqNo:2283; #beams:4 2024-11-06T03:34:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.49 , 32.88 , 38.08 , 30.59 2024-11-06T03:34:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.05 2024-11-06T03:34:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864048.12 epoch sec; seqNo:6653; 4061749.21 north; 594381.10 east; 36.70 lat; -121.94 lon; 49.41 depth; utmZone: 10 2024-11-06T03:34:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864047.28 2024-11-06T03:34:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:08Z- waiting for 1730864051.28, time is 1730864048.12 2024-11-06T03:34:08Z- AHRS_M2 msg: 1730864048.47 epoch sec; seqNo:6241 2024-11-06T03:34:08Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.15 psi 2024-11-06T03:34:08Z- handleDepth()- Depth_Keller msg: 1730864048.48 epoch sec; seqNo:6654; depth 49.42 2024-11-06T03:34:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864048.50 epoch sec; seqNo:6654; 4061748.83 north; 594381.11 east; 36.70 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T03:34:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864047.28 2024-11-06T03:34:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:08Z- waiting for 1730864051.28, time is 1730864048.50 2024-11-06T03:34:08Z- AHRS_M2 msg: 1730864048.88 epoch sec; seqNo:6242 2024-11-06T03:34:08Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.15 psi 2024-11-06T03:34:08Z- handleDepth()- Depth_Keller msg: 1730864048.89 epoch sec; seqNo:6655; depth 49.42 2024-11-06T03:34:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864048.90 epoch sec; seqNo:6655; 4061748.83 north; 594381.11 east; 36.70 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T03:34:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864047.28 2024-11-06T03:34:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:09Z- waiting for 1730864051.28, time is 1730864048.90 2024-11-06T03:34:09Z- AHRS_M2 msg: 1730864049.28 epoch sec; seqNo:6243 2024-11-06T03:34:09Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.15 psi 2024-11-06T03:34:09Z- handleDepth()- Depth_Keller msg: 1730864049.29 epoch sec; seqNo:6656; depth 49.43 2024-11-06T03:34:09Z- handleDVL() - DVL msg epochMillisec: 1730864049.29 2024-11-06T03:34:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:09Z- handleDvl() - RDI_Pathfinder msg: 1730864049.29 sec; seqNo:2284; #beams:0 2024-11-06T03:34:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.05 2024-11-06T03:34:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864049.30 epoch sec; seqNo:6656; 4061748.83 north; 594381.11 east; 36.70 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T03:34:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864047.28 2024-11-06T03:34:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:09Z- waiting for 1730864051.28, time is 1730864049.30 2024-11-06T03:34:09Z- AHRS_M2 msg: 1730864049.69 epoch sec; seqNo:6244 2024-11-06T03:34:09Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.15 psi 2024-11-06T03:34:09Z- handleDepth()- Depth_Keller msg: 1730864049.70 epoch sec; seqNo:6657; depth 49.43 2024-11-06T03:34:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864049.71 epoch sec; seqNo:6657; 4061748.44 north; 594379.89 east; 36.70 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T03:34:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864047.28 2024-11-06T03:34:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:09Z- waiting for 1730864051.28, time is 1730864049.71 2024-11-06T03:34:10Z- AHRS_M2 msg: 1730864050.09 epoch sec; seqNo:6245 2024-11-06T03:34:10Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.15 psi 2024-11-06T03:34:10Z- handleDepth()- Depth_Keller msg: 1730864050.10 epoch sec; seqNo:6658; depth 49.43 2024-11-06T03:34:10Z- handleDVL() - DVL msg epochMillisec: 1730864050.13 2024-11-06T03:34:10Z- handleDvl() - RDI_Pathfinder msg: 1730864050.13 sec; seqNo:2285; #beams:4 2024-11-06T03:34:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.26 , 32.88 , 38.54 , 30.44 2024-11-06T03:34:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , 0.02 2024-11-06T03:34:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864050.15 epoch sec; seqNo:6658; 4061748.06 north; 594379.89 east; 36.70 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T03:34:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864047.28 2024-11-06T03:34:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:10Z- waiting for 1730864051.28, time is 1730864050.15 2024-11-06T03:34:10Z- AHRS_M2 msg: 1730864050.49 epoch sec; seqNo:6246 2024-11-06T03:34:10Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.15 psi 2024-11-06T03:34:10Z- handleDepth()- Depth_Keller msg: 1730864050.50 epoch sec; seqNo:6659; depth 49.45 2024-11-06T03:34:10Z- handleDVL() - DVL msg epochMillisec: 1730864050.50 2024-11-06T03:34:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:10Z- handleDvl() - RDI_Pathfinder msg: 1730864050.50 sec; seqNo:2286; #beams:0 2024-11-06T03:34:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , 0.02 2024-11-06T03:34:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864050.52 epoch sec; seqNo:6659; 4061748.06 north; 594379.89 east; 36.70 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:34:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864047.28 2024-11-06T03:34:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:10Z- waiting for 1730864051.28, time is 1730864050.52 2024-11-06T03:34:10Z- AHRS_M2 msg: 1730864050.91 epoch sec; seqNo:6247 2024-11-06T03:34:10Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.15 psi 2024-11-06T03:34:10Z- handleDepth()- Depth_Keller msg: 1730864050.92 epoch sec; seqNo:6660; depth 49.45 2024-11-06T03:34:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864050.94 epoch sec; seqNo:6660; 4061747.68 north; 594379.90 east; 36.70 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:34:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864047.28 2024-11-06T03:34:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:11Z- waiting for 1730864051.28, time is 1730864050.94 2024-11-06T03:34:11Z- AHRS_M2 msg: 1730864051.30 epoch sec; seqNo:6248 2024-11-06T03:34:11Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.15 psi 2024-11-06T03:34:11Z- handleDepth()- Depth_Keller msg: 1730864051.31 epoch sec; seqNo:6661; depth 49.47 2024-11-06T03:34:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864051.32 epoch sec; seqNo:6661; 4061747.67 north; 594378.68 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:34:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864047.28 2024-11-06T03:34:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:11Z- waiting for 1730864051.28, time is 1730864051.32 2024-11-06T03:34:11Z- LcmTrn::updateTrn() >>>> pose time 1730864051.32, meas time 1730864050.50 <<<< 2024-11-06T03:34:11Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864051.32 2024-11-06T03:34:11Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:34:11Z- TerrainNav::motionUpdate() - currEstimate = (4061748, 594378.68, 49.47) 2024-11-06T03:34:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:34:11Z- TerrainNav:: SubcloudNIS is 0.64, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:34:11Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:34:11Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:34:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:34:11Z- variances([0], [2], and psi_berg) = 0.295697 0.198821 0.000000 2024-11-06T03:34:11Z- publishEstimatesDecoder() - at 1730864051.32 sec 2024-11-06T03:34:11Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:34:11Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061776.31 594361.44 52.99 2024-11-06T03:34:11Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061775.44 594361.63 53.03 lat/lon: 36.70 -121.94 2024-11-06T03:34:11Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.30 0.20 0.91 0.00 2024-11-06T03:34:11Z- AHRS_M2 msg: 1730864051.71 epoch sec; seqNo:6249 2024-11-06T03:34:11Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.16 psi 2024-11-06T03:34:11Z- handleDepth()- Depth_Keller msg: 1730864051.71 epoch sec; seqNo:6662; depth 49.45 2024-11-06T03:34:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864051.72 epoch sec; seqNo:6662; 4061747.29 north; 594378.68 east; 36.70 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:34:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864051.32 2024-11-06T03:34:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:11Z- waiting for 1730864055.32, time is 1730864051.72 2024-11-06T03:34:12Z- AHRS_M2 msg: 1730864052.12 epoch sec; seqNo:6250 2024-11-06T03:34:12Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.16 psi 2024-11-06T03:34:12Z- handleDepth()- Depth_Keller msg: 1730864052.13 epoch sec; seqNo:6663; depth 49.46 2024-11-06T03:34:12Z- handleDVL() - DVL msg epochMillisec: 1730864052.16 2024-11-06T03:34:12Z- handleDvl() - RDI_Pathfinder msg: 1730864052.16 sec; seqNo:2287; #beams:4 2024-11-06T03:34:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.87 , 32.88 , 37.78 , 30.59 2024-11-06T03:34:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , 0.00 2024-11-06T03:34:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864052.17 epoch sec; seqNo:6663; 4061747.29 north; 594378.68 east; 36.70 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T03:34:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864051.32 2024-11-06T03:34:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:12Z- waiting for 1730864055.32, time is 1730864052.17 2024-11-06T03:34:12Z- AHRS_M2 msg: 1730864052.51 epoch sec; seqNo:6251 2024-11-06T03:34:12Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.16 psi 2024-11-06T03:34:12Z- handleDepth()- Depth_Keller msg: 1730864052.52 epoch sec; seqNo:6664; depth 49.45 2024-11-06T03:34:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864052.53 epoch sec; seqNo:6664; 4061746.91 north; 594378.69 east; 36.70 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T03:34:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864051.32 2024-11-06T03:34:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:12Z- waiting for 1730864055.32, time is 1730864052.53 2024-11-06T03:34:12Z- AHRS_M2 msg: 1730864052.92 epoch sec; seqNo:6252 2024-11-06T03:34:12Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.16 psi 2024-11-06T03:34:12Z- handleDepth()- Depth_Keller msg: 1730864052.93 epoch sec; seqNo:6665; depth 49.47 2024-11-06T03:34:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864052.93 epoch sec; seqNo:6665; 4061746.89 north; 594377.47 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:34:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864051.32 2024-11-06T03:34:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:13Z- waiting for 1730864055.32, time is 1730864052.93 2024-11-06T03:34:13Z- AHRS_M2 msg: 1730864053.32 epoch sec; seqNo:6253 2024-11-06T03:34:13Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.16 psi 2024-11-06T03:34:13Z- handleDepth()- Depth_Keller msg: 1730864053.33 epoch sec; seqNo:6666; depth 49.47 2024-11-06T03:34:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864053.35 epoch sec; seqNo:6666; 4061746.52 north; 594377.47 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:34:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864051.32 2024-11-06T03:34:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:13Z- waiting for 1730864055.32, time is 1730864053.35 2024-11-06T03:34:13Z- AHRS_M2 msg: 1730864053.73 epoch sec; seqNo:6254 2024-11-06T03:34:13Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.16 psi 2024-11-06T03:34:13Z- handleDepth()- Depth_Keller msg: 1730864053.73 epoch sec; seqNo:6667; depth 49.47 2024-11-06T03:34:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864053.76 epoch sec; seqNo:6667; 4061746.52 north; 594377.47 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:34:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864051.32 2024-11-06T03:34:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:13Z- waiting for 1730864055.32, time is 1730864053.76 2024-11-06T03:34:14Z- AHRS_M2 msg: 1730864054.13 epoch sec; seqNo:6255 2024-11-06T03:34:14Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.16 psi 2024-11-06T03:34:14Z- handleDepth()- Depth_Keller msg: 1730864054.14 epoch sec; seqNo:6668; depth 49.47 2024-11-06T03:34:14Z- handleDVL() - DVL msg epochMillisec: 1730864054.16 2024-11-06T03:34:14Z- handleDvl() - RDI_Pathfinder msg: 1730864054.16 sec; seqNo:2288; #beams:4 2024-11-06T03:34:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.79 , 33.49 , 38.69 , 30.74 2024-11-06T03:34:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.01 2024-11-06T03:34:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864054.19 epoch sec; seqNo:6668; 4061746.14 north; 594377.47 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T03:34:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864051.32 2024-11-06T03:34:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:14Z- waiting for 1730864055.32, time is 1730864054.19 2024-11-06T03:34:14Z- AHRS_M2 msg: 1730864054.54 epoch sec; seqNo:6256 2024-11-06T03:34:14Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.17 psi 2024-11-06T03:34:14Z- handleDepth()- Depth_Keller msg: 1730864054.54 epoch sec; seqNo:6669; depth 49.49 2024-11-06T03:34:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864054.56 epoch sec; seqNo:6669; 4061746.12 north; 594376.25 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:34:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864051.32 2024-11-06T03:34:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:14Z- waiting for 1730864055.32, time is 1730864054.56 2024-11-06T03:34:14Z- AHRS_M2 msg: 1730864054.94 epoch sec; seqNo:6257 2024-11-06T03:34:14Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.17 psi 2024-11-06T03:34:14Z- handleDepth()- Depth_Keller msg: 1730864054.95 epoch sec; seqNo:6670; depth 49.48 2024-11-06T03:34:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864054.96 epoch sec; seqNo:6670; 4061745.74 north; 594376.26 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:34:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864051.32 2024-11-06T03:34:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:15Z- waiting for 1730864055.32, time is 1730864054.96 2024-11-06T03:34:15Z- AHRS_M2 msg: 1730864055.34 epoch sec; seqNo:6258 2024-11-06T03:34:15Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.17 psi 2024-11-06T03:34:15Z- handleDepth()- Depth_Keller msg: 1730864055.35 epoch sec; seqNo:6671; depth 49.48 2024-11-06T03:34:15Z- handleDVL() - DVL msg epochMillisec: 1730864055.36 2024-11-06T03:34:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:15Z- handleDvl() - RDI_Pathfinder msg: 1730864055.36 sec; seqNo:2289; #beams:0 2024-11-06T03:34:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.01 2024-11-06T03:34:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864055.36 epoch sec; seqNo:6671; 4061745.74 north; 594376.26 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:34:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864051.32 2024-11-06T03:34:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:15Z- waiting for 1730864055.32, time is 1730864055.36 2024-11-06T03:34:15Z- LcmTrn::updateTrn() >>>> pose time 1730864055.36, meas time 1730864055.36 <<<< 2024-11-06T03:34:15Z- LcmTrn::updateTrn() equating pose 1730864055.36 and meas times 1730864055.36 2024-11-06T03:34:15Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730864055.36 2024-11-06T03:34:15Z- TerrainNav::motionUpdate() - currEstimate = (4061746, 594376.26, 49.48) 2024-11-06T03:34:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:34:15Z- TerrainNav:: SubcloudNIS is 0.64, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:34:15Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:34:15Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:34:15Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:34:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:34:15Z- variances([0], [2], and psi_berg) = 0.296524 0.200656 0.000000 2024-11-06T03:34:15Z- publishEstimatesDecoder() - at 1730864055.36 sec 2024-11-06T03:34:15Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:34:15Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061774.42 594359.02 54.14 2024-11-06T03:34:15Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061773.52 594359.21 53.05 lat/lon: 36.70 -121.94 2024-11-06T03:34:15Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.30 0.20 1.16 0.00 2024-11-06T03:34:15Z- AHRS_M2 msg: 1730864055.76 epoch sec; seqNo:6259 2024-11-06T03:34:15Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.17 psi 2024-11-06T03:34:15Z- handleDepth()- Depth_Keller msg: 1730864055.77 epoch sec; seqNo:6672; depth 49.48 2024-11-06T03:34:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864055.78 epoch sec; seqNo:6672; 4061745.37 north; 594376.26 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:34:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864055.36 2024-11-06T03:34:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:15Z- waiting for 1730864059.36, time is 1730864055.78 2024-11-06T03:34:16Z- AHRS_M2 msg: 1730864056.15 epoch sec; seqNo:6260 2024-11-06T03:34:16Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.17 psi 2024-11-06T03:34:16Z- handleDepth()- Depth_Keller msg: 1730864056.16 epoch sec; seqNo:6673; depth 49.48 2024-11-06T03:34:16Z- handleDVL() - DVL msg epochMillisec: 1730864056.19 2024-11-06T03:34:16Z- handleDvl() - RDI_Pathfinder msg: 1730864056.19 sec; seqNo:2290; #beams:4 2024-11-06T03:34:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.37 , 33.99 , 39.43 , 30.76 2024-11-06T03:34:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , 0.01 2024-11-06T03:34:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864056.20 epoch sec; seqNo:6673; 4061745.35 north; 594375.04 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:34:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864055.36 2024-11-06T03:34:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:16Z- waiting for 1730864059.36, time is 1730864056.20 2024-11-06T03:34:16Z- AHRS_M2 msg: 1730864056.56 epoch sec; seqNo:6261 2024-11-06T03:34:16Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.17 psi 2024-11-06T03:34:16Z- handleDepth()- Depth_Keller msg: 1730864056.57 epoch sec; seqNo:6674; depth 49.49 2024-11-06T03:34:16Z- handleDVL() - DVL msg epochMillisec: 1730864056.57 2024-11-06T03:34:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:16Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:16Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:16Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:16Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:16Z- handleDvl() - RDI_Pathfinder msg: 1730864056.57 sec; seqNo:2291; #beams:0 2024-11-06T03:34:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , 0.01 2024-11-06T03:34:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864056.59 epoch sec; seqNo:6674; 4061744.97 north; 594375.05 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:34:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864055.36 2024-11-06T03:34:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:16Z- waiting for 1730864059.36, time is 1730864056.59 2024-11-06T03:34:16Z- AHRS_M2 msg: 1730864056.97 epoch sec; seqNo:6262 2024-11-06T03:34:16Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.18 psi 2024-11-06T03:34:16Z- handleDepth()- Depth_Keller msg: 1730864056.98 epoch sec; seqNo:6675; depth 49.49 2024-11-06T03:34:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864056.99 epoch sec; seqNo:6675; 4061744.97 north; 594375.05 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:34:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864055.36 2024-11-06T03:34:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:17Z- waiting for 1730864059.36, time is 1730864056.99 2024-11-06T03:34:17Z- AHRS_M2 msg: 1730864057.36 epoch sec; seqNo:6263 2024-11-06T03:34:17Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.18 psi 2024-11-06T03:34:17Z- handleDepth()- Depth_Keller msg: 1730864057.37 epoch sec; seqNo:6676; depth 49.49 2024-11-06T03:34:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864057.38 epoch sec; seqNo:6676; 4061744.59 north; 594375.05 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T03:34:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864055.36 2024-11-06T03:34:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:17Z- waiting for 1730864059.36, time is 1730864057.38 2024-11-06T03:34:17Z- AHRS_M2 msg: 1730864057.77 epoch sec; seqNo:6264 2024-11-06T03:34:17Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.18 psi 2024-11-06T03:34:17Z- handleDepth()- Depth_Keller msg: 1730864057.78 epoch sec; seqNo:6677; depth 49.48 2024-11-06T03:34:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864057.80 epoch sec; seqNo:6677; 4061744.58 north; 594373.83 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T03:34:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864055.36 2024-11-06T03:34:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:17Z- waiting for 1730864059.36, time is 1730864057.80 2024-11-06T03:34:18Z- AHRS_M2 msg: 1730864058.17 epoch sec; seqNo:6265 2024-11-06T03:34:18Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.18 psi 2024-11-06T03:34:18Z- handleDepth()- Depth_Keller msg: 1730864058.18 epoch sec; seqNo:6678; depth 49.50 2024-11-06T03:34:18Z- handleDVL() - DVL msg epochMillisec: 1730864058.21 2024-11-06T03:34:18Z- handleDvl() - RDI_Pathfinder msg: 1730864058.21 sec; seqNo:2292; #beams:4 2024-11-06T03:34:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.39 , 34.16 , 39.94 , 30.42 2024-11-06T03:34:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.04 , 0.02 2024-11-06T03:34:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864058.24 epoch sec; seqNo:6678; 4061744.20 north; 594373.83 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:34:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864055.36 2024-11-06T03:34:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:18Z- waiting for 1730864059.36, time is 1730864058.24 2024-11-06T03:34:18Z- AHRS_M2 msg: 1730864058.57 epoch sec; seqNo:6266 2024-11-06T03:34:18Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.18 psi 2024-11-06T03:34:18Z- handleDepth()- Depth_Keller msg: 1730864058.58 epoch sec; seqNo:6679; depth 49.51 2024-11-06T03:34:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864058.61 epoch sec; seqNo:6679; 4061744.20 north; 594373.83 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T03:34:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864055.36 2024-11-06T03:34:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:18Z- waiting for 1730864059.36, time is 1730864058.61 2024-11-06T03:34:18Z- AHRS_M2 msg: 1730864058.98 epoch sec; seqNo:6267 2024-11-06T03:34:18Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.18 psi 2024-11-06T03:34:18Z- handleDepth()- Depth_Keller msg: 1730864058.99 epoch sec; seqNo:6680; depth 49.52 2024-11-06T03:34:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864059.00 epoch sec; seqNo:6680; 4061744.19 north; 594372.61 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T03:34:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864055.36 2024-11-06T03:34:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:19Z- waiting for 1730864059.36, time is 1730864059.00 2024-11-06T03:34:19Z- AHRS_M2 msg: 1730864059.39 epoch sec; seqNo:6268 2024-11-06T03:34:19Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.18 psi 2024-11-06T03:34:19Z- handleDepth()- Depth_Keller msg: 1730864059.40 epoch sec; seqNo:6681; depth 49.50 2024-11-06T03:34:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864059.41 epoch sec; seqNo:6681; 4061743.81 north; 594372.62 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T03:34:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864055.36 2024-11-06T03:34:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:19Z- waiting for 1730864059.36, time is 1730864059.41 2024-11-06T03:34:19Z- LcmTrn::updateTrn() >>>> pose time 1730864059.41, meas time 1730864058.21 <<<< 2024-11-06T03:34:19Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864059.41 2024-11-06T03:34:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864058.21 sec, ping # 0; waiting for INS data more recent than 1730864055.36... 2024-11-06T03:34:19Z- TerrainNav::motionUpdate() - currEstimate = (4061744, 594372.62, 49.50) 2024-11-06T03:34:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:34:19Z- TerrainNav:: SubcloudNIS is 0.64, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:34:19Z- TNavPF::Projecting Measurements 2024-11-06T03:34:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:34:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:34:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:34:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:34:19Z- TNavFilter::4 good beams. 2024-11-06T03:34:19Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:34:19Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:34:19Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:34:19Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:34:19Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:34:19Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:34:19Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:34:19Z- TerrainMapOctree: (vehN, vehE ) = (4061744.38, 594373.69). 2024-11-06T03:34:19Z- TerrainMapOctree: Min Distance = 123.82. 2024-11-06T03:34:19Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:34:19Z- Forcing Subcloud Comparison 2024-11-06T03:34:19Z- Weighting particles with subcloud comparison 2024-11-06T03:34:19Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:19Z- meanExpectedMeasDiff: 1.978628 delta_rms_squared: 2.315350 alpha: 0.991436 2024-11-06T03:34:19Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:19Z- meanExpectedMeasDiff: 3.440442 delta_rms_squared: 1.629350 alpha: 0.987874 2024-11-06T03:34:19Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:19Z- meanExpectedMeasDiff: 9.056611 delta_rms_squared: 2.042624 alpha: 0.990304 2024-11-06T03:34:19Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:19Z- meanExpectedMeasDiff: -2.459440 delta_rms_squared: 2.403755 alpha: 0.991748 2024-11-06T03:34:19Z- Subcloud NIS: 0.687650 2024-11-06T03:34:19Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:34:19Z- TNavPF:: sumWeights = 0.000026 2024-11-06T03:34:19Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:34:19Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:34:19Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:34:19Z- TerrainNav::Resampling particle filter... 2024-11-06T03:34:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864058.21 sec. 2024-11-06T03:34:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:34:19Z- variances([0], [2], and psi_berg) = 0.438638 0.161284 0.000000 2024-11-06T03:34:19Z- publishEstimatesDecoder() - at 1730864059.41 sec 2024-11-06T03:34:19Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:34:19Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061772.51 594355.39 52.99 2024-11-06T03:34:19Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061771.62 594355.51 51.58 lat/lon: 36.70 -121.94 2024-11-06T03:34:19Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.44 0.16 0.75 0.00 2024-11-06T03:34:19Z- AHRS_M2 msg: 1730864059.80 epoch sec; seqNo:6269 2024-11-06T03:34:19Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.18 psi 2024-11-06T03:34:19Z- handleDepth()- Depth_Keller msg: 1730864059.82 epoch sec; seqNo:6682; depth 49.53 2024-11-06T03:34:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864059.84 epoch sec; seqNo:6682; 4061743.81 north; 594372.62 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T03:34:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864059.41 2024-11-06T03:34:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:19Z- waiting for 1730864063.41, time is 1730864059.84 2024-11-06T03:34:20Z- AHRS_M2 msg: 1730864060.19 epoch sec; seqNo:6270 2024-11-06T03:34:20Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.18 psi 2024-11-06T03:34:20Z- handleDepth()- Depth_Keller msg: 1730864060.19 epoch sec; seqNo:6683; depth 49.53 2024-11-06T03:34:20Z- handleDVL() - DVL msg epochMillisec: 1730864060.22 2024-11-06T03:34:20Z- handleDvl() - RDI_Pathfinder msg: 1730864060.22 sec; seqNo:2293; #beams:4 2024-11-06T03:34:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.75 , 34.67 , 39.77 , 31.10 2024-11-06T03:34:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , 0.04 2024-11-06T03:34:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864060.24 epoch sec; seqNo:6683; 4061743.43 north; 594372.62 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T03:34:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864059.41 2024-11-06T03:34:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:20Z- waiting for 1730864063.41, time is 1730864060.24 2024-11-06T03:34:20Z- AHRS_M2 msg: 1730864060.60 epoch sec; seqNo:6271 2024-11-06T03:34:20Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.18 psi 2024-11-06T03:34:20Z- handleDepth()- Depth_Keller msg: 1730864060.60 epoch sec; seqNo:6684; depth 49.55 2024-11-06T03:34:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864060.63 epoch sec; seqNo:6684; 4061743.42 north; 594371.40 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:34:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864059.41 2024-11-06T03:34:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:20Z- waiting for 1730864063.41, time is 1730864060.63 2024-11-06T03:34:21Z- AHRS_M2 msg: 1730864061.00 epoch sec; seqNo:6272 2024-11-06T03:34:21Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.18 psi 2024-11-06T03:34:21Z- handleDepth()- Depth_Keller msg: 1730864061.01 epoch sec; seqNo:6685; depth 49.55 2024-11-06T03:34:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864061.02 epoch sec; seqNo:6685; 4061743.04 north; 594371.41 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T03:34:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864059.41 2024-11-06T03:34:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:21Z- waiting for 1730864063.41, time is 1730864061.02 2024-11-06T03:34:21Z- AHRS_M2 msg: 1730864061.41 epoch sec; seqNo:6273 2024-11-06T03:34:21Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.18 psi 2024-11-06T03:34:21Z- handleDepth()- Depth_Keller msg: 1730864061.42 epoch sec; seqNo:6686; depth 49.56 2024-11-06T03:34:21Z- handleDVL() - DVL msg epochMillisec: 1730864061.42 2024-11-06T03:34:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:21Z- handleDvl() - RDI_Pathfinder msg: 1730864061.42 sec; seqNo:2294; #beams:0 2024-11-06T03:34:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , 0.04 2024-11-06T03:34:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864061.43 epoch sec; seqNo:6686; 4061743.04 north; 594371.41 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T03:34:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864059.41 2024-11-06T03:34:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:21Z- waiting for 1730864063.41, time is 1730864061.43 2024-11-06T03:34:21Z- AHRS_M2 msg: 1730864061.81 epoch sec; seqNo:6274 2024-11-06T03:34:21Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.18 psi 2024-11-06T03:34:21Z- handleDepth()- Depth_Keller msg: 1730864061.81 epoch sec; seqNo:6687; depth 49.57 2024-11-06T03:34:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864061.83 epoch sec; seqNo:6687; 4061742.66 north; 594371.41 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:34:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864059.41 2024-11-06T03:34:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:21Z- waiting for 1730864063.41, time is 1730864061.83 2024-11-06T03:34:22Z- AHRS_M2 msg: 1730864062.21 epoch sec; seqNo:6275 2024-11-06T03:34:22Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.19 psi 2024-11-06T03:34:22Z- handleDepth()- Depth_Keller msg: 1730864062.22 epoch sec; seqNo:6688; depth 49.57 2024-11-06T03:34:22Z- handleDVL() - DVL msg epochMillisec: 1730864062.25 2024-11-06T03:34:22Z- handleDvl() - RDI_Pathfinder msg: 1730864062.25 sec; seqNo:2295; #beams:4 2024-11-06T03:34:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.92 , 35.52 , 40.28 , 31.44 2024-11-06T03:34:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , 0.05 2024-11-06T03:34:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864062.26 epoch sec; seqNo:6688; 4061742.65 north; 594370.19 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:34:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864059.41 2024-11-06T03:34:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:22Z- waiting for 1730864063.41, time is 1730864062.26 2024-11-06T03:34:22Z- AHRS_M2 msg: 1730864062.62 epoch sec; seqNo:6276 2024-11-06T03:34:22Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.19 psi 2024-11-06T03:34:22Z- handleDepth()- Depth_Keller msg: 1730864062.63 epoch sec; seqNo:6689; depth 49.59 2024-11-06T03:34:22Z- handleDVL() - DVL msg epochMillisec: 1730864062.63 2024-11-06T03:34:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:22Z- handleDvl() - RDI_Pathfinder msg: 1730864062.63 sec; seqNo:2296; #beams:0 2024-11-06T03:34:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , 0.05 2024-11-06T03:34:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864062.65 epoch sec; seqNo:6689; 4061742.27 north; 594370.19 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:34:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864059.41 2024-11-06T03:34:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:22Z- waiting for 1730864063.41, time is 1730864062.65 2024-11-06T03:34:23Z- AHRS_M2 msg: 1730864063.02 epoch sec; seqNo:6277 2024-11-06T03:34:23Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.19 psi 2024-11-06T03:34:23Z- handleDepth()- Depth_Keller msg: 1730864063.03 epoch sec; seqNo:6690; depth 49.58 2024-11-06T03:34:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864063.04 epoch sec; seqNo:6690; 4061742.27 north; 594370.19 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:34:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864059.41 2024-11-06T03:34:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:23Z- waiting for 1730864063.41, time is 1730864063.04 2024-11-06T03:34:23Z- AHRS_M2 msg: 1730864063.42 epoch sec; seqNo:6278 2024-11-06T03:34:23Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.19 psi 2024-11-06T03:34:23Z- handleDepth()- Depth_Keller msg: 1730864063.43 epoch sec; seqNo:6691; depth 49.60 2024-11-06T03:34:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864063.44 epoch sec; seqNo:6691; 4061742.27 north; 594370.19 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:34:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864059.41 2024-11-06T03:34:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:23Z- waiting for 1730864063.41, time is 1730864063.44 2024-11-06T03:34:23Z- LcmTrn::updateTrn() >>>> pose time 1730864063.44, meas time 1730864062.63 <<<< 2024-11-06T03:34:23Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864063.44 2024-11-06T03:34:23Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:34:23Z- TerrainNav::motionUpdate() - currEstimate = (4061742, 594370.19, 49.60) 2024-11-06T03:34:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:34:23Z- TerrainNav:: SubcloudNIS is 0.69, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:34:23Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:34:23Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:34:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:34:23Z- variances([0], [2], and psi_berg) = 0.439559 0.162760 0.000000 2024-11-06T03:34:23Z- publishEstimatesDecoder() - at 1730864063.44 sec 2024-11-06T03:34:23Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:34:23Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061770.98 594352.99 53.98 2024-11-06T03:34:23Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061770.07 594353.09 51.67 lat/lon: 36.70 -121.94 2024-11-06T03:34:23Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.44 0.16 0.99 0.00 2024-11-06T03:34:23Z- AHRS_M2 msg: 1730864063.83 epoch sec; seqNo:6279 2024-11-06T03:34:23Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.19 psi 2024-11-06T03:34:23Z- handleDepth()- Depth_Keller msg: 1730864063.83 epoch sec; seqNo:6692; depth 49.60 2024-11-06T03:34:23Z- handleDVL() - DVL msg epochMillisec: 1730864063.84 2024-11-06T03:34:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:23Z- handleDvl() - RDI_Pathfinder msg: 1730864063.84 sec; seqNo:2297; #beams:0 2024-11-06T03:34:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.03 , 0.05 2024-11-06T03:34:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864063.86 epoch sec; seqNo:6692; 4061741.88 north; 594368.98 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:34:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864063.44 2024-11-06T03:34:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:23Z- waiting for 1730864067.44, time is 1730864063.86 2024-11-06T03:34:24Z- AHRS_M2 msg: 1730864064.23 epoch sec; seqNo:6280 2024-11-06T03:34:24Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.19 psi 2024-11-06T03:34:24Z- handleDepth()- Depth_Keller msg: 1730864064.24 epoch sec; seqNo:6693; depth 49.61 2024-11-06T03:34:24Z- handleDVL() - DVL msg epochMillisec: 1730864064.26 2024-11-06T03:34:24Z- handleDvl() - RDI_Pathfinder msg: 1730864064.26 sec; seqNo:2298; #beams:4 2024-11-06T03:34:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.43 , 35.69 , 40.28 , 31.44 2024-11-06T03:34:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.03 2024-11-06T03:34:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864064.29 epoch sec; seqNo:6693; 4061741.88 north; 594368.98 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:34:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864063.44 2024-11-06T03:34:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:24Z- waiting for 1730864067.44, time is 1730864064.29 2024-11-06T03:34:24Z- AHRS_M2 msg: 1730864064.64 epoch sec; seqNo:6281 2024-11-06T03:34:24Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.19 psi 2024-11-06T03:34:24Z- handleDepth()- Depth_Keller msg: 1730864064.65 epoch sec; seqNo:6694; depth 49.60 2024-11-06T03:34:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864064.66 epoch sec; seqNo:6694; 4061741.50 north; 594368.98 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:34:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864063.44 2024-11-06T03:34:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:24Z- waiting for 1730864067.44, time is 1730864064.66 2024-11-06T03:34:25Z- AHRS_M2 msg: 1730864065.04 epoch sec; seqNo:6282 2024-11-06T03:34:25Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.19 psi 2024-11-06T03:34:25Z- handleDepth()- Depth_Keller msg: 1730864065.04 epoch sec; seqNo:6695; depth 49.60 2024-11-06T03:34:25Z- handleDVL() - DVL msg epochMillisec: 1730864065.05 2024-11-06T03:34:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:25Z- handleDvl() - RDI_Pathfinder msg: 1730864065.05 sec; seqNo:2299; #beams:0 2024-11-06T03:34:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.03 2024-11-06T03:34:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864065.06 epoch sec; seqNo:6695; 4061741.50 north; 594368.98 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:34:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864063.44 2024-11-06T03:34:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:25Z- waiting for 1730864067.44, time is 1730864065.06 2024-11-06T03:34:25Z- AHRS_M2 msg: 1730864065.44 epoch sec; seqNo:6283 2024-11-06T03:34:25Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.19 psi 2024-11-06T03:34:25Z- handleDepth()- Depth_Keller msg: 1730864065.45 epoch sec; seqNo:6696; depth 49.62 2024-11-06T03:34:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864065.46 epoch sec; seqNo:6696; 4061741.10 north; 594367.77 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:34:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864063.44 2024-11-06T03:34:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:25Z- waiting for 1730864067.44, time is 1730864065.46 2024-11-06T03:34:25Z- AHRS_M2 msg: 1730864065.85 epoch sec; seqNo:6284 2024-11-06T03:34:25Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.20 psi 2024-11-06T03:34:25Z- handleDepth()- Depth_Keller msg: 1730864065.85 epoch sec; seqNo:6697; depth 49.61 2024-11-06T03:34:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864065.86 epoch sec; seqNo:6697; 4061741.10 north; 594367.77 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:34:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864063.44 2024-11-06T03:34:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:26Z- waiting for 1730864067.44, time is 1730864065.86 2024-11-06T03:34:26Z- AHRS_M2 msg: 1730864066.25 epoch sec; seqNo:6285 2024-11-06T03:34:26Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.20 psi 2024-11-06T03:34:26Z- handleDepth()- Depth_Keller msg: 1730864066.26 epoch sec; seqNo:6698; depth 49.63 2024-11-06T03:34:26Z- handleDVL() - DVL msg epochMillisec: 1730864066.29 2024-11-06T03:34:26Z- handleDvl() - RDI_Pathfinder msg: 1730864066.29 sec; seqNo:2300; #beams:4 2024-11-06T03:34:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.77 , 35.35 , 40.28 , 31.95 2024-11-06T03:34:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , -0.01 2024-11-06T03:34:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864066.30 epoch sec; seqNo:6698; 4061740.73 north; 594367.77 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:34:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864063.44 2024-11-06T03:34:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:26Z- waiting for 1730864067.44, time is 1730864066.30 2024-11-06T03:34:26Z- AHRS_M2 msg: 1730864066.65 epoch sec; seqNo:6286 2024-11-06T03:34:26Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.20 psi 2024-11-06T03:34:26Z- handleDepth()- Depth_Keller msg: 1730864066.66 epoch sec; seqNo:6699; depth 49.63 2024-11-06T03:34:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864066.68 epoch sec; seqNo:6699; 4061740.71 north; 594366.55 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:34:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864063.44 2024-11-06T03:34:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:26Z- waiting for 1730864067.44, time is 1730864066.68 2024-11-06T03:34:27Z- AHRS_M2 msg: 1730864067.06 epoch sec; seqNo:6287 2024-11-06T03:34:27Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.20 psi 2024-11-06T03:34:27Z- handleDepth()- Depth_Keller msg: 1730864067.07 epoch sec; seqNo:6700; depth 49.63 2024-11-06T03:34:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864067.08 epoch sec; seqNo:6700; 4061740.33 north; 594366.55 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:34:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864063.44 2024-11-06T03:34:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:27Z- waiting for 1730864067.44, time is 1730864067.08 2024-11-06T03:34:27Z- AHRS_M2 msg: 1730864067.47 epoch sec; seqNo:6288 2024-11-06T03:34:27Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.20 psi 2024-11-06T03:34:27Z- handleDepth()- Depth_Keller msg: 1730864067.48 epoch sec; seqNo:6701; depth 49.64 2024-11-06T03:34:27Z- handleDVL() - DVL msg epochMillisec: 1730864067.49 2024-11-06T03:34:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:27Z- handleDvl() - RDI_Pathfinder msg: 1730864067.49 sec; seqNo:2301; #beams:0 2024-11-06T03:34:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , -0.01 2024-11-06T03:34:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864067.50 epoch sec; seqNo:6701; 4061740.33 north; 594366.55 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:34:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864063.44 2024-11-06T03:34:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:27Z- waiting for 1730864067.44, time is 1730864067.50 2024-11-06T03:34:27Z- LcmTrn::updateTrn() >>>> pose time 1730864067.50, meas time 1730864067.49 <<<< 2024-11-06T03:34:27Z- LcmTrn::updateTrn() equating pose 1730864067.49 and meas times 1730864067.49 2024-11-06T03:34:27Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730864067.49 2024-11-06T03:34:27Z- TerrainNav::motionUpdate() - currEstimate = (4061740, 594366.55, 49.64) 2024-11-06T03:34:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:34:27Z- TerrainNav:: SubcloudNIS is 0.69, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:34:27Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:34:27Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:34:27Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:34:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:34:27Z- variances([0], [2], and psi_berg) = 0.441757 0.166371 0.000000 2024-11-06T03:34:27Z- publishEstimatesDecoder() - at 1730864067.49 sec 2024-11-06T03:34:27Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:34:27Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061769.12 594349.41 53.91 2024-11-06T03:34:27Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061768.14 594349.45 51.72 lat/lon: 36.70 -121.94 2024-11-06T03:34:27Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.44 0.17 1.23 0.00 2024-11-06T03:34:27Z- AHRS_M2 msg: 1730864067.87 epoch sec; seqNo:6289 2024-11-06T03:34:27Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.20 psi 2024-11-06T03:34:27Z- handleDepth()- Depth_Keller msg: 1730864067.87 epoch sec; seqNo:6702; depth 49.64 2024-11-06T03:34:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864067.88 epoch sec; seqNo:6702; 4061739.95 north; 594366.56 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:34:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864067.49 2024-11-06T03:34:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:28Z- waiting for 1730864071.49, time is 1730864067.88 2024-11-06T03:34:28Z- AHRS_M2 msg: 1730864068.27 epoch sec; seqNo:6290 2024-11-06T03:34:28Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.20 psi 2024-11-06T03:34:28Z- handleDepth()- Depth_Keller msg: 1730864068.28 epoch sec; seqNo:6703; depth 49.65 2024-11-06T03:34:28Z- handleDVL() - DVL msg epochMillisec: 1730864068.30 2024-11-06T03:34:28Z- handleDvl() - RDI_Pathfinder msg: 1730864068.30 sec; seqNo:2302; #beams:4 2024-11-06T03:34:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.96 , 35.35 , 39.94 , 32.12 2024-11-06T03:34:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , -0.04 2024-11-06T03:34:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864068.31 epoch sec; seqNo:6703; 4061739.94 north; 594365.34 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:34:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864067.49 2024-11-06T03:34:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:28Z- waiting for 1730864071.49, time is 1730864068.31 2024-11-06T03:34:28Z- AHRS_M2 msg: 1730864068.67 epoch sec; seqNo:6291 2024-11-06T03:34:28Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.20 psi 2024-11-06T03:34:28Z- handleDepth()- Depth_Keller msg: 1730864068.68 epoch sec; seqNo:6704; depth 49.66 2024-11-06T03:34:28Z- handleDVL() - DVL msg epochMillisec: 1730864068.69 2024-11-06T03:34:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:28Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:28Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:28Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:28Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:28Z- handleDvl() - RDI_Pathfinder msg: 1730864068.69 sec; seqNo:2303; #beams:0 2024-11-06T03:34:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , -0.04 2024-11-06T03:34:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864068.70 epoch sec; seqNo:6704; 4061739.56 north; 594365.34 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:34:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864067.49 2024-11-06T03:34:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:28Z- waiting for 1730864071.49, time is 1730864068.70 2024-11-06T03:34:29Z- AHRS_M2 msg: 1730864069.08 epoch sec; seqNo:6292 2024-11-06T03:34:29Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.20 psi 2024-11-06T03:34:29Z- handleDepth()- Depth_Keller msg: 1730864069.09 epoch sec; seqNo:6705; depth 49.67 2024-11-06T03:34:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864069.10 epoch sec; seqNo:6705; 4061739.56 north; 594365.34 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:34:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864067.49 2024-11-06T03:34:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:29Z- waiting for 1730864071.49, time is 1730864069.10 2024-11-06T03:34:29Z- AHRS_M2 msg: 1730864069.48 epoch sec; seqNo:6293 2024-11-06T03:34:29Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.20 psi 2024-11-06T03:34:29Z- handleDepth()- Depth_Keller msg: 1730864069.49 epoch sec; seqNo:6706; depth 49.68 2024-11-06T03:34:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864069.50 epoch sec; seqNo:6706; 4061739.18 north; 594365.35 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:34:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864067.49 2024-11-06T03:34:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:29Z- waiting for 1730864071.49, time is 1730864069.50 2024-11-06T03:34:29Z- AHRS_M2 msg: 1730864069.89 epoch sec; seqNo:6294 2024-11-06T03:34:29Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.20 psi 2024-11-06T03:34:29Z- handleDepth()- Depth_Keller msg: 1730864069.89 epoch sec; seqNo:6707; depth 49.69 2024-11-06T03:34:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864069.90 epoch sec; seqNo:6707; 4061739.17 north; 594364.13 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:34:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864067.49 2024-11-06T03:34:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:30Z- waiting for 1730864071.49, time is 1730864069.90 2024-11-06T03:34:30Z- handleDepth()- Depth_Keller msg: 1730864070.31 epoch sec; seqNo:6708; depth 49.71 2024-11-06T03:34:30Z- handleDVL() - DVL msg epochMillisec: 1730864070.33 2024-11-06T03:34:30Z- handleDvl() - RDI_Pathfinder msg: 1730864070.33 sec; seqNo:2304; #beams:4 2024-11-06T03:34:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.47 , 35.35 , 39.77 , 32.80 2024-11-06T03:34:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.10 , -0.03 2024-11-06T03:34:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864070.35 epoch sec; seqNo:6708; 4061738.79 north; 594364.13 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:34:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864067.49 2024-11-06T03:34:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:30Z- waiting for 1730864071.49, time is 1730864070.35 2024-11-06T03:34:30Z- AHRS_M2 msg: 1730864070.70 epoch sec; seqNo:6295 2024-11-06T03:34:30Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.20 psi 2024-11-06T03:34:30Z- handleDepth()- Depth_Keller msg: 1730864070.71 epoch sec; seqNo:6709; depth 49.72 2024-11-06T03:34:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864070.72 epoch sec; seqNo:6709; 4061738.79 north; 594364.13 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:34:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864067.49 2024-11-06T03:34:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:30Z- waiting for 1730864071.49, time is 1730864070.72 2024-11-06T03:34:31Z- AHRS_M2 msg: 1730864071.10 epoch sec; seqNo:6296 2024-11-06T03:34:31Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.20 psi 2024-11-06T03:34:31Z- handleDepth()- Depth_Keller msg: 1730864071.10 epoch sec; seqNo:6710; depth 49.75 2024-11-06T03:34:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864071.12 epoch sec; seqNo:6710; 4061738.78 north; 594362.91 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:34:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864067.49 2024-11-06T03:34:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:31Z- waiting for 1730864071.49, time is 1730864071.12 2024-11-06T03:34:31Z- AHRS_M2 msg: 1730864071.50 epoch sec; seqNo:6297 2024-11-06T03:34:31Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.19 psi 2024-11-06T03:34:31Z- handleDepth()- Depth_Keller msg: 1730864071.51 epoch sec; seqNo:6711; depth 49.75 2024-11-06T03:34:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864071.52 epoch sec; seqNo:6711; 4061738.40 north; 594362.91 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:34:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864067.49 2024-11-06T03:34:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:31Z- waiting for 1730864071.49, time is 1730864071.52 2024-11-06T03:34:31Z- LcmTrn::updateTrn() >>>> pose time 1730864071.52, meas time 1730864070.33 <<<< 2024-11-06T03:34:31Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864071.52 2024-11-06T03:34:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864070.33 sec, ping # 0; waiting for INS data more recent than 1730864067.49... 2024-11-06T03:34:31Z- TerrainNav::motionUpdate() - currEstimate = (4061738, 594362.91, 49.75) 2024-11-06T03:34:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:34:31Z- TerrainNav:: SubcloudNIS is 0.69, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:34:31Z- TNavPF::Projecting Measurements 2024-11-06T03:34:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:34:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:34:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:34:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:34:31Z- TNavFilter::4 good beams. 2024-11-06T03:34:31Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:34:31Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:34:31Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:34:31Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:34:31Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:34:31Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:34:31Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:34:31Z- TerrainMapOctree: (vehN, vehE ) = (4061738.97, 594363.99). 2024-11-06T03:34:31Z- TerrainMapOctree: Min Distance = 114.52. 2024-11-06T03:34:31Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:34:31Z- Forcing Subcloud Comparison 2024-11-06T03:34:31Z- Weighting particles with subcloud comparison 2024-11-06T03:34:31Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:31Z- meanExpectedMeasDiff: 6.058018 delta_rms_squared: 2.386788 alpha: 0.991690 2024-11-06T03:34:31Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:31Z- meanExpectedMeasDiff: 3.098746 delta_rms_squared: 1.734570 alpha: 0.988601 2024-11-06T03:34:31Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:31Z- meanExpectedMeasDiff: 6.960492 delta_rms_squared: 1.912809 alpha: 0.989652 2024-11-06T03:34:31Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:31Z- meanExpectedMeasDiff: -1.249048 delta_rms_squared: 2.281866 alpha: 0.991311 2024-11-06T03:34:31Z- Subcloud NIS: 0.733064 2024-11-06T03:34:31Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:34:31Z- TNavPF:: sumWeights = 0.000508 2024-11-06T03:34:31Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:34:31Z- TNavPF::First Term of Map Covariance Matrix is 0.01 2024-11-06T03:34:31Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:34:31Z- TerrainNav::Resampling particle filter... 2024-11-06T03:34:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864070.33 sec. 2024-11-06T03:34:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:34:31Z- variances([0], [2], and psi_berg) = 0.168075 0.158136 0.000000 2024-11-06T03:34:31Z- publishEstimatesDecoder() - at 1730864071.52 sec 2024-11-06T03:34:31Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:34:31Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061767.09 594345.60 52.62 2024-11-06T03:34:31Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061765.94 594345.88 49.82 lat/lon: 36.70 -121.94 2024-11-06T03:34:31Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.16 0.78 0.00 2024-11-06T03:34:31Z- AHRS_M2 msg: 1730864071.91 epoch sec; seqNo:6298 2024-11-06T03:34:31Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.19 psi 2024-11-06T03:34:31Z- handleDepth()- Depth_Keller msg: 1730864071.91 epoch sec; seqNo:6712; depth 49.78 2024-11-06T03:34:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864071.93 epoch sec; seqNo:6712; 4061738.40 north; 594362.91 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:34:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864071.52 2024-11-06T03:34:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:32Z- waiting for 1730864075.52, time is 1730864071.93 2024-11-06T03:34:32Z- AHRS_M2 msg: 1730864072.31 epoch sec; seqNo:6299 2024-11-06T03:34:32Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.19 psi 2024-11-06T03:34:32Z- handleDepth()- Depth_Keller msg: 1730864072.32 epoch sec; seqNo:6713; depth 49.81 2024-11-06T03:34:32Z- handleDVL() - DVL msg epochMillisec: 1730864072.36 2024-11-06T03:34:32Z- handleDvl() - RDI_Pathfinder msg: 1730864072.36 sec; seqNo:2305; #beams:4 2024-11-06T03:34:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.15 , 35.69 , 40.11 , 33.14 2024-11-06T03:34:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.11 , 0.06 2024-11-06T03:34:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864072.39 epoch sec; seqNo:6713; 4061738.02 north; 594362.92 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:34:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864071.52 2024-11-06T03:34:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:32Z- waiting for 1730864075.52, time is 1730864072.39 2024-11-06T03:34:32Z- AHRS_M2 msg: 1730864072.71 epoch sec; seqNo:6300 2024-11-06T03:34:32Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 4.18 psi 2024-11-06T03:34:32Z- handleDepth()- Depth_Keller msg: 1730864072.72 epoch sec; seqNo:6714; depth 49.81 2024-11-06T03:34:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864072.74 epoch sec; seqNo:6714; 4061738.01 north; 594361.70 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T03:34:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864071.52 2024-11-06T03:34:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:32Z- waiting for 1730864075.52, time is 1730864072.74 2024-11-06T03:34:33Z- AHRS_M2 msg: 1730864073.12 epoch sec; seqNo:6301 2024-11-06T03:34:33Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.18 psi 2024-11-06T03:34:33Z- handleDepth()- Depth_Keller msg: 1730864073.12 epoch sec; seqNo:6715; depth 49.84 2024-11-06T03:34:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864073.13 epoch sec; seqNo:6715; 4061738.01 north; 594361.70 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:34:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864071.52 2024-11-06T03:34:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:33Z- waiting for 1730864075.52, time is 1730864073.13 2024-11-06T03:34:33Z- AHRS_M2 msg: 1730864073.52 epoch sec; seqNo:6302 2024-11-06T03:34:33Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.18 psi 2024-11-06T03:34:33Z- handleDepth()- Depth_Keller msg: 1730864073.53 epoch sec; seqNo:6716; depth 49.85 2024-11-06T03:34:33Z- handleDVL() - DVL msg epochMillisec: 1730864073.54 2024-11-06T03:34:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:33Z- handleDvl() - RDI_Pathfinder msg: 1730864073.54 sec; seqNo:2306; #beams:0 2024-11-06T03:34:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.11 , 0.06 2024-11-06T03:34:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864073.54 epoch sec; seqNo:6716; 4061737.63 north; 594361.70 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:34:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864071.52 2024-11-06T03:34:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:33Z- waiting for 1730864075.52, time is 1730864073.54 2024-11-06T03:34:33Z- AHRS_M2 msg: 1730864073.93 epoch sec; seqNo:6303 2024-11-06T03:34:33Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.18 psi 2024-11-06T03:34:33Z- handleDepth()- Depth_Keller msg: 1730864073.98 epoch sec; seqNo:6717; depth 49.87 2024-11-06T03:34:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864074.01 epoch sec; seqNo:6717; 4061737.61 north; 594360.48 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:34:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864071.52 2024-11-06T03:34:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:34Z- waiting for 1730864075.52, time is 1730864074.01 2024-11-06T03:34:34Z- AHRS_M2 msg: 1730864074.33 epoch sec; seqNo:6304 2024-11-06T03:34:34Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.18 psi 2024-11-06T03:34:34Z- handleDepth()- Depth_Keller msg: 1730864074.34 epoch sec; seqNo:6718; depth 49.90 2024-11-06T03:34:34Z- handleDVL() - DVL msg epochMillisec: 1730864074.37 2024-11-06T03:34:34Z- handleDvl() - RDI_Pathfinder msg: 1730864074.37 sec; seqNo:2307; #beams:4 2024-11-06T03:34:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.15 , 35.86 , 40.28 , 33.99 2024-11-06T03:34:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , 0.10 2024-11-06T03:34:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864074.39 epoch sec; seqNo:6718; 4061737.24 north; 594360.49 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:34:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864071.52 2024-11-06T03:34:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:34Z- waiting for 1730864075.52, time is 1730864074.39 2024-11-06T03:34:34Z- AHRS_M2 msg: 1730864074.74 epoch sec; seqNo:6305 2024-11-06T03:34:34Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.18 psi 2024-11-06T03:34:34Z- handleDepth()- Depth_Keller msg: 1730864074.74 epoch sec; seqNo:6719; depth 49.91 2024-11-06T03:34:34Z- handleDVL() - DVL msg epochMillisec: 1730864074.75 2024-11-06T03:34:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:34Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:34Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:34Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:34Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:34Z- handleDvl() - RDI_Pathfinder msg: 1730864074.75 sec; seqNo:2308; #beams:0 2024-11-06T03:34:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , 0.10 2024-11-06T03:34:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864074.76 epoch sec; seqNo:6719; 4061737.24 north; 594360.49 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:34:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864071.52 2024-11-06T03:34:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:34Z- waiting for 1730864075.52, time is 1730864074.76 2024-11-06T03:34:35Z- AHRS_M2 msg: 1730864075.14 epoch sec; seqNo:6306 2024-11-06T03:34:35Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.18 psi 2024-11-06T03:34:35Z- handleDepth()- Depth_Keller msg: 1730864075.14 epoch sec; seqNo:6720; depth 49.92 2024-11-06T03:34:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864075.17 epoch sec; seqNo:6720; 4061736.86 north; 594360.49 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:34:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864071.52 2024-11-06T03:34:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:35Z- waiting for 1730864075.52, time is 1730864075.17 2024-11-06T03:34:35Z- AHRS_M2 msg: 1730864075.54 epoch sec; seqNo:6307 2024-11-06T03:34:35Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.18 psi 2024-11-06T03:34:35Z- handleDepth()- Depth_Keller msg: 1730864075.56 epoch sec; seqNo:6721; depth 49.93 2024-11-06T03:34:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864075.58 epoch sec; seqNo:6721; 4061736.84 north; 594359.27 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:34:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864071.52 2024-11-06T03:34:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:35Z- waiting for 1730864075.52, time is 1730864075.58 2024-11-06T03:34:35Z- LcmTrn::updateTrn() >>>> pose time 1730864075.58, meas time 1730864074.75 <<<< 2024-11-06T03:34:35Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864075.58 2024-11-06T03:34:35Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:34:35Z- TerrainNav::motionUpdate() - currEstimate = (4061737, 594359.27, 49.93) 2024-11-06T03:34:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:34:35Z- TerrainNav:: SubcloudNIS is 0.73, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:34:35Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:34:35Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:34:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:34:35Z- variances([0], [2], and psi_berg) = 0.170002 0.160455 0.000000 2024-11-06T03:34:35Z- publishEstimatesDecoder() - at 1730864075.58 sec 2024-11-06T03:34:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:34:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061765.63 594341.98 53.08 2024-11-06T03:34:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061764.38 594342.23 50.01 lat/lon: 36.70 -121.94 2024-11-06T03:34:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.16 1.04 0.00 2024-11-06T03:34:36Z- AHRS_M2 msg: 1730864076.00 epoch sec; seqNo:6308 2024-11-06T03:34:36Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.19 psi 2024-11-06T03:34:36Z- handleDepth()- Depth_Keller msg: 1730864076.01 epoch sec; seqNo:6722; depth 49.95 2024-11-06T03:34:36Z- handleDVL() - DVL msg epochMillisec: 1730864076.03 2024-11-06T03:34:36Z- handleDvl() - RDI_Pathfinder msg: 1730864076.03 sec; seqNo:2309; #beams:4 2024-11-06T03:34:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.25 , 36.26 , 39.82 , 34.58 2024-11-06T03:34:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.04 2024-11-06T03:34:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864076.06 epoch sec; seqNo:6722; 4061736.46 north; 594359.27 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T03:34:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864075.58 2024-11-06T03:34:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:36Z- waiting for 1730864079.58, time is 1730864076.06 2024-11-06T03:34:36Z- AHRS_M2 msg: 1730864076.36 epoch sec; seqNo:6309 2024-11-06T03:34:36Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.19 psi 2024-11-06T03:34:36Z- handleDepth()- Depth_Keller msg: 1730864076.36 epoch sec; seqNo:6723; depth 49.96 2024-11-06T03:34:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864076.38 epoch sec; seqNo:6723; 4061736.46 north; 594359.27 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:34:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864075.58 2024-11-06T03:34:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:36Z- waiting for 1730864079.58, time is 1730864076.38 2024-11-06T03:34:36Z- AHRS_M2 msg: 1730864076.76 epoch sec; seqNo:6310 2024-11-06T03:34:36Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.19 psi 2024-11-06T03:34:36Z- handleDepth()- Depth_Keller msg: 1730864076.76 epoch sec; seqNo:6724; depth 49.97 2024-11-06T03:34:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864076.77 epoch sec; seqNo:6724; 4061736.09 north; 594359.28 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:34:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864075.58 2024-11-06T03:34:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:36Z- waiting for 1730864079.58, time is 1730864076.77 2024-11-06T03:34:37Z- AHRS_M2 msg: 1730864077.16 epoch sec; seqNo:6311 2024-11-06T03:34:37Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.19 psi 2024-11-06T03:34:37Z- handleDepth()- Depth_Keller msg: 1730864077.17 epoch sec; seqNo:6725; depth 49.97 2024-11-06T03:34:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864077.18 epoch sec; seqNo:6725; 4061736.07 north; 594358.06 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:34:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864075.58 2024-11-06T03:34:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:37Z- waiting for 1730864079.58, time is 1730864077.18 2024-11-06T03:34:37Z- AHRS_M2 msg: 1730864077.56 epoch sec; seqNo:6312 2024-11-06T03:34:37Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.20 psi 2024-11-06T03:34:37Z- handleDepth()- Depth_Keller msg: 1730864077.57 epoch sec; seqNo:6726; depth 49.97 2024-11-06T03:34:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864077.58 epoch sec; seqNo:6726; 4061735.69 north; 594358.06 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:34:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864075.58 2024-11-06T03:34:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:37Z- waiting for 1730864079.58, time is 1730864077.58 2024-11-06T03:34:38Z- AHRS_M2 msg: 1730864078.00 epoch sec; seqNo:6313 2024-11-06T03:34:38Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.20 psi 2024-11-06T03:34:38Z- handleDepth()- Depth_Keller msg: 1730864078.01 epoch sec; seqNo:6727; depth 49.97 2024-11-06T03:34:38Z- handleDVL() - DVL msg epochMillisec: 1730864078.04 2024-11-06T03:34:38Z- handleDvl() - RDI_Pathfinder msg: 1730864078.04 sec; seqNo:2310; #beams:4 2024-11-06T03:34:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.81 , 36.45 , 39.07 , 34.96 2024-11-06T03:34:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , -0.04 2024-11-06T03:34:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864078.05 epoch sec; seqNo:6727; 4061735.69 north; 594358.06 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:34:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864075.58 2024-11-06T03:34:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:38Z- waiting for 1730864079.58, time is 1730864078.05 2024-11-06T03:34:38Z- AHRS_M2 msg: 1730864078.37 epoch sec; seqNo:6314 2024-11-06T03:34:38Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.20 psi 2024-11-06T03:34:38Z- handleDepth()- Depth_Keller msg: 1730864078.38 epoch sec; seqNo:6728; depth 49.98 2024-11-06T03:34:38Z- handleDVL() - DVL msg epochMillisec: 1730864078.38 2024-11-06T03:34:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:38Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:38Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:38Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:38Z- handleDvl() - RDI_Pathfinder msg: 1730864078.38 sec; seqNo:2311; #beams:0 2024-11-06T03:34:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , -0.04 2024-11-06T03:34:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864078.40 epoch sec; seqNo:6728; 4061735.31 north; 594358.07 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:34:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864075.58 2024-11-06T03:34:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:38Z- waiting for 1730864079.58, time is 1730864078.40 2024-11-06T03:34:38Z- AHRS_M2 msg: 1730864078.78 epoch sec; seqNo:6315 2024-11-06T03:34:38Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.20 psi 2024-11-06T03:34:38Z- handleDepth()- Depth_Keller msg: 1730864078.78 epoch sec; seqNo:6729; depth 49.98 2024-11-06T03:34:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864078.79 epoch sec; seqNo:6729; 4061735.30 north; 594356.85 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:34:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864075.58 2024-11-06T03:34:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:38Z- waiting for 1730864079.58, time is 1730864078.79 2024-11-06T03:34:39Z- AHRS_M2 msg: 1730864079.18 epoch sec; seqNo:6316 2024-11-06T03:34:39Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.20 psi 2024-11-06T03:34:39Z- handleDepth()- Depth_Keller msg: 1730864079.18 epoch sec; seqNo:6730; depth 49.98 2024-11-06T03:34:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864079.19 epoch sec; seqNo:6730; 4061734.92 north; 594356.85 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:34:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864075.58 2024-11-06T03:34:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:39Z- waiting for 1730864079.58, time is 1730864079.19 2024-11-06T03:34:39Z- AHRS_M2 msg: 1730864079.58 epoch sec; seqNo:6317 2024-11-06T03:34:39Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 4.19 psi 2024-11-06T03:34:39Z- handleDepth()- Depth_Keller msg: 1730864079.59 epoch sec; seqNo:6731; depth 49.99 2024-11-06T03:34:39Z- handleDVL() - DVL msg epochMillisec: 1730864079.59 2024-11-06T03:34:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:39Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:39Z- handleDvl() - RDI_Pathfinder msg: 1730864079.59 sec; seqNo:2312; #beams:0 2024-11-06T03:34:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , -0.04 2024-11-06T03:34:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864079.61 epoch sec; seqNo:6731; 4061734.92 north; 594356.85 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:34:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864075.58 2024-11-06T03:34:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:39Z- waiting for 1730864079.58, time is 1730864079.61 2024-11-06T03:34:39Z- LcmTrn::updateTrn() >>>> pose time 1730864079.61, meas time 1730864079.59 <<<< 2024-11-06T03:34:39Z- LcmTrn::updateTrn() equating pose 1730864079.59 and meas times 1730864079.59 2024-11-06T03:34:39Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730864079.59 2024-11-06T03:34:39Z- TerrainNav::motionUpdate() - currEstimate = (4061735, 594356.85, 49.99) 2024-11-06T03:34:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:34:39Z- TerrainNav:: SubcloudNIS is 0.73, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:34:39Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:34:39Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:34:39Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:34:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:34:39Z- variances([0], [2], and psi_berg) = 0.171176 0.162027 0.000000 2024-11-06T03:34:39Z- publishEstimatesDecoder() - at 1730864079.59 sec 2024-11-06T03:34:39Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:34:39Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061763.80 594339.57 53.47 2024-11-06T03:34:39Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061762.46 594339.81 50.07 lat/lon: 36.70 -121.94 2024-11-06T03:34:39Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.16 1.29 0.00 2024-11-06T03:34:40Z- AHRS_M2 msg: 1730864080.01 epoch sec; seqNo:6318 2024-11-06T03:34:40Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.19 psi 2024-11-06T03:34:40Z- handleDepth()- Depth_Keller msg: 1730864080.02 epoch sec; seqNo:6732; depth 50.00 2024-11-06T03:34:40Z- handleDVL() - DVL msg epochMillisec: 1730864080.04 2024-11-06T03:34:40Z- handleDvl() - RDI_Pathfinder msg: 1730864080.04 sec; seqNo:2313; #beams:4 2024-11-06T03:34:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.37 , 36.26 , 40.19 , 35.14 2024-11-06T03:34:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , -0.05 2024-11-06T03:34:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864080.05 epoch sec; seqNo:6732; 4061734.54 north; 594356.85 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:34:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864079.59 2024-11-06T03:34:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:40Z- waiting for 1730864083.59, time is 1730864080.05 2024-11-06T03:34:40Z- AHRS_M2 msg: 1730864080.39 epoch sec; seqNo:6319 2024-11-06T03:34:40Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.19 psi 2024-11-06T03:34:40Z- handleDepth()- Depth_Keller msg: 1730864080.40 epoch sec; seqNo:6733; depth 49.99 2024-11-06T03:34:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864080.41 epoch sec; seqNo:6733; 4061734.53 north; 594355.63 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:34:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864079.59 2024-11-06T03:34:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:40Z- waiting for 1730864083.59, time is 1730864080.41 2024-11-06T03:34:40Z- AHRS_M2 msg: 1730864080.80 epoch sec; seqNo:6320 2024-11-06T03:34:40Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.19 psi 2024-11-06T03:34:40Z- handleDepth()- Depth_Keller msg: 1730864080.81 epoch sec; seqNo:6734; depth 49.99 2024-11-06T03:34:40Z- handleDVL() - DVL msg epochMillisec: 1730864080.81 2024-11-06T03:34:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:40Z- handleDvl() - RDI_Pathfinder msg: 1730864080.81 sec; seqNo:2314; #beams:0 2024-11-06T03:34:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , -0.05 2024-11-06T03:34:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864080.82 epoch sec; seqNo:6734; 4061734.53 north; 594355.63 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:34:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864079.59 2024-11-06T03:34:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:40Z- waiting for 1730864083.59, time is 1730864080.82 2024-11-06T03:34:41Z- AHRS_M2 msg: 1730864081.20 epoch sec; seqNo:6321 2024-11-06T03:34:41Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.18 psi 2024-11-06T03:34:41Z- handleDepth()- Depth_Keller msg: 1730864081.20 epoch sec; seqNo:6735; depth 50.00 2024-11-06T03:34:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864081.22 epoch sec; seqNo:6735; 4061734.15 north; 594355.64 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:34:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864079.59 2024-11-06T03:34:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:41Z- waiting for 1730864083.59, time is 1730864081.22 2024-11-06T03:34:41Z- AHRS_M2 msg: 1730864081.61 epoch sec; seqNo:6322 2024-11-06T03:34:41Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.18 psi 2024-11-06T03:34:41Z- handleDepth()- Depth_Keller msg: 1730864081.65 epoch sec; seqNo:6736; depth 50.00 2024-11-06T03:34:41Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:34:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864079.59 2024-11-06T03:34:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:41Z- waiting for 1730864083.59, time is 1730864081.61 2024-11-06T03:34:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864081.78 epoch sec; seqNo:6736; 4061733.76 north; 594354.42 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:34:41Z- 1 msgs in 200 ms and 2 handle calls 2024-11-06T03:34:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864079.59 2024-11-06T03:34:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:41Z- waiting for 1730864083.59, time is 1730864081.78 2024-11-06T03:34:42Z- AHRS_M2 msg: 1730864082.01 epoch sec; seqNo:6323 2024-11-06T03:34:42Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.18 psi 2024-11-06T03:34:42Z- handleDepth()- Depth_Keller msg: 1730864082.02 epoch sec; seqNo:6737; depth 50.00 2024-11-06T03:34:42Z- handleDVL() - DVL msg epochMillisec: 1730864082.04 2024-11-06T03:34:42Z- handleDvl() - RDI_Pathfinder msg: 1730864082.04 sec; seqNo:2315; #beams:4 2024-11-06T03:34:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.81 , 36.26 , 40.19 , 35.33 2024-11-06T03:34:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.08 , 0.00 2024-11-06T03:34:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864082.07 epoch sec; seqNo:6737; 4061733.76 north; 594354.42 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:34:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864079.59 2024-11-06T03:34:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:42Z- waiting for 1730864083.59, time is 1730864082.07 2024-11-06T03:34:42Z- AHRS_M2 msg: 1730864082.41 epoch sec; seqNo:6324 2024-11-06T03:34:42Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.17 psi 2024-11-06T03:34:42Z- handleDepth()- Depth_Keller msg: 1730864082.42 epoch sec; seqNo:6738; depth 50.02 2024-11-06T03:34:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864082.43 epoch sec; seqNo:6738; 4061733.76 north; 594354.42 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:34:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864079.59 2024-11-06T03:34:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:42Z- waiting for 1730864083.59, time is 1730864082.43 2024-11-06T03:34:42Z- AHRS_M2 msg: 1730864082.81 epoch sec; seqNo:6325 2024-11-06T03:34:42Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.17 psi 2024-11-06T03:34:42Z- handleDepth()- Depth_Keller msg: 1730864082.83 epoch sec; seqNo:6739; depth 50.04 2024-11-06T03:34:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864082.84 epoch sec; seqNo:6739; 4061733.38 north; 594354.43 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:34:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864079.59 2024-11-06T03:34:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:42Z- waiting for 1730864083.59, time is 1730864082.84 2024-11-06T03:34:43Z- AHRS_M2 msg: 1730864083.22 epoch sec; seqNo:6326 2024-11-06T03:34:43Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.17 psi 2024-11-06T03:34:43Z- handleDepth()- Depth_Keller msg: 1730864083.23 epoch sec; seqNo:6740; depth 50.03 2024-11-06T03:34:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864083.24 epoch sec; seqNo:6740; 4061733.37 north; 594353.21 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:34:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864079.59 2024-11-06T03:34:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:43Z- waiting for 1730864083.59, time is 1730864083.24 2024-11-06T03:34:43Z- AHRS_M2 msg: 1730864083.63 epoch sec; seqNo:6327 2024-11-06T03:34:43Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 4.16 psi 2024-11-06T03:34:43Z- handleDepth()- Depth_Keller msg: 1730864083.64 epoch sec; seqNo:6741; depth 50.05 2024-11-06T03:34:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864083.65 epoch sec; seqNo:6741; 4061732.99 north; 594353.21 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:34:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864079.59 2024-11-06T03:34:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:43Z- waiting for 1730864083.59, time is 1730864083.65 2024-11-06T03:34:43Z- LcmTrn::updateTrn() >>>> pose time 1730864083.65, meas time 1730864082.04 <<<< 2024-11-06T03:34:43Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864083.65 2024-11-06T03:34:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864082.04 sec, ping # 0; waiting for INS data more recent than 1730864079.59... 2024-11-06T03:34:43Z- TerrainNav::motionUpdate() - currEstimate = (4061733, 594353.21, 50.05) 2024-11-06T03:34:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:34:43Z- TerrainNav:: SubcloudNIS is 0.73, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:34:43Z- TNavPF::Projecting Measurements 2024-11-06T03:34:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:34:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:34:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:34:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:34:43Z- TNavFilter::4 good beams. 2024-11-06T03:34:43Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:34:43Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:34:43Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:34:43Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:34:43Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:34:43Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:34:43Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:34:43Z- TerrainMapOctree: (vehN, vehE ) = (4061733.75, 594354.65). 2024-11-06T03:34:43Z- TerrainMapOctree: Min Distance = 105.91. 2024-11-06T03:34:43Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:34:43Z- Forcing Subcloud Comparison 2024-11-06T03:34:43Z- Weighting particles with subcloud comparison 2024-11-06T03:34:43Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:43Z- meanExpectedMeasDiff: 4.201769 delta_rms_squared: 6.092826 alpha: 0.996728 2024-11-06T03:34:43Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:43Z- meanExpectedMeasDiff: 2.540760 delta_rms_squared: 2.052313 alpha: 0.990349 2024-11-06T03:34:43Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:43Z- meanExpectedMeasDiff: 5.570103 delta_rms_squared: 2.186543 alpha: 0.990936 2024-11-06T03:34:43Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:43Z- meanExpectedMeasDiff: -0.490949 delta_rms_squared: 1.269098 alpha: 0.984485 2024-11-06T03:34:43Z- Subcloud NIS: 0.781953 2024-11-06T03:34:43Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:34:43Z- TNavPF:: sumWeights = 0.022728 2024-11-06T03:34:43Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:34:43Z- TNavPF::First Term of Map Covariance Matrix is 0.05 2024-11-06T03:34:43Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:34:43Z- TerrainNav::Resampling particle filter... 2024-11-06T03:34:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864082.04 sec. 2024-11-06T03:34:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:34:43Z- variances([0], [2], and psi_berg) = 0.099785 0.107781 0.000000 2024-11-06T03:34:43Z- publishEstimatesDecoder() - at 1730864083.65 sec 2024-11-06T03:34:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:34:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061760.79 594336.43 47.49 2024-11-06T03:34:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061760.53 594336.23 48.64 lat/lon: 36.70 -121.94 2024-11-06T03:34:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.10 0.11 0.71 0.00 2024-11-06T03:34:44Z- AHRS_M2 msg: 1730864084.03 epoch sec; seqNo:6328 2024-11-06T03:34:44Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 4.16 psi 2024-11-06T03:34:44Z- handleDepth()- Depth_Keller msg: 1730864084.04 epoch sec; seqNo:6742; depth 50.05 2024-11-06T03:34:44Z- handleDVL() - DVL msg epochMillisec: 1730864084.12 2024-11-06T03:34:44Z- handleDvl() - RDI_Pathfinder msg: 1730864084.12 sec; seqNo:2316; #beams:4 2024-11-06T03:34:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.99 , 36.83 , 40.56 , 36.26 2024-11-06T03:34:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.06 , 0.04 2024-11-06T03:34:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864084.15 epoch sec; seqNo:6742; 4061732.99 north; 594353.21 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:34:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864083.65 2024-11-06T03:34:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:44Z- waiting for 1730864087.65, time is 1730864084.15 2024-11-06T03:34:44Z- AHRS_M2 msg: 1730864084.43 epoch sec; seqNo:6329 2024-11-06T03:34:44Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 4.16 psi 2024-11-06T03:34:44Z- handleDepth()- Depth_Keller msg: 1730864084.44 epoch sec; seqNo:6743; depth 50.06 2024-11-06T03:34:44Z- handleDVL() - DVL msg epochMillisec: 1730864084.44 2024-11-06T03:34:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:44Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:44Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:44Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:44Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:44Z- handleDvl() - RDI_Pathfinder msg: 1730864084.44 sec; seqNo:2317; #beams:0 2024-11-06T03:34:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.06 , 0.04 2024-11-06T03:34:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864084.46 epoch sec; seqNo:6743; 4061732.60 north; 594351.99 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:34:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864083.65 2024-11-06T03:34:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:44Z- waiting for 1730864087.65, time is 1730864084.46 2024-11-06T03:34:44Z- AHRS_M2 msg: 1730864084.84 epoch sec; seqNo:6330 2024-11-06T03:34:44Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 4.16 psi 2024-11-06T03:34:44Z- handleDepth()- Depth_Keller msg: 1730864084.84 epoch sec; seqNo:6744; depth 50.08 2024-11-06T03:34:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864084.85 epoch sec; seqNo:6744; 4061732.60 north; 594351.99 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:34:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864083.65 2024-11-06T03:34:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:44Z- waiting for 1730864087.65, time is 1730864084.85 2024-11-06T03:34:45Z- AHRS_M2 msg: 1730864085.24 epoch sec; seqNo:6331 2024-11-06T03:34:45Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 4.16 psi 2024-11-06T03:34:45Z- handleDepth()- Depth_Keller msg: 1730864085.24 epoch sec; seqNo:6745; depth 50.09 2024-11-06T03:34:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864085.25 epoch sec; seqNo:6745; 4061732.22 north; 594352.00 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:34:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864083.65 2024-11-06T03:34:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:45Z- waiting for 1730864087.65, time is 1730864085.25 2024-11-06T03:34:45Z- AHRS_M2 msg: 1730864085.65 epoch sec; seqNo:6332 2024-11-06T03:34:45Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 4.16 psi 2024-11-06T03:34:45Z- handleDepth()- Depth_Keller msg: 1730864085.65 epoch sec; seqNo:6746; depth 50.10 2024-11-06T03:34:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864085.66 epoch sec; seqNo:6746; 4061732.22 north; 594352.00 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:34:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864083.65 2024-11-06T03:34:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:45Z- waiting for 1730864087.65, time is 1730864085.66 2024-11-06T03:34:46Z- AHRS_M2 msg: 1730864086.05 epoch sec; seqNo:6333 2024-11-06T03:34:46Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 4.16 psi 2024-11-06T03:34:46Z- handleDepth()- Depth_Keller msg: 1730864086.05 epoch sec; seqNo:6747; depth 50.12 2024-11-06T03:34:46Z- handleDVL() - DVL msg epochMillisec: 1730864086.08 2024-11-06T03:34:46Z- handleDvl() - RDI_Pathfinder msg: 1730864086.08 sec; seqNo:2318; #beams:4 2024-11-06T03:34:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.18 , 37.20 , 40.38 , 36.64 2024-11-06T03:34:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.04 , 0.04 2024-11-06T03:34:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864086.09 epoch sec; seqNo:6747; 4061731.82 north; 594350.78 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:34:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864083.65 2024-11-06T03:34:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:46Z- waiting for 1730864087.65, time is 1730864086.09 2024-11-06T03:34:46Z- AHRS_M2 msg: 1730864086.45 epoch sec; seqNo:6334 2024-11-06T03:34:46Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 4.16 psi 2024-11-06T03:34:46Z- handleDepth()- Depth_Keller msg: 1730864086.46 epoch sec; seqNo:6748; depth 50.13 2024-11-06T03:34:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864086.47 epoch sec; seqNo:6748; 4061731.82 north; 594350.78 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:34:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864083.65 2024-11-06T03:34:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:46Z- waiting for 1730864087.65, time is 1730864086.47 2024-11-06T03:34:46Z- AHRS_M2 msg: 1730864086.86 epoch sec; seqNo:6335 2024-11-06T03:34:46Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.16 psi 2024-11-06T03:34:46Z- handleDepth()- Depth_Keller msg: 1730864086.87 epoch sec; seqNo:6749; depth 50.15 2024-11-06T03:34:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864086.87 epoch sec; seqNo:6749; 4061731.45 north; 594350.79 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:34:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864083.65 2024-11-06T03:34:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:47Z- waiting for 1730864087.65, time is 1730864086.87 2024-11-06T03:34:47Z- AHRS_M2 msg: 1730864087.26 epoch sec; seqNo:6336 2024-11-06T03:34:47Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.16 psi 2024-11-06T03:34:47Z- handleDepth()- Depth_Keller msg: 1730864087.27 epoch sec; seqNo:6750; depth 50.16 2024-11-06T03:34:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864087.27 epoch sec; seqNo:6750; 4061731.45 north; 594350.79 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:34:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864083.65 2024-11-06T03:34:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:47Z- waiting for 1730864087.65, time is 1730864087.27 2024-11-06T03:34:47Z- AHRS_M2 msg: 1730864087.66 epoch sec; seqNo:6337 2024-11-06T03:34:47Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.16 psi 2024-11-06T03:34:47Z- handleDepth()- Depth_Keller msg: 1730864087.67 epoch sec; seqNo:6751; depth 50.18 2024-11-06T03:34:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864087.68 epoch sec; seqNo:6751; 4061731.05 north; 594349.57 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:34:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864083.65 2024-11-06T03:34:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:47Z- waiting for 1730864087.65, time is 1730864087.68 2024-11-06T03:34:47Z- LcmTrn::updateTrn() >>>> pose time 1730864087.68, meas time 1730864086.08 <<<< 2024-11-06T03:34:47Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864087.68 2024-11-06T03:34:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864086.08 sec, ping # 0; waiting for INS data more recent than 1730864083.65... 2024-11-06T03:34:47Z- TerrainNav::motionUpdate() - currEstimate = (4061731, 594349.57, 50.18) 2024-11-06T03:34:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:34:47Z- TerrainNav:: SubcloudNIS is 0.78, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:34:47Z- TNavPF::Projecting Measurements 2024-11-06T03:34:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:34:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:34:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:34:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:34:47Z- TNavFilter::4 good beams. 2024-11-06T03:34:47Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:34:47Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:34:47Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:34:47Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:34:47Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:34:47Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:34:47Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:34:47Z- TerrainMapOctree: (vehN, vehE ) = (4061731.82, 594351.02). 2024-11-06T03:34:47Z- TerrainMapOctree: Min Distance = 102.64. 2024-11-06T03:34:47Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:34:47Z- Forcing Subcloud Comparison 2024-11-06T03:34:47Z- Weighting particles with subcloud comparison 2024-11-06T03:34:47Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:47Z- meanExpectedMeasDiff: 1.943846 delta_rms_squared: 1.906803 alpha: 0.989620 2024-11-06T03:34:47Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:47Z- meanExpectedMeasDiff: 2.513626 delta_rms_squared: 1.117495 alpha: 0.982418 2024-11-06T03:34:47Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:47Z- meanExpectedMeasDiff: 6.267749 delta_rms_squared: 1.223077 alpha: 0.983911 2024-11-06T03:34:47Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:47Z- meanExpectedMeasDiff: -2.125849 delta_rms_squared: 0.651795 alpha: 0.970229 2024-11-06T03:34:47Z- Subcloud NIS: 0.800993 2024-11-06T03:34:47Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:34:47Z- TNavPF:: sumWeights = 0.005570 2024-11-06T03:34:47Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:34:47Z- TNavPF::First Term of Map Covariance Matrix is 0.01 2024-11-06T03:34:47Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:34:47Z- TerrainNav::Resampling particle filter... 2024-11-06T03:34:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864086.08 sec. 2024-11-06T03:34:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:34:47Z- variances([0], [2], and psi_berg) = 0.125045 0.166037 0.000000 2024-11-06T03:34:47Z- publishEstimatesDecoder() - at 1730864087.68 sec 2024-11-06T03:34:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:34:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061758.87 594332.71 48.71 2024-11-06T03:34:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061758.62 594332.63 47.69 lat/lon: 36.70 -121.94 2024-11-06T03:34:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.13 0.17 0.66 0.00 2024-11-06T03:34:48Z- AHRS_M2 msg: 1730864088.07 epoch sec; seqNo:6338 2024-11-06T03:34:48Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.16 psi 2024-11-06T03:34:48Z- handleDepth()- Depth_Keller msg: 1730864088.07 epoch sec; seqNo:6752; depth 50.19 2024-11-06T03:34:48Z- handleDVL() - DVL msg epochMillisec: 1730864088.13 2024-11-06T03:34:48Z- handleDvl() - RDI_Pathfinder msg: 1730864088.13 sec; seqNo:2319; #beams:4 2024-11-06T03:34:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.62 , 37.20 , 40.56 , 37.20 2024-11-06T03:34:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.03 , 0.01 2024-11-06T03:34:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864088.15 epoch sec; seqNo:6752; 4061731.05 north; 594349.57 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:34:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864087.68 2024-11-06T03:34:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:48Z- waiting for 1730864091.68, time is 1730864088.15 2024-11-06T03:34:48Z- AHRS_M2 msg: 1730864088.47 epoch sec; seqNo:6339 2024-11-06T03:34:48Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.16 psi 2024-11-06T03:34:48Z- handleDepth()- Depth_Keller msg: 1730864088.48 epoch sec; seqNo:6753; depth 50.22 2024-11-06T03:34:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864088.49 epoch sec; seqNo:6753; 4061730.67 north; 594349.57 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:34:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864087.68 2024-11-06T03:34:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:48Z- waiting for 1730864091.68, time is 1730864088.49 2024-11-06T03:34:48Z- AHRS_M2 msg: 1730864088.88 epoch sec; seqNo:6340 2024-11-06T03:34:48Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.15 psi 2024-11-06T03:34:48Z- handleDepth()- Depth_Keller msg: 1730864088.88 epoch sec; seqNo:6754; depth 50.22 2024-11-06T03:34:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864088.89 epoch sec; seqNo:6754; 4061730.67 north; 594349.57 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:34:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864087.68 2024-11-06T03:34:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:49Z- waiting for 1730864091.68, time is 1730864088.89 2024-11-06T03:34:49Z- AHRS_M2 msg: 1730864089.28 epoch sec; seqNo:6341 2024-11-06T03:34:49Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.15 psi 2024-11-06T03:34:49Z- handleDepth()- Depth_Keller msg: 1730864089.28 epoch sec; seqNo:6755; depth 50.24 2024-11-06T03:34:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864089.30 epoch sec; seqNo:6755; 4061730.28 north; 594348.36 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:34:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864087.68 2024-11-06T03:34:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:49Z- waiting for 1730864091.68, time is 1730864089.30 2024-11-06T03:34:49Z- AHRS_M2 msg: 1730864089.68 epoch sec; seqNo:6342 2024-11-06T03:34:49Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.15 psi 2024-11-06T03:34:49Z- handleDepth()- Depth_Keller msg: 1730864089.69 epoch sec; seqNo:6756; depth 50.25 2024-11-06T03:34:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864089.70 epoch sec; seqNo:6756; 4061730.28 north; 594348.36 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:34:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864087.68 2024-11-06T03:34:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:49Z- waiting for 1730864091.68, time is 1730864089.70 2024-11-06T03:34:50Z- AHRS_M2 msg: 1730864090.09 epoch sec; seqNo:6343 2024-11-06T03:34:50Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.15 psi 2024-11-06T03:34:50Z- handleDepth()- Depth_Keller msg: 1730864090.09 epoch sec; seqNo:6757; depth 50.27 2024-11-06T03:34:50Z- handleDVL() - DVL msg epochMillisec: 1730864090.11 2024-11-06T03:34:50Z- handleDvl() - RDI_Pathfinder msg: 1730864090.11 sec; seqNo:2320; #beams:4 2024-11-06T03:34:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.37 , 37.20 , 40.00 , 37.20 2024-11-06T03:34:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.04 , 0.02 2024-11-06T03:34:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864090.14 epoch sec; seqNo:6757; 4061729.90 north; 594348.36 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:34:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864087.68 2024-11-06T03:34:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:50Z- waiting for 1730864091.68, time is 1730864090.14 2024-11-06T03:34:50Z- AHRS_M2 msg: 1730864090.49 epoch sec; seqNo:6344 2024-11-06T03:34:50Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.15 psi 2024-11-06T03:34:50Z- handleDepth()- Depth_Keller msg: 1730864090.50 epoch sec; seqNo:6758; depth 50.29 2024-11-06T03:34:50Z- handleDVL() - DVL msg epochMillisec: 1730864090.50 2024-11-06T03:34:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:50Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:50Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:50Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:50Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:50Z- handleDvl() - RDI_Pathfinder msg: 1730864090.50 sec; seqNo:2321; #beams:0 2024-11-06T03:34:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.04 , 0.02 2024-11-06T03:34:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864090.52 epoch sec; seqNo:6758; 4061729.89 north; 594347.14 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:34:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864087.68 2024-11-06T03:34:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:50Z- waiting for 1730864091.68, time is 1730864090.52 2024-11-06T03:34:50Z- AHRS_M2 msg: 1730864090.90 epoch sec; seqNo:6345 2024-11-06T03:34:50Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.15 psi 2024-11-06T03:34:50Z- handleDepth()- Depth_Keller msg: 1730864090.91 epoch sec; seqNo:6759; depth 50.30 2024-11-06T03:34:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864090.93 epoch sec; seqNo:6759; 4061729.51 north; 594347.15 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:34:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864087.68 2024-11-06T03:34:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:51Z- waiting for 1730864091.68, time is 1730864090.93 2024-11-06T03:34:51Z- AHRS_M2 msg: 1730864091.30 epoch sec; seqNo:6346 2024-11-06T03:34:51Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.15 psi 2024-11-06T03:34:51Z- handleDepth()- Depth_Keller msg: 1730864091.31 epoch sec; seqNo:6760; depth 50.31 2024-11-06T03:34:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864091.32 epoch sec; seqNo:6760; 4061729.51 north; 594347.15 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T03:34:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864087.68 2024-11-06T03:34:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:51Z- waiting for 1730864091.68, time is 1730864091.32 2024-11-06T03:34:51Z- AHRS_M2 msg: 1730864091.70 epoch sec; seqNo:6347 2024-11-06T03:34:51Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.15 psi 2024-11-06T03:34:51Z- handleDepth()- Depth_Keller msg: 1730864091.71 epoch sec; seqNo:6761; depth 50.33 2024-11-06T03:34:51Z- handleDVL() - DVL msg epochMillisec: 1730864091.71 2024-11-06T03:34:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:51Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:51Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:51Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:51Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:51Z- handleDvl() - RDI_Pathfinder msg: 1730864091.71 sec; seqNo:2322; #beams:0 2024-11-06T03:34:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.04 , 0.02 2024-11-06T03:34:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864091.72 epoch sec; seqNo:6761; 4061729.13 north; 594347.15 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:34:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864087.68 2024-11-06T03:34:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:51Z- waiting for 1730864091.68, time is 1730864091.72 2024-11-06T03:34:51Z- LcmTrn::updateTrn() >>>> pose time 1730864091.72, meas time 1730864091.71 <<<< 2024-11-06T03:34:51Z- LcmTrn::updateTrn() equating pose 1730864091.71 and meas times 1730864091.71 2024-11-06T03:34:51Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730864091.71 2024-11-06T03:34:51Z- TerrainNav::motionUpdate() - currEstimate = (4061729, 594347.15, 50.33) 2024-11-06T03:34:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:34:51Z- TerrainNav:: SubcloudNIS is 0.80, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:34:51Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:34:51Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:34:51Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:34:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:34:51Z- variances([0], [2], and psi_berg) = 0.126579 0.167398 0.000000 2024-11-06T03:34:51Z- publishEstimatesDecoder() - at 1730864091.71 sec 2024-11-06T03:34:51Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:34:51Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061757.00 594330.32 48.38 2024-11-06T03:34:51Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061756.70 594330.21 47.83 lat/lon: 36.70 -121.94 2024-11-06T03:34:51Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.13 0.17 0.91 0.00 2024-11-06T03:34:52Z- AHRS_M2 msg: 1730864092.11 epoch sec; seqNo:6348 2024-11-06T03:34:52Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.15 psi 2024-11-06T03:34:52Z- handleDepth()- Depth_Keller msg: 1730864092.12 epoch sec; seqNo:6762; depth 50.34 2024-11-06T03:34:52Z- handleDVL() - DVL msg epochMillisec: 1730864092.14 2024-11-06T03:34:52Z- handleDvl() - RDI_Pathfinder msg: 1730864092.14 sec; seqNo:2323; #beams:4 2024-11-06T03:34:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.18 , 37.01 , 40.19 , 37.39 2024-11-06T03:34:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , 0.04 2024-11-06T03:34:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864092.16 epoch sec; seqNo:6762; 4061729.12 north; 594345.93 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:34:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864091.71 2024-11-06T03:34:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:52Z- waiting for 1730864095.71, time is 1730864092.16 2024-11-06T03:34:52Z- AHRS_M2 msg: 1730864092.51 epoch sec; seqNo:6349 2024-11-06T03:34:52Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 4.15 psi 2024-11-06T03:34:52Z- handleDepth()- Depth_Keller msg: 1730864092.52 epoch sec; seqNo:6763; depth 50.35 2024-11-06T03:34:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864092.54 epoch sec; seqNo:6763; 4061728.74 north; 594345.93 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:34:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864091.71 2024-11-06T03:34:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:52Z- waiting for 1730864095.71, time is 1730864092.54 2024-11-06T03:34:52Z- AHRS_M2 msg: 1730864092.93 epoch sec; seqNo:6350 2024-11-06T03:34:52Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.15 psi 2024-11-06T03:34:52Z- handleDepth()- Depth_Keller msg: 1730864092.93 epoch sec; seqNo:6764; depth 50.36 2024-11-06T03:34:52Z- handleDVL() - DVL msg epochMillisec: 1730864092.93 2024-11-06T03:34:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:52Z- handleDvl() - RDI_Pathfinder msg: 1730864092.93 sec; seqNo:2324; #beams:0 2024-11-06T03:34:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , 0.04 2024-11-06T03:34:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864092.94 epoch sec; seqNo:6764; 4061728.74 north; 594345.93 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:34:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864091.71 2024-11-06T03:34:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:53Z- waiting for 1730864095.71, time is 1730864092.94 2024-11-06T03:34:53Z- AHRS_M2 msg: 1730864093.32 epoch sec; seqNo:6351 2024-11-06T03:34:53Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.15 psi 2024-11-06T03:34:53Z- handleDepth()- Depth_Keller msg: 1730864093.33 epoch sec; seqNo:6765; depth 50.35 2024-11-06T03:34:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864093.34 epoch sec; seqNo:6765; 4061728.36 north; 594345.94 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:34:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864091.71 2024-11-06T03:34:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:53Z- waiting for 1730864095.71, time is 1730864093.34 2024-11-06T03:34:53Z- AHRS_M2 msg: 1730864093.72 epoch sec; seqNo:6352 2024-11-06T03:34:53Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.15 psi 2024-11-06T03:34:53Z- handleDepth()- Depth_Keller msg: 1730864093.73 epoch sec; seqNo:6766; depth 50.36 2024-11-06T03:34:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864093.75 epoch sec; seqNo:6766; 4061728.35 north; 594344.72 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:34:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864091.71 2024-11-06T03:34:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:53Z- waiting for 1730864095.71, time is 1730864093.75 2024-11-06T03:34:54Z- AHRS_M2 msg: 1730864094.12 epoch sec; seqNo:6353 2024-11-06T03:34:54Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.15 psi 2024-11-06T03:34:54Z- handleDepth()- Depth_Keller msg: 1730864094.14 epoch sec; seqNo:6767; depth 50.37 2024-11-06T03:34:54Z- handleDVL() - DVL msg epochMillisec: 1730864094.16 2024-11-06T03:34:54Z- handleDvl() - RDI_Pathfinder msg: 1730864094.16 sec; seqNo:2325; #beams:4 2024-11-06T03:34:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.37 , 36.64 , 41.31 , 37.57 2024-11-06T03:34:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.01 2024-11-06T03:34:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864094.18 epoch sec; seqNo:6767; 4061727.97 north; 594344.72 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:34:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864091.71 2024-11-06T03:34:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:54Z- waiting for 1730864095.71, time is 1730864094.18 2024-11-06T03:34:54Z- AHRS_M2 msg: 1730864094.53 epoch sec; seqNo:6354 2024-11-06T03:34:54Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.15 psi 2024-11-06T03:34:54Z- handleDepth()- Depth_Keller msg: 1730864094.54 epoch sec; seqNo:6768; depth 50.37 2024-11-06T03:34:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864094.55 epoch sec; seqNo:6768; 4061727.97 north; 594344.72 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:34:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864091.71 2024-11-06T03:34:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:54Z- waiting for 1730864095.71, time is 1730864094.55 2024-11-06T03:34:54Z- AHRS_M2 msg: 1730864094.94 epoch sec; seqNo:6355 2024-11-06T03:34:54Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.15 psi 2024-11-06T03:34:54Z- handleDepth()- Depth_Keller msg: 1730864094.95 epoch sec; seqNo:6769; depth 50.37 2024-11-06T03:34:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864094.97 epoch sec; seqNo:6769; 4061727.59 north; 594344.73 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:34:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864091.71 2024-11-06T03:34:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:55Z- waiting for 1730864095.71, time is 1730864094.97 2024-11-06T03:34:55Z- AHRS_M2 msg: 1730864095.34 epoch sec; seqNo:6356 2024-11-06T03:34:55Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.15 psi 2024-11-06T03:34:55Z- handleDepth()- Depth_Keller msg: 1730864095.35 epoch sec; seqNo:6770; depth 50.37 2024-11-06T03:34:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864095.36 epoch sec; seqNo:6770; 4061727.58 north; 594343.51 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:34:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864091.71 2024-11-06T03:34:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:55Z- waiting for 1730864095.71, time is 1730864095.36 2024-11-06T03:34:55Z- AHRS_M2 msg: 1730864095.74 epoch sec; seqNo:6357 2024-11-06T03:34:55Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.15 psi 2024-11-06T03:34:55Z- handleDepth()- Depth_Keller msg: 1730864095.75 epoch sec; seqNo:6771; depth 50.38 2024-11-06T03:34:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864095.76 epoch sec; seqNo:6771; 4061727.20 north; 594343.51 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:34:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864091.71 2024-11-06T03:34:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:55Z- waiting for 1730864095.71, time is 1730864095.76 2024-11-06T03:34:55Z- LcmTrn::updateTrn() >>>> pose time 1730864095.76, meas time 1730864094.16 <<<< 2024-11-06T03:34:55Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864095.76 2024-11-06T03:34:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864094.16 sec, ping # 0; waiting for INS data more recent than 1730864091.71... 2024-11-06T03:34:55Z- TerrainNav::motionUpdate() - currEstimate = (4061727, 594343.51, 50.38) 2024-11-06T03:34:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:34:55Z- TerrainNav:: SubcloudNIS is 0.80, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:34:55Z- TNavPF::Projecting Measurements 2024-11-06T03:34:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:34:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:34:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:34:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:34:55Z- TNavFilter::4 good beams. 2024-11-06T03:34:55Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:34:55Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:34:55Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:34:55Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:34:55Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:34:55Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:34:55Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:34:55Z- TerrainMapOctree: (vehN, vehE ) = (4061727.96, 594344.95). 2024-11-06T03:34:55Z- TerrainMapOctree: Min Distance = 97.47. 2024-11-06T03:34:55Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:34:56Z- Forcing Subcloud Comparison 2024-11-06T03:34:56Z- Weighting particles with subcloud comparison 2024-11-06T03:34:56Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:56Z- meanExpectedMeasDiff: 2.262345 delta_rms_squared: 1.687106 alpha: 0.988284 2024-11-06T03:34:56Z- beam number: 1 num in subcloud: 9999 num not in subcloud: 1 2024-11-06T03:34:56Z- meanExpectedMeasDiff: -0.172322 delta_rms_squared: 1.276603 alpha: 0.984575 2024-11-06T03:34:56Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:56Z- meanExpectedMeasDiff: -0.252015 delta_rms_squared: 2.477962 alpha: 0.991993 2024-11-06T03:34:56Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:34:56Z- meanExpectedMeasDiff: -1.544075 delta_rms_squared: 2.454979 alpha: 0.991919 2024-11-06T03:34:56Z- Subcloud NIS: 0.790568 2024-11-06T03:34:56Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:34:56Z- TNavPF:: sumWeights = 0.261908 2024-11-06T03:34:56Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:34:56Z- TNavPF::First Term of Map Covariance Matrix is 0.88 2024-11-06T03:34:56Z- TNavPF::Calculated NIS Value : -nan numBeams normalized NIS: -nan 2024-11-06T03:34:56Z- TerrainNav::Resampling particle filter... 2024-11-06T03:34:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864094.16 sec. 2024-11-06T03:34:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:34:56Z- variances([0], [2], and psi_berg) = 0.108787 0.091571 0.000000 2024-11-06T03:34:56Z- publishEstimatesDecoder() - at 1730864095.76 sec 2024-11-06T03:34:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:34:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061755.06 594326.74 49.69 2024-11-06T03:34:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061754.75 594326.60 47.90 lat/lon: 36.70 -121.94 2024-11-06T03:34:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.11 0.09 0.72 0.00 2024-11-06T03:34:56Z- AHRS_M2 msg: 1730864096.15 epoch sec; seqNo:6358 2024-11-06T03:34:56Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 4.15 psi 2024-11-06T03:34:56Z- handleDepth()- Depth_Keller msg: 1730864096.16 epoch sec; seqNo:6772; depth 50.38 2024-11-06T03:34:56Z- handleDVL() - DVL msg epochMillisec: 1730864096.20 2024-11-06T03:34:56Z- handleDvl() - RDI_Pathfinder msg: 1730864096.20 sec; seqNo:2326; #beams:4 2024-11-06T03:34:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.56 , 37.57 , 41.31 , 37.76 2024-11-06T03:34:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , -0.02 2024-11-06T03:34:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864096.23 epoch sec; seqNo:6772; 4061727.20 north; 594343.51 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:34:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864095.76 2024-11-06T03:34:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:56Z- waiting for 1730864099.76, time is 1730864096.23 2024-11-06T03:34:56Z- AHRS_M2 msg: 1730864096.55 epoch sec; seqNo:6359 2024-11-06T03:34:56Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 4.14 psi 2024-11-06T03:34:56Z- handleDepth()- Depth_Keller msg: 1730864096.56 epoch sec; seqNo:6773; depth 50.37 2024-11-06T03:34:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864096.57 epoch sec; seqNo:6773; 4061726.82 north; 594343.51 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:34:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864095.76 2024-11-06T03:34:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:56Z- waiting for 1730864099.76, time is 1730864096.57 2024-11-06T03:34:56Z- AHRS_M2 msg: 1730864096.95 epoch sec; seqNo:6360 2024-11-06T03:34:56Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.14 psi 2024-11-06T03:34:56Z- handleDepth()- Depth_Keller msg: 1730864096.96 epoch sec; seqNo:6774; depth 50.36 2024-11-06T03:34:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864096.98 epoch sec; seqNo:6774; 4061726.81 north; 594342.29 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:34:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864095.76 2024-11-06T03:34:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:57Z- waiting for 1730864099.76, time is 1730864096.98 2024-11-06T03:34:57Z- AHRS_M2 msg: 1730864097.36 epoch sec; seqNo:6361 2024-11-06T03:34:57Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.14 psi 2024-11-06T03:34:57Z- handleDepth()- Depth_Keller msg: 1730864097.37 epoch sec; seqNo:6775; depth 50.37 2024-11-06T03:34:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864097.37 epoch sec; seqNo:6775; 4061726.43 north; 594342.30 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:34:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864095.76 2024-11-06T03:34:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:57Z- waiting for 1730864099.76, time is 1730864097.37 2024-11-06T03:34:57Z- AHRS_M2 msg: 1730864097.76 epoch sec; seqNo:6362 2024-11-06T03:34:57Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.14 psi 2024-11-06T03:34:57Z- handleDepth()- Depth_Keller msg: 1730864097.77 epoch sec; seqNo:6776; depth 50.36 2024-11-06T03:34:57Z- handleDVL() - DVL msg epochMillisec: 1730864097.77 2024-11-06T03:34:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:57Z- handleDvl() - RDI_Pathfinder msg: 1730864097.77 sec; seqNo:2327; #beams:0 2024-11-06T03:34:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , -0.02 2024-11-06T03:34:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864097.78 epoch sec; seqNo:6776; 4061726.43 north; 594342.30 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:34:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864095.76 2024-11-06T03:34:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:57Z- waiting for 1730864099.76, time is 1730864097.78 2024-11-06T03:34:58Z- AHRS_M2 msg: 1730864098.17 epoch sec; seqNo:6363 2024-11-06T03:34:58Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.14 psi 2024-11-06T03:34:58Z- handleDepth()- Depth_Keller msg: 1730864098.17 epoch sec; seqNo:6777; depth 50.36 2024-11-06T03:34:58Z- handleDVL() - DVL msg epochMillisec: 1730864098.20 2024-11-06T03:34:58Z- handleDvl() - RDI_Pathfinder msg: 1730864098.20 sec; seqNo:2328; #beams:4 2024-11-06T03:34:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.56 , 37.20 , 41.12 , 38.13 2024-11-06T03:34:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.05 , -0.01 2024-11-06T03:34:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864098.21 epoch sec; seqNo:6777; 4061726.03 north; 594341.08 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:34:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864095.76 2024-11-06T03:34:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:58Z- waiting for 1730864099.76, time is 1730864098.21 2024-11-06T03:34:58Z- AHRS_M2 msg: 1730864098.57 epoch sec; seqNo:6364 2024-11-06T03:34:58Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.14 psi 2024-11-06T03:34:58Z- handleDepth()- Depth_Keller msg: 1730864098.58 epoch sec; seqNo:6778; depth 50.37 2024-11-06T03:34:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864098.59 epoch sec; seqNo:6778; 4061726.03 north; 594341.08 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:34:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864095.76 2024-11-06T03:34:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:58Z- waiting for 1730864099.76, time is 1730864098.59 2024-11-06T03:34:58Z- AHRS_M2 msg: 1730864098.98 epoch sec; seqNo:6365 2024-11-06T03:34:58Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.13 psi 2024-11-06T03:34:58Z- handleDepth()- Depth_Keller msg: 1730864098.98 epoch sec; seqNo:6779; depth 50.36 2024-11-06T03:34:58Z- handleDVL() - DVL msg epochMillisec: 1730864098.99 2024-11-06T03:34:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:34:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:34:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:34:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:34:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:34:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:34:58Z- handleDvl() - RDI_Pathfinder msg: 1730864098.99 sec; seqNo:2329; #beams:0 2024-11-06T03:34:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:34:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.05 , -0.01 2024-11-06T03:34:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864098.99 epoch sec; seqNo:6779; 4061725.66 north; 594341.09 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T03:34:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:34:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864095.76 2024-11-06T03:34:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:59Z- waiting for 1730864099.76, time is 1730864098.99 2024-11-06T03:34:59Z- AHRS_M2 msg: 1730864099.38 epoch sec; seqNo:6366 2024-11-06T03:34:59Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.13 psi 2024-11-06T03:34:59Z- handleDepth()- Depth_Keller msg: 1730864099.39 epoch sec; seqNo:6780; depth 50.37 2024-11-06T03:34:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864099.40 epoch sec; seqNo:6780; 4061725.66 north; 594341.09 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:34:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864095.76 2024-11-06T03:34:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:59Z- waiting for 1730864099.76, time is 1730864099.40 2024-11-06T03:34:59Z- AHRS_M2 msg: 1730864099.78 epoch sec; seqNo:6367 2024-11-06T03:34:59Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.13 psi 2024-11-06T03:34:59Z- handleDepth()- Depth_Keller msg: 1730864099.79 epoch sec; seqNo:6781; depth 50.37 2024-11-06T03:34:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864099.80 epoch sec; seqNo:6781; 4061725.26 north; 594339.87 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:34:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:34:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864095.76 2024-11-06T03:34:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:34:59Z- waiting for 1730864099.76, time is 1730864099.80 2024-11-06T03:34:59Z- LcmTrn::updateTrn() >>>> pose time 1730864099.80, meas time 1730864098.99 <<<< 2024-11-06T03:34:59Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864099.80 2024-11-06T03:34:59Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:34:59Z- TerrainNav::motionUpdate() - currEstimate = (4061725, 594339.87, 50.37) 2024-11-06T03:34:59Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:34:59Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:34:59Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:34:59Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:34:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:34:59Z- variances([0], [2], and psi_berg) = 0.111645 0.094478 0.000000 2024-11-06T03:34:59Z- publishEstimatesDecoder() - at 1730864099.80 sec 2024-11-06T03:34:59Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:34:59Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061753.02 594323.12 49.88 2024-11-06T03:34:59Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061752.81 594322.96 47.89 lat/lon: 36.70 -121.94 2024-11-06T03:34:59Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.11 0.09 0.97 0.00 2024-11-06T03:35:00Z- AHRS_M2 msg: 1730864100.19 epoch sec; seqNo:6368 2024-11-06T03:35:00Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.13 psi 2024-11-06T03:35:00Z- handleDepth()- Depth_Keller msg: 1730864100.19 epoch sec; seqNo:6782; depth 50.38 2024-11-06T03:35:00Z- handleDVL() - DVL msg epochMillisec: 1730864100.21 2024-11-06T03:35:00Z- handleDvl() - RDI_Pathfinder msg: 1730864100.21 sec; seqNo:2330; #beams:4 2024-11-06T03:35:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.74 , 37.57 , 41.12 , 37.94 2024-11-06T03:35:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.06 , 0.02 2024-11-06T03:35:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864100.24 epoch sec; seqNo:6782; 4061724.88 north; 594339.87 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:35:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864099.80 2024-11-06T03:35:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:00Z- waiting for 1730864103.80, time is 1730864100.24 2024-11-06T03:35:00Z- AHRS_M2 msg: 1730864100.59 epoch sec; seqNo:6369 2024-11-06T03:35:00Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.13 psi 2024-11-06T03:35:00Z- handleDepth()- Depth_Keller msg: 1730864100.60 epoch sec; seqNo:6783; depth 50.37 2024-11-06T03:35:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864100.62 epoch sec; seqNo:6783; 4061724.88 north; 594339.87 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:35:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864099.80 2024-11-06T03:35:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:00Z- waiting for 1730864103.80, time is 1730864100.62 2024-11-06T03:35:01Z- AHRS_M2 msg: 1730864101.00 epoch sec; seqNo:6370 2024-11-06T03:35:01Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.13 psi 2024-11-06T03:35:01Z- handleDepth()- Depth_Keller msg: 1730864101.01 epoch sec; seqNo:6784; depth 50.37 2024-11-06T03:35:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864101.02 epoch sec; seqNo:6784; 4061724.51 north; 594339.88 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:35:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864099.80 2024-11-06T03:35:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:01Z- waiting for 1730864103.80, time is 1730864101.02 2024-11-06T03:35:01Z- AHRS_M2 msg: 1730864101.40 epoch sec; seqNo:6371 2024-11-06T03:35:01Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.13 psi 2024-11-06T03:35:01Z- handleDepth()- Depth_Keller msg: 1730864101.41 epoch sec; seqNo:6785; depth 50.37 2024-11-06T03:35:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864101.42 epoch sec; seqNo:6785; 4061724.49 north; 594338.66 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:35:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864099.80 2024-11-06T03:35:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:01Z- waiting for 1730864103.80, time is 1730864101.42 2024-11-06T03:35:01Z- AHRS_M2 msg: 1730864101.80 epoch sec; seqNo:6372 2024-11-06T03:35:01Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.13 psi 2024-11-06T03:35:01Z- handleDepth()- Depth_Keller msg: 1730864101.81 epoch sec; seqNo:6786; depth 50.38 2024-11-06T03:35:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864101.82 epoch sec; seqNo:6786; 4061724.11 north; 594338.66 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:35:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864099.80 2024-11-06T03:35:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:01Z- waiting for 1730864103.80, time is 1730864101.82 2024-11-06T03:35:02Z- handleDepth()- Depth_Keller msg: 1730864102.23 epoch sec; seqNo:6787; depth 50.38 2024-11-06T03:35:02Z- handleDVL() - DVL msg epochMillisec: 1730864102.25 2024-11-06T03:35:02Z- handleDvl() - RDI_Pathfinder msg: 1730864102.25 sec; seqNo:2331; #beams:4 2024-11-06T03:35:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.74 , 37.57 , 41.49 , 38.32 2024-11-06T03:35:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , 0.04 2024-11-06T03:35:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864102.26 epoch sec; seqNo:6787; 4061724.11 north; 594338.66 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:35:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864099.80 2024-11-06T03:35:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:02Z- waiting for 1730864103.80, time is 1730864102.26 2024-11-06T03:35:02Z- AHRS_M2 msg: 1730864102.62 epoch sec; seqNo:6373 2024-11-06T03:35:02Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.13 psi 2024-11-06T03:35:02Z- handleDepth()- Depth_Keller msg: 1730864102.63 epoch sec; seqNo:6788; depth 50.38 2024-11-06T03:35:02Z- handleDVL() - DVL msg epochMillisec: 1730864102.63 2024-11-06T03:35:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:02Z- handleDvl() - RDI_Pathfinder msg: 1730864102.63 sec; seqNo:2332; #beams:0 2024-11-06T03:35:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , 0.04 2024-11-06T03:35:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864102.66 epoch sec; seqNo:6788; 4061723.72 north; 594337.45 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:35:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864099.80 2024-11-06T03:35:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:02Z- waiting for 1730864103.80, time is 1730864102.66 2024-11-06T03:35:03Z- AHRS_M2 msg: 1730864103.03 epoch sec; seqNo:6374 2024-11-06T03:35:03Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.12 psi 2024-11-06T03:35:03Z- handleDepth()- Depth_Keller msg: 1730864103.03 epoch sec; seqNo:6789; depth 50.39 2024-11-06T03:35:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864103.04 epoch sec; seqNo:6789; 4061723.72 north; 594337.45 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:35:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864099.80 2024-11-06T03:35:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:03Z- waiting for 1730864103.80, time is 1730864103.04 2024-11-06T03:35:03Z- handleDepth()- Depth_Keller msg: 1730864103.44 epoch sec; seqNo:6790; depth 50.39 2024-11-06T03:35:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864103.45 epoch sec; seqNo:6790; 4061723.34 north; 594337.45 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:35:03Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:35:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864099.80 2024-11-06T03:35:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:03Z- waiting for 1730864103.80, time is 1730864103.45 2024-11-06T03:35:03Z- AHRS_M2 msg: 1730864103.82 epoch sec; seqNo:6375 2024-11-06T03:35:03Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.13 psi 2024-11-06T03:35:03Z- handleDepth()- Depth_Keller msg: 1730864103.83 epoch sec; seqNo:6791; depth 50.39 2024-11-06T03:35:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864103.84 epoch sec; seqNo:6791; 4061723.34 north; 594337.45 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:35:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864099.80 2024-11-06T03:35:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:03Z- waiting for 1730864103.80, time is 1730864103.84 2024-11-06T03:35:03Z- LcmTrn::updateTrn() >>>> pose time 1730864103.84, meas time 1730864102.63 <<<< 2024-11-06T03:35:03Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864103.84 2024-11-06T03:35:03Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:35:03Z- TerrainNav::motionUpdate() - currEstimate = (4061723, 594337.45, 50.39) 2024-11-06T03:35:03Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:35:03Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:35:03Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:35:03Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:35:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:35:04Z- variances([0], [2], and psi_berg) = 0.113111 0.095756 0.000000 2024-11-06T03:35:04Z- publishEstimatesDecoder() - at 1730864103.84 sec 2024-11-06T03:35:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:35:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061751.12 594320.71 49.56 2024-11-06T03:35:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061750.89 594320.54 47.92 lat/lon: 36.70 -121.94 2024-11-06T03:35:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.11 0.10 1.22 0.00 2024-11-06T03:35:04Z- AHRS_M2 msg: 1730864104.23 epoch sec; seqNo:6376 2024-11-06T03:35:04Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.13 psi 2024-11-06T03:35:04Z- handleDepth()- Depth_Keller msg: 1730864104.24 epoch sec; seqNo:6792; depth 50.40 2024-11-06T03:35:04Z- handleDVL() - DVL msg epochMillisec: 1730864104.27 2024-11-06T03:35:04Z- handleDvl() - RDI_Pathfinder msg: 1730864104.27 sec; seqNo:2333; #beams:4 2024-11-06T03:35:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.74 , 38.13 , 41.50 , 38.88 2024-11-06T03:35:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , 0.03 2024-11-06T03:35:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864104.28 epoch sec; seqNo:6792; 4061722.95 north; 594336.23 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:35:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864103.84 2024-11-06T03:35:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:04Z- waiting for 1730864107.84, time is 1730864104.28 2024-11-06T03:35:04Z- AHRS_M2 msg: 1730864104.64 epoch sec; seqNo:6377 2024-11-06T03:35:04Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.13 psi 2024-11-06T03:35:04Z- handleDepth()- Depth_Keller msg: 1730864104.64 epoch sec; seqNo:6793; depth 50.40 2024-11-06T03:35:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864104.66 epoch sec; seqNo:6793; 4061722.95 north; 594336.23 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:35:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864103.84 2024-11-06T03:35:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:04Z- waiting for 1730864107.84, time is 1730864104.66 2024-11-06T03:35:05Z- AHRS_M2 msg: 1730864105.05 epoch sec; seqNo:6378 2024-11-06T03:35:05Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.13 psi 2024-11-06T03:35:05Z- handleDepth()- Depth_Keller msg: 1730864105.06 epoch sec; seqNo:6794; depth 50.40 2024-11-06T03:35:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864105.07 epoch sec; seqNo:6794; 4061722.57 north; 594336.24 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:35:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864103.84 2024-11-06T03:35:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:05Z- waiting for 1730864107.84, time is 1730864105.07 2024-11-06T03:35:05Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:35:05Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:35:05Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:35:05Z- AHRS_M2 msg: 1730864105.43 epoch sec; seqNo:6379 2024-11-06T03:35:05Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.13 psi 2024-11-06T03:35:05Z- handleDepth()- Depth_Keller msg: 1730864105.46 epoch sec; seqNo:6795; depth 50.40 2024-11-06T03:35:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864105.47 epoch sec; seqNo:6795; 4061722.57 north; 594336.24 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:35:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864103.84 2024-11-06T03:35:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:05Z- waiting for 1730864107.84, time is 1730864105.47 2024-11-06T03:35:05Z- AHRS_M2 msg: 1730864105.84 epoch sec; seqNo:6380 2024-11-06T03:35:05Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.13 psi 2024-11-06T03:35:05Z- handleDepth()- Depth_Keller msg: 1730864105.85 epoch sec; seqNo:6796; depth 50.40 2024-11-06T03:35:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864105.86 epoch sec; seqNo:6796; 4061722.18 north; 594335.02 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:35:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864103.84 2024-11-06T03:35:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:06Z- waiting for 1730864107.84, time is 1730864105.86 2024-11-06T03:35:06Z- handleDepth()- Depth_Keller msg: 1730864106.27 epoch sec; seqNo:6797; depth 50.41 2024-11-06T03:35:06Z- handleDVL() - DVL msg epochMillisec: 1730864106.29 2024-11-06T03:35:06Z- handleDvl() - RDI_Pathfinder msg: 1730864106.29 sec; seqNo:2334; #beams:4 2024-11-06T03:35:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.49 , 38.13 , 41.87 , 39.25 2024-11-06T03:35:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , 0.03 2024-11-06T03:35:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864106.30 epoch sec; seqNo:6797; 4061722.18 north; 594335.02 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:35:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864103.84 2024-11-06T03:35:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:06Z- waiting for 1730864107.84, time is 1730864106.30 2024-11-06T03:35:06Z- AHRS_M2 msg: 1730864106.66 epoch sec; seqNo:6381 2024-11-06T03:35:06Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.13 psi 2024-11-06T03:35:06Z- handleDepth()- Depth_Keller msg: 1730864106.67 epoch sec; seqNo:6798; depth 50.42 2024-11-06T03:35:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864106.69 epoch sec; seqNo:6798; 4061721.80 north; 594335.03 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T03:35:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864103.84 2024-11-06T03:35:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:06Z- waiting for 1730864107.84, time is 1730864106.69 2024-11-06T03:35:07Z- AHRS_M2 msg: 1730864107.05 epoch sec; seqNo:6382 2024-11-06T03:35:07Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.13 psi 2024-11-06T03:35:07Z- handleDepth()- Depth_Keller msg: 1730864107.07 epoch sec; seqNo:6799; depth 50.41 2024-11-06T03:35:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864107.08 epoch sec; seqNo:6799; 4061721.80 north; 594335.03 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:35:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864103.84 2024-11-06T03:35:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:07Z- waiting for 1730864107.84, time is 1730864107.08 2024-11-06T03:35:07Z- AHRS_M2 msg: 1730864107.46 epoch sec; seqNo:6383 2024-11-06T03:35:07Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.13 psi 2024-11-06T03:35:07Z- handleDepth()- Depth_Keller msg: 1730864107.47 epoch sec; seqNo:6800; depth 50.40 2024-11-06T03:35:07Z- handleDVL() - DVL msg epochMillisec: 1730864107.47 2024-11-06T03:35:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:07Z- handleDvl() - RDI_Pathfinder msg: 1730864107.47 sec; seqNo:2335; #beams:0 2024-11-06T03:35:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , 0.03 2024-11-06T03:35:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864107.48 epoch sec; seqNo:6800; 4061721.41 north; 594333.81 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:35:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864103.84 2024-11-06T03:35:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:07Z- waiting for 1730864107.84, time is 1730864107.48 2024-11-06T03:35:07Z- AHRS_M2 msg: 1730864107.86 epoch sec; seqNo:6384 2024-11-06T03:35:07Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.13 psi 2024-11-06T03:35:07Z- handleDepth()- Depth_Keller msg: 1730864107.87 epoch sec; seqNo:6801; depth 50.40 2024-11-06T03:35:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864107.88 epoch sec; seqNo:6801; 4061721.03 north; 594333.81 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:35:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864103.84 2024-11-06T03:35:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:08Z- waiting for 1730864107.84, time is 1730864107.88 2024-11-06T03:35:08Z- LcmTrn::updateTrn() >>>> pose time 1730864107.88, meas time 1730864107.47 <<<< 2024-11-06T03:35:08Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864107.88 2024-11-06T03:35:08Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:35:08Z- TerrainNav::motionUpdate() - currEstimate = (4061721, 594333.81, 50.40) 2024-11-06T03:35:08Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:35:08Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:35:08Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:35:08Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:35:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:35:08Z- variances([0], [2], and psi_berg) = 0.116000 0.098661 0.000000 2024-11-06T03:35:08Z- publishEstimatesDecoder() - at 1730864107.88 sec 2024-11-06T03:35:08Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:35:08Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061748.77 594317.06 48.77 2024-11-06T03:35:08Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061748.58 594316.91 47.93 lat/lon: 36.70 -121.94 2024-11-06T03:35:08Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.12 0.10 1.47 0.00 2024-11-06T03:35:08Z- AHRS_M2 msg: 1730864108.27 epoch sec; seqNo:6385 2024-11-06T03:35:08Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.13 psi 2024-11-06T03:35:08Z- handleDepth()- Depth_Keller msg: 1730864108.27 epoch sec; seqNo:6802; depth 50.41 2024-11-06T03:35:08Z- handleDVL() - DVL msg epochMillisec: 1730864108.29 2024-11-06T03:35:08Z- handleDvl() - RDI_Pathfinder msg: 1730864108.29 sec; seqNo:2336; #beams:4 2024-11-06T03:35:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.23 , 38.13 , 42.06 , 39.63 2024-11-06T03:35:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , 0.03 2024-11-06T03:35:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864108.32 epoch sec; seqNo:6802; 4061721.03 north; 594333.81 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:35:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864107.88 2024-11-06T03:35:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:08Z- waiting for 1730864111.88, time is 1730864108.32 2024-11-06T03:35:08Z- AHRS_M2 msg: 1730864108.68 epoch sec; seqNo:6386 2024-11-06T03:35:08Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.13 psi 2024-11-06T03:35:08Z- handleDepth()- Depth_Keller msg: 1730864108.68 epoch sec; seqNo:6803; depth 50.39 2024-11-06T03:35:08Z- handleDVL() - DVL msg epochMillisec: 1730864108.69 2024-11-06T03:35:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:08Z- handleDvl() - RDI_Pathfinder msg: 1730864108.69 sec; seqNo:2337; #beams:0 2024-11-06T03:35:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , 0.03 2024-11-06T03:35:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864108.71 epoch sec; seqNo:6803; 4061720.65 north; 594333.82 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:35:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864107.88 2024-11-06T03:35:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:08Z- waiting for 1730864111.88, time is 1730864108.71 2024-11-06T03:35:09Z- AHRS_M2 msg: 1730864109.11 epoch sec; seqNo:6387 2024-11-06T03:35:09Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.13 psi 2024-11-06T03:35:09Z- handleDepth()- Depth_Keller msg: 1730864109.12 epoch sec; seqNo:6804; depth 50.37 2024-11-06T03:35:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864109.13 epoch sec; seqNo:6804; 4061720.64 north; 594332.60 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:35:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864107.88 2024-11-06T03:35:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:09Z- waiting for 1730864111.88, time is 1730864109.13 2024-11-06T03:35:09Z- handleDepth()- Depth_Keller msg: 1730864109.50 epoch sec; seqNo:6805; depth 50.38 2024-11-06T03:35:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864109.52 epoch sec; seqNo:6805; 4061720.26 north; 594332.60 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T03:35:09Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:35:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864107.88 2024-11-06T03:35:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:09Z- waiting for 1730864111.88, time is 1730864109.52 2024-11-06T03:35:09Z- AHRS_M2 msg: 1730864109.89 epoch sec; seqNo:6388 2024-11-06T03:35:09Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.14 psi 2024-11-06T03:35:09Z- handleDepth()- Depth_Keller msg: 1730864109.90 epoch sec; seqNo:6806; depth 50.37 2024-11-06T03:35:09Z- handleDVL() - DVL msg epochMillisec: 1730864109.90 2024-11-06T03:35:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:09Z- handleDvl() - RDI_Pathfinder msg: 1730864109.90 sec; seqNo:2338; #beams:0 2024-11-06T03:35:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , 0.03 2024-11-06T03:35:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864109.91 epoch sec; seqNo:6806; 4061720.26 north; 594332.60 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T03:35:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864107.88 2024-11-06T03:35:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:10Z- waiting for 1730864111.88, time is 1730864109.91 2024-11-06T03:35:10Z- AHRS_M2 msg: 1730864110.29 epoch sec; seqNo:6389 2024-11-06T03:35:10Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.14 psi 2024-11-06T03:35:10Z- handleDepth()- Depth_Keller msg: 1730864110.30 epoch sec; seqNo:6807; depth 50.35 2024-11-06T03:35:10Z- handleDVL() - DVL msg epochMillisec: 1730864110.32 2024-11-06T03:35:10Z- handleDvl() - RDI_Pathfinder msg: 1730864110.32 sec; seqNo:2339; #beams:4 2024-11-06T03:35:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.23 , 38.32 , 42.24 , 40.19 2024-11-06T03:35:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , 0.01 2024-11-06T03:35:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864110.34 epoch sec; seqNo:6807; 4061719.88 north; 594332.61 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:35:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864107.88 2024-11-06T03:35:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:10Z- waiting for 1730864111.88, time is 1730864110.34 2024-11-06T03:35:10Z- AHRS_M2 msg: 1730864110.70 epoch sec; seqNo:6390 2024-11-06T03:35:10Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.14 psi 2024-11-06T03:35:10Z- handleDepth()- Depth_Keller msg: 1730864110.70 epoch sec; seqNo:6808; depth 50.34 2024-11-06T03:35:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864110.72 epoch sec; seqNo:6808; 4061719.86 north; 594331.39 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:35:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864107.88 2024-11-06T03:35:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:10Z- waiting for 1730864111.88, time is 1730864110.72 2024-11-06T03:35:11Z- AHRS_M2 msg: 1730864111.09 epoch sec; seqNo:6391 2024-11-06T03:35:11Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.14 psi 2024-11-06T03:35:11Z- handleDepth()- Depth_Keller msg: 1730864111.12 epoch sec; seqNo:6809; depth 50.32 2024-11-06T03:35:11Z- handleDVL() - DVL msg epochMillisec: 1730864111.12 2024-11-06T03:35:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:11Z- handleDvl() - RDI_Pathfinder msg: 1730864111.12 sec; seqNo:2340; #beams:0 2024-11-06T03:35:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , 0.01 2024-11-06T03:35:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864111.12 epoch sec; seqNo:6809; 4061719.49 north; 594331.39 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:35:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864107.88 2024-11-06T03:35:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:11Z- waiting for 1730864111.88, time is 1730864111.12 2024-11-06T03:35:11Z- AHRS_M2 msg: 1730864111.50 epoch sec; seqNo:6392 2024-11-06T03:35:11Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.14 psi 2024-11-06T03:35:11Z- handleDepth()- Depth_Keller msg: 1730864111.51 epoch sec; seqNo:6810; depth 50.32 2024-11-06T03:35:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864111.54 epoch sec; seqNo:6810; 4061719.49 north; 594331.39 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T03:35:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864107.88 2024-11-06T03:35:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:11Z- waiting for 1730864111.88, time is 1730864111.54 2024-11-06T03:35:11Z- AHRS_M2 msg: 1730864111.90 epoch sec; seqNo:6393 2024-11-06T03:35:11Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.14 psi 2024-11-06T03:35:11Z- handleDepth()- Depth_Keller msg: 1730864111.91 epoch sec; seqNo:6811; depth 50.30 2024-11-06T03:35:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864111.92 epoch sec; seqNo:6811; 4061719.11 north; 594331.39 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T03:35:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864107.88 2024-11-06T03:35:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:12Z- waiting for 1730864111.88, time is 1730864111.92 2024-11-06T03:35:12Z- LcmTrn::updateTrn() >>>> pose time 1730864111.92, meas time 1730864111.12 <<<< 2024-11-06T03:35:12Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864111.92 2024-11-06T03:35:12Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:35:12Z- TerrainNav::motionUpdate() - currEstimate = (4061719, 594331.39, 50.30) 2024-11-06T03:35:12Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:35:12Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:35:12Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:35:12Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:35:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:35:12Z- variances([0], [2], and psi_berg) = 0.117475 0.100149 0.000000 2024-11-06T03:35:12Z- publishEstimatesDecoder() - at 1730864111.92 sec 2024-11-06T03:35:12Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:35:12Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061746.87 594314.64 48.56 2024-11-06T03:35:12Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061746.66 594314.49 47.82 lat/lon: 36.70 -121.94 2024-11-06T03:35:12Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.12 0.10 1.71 0.00 2024-11-06T03:35:12Z- AHRS_M2 msg: 1730864112.31 epoch sec; seqNo:6394 2024-11-06T03:35:12Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.14 psi 2024-11-06T03:35:12Z- handleDepth()- Depth_Keller msg: 1730864112.32 epoch sec; seqNo:6812; depth 50.29 2024-11-06T03:35:12Z- handleDVL() - DVL msg epochMillisec: 1730864112.35 2024-11-06T03:35:12Z- handleDvl() - RDI_Pathfinder msg: 1730864112.35 sec; seqNo:2341; #beams:4 2024-11-06T03:35:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.61 , 38.32 , 42.80 , 40.19 2024-11-06T03:35:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , 0.01 2024-11-06T03:35:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864112.36 epoch sec; seqNo:6812; 4061719.09 north; 594330.17 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:35:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864111.92 2024-11-06T03:35:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:12Z- waiting for 1730864115.92, time is 1730864112.36 2024-11-06T03:35:12Z- AHRS_M2 msg: 1730864112.71 epoch sec; seqNo:6395 2024-11-06T03:35:12Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.14 psi 2024-11-06T03:35:12Z- handleDepth()- Depth_Keller msg: 1730864112.72 epoch sec; seqNo:6813; depth 50.28 2024-11-06T03:35:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864112.73 epoch sec; seqNo:6813; 4061718.71 north; 594330.18 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T03:35:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864111.92 2024-11-06T03:35:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:12Z- waiting for 1730864115.92, time is 1730864112.73 2024-11-06T03:35:13Z- AHRS_M2 msg: 1730864113.11 epoch sec; seqNo:6396 2024-11-06T03:35:13Z- AHRS_M2 msg: 0.17 phi; -0.01 theta; 4.14 psi 2024-11-06T03:35:13Z- handleDepth()- Depth_Keller msg: 1730864113.13 epoch sec; seqNo:6814; depth 50.26 2024-11-06T03:35:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864113.14 epoch sec; seqNo:6814; 4061718.34 north; 594330.18 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:35:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864111.92 2024-11-06T03:35:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:13Z- waiting for 1730864115.92, time is 1730864113.14 2024-11-06T03:35:13Z- AHRS_M2 msg: 1730864113.52 epoch sec; seqNo:6397 2024-11-06T03:35:13Z- AHRS_M2 msg: 0.17 phi; -0.01 theta; 4.14 psi 2024-11-06T03:35:13Z- handleDepth()- Depth_Keller msg: 1730864113.52 epoch sec; seqNo:6815; depth 50.25 2024-11-06T03:35:13Z- handleDVL() - DVL msg epochMillisec: 1730864113.53 2024-11-06T03:35:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:13Z- handleDvl() - RDI_Pathfinder msg: 1730864113.53 sec; seqNo:2342; #beams:0 2024-11-06T03:35:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , 0.01 2024-11-06T03:35:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864113.55 epoch sec; seqNo:6815; 4061718.34 north; 594330.18 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T03:35:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864111.92 2024-11-06T03:35:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:13Z- waiting for 1730864115.92, time is 1730864113.55 2024-11-06T03:35:13Z- AHRS_M2 msg: 1730864113.93 epoch sec; seqNo:6398 2024-11-06T03:35:13Z- AHRS_M2 msg: 0.17 phi; -0.01 theta; 4.14 psi 2024-11-06T03:35:13Z- handleDepth()- Depth_Keller msg: 1730864113.94 epoch sec; seqNo:6816; depth 50.24 2024-11-06T03:35:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864114.02 epoch sec; seqNo:6816; 4061717.94 north; 594328.97 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:35:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864111.92 2024-11-06T03:35:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:14Z- waiting for 1730864115.92, time is 1730864114.02 2024-11-06T03:35:14Z- AHRS_M2 msg: 1730864114.33 epoch sec; seqNo:6399 2024-11-06T03:35:14Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.15 psi 2024-11-06T03:35:14Z- handleDepth()- Depth_Keller msg: 1730864114.34 epoch sec; seqNo:6817; depth 50.23 2024-11-06T03:35:14Z- handleDVL() - DVL msg epochMillisec: 1730864114.36 2024-11-06T03:35:14Z- handleDvl() - RDI_Pathfinder msg: 1730864114.36 sec; seqNo:2343; #beams:4 2024-11-06T03:35:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 38.54 , 42.41 , 40.58 2024-11-06T03:35:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , 0.00 2024-11-06T03:35:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864114.37 epoch sec; seqNo:6817; 4061717.94 north; 594328.97 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T03:35:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864111.92 2024-11-06T03:35:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:14Z- waiting for 1730864115.92, time is 1730864114.37 2024-11-06T03:35:14Z- AHRS_M2 msg: 1730864114.73 epoch sec; seqNo:6400 2024-11-06T03:35:14Z- AHRS_M2 msg: 0.17 phi; -0.01 theta; 4.15 psi 2024-11-06T03:35:14Z- handleDepth()- Depth_Keller msg: 1730864114.74 epoch sec; seqNo:6818; depth 50.21 2024-11-06T03:35:14Z- handleDVL() - DVL msg epochMillisec: 1730864114.74 2024-11-06T03:35:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:14Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:14Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:14Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:14Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:14Z- handleDvl() - RDI_Pathfinder msg: 1730864114.74 sec; seqNo:2344; #beams:0 2024-11-06T03:35:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , 0.00 2024-11-06T03:35:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864114.76 epoch sec; seqNo:6818; 4061717.56 north; 594328.97 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T03:35:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864111.92 2024-11-06T03:35:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:14Z- waiting for 1730864115.92, time is 1730864114.76 2024-11-06T03:35:15Z- AHRS_M2 msg: 1730864115.13 epoch sec; seqNo:6401 2024-11-06T03:35:15Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.15 psi 2024-11-06T03:35:15Z- handleDepth()- Depth_Keller msg: 1730864115.15 epoch sec; seqNo:6819; depth 50.20 2024-11-06T03:35:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864115.15 epoch sec; seqNo:6819; 4061717.55 north; 594327.75 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:35:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864111.92 2024-11-06T03:35:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:15Z- waiting for 1730864115.92, time is 1730864115.15 2024-11-06T03:35:15Z- AHRS_M2 msg: 1730864115.54 epoch sec; seqNo:6402 2024-11-06T03:35:15Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.15 psi 2024-11-06T03:35:15Z- handleDepth()- Depth_Keller msg: 1730864115.54 epoch sec; seqNo:6820; depth 50.18 2024-11-06T03:35:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864115.55 epoch sec; seqNo:6820; 4061717.17 north; 594327.75 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:35:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864111.92 2024-11-06T03:35:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:15Z- waiting for 1730864115.92, time is 1730864115.55 2024-11-06T03:35:16Z- AHRS_M2 msg: 1730864115.99 epoch sec; seqNo:6403 2024-11-06T03:35:16Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.15 psi 2024-11-06T03:35:16Z- handleDepth()- Depth_Keller msg: 1730864116.01 epoch sec; seqNo:6821; depth 50.19 2024-11-06T03:35:16Z- handleDVL() - DVL msg epochMillisec: 1730864116.04 2024-11-06T03:35:16Z- handleDvl() - RDI_Pathfinder msg: 1730864116.04 sec; seqNo:2345; #beams:4 2024-11-06T03:35:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 38.13 , 42.41 , 40.37 2024-11-06T03:35:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , 0.01 2024-11-06T03:35:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864116.05 epoch sec; seqNo:6821; 4061717.17 north; 594327.75 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:35:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864111.92 2024-11-06T03:35:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:16Z- waiting for 1730864115.92, time is 1730864116.05 2024-11-06T03:35:16Z- LcmTrn::updateTrn() >>>> pose time 1730864116.05, meas time 1730864116.04 <<<< 2024-11-06T03:35:16Z- LcmTrn::updateTrn() equating pose 1730864116.04 and meas times 1730864116.04 2024-11-06T03:35:16Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730864116.04 2024-11-06T03:35:16Z- TerrainNav::motionUpdate() - currEstimate = (4061717, 594327.75, 50.19) 2024-11-06T03:35:16Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:35:16Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:35:16Z- TNavPF::Projecting Measurements 2024-11-06T03:35:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:35:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:35:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:35:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:35:16Z- TNavFilter::4 good beams. 2024-11-06T03:35:16Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:35:16Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:35:16Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:35:16Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:35:16Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:35:16Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:35:16Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:35:16Z- TerrainMapOctree: (vehN, vehE ) = (4061717.17, 594327.75). 2024-11-06T03:35:16Z- TerrainMapOctree: Min Distance = 84.40. 2024-11-06T03:35:16Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:35:16Z- Forcing Subcloud Comparison 2024-11-06T03:35:16Z- Weighting particles with subcloud comparison 2024-11-06T03:35:16Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:16Z- meanExpectedMeasDiff: 3.576428 delta_rms_squared: 2.245836 alpha: 0.991173 2024-11-06T03:35:16Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:16Z- meanExpectedMeasDiff: -5.766676 delta_rms_squared: 2.804779 alpha: 0.992920 2024-11-06T03:35:16Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:16Z- meanExpectedMeasDiff: -6.193589 delta_rms_squared: 2.904032 alpha: 0.993160 2024-11-06T03:35:16Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:16Z- meanExpectedMeasDiff: 3.072678 delta_rms_squared: 1.639404 alpha: 0.987947 2024-11-06T03:35:16Z- Subcloud NIS: 0.863053 2024-11-06T03:35:16Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:35:16Z- TNavPF:: sumWeights = 0.000051 2024-11-06T03:35:16Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:35:16Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:35:16Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:35:16Z- TerrainNav::Resampling particle filter... 2024-11-06T03:35:16Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730864116.04 sec, ping # 0. 2024-11-06T03:35:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:35:16Z- variances([0], [2], and psi_berg) = 0.156426 0.113628 0.000000 2024-11-06T03:35:16Z- publishEstimatesDecoder() - at 1730864116.04 sec 2024-11-06T03:35:16Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:35:16Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061744.96 594310.99 48.22 2024-11-06T03:35:16Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061744.80 594310.87 48.67 lat/lon: 36.70 -121.94 2024-11-06T03:35:16Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.11 0.60 0.00 2024-11-06T03:35:16Z- AHRS_M2 msg: 1730864116.36 epoch sec; seqNo:6404 2024-11-06T03:35:16Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.15 psi 2024-11-06T03:35:16Z- handleDepth()- Depth_Keller msg: 1730864116.36 epoch sec; seqNo:6822; depth 50.18 2024-11-06T03:35:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864116.41 epoch sec; seqNo:6822; 4061716.79 north; 594327.76 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:35:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864116.04 2024-11-06T03:35:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:16Z- waiting for 1730864120.04, time is 1730864116.41 2024-11-06T03:35:16Z- AHRS_M2 msg: 1730864116.75 epoch sec; seqNo:6405 2024-11-06T03:35:16Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.15 psi 2024-11-06T03:35:16Z- handleDepth()- Depth_Keller msg: 1730864116.76 epoch sec; seqNo:6823; depth 50.18 2024-11-06T03:35:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864116.77 epoch sec; seqNo:6823; 4061716.78 north; 594326.54 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:35:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864116.04 2024-11-06T03:35:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:16Z- waiting for 1730864120.04, time is 1730864116.77 2024-11-06T03:35:17Z- AHRS_M2 msg: 1730864117.15 epoch sec; seqNo:6406 2024-11-06T03:35:17Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.15 psi 2024-11-06T03:35:17Z- handleDepth()- Depth_Keller msg: 1730864117.16 epoch sec; seqNo:6824; depth 50.17 2024-11-06T03:35:17Z- handleDVL() - DVL msg epochMillisec: 1730864117.17 2024-11-06T03:35:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:17Z- handleDvl() - RDI_Pathfinder msg: 1730864117.17 sec; seqNo:2346; #beams:0 2024-11-06T03:35:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , 0.01 2024-11-06T03:35:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864117.17 epoch sec; seqNo:6824; 4061716.40 north; 594326.54 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:35:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864116.04 2024-11-06T03:35:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:17Z- waiting for 1730864120.04, time is 1730864117.17 2024-11-06T03:35:17Z- AHRS_M2 msg: 1730864117.56 epoch sec; seqNo:6407 2024-11-06T03:35:17Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.15 psi 2024-11-06T03:35:17Z- handleDepth()- Depth_Keller msg: 1730864117.57 epoch sec; seqNo:6825; depth 50.16 2024-11-06T03:35:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864117.58 epoch sec; seqNo:6825; 4061716.40 north; 594326.54 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:35:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864116.04 2024-11-06T03:35:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:17Z- waiting for 1730864120.04, time is 1730864117.58 2024-11-06T03:35:18Z- AHRS_M2 msg: 1730864118.00 epoch sec; seqNo:6408 2024-11-06T03:35:18Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.15 psi 2024-11-06T03:35:18Z- handleDepth()- Depth_Keller msg: 1730864118.01 epoch sec; seqNo:6826; depth 50.18 2024-11-06T03:35:18Z- handleDVL() - DVL msg epochMillisec: 1730864118.03 2024-11-06T03:35:18Z- handleDvl() - RDI_Pathfinder msg: 1730864118.03 sec; seqNo:2347; #beams:4 2024-11-06T03:35:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.05 , 38.69 , 42.24 , 40.56 2024-11-06T03:35:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.06 , 0.03 2024-11-06T03:35:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864118.06 epoch sec; seqNo:6826; 4061716.01 north; 594325.33 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:35:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864116.04 2024-11-06T03:35:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:18Z- waiting for 1730864120.04, time is 1730864118.06 2024-11-06T03:35:18Z- handleDepth()- Depth_Keller msg: 1730864118.39 epoch sec; seqNo:6827; depth 50.16 2024-11-06T03:35:18Z- handleDVL() - DVL msg epochMillisec: 1730864118.39 2024-11-06T03:35:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:18Z- handleDvl() - RDI_Pathfinder msg: 1730864118.39 sec; seqNo:2348; #beams:0 2024-11-06T03:35:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.06 , 0.03 2024-11-06T03:35:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864118.42 epoch sec; seqNo:6827; 4061716.01 north; 594325.33 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:35:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864116.04 2024-11-06T03:35:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:18Z- waiting for 1730864120.04, time is 1730864118.42 2024-11-06T03:35:18Z- AHRS_M2 msg: 1730864118.77 epoch sec; seqNo:6409 2024-11-06T03:35:18Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.15 psi 2024-11-06T03:35:18Z- handleDepth()- Depth_Keller msg: 1730864118.78 epoch sec; seqNo:6828; depth 50.17 2024-11-06T03:35:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864118.79 epoch sec; seqNo:6828; 4061715.63 north; 594325.33 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:35:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864116.04 2024-11-06T03:35:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:18Z- waiting for 1730864120.04, time is 1730864118.79 2024-11-06T03:35:19Z- AHRS_M2 msg: 1730864119.17 epoch sec; seqNo:6410 2024-11-06T03:35:19Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.15 psi 2024-11-06T03:35:19Z- handleDepth()- Depth_Keller msg: 1730864119.18 epoch sec; seqNo:6829; depth 50.17 2024-11-06T03:35:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864119.20 epoch sec; seqNo:6829; 4061715.63 north; 594325.33 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:35:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864116.04 2024-11-06T03:35:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:19Z- waiting for 1730864120.04, time is 1730864119.20 2024-11-06T03:35:19Z- AHRS_M2 msg: 1730864119.58 epoch sec; seqNo:6411 2024-11-06T03:35:19Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.15 psi 2024-11-06T03:35:19Z- handleDepth()- Depth_Keller msg: 1730864119.59 epoch sec; seqNo:6830; depth 50.17 2024-11-06T03:35:19Z- handleDVL() - DVL msg epochMillisec: 1730864119.59 2024-11-06T03:35:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:19Z- handleDvl() - RDI_Pathfinder msg: 1730864119.59 sec; seqNo:2349; #beams:0 2024-11-06T03:35:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.06 , 0.03 2024-11-06T03:35:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864119.60 epoch sec; seqNo:6830; 4061715.24 north; 594324.11 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:35:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864116.04 2024-11-06T03:35:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:19Z- waiting for 1730864120.04, time is 1730864119.60 2024-11-06T03:35:20Z- AHRS_M2 msg: 1730864120.01 epoch sec; seqNo:6412 2024-11-06T03:35:20Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.15 psi 2024-11-06T03:35:20Z- handleDepth()- Depth_Keller msg: 1730864120.02 epoch sec; seqNo:6831; depth 50.19 2024-11-06T03:35:20Z- handleDVL() - DVL msg epochMillisec: 1730864120.06 2024-11-06T03:35:20Z- handleDvl() - RDI_Pathfinder msg: 1730864120.06 sec; seqNo:2350; #beams:4 2024-11-06T03:35:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.61 , 38.88 , 42.43 , 40.37 2024-11-06T03:35:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , 0.06 2024-11-06T03:35:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864120.07 epoch sec; seqNo:6831; 4061715.24 north; 594324.11 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:35:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864116.04 2024-11-06T03:35:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:20Z- waiting for 1730864120.04, time is 1730864120.07 2024-11-06T03:35:20Z- LcmTrn::updateTrn() >>>> pose time 1730864120.07, meas time 1730864120.06 <<<< 2024-11-06T03:35:20Z- LcmTrn::updateTrn() equating pose 1730864120.06 and meas times 1730864120.06 2024-11-06T03:35:20Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730864120.06 2024-11-06T03:35:20Z- TerrainNav::motionUpdate() - currEstimate = (4061715, 594324.11, 50.19) 2024-11-06T03:35:20Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:35:20Z- TerrainNav:: SubcloudNIS is 0.86, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:35:20Z- TNavPF::Projecting Measurements 2024-11-06T03:35:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:35:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:35:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:35:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:35:20Z- TNavFilter::4 good beams. 2024-11-06T03:35:20Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:35:20Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:35:20Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:35:20Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:35:20Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:35:20Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:35:20Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:35:20Z- TerrainMapOctree: (vehN, vehE ) = (4061715.24, 594324.11). 2024-11-06T03:35:20Z- TerrainMapOctree: Min Distance = 81.86. 2024-11-06T03:35:20Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:35:20Z- Forcing Subcloud Comparison 2024-11-06T03:35:20Z- Weighting particles with subcloud comparison 2024-11-06T03:35:20Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:20Z- meanExpectedMeasDiff: 4.114592 delta_rms_squared: 0.783642 alpha: 0.975113 2024-11-06T03:35:20Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:20Z- meanExpectedMeasDiff: -5.267346 delta_rms_squared: 1.670700 alpha: 0.988171 2024-11-06T03:35:20Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:20Z- meanExpectedMeasDiff: -5.003531 delta_rms_squared: 1.100914 alpha: 0.982157 2024-11-06T03:35:20Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:20Z- meanExpectedMeasDiff: 3.554508 delta_rms_squared: 2.258284 alpha: 0.991221 2024-11-06T03:35:20Z- Subcloud NIS: 0.920044 2024-11-06T03:35:20Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:35:20Z- TNavPF:: sumWeights = 0.000177 2024-11-06T03:35:20Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:35:20Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:35:20Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:35:20Z- TerrainNav::Resampling particle filter... 2024-11-06T03:35:20Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730864120.06 sec, ping # 0. 2024-11-06T03:35:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:35:20Z- variances([0], [2], and psi_berg) = 0.123265 0.123005 0.000000 2024-11-06T03:35:20Z- publishEstimatesDecoder() - at 1730864120.06 sec 2024-11-06T03:35:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:35:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061742.93 594307.33 48.52 2024-11-06T03:35:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061743.01 594307.33 49.19 lat/lon: 36.70 -121.94 2024-11-06T03:35:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.12 0.12 0.36 0.00 2024-11-06T03:35:20Z- AHRS_M2 msg: 1730864120.40 epoch sec; seqNo:6413 2024-11-06T03:35:20Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.15 psi 2024-11-06T03:35:20Z- handleDepth()- Depth_Keller msg: 1730864120.40 epoch sec; seqNo:6832; depth 50.18 2024-11-06T03:35:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864120.41 epoch sec; seqNo:6832; 4061714.86 north; 594324.12 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:35:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864120.06 2024-11-06T03:35:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:20Z- waiting for 1730864124.06, time is 1730864120.41 2024-11-06T03:35:20Z- AHRS_M2 msg: 1730864120.79 epoch sec; seqNo:6414 2024-11-06T03:35:20Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.15 psi 2024-11-06T03:35:20Z- handleDepth()- Depth_Keller msg: 1730864120.80 epoch sec; seqNo:6833; depth 50.20 2024-11-06T03:35:20Z- handleDVL() - DVL msg epochMillisec: 1730864120.80 2024-11-06T03:35:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:20Z- handleDvl() - RDI_Pathfinder msg: 1730864120.80 sec; seqNo:2351; #beams:0 2024-11-06T03:35:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.01 , 0.06 2024-11-06T03:35:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864120.83 epoch sec; seqNo:6833; 4061714.86 north; 594324.12 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:35:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864120.06 2024-11-06T03:35:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:20Z- waiting for 1730864124.06, time is 1730864120.83 2024-11-06T03:35:21Z- AHRS_M2 msg: 1730864121.20 epoch sec; seqNo:6415 2024-11-06T03:35:21Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.15 psi 2024-11-06T03:35:21Z- handleDepth()- Depth_Keller msg: 1730864121.21 epoch sec; seqNo:6834; depth 50.19 2024-11-06T03:35:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864121.21 epoch sec; seqNo:6834; 4061714.47 north; 594322.90 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:35:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864120.06 2024-11-06T03:35:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:21Z- waiting for 1730864124.06, time is 1730864121.21 2024-11-06T03:35:21Z- AHRS_M2 msg: 1730864121.60 epoch sec; seqNo:6416 2024-11-06T03:35:21Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.15 psi 2024-11-06T03:35:21Z- handleDepth()- Depth_Keller msg: 1730864121.61 epoch sec; seqNo:6835; depth 50.19 2024-11-06T03:35:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864121.62 epoch sec; seqNo:6835; 4061714.47 north; 594322.90 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:35:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864120.06 2024-11-06T03:35:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:21Z- waiting for 1730864124.06, time is 1730864121.62 2024-11-06T03:35:22Z- AHRS_M2 msg: 1730864122.02 epoch sec; seqNo:6417 2024-11-06T03:35:22Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.15 psi 2024-11-06T03:35:22Z- handleDepth()- Depth_Keller msg: 1730864122.02 epoch sec; seqNo:6836; depth 50.20 2024-11-06T03:35:22Z- handleDVL() - DVL msg epochMillisec: 1730864122.05 2024-11-06T03:35:22Z- handleDvl() - RDI_Pathfinder msg: 1730864122.05 sec; seqNo:2352; #beams:4 2024-11-06T03:35:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.47 , 39.15 , 42.62 , 40.99 2024-11-06T03:35:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , 0.07 2024-11-06T03:35:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864122.06 epoch sec; seqNo:6836; 4061714.09 north; 594322.91 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:35:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864120.06 2024-11-06T03:35:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:22Z- waiting for 1730864124.06, time is 1730864122.06 2024-11-06T03:35:22Z- AHRS_M2 msg: 1730864122.41 epoch sec; seqNo:6418 2024-11-06T03:35:22Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.15 psi 2024-11-06T03:35:22Z- handleDepth()- Depth_Keller msg: 1730864122.41 epoch sec; seqNo:6837; depth 50.19 2024-11-06T03:35:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864122.44 epoch sec; seqNo:6837; 4061714.09 north; 594322.91 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:35:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864120.06 2024-11-06T03:35:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:22Z- waiting for 1730864124.06, time is 1730864122.44 2024-11-06T03:35:22Z- AHRS_M2 msg: 1730864122.81 epoch sec; seqNo:6419 2024-11-06T03:35:22Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.15 psi 2024-11-06T03:35:22Z- handleDepth()- Depth_Keller msg: 1730864122.82 epoch sec; seqNo:6838; depth 50.19 2024-11-06T03:35:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864122.83 epoch sec; seqNo:6838; 4061713.70 north; 594321.69 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:35:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864120.06 2024-11-06T03:35:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:22Z- waiting for 1730864124.06, time is 1730864122.83 2024-11-06T03:35:23Z- AHRS_M2 msg: 1730864123.22 epoch sec; seqNo:6420 2024-11-06T03:35:23Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.15 psi 2024-11-06T03:35:23Z- handleDepth()- Depth_Keller msg: 1730864123.22 epoch sec; seqNo:6839; depth 50.18 2024-11-06T03:35:23Z- handleDVL() - DVL msg epochMillisec: 1730864123.22 2024-11-06T03:35:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:23Z- handleDvl() - RDI_Pathfinder msg: 1730864123.22 sec; seqNo:2353; #beams:0 2024-11-06T03:35:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , 0.07 2024-11-06T03:35:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864123.23 epoch sec; seqNo:6839; 4061713.32 north; 594321.69 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:35:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864120.06 2024-11-06T03:35:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:23Z- waiting for 1730864124.06, time is 1730864123.23 2024-11-06T03:35:23Z- AHRS_M2 msg: 1730864123.62 epoch sec; seqNo:6421 2024-11-06T03:35:23Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.15 psi 2024-11-06T03:35:23Z- handleDepth()- Depth_Keller msg: 1730864123.63 epoch sec; seqNo:6840; depth 50.19 2024-11-06T03:35:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864123.65 epoch sec; seqNo:6840; 4061713.32 north; 594321.69 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:35:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864120.06 2024-11-06T03:35:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:23Z- waiting for 1730864124.06, time is 1730864123.65 2024-11-06T03:35:24Z- AHRS_M2 msg: 1730864124.02 epoch sec; seqNo:6422 2024-11-06T03:35:24Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.16 psi 2024-11-06T03:35:24Z- handleDepth()- Depth_Keller msg: 1730864124.04 epoch sec; seqNo:6841; depth 50.18 2024-11-06T03:35:24Z- handleDVL() - DVL msg epochMillisec: 1730864124.07 2024-11-06T03:35:24Z- handleDvl() - RDI_Pathfinder msg: 1730864124.07 sec; seqNo:2354; #beams:4 2024-11-06T03:35:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.29 , 39.35 , 42.82 , 40.99 2024-11-06T03:35:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.05 2024-11-06T03:35:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864124.12 epoch sec; seqNo:6841; 4061712.92 north; 594320.48 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:35:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864120.06 2024-11-06T03:35:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:24Z- waiting for 1730864124.06, time is 1730864124.12 2024-11-06T03:35:24Z- LcmTrn::updateTrn() >>>> pose time 1730864124.12, meas time 1730864124.07 <<<< 2024-11-06T03:35:24Z- LcmTrn::updateTrn() equating pose 1730864124.07 and meas times 1730864124.07 2024-11-06T03:35:24Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730864124.07 2024-11-06T03:35:24Z- TerrainNav::motionUpdate() - currEstimate = (4061713, 594320.48, 50.18) 2024-11-06T03:35:24Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:35:24Z- TerrainNav:: SubcloudNIS is 0.92, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:35:24Z- TNavPF::Projecting Measurements 2024-11-06T03:35:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:35:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:35:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:35:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:35:24Z- TNavFilter::4 good beams. 2024-11-06T03:35:24Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:35:24Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:35:24Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:35:24Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:35:24Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:35:24Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:35:24Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:35:24Z- TerrainMapOctree: (vehN, vehE ) = (4061712.92, 594320.48). 2024-11-06T03:35:24Z- TerrainMapOctree: Min Distance = 79.64. 2024-11-06T03:35:24Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:35:24Z- Forcing Subcloud Comparison 2024-11-06T03:35:24Z- Weighting particles with subcloud comparison 2024-11-06T03:35:24Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:24Z- meanExpectedMeasDiff: 3.232076 delta_rms_squared: 0.971967 alpha: 0.979838 2024-11-06T03:35:24Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:24Z- meanExpectedMeasDiff: -5.609314 delta_rms_squared: 1.318951 alpha: 0.985063 2024-11-06T03:35:24Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:24Z- meanExpectedMeasDiff: -4.659014 delta_rms_squared: 1.633649 alpha: 0.987906 2024-11-06T03:35:24Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:24Z- meanExpectedMeasDiff: 6.097698 delta_rms_squared: 0.812643 alpha: 0.975980 2024-11-06T03:35:24Z- Subcloud NIS: 0.955316 2024-11-06T03:35:24Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:35:24Z- TNavPF:: sumWeights = 0.000019 2024-11-06T03:35:24Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:35:24Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:35:24Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:35:24Z- TerrainNav::Resampling particle filter... 2024-11-06T03:35:24Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730864124.07 sec, ping # 0. 2024-11-06T03:35:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:35:24Z- variances([0], [2], and psi_berg) = 0.114958 0.087191 0.000000 2024-11-06T03:35:24Z- publishEstimatesDecoder() - at 1730864124.07 sec 2024-11-06T03:35:24Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:35:24Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061740.78 594303.77 49.49 2024-11-06T03:35:24Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061740.65 594303.89 49.45 lat/lon: 36.70 -121.94 2024-11-06T03:35:24Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.11 0.09 0.31 0.00 2024-11-06T03:35:24Z- AHRS_M2 msg: 1730864124.42 epoch sec; seqNo:6423 2024-11-06T03:35:24Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.16 psi 2024-11-06T03:35:24Z- handleDepth()- Depth_Keller msg: 1730864124.43 epoch sec; seqNo:6842; depth 50.18 2024-11-06T03:35:24Z- handleDVL() - DVL msg epochMillisec: 1730864124.44 2024-11-06T03:35:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:24Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:24Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:24Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:24Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:24Z- handleDvl() - RDI_Pathfinder msg: 1730864124.44 sec; seqNo:2355; #beams:0 2024-11-06T03:35:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.05 2024-11-06T03:35:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864124.45 epoch sec; seqNo:6842; 4061712.92 north; 594320.48 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:35:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864124.07 2024-11-06T03:35:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:24Z- waiting for 1730864128.07, time is 1730864124.45 2024-11-06T03:35:24Z- AHRS_M2 msg: 1730864124.83 epoch sec; seqNo:6424 2024-11-06T03:35:24Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.16 psi 2024-11-06T03:35:24Z- handleDepth()- Depth_Keller msg: 1730864124.84 epoch sec; seqNo:6843; depth 50.17 2024-11-06T03:35:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864124.85 epoch sec; seqNo:6843; 4061712.55 north; 594320.48 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:35:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864124.07 2024-11-06T03:35:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:24Z- waiting for 1730864128.07, time is 1730864124.85 2024-11-06T03:35:25Z- AHRS_M2 msg: 1730864125.23 epoch sec; seqNo:6425 2024-11-06T03:35:25Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.16 psi 2024-11-06T03:35:25Z- handleDepth()- Depth_Keller msg: 1730864125.24 epoch sec; seqNo:6844; depth 50.17 2024-11-06T03:35:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864125.26 epoch sec; seqNo:6844; 4061712.55 north; 594320.48 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:35:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864124.07 2024-11-06T03:35:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:25Z- waiting for 1730864128.07, time is 1730864125.26 2024-11-06T03:35:25Z- AHRS_M2 msg: 1730864125.64 epoch sec; seqNo:6426 2024-11-06T03:35:25Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.16 psi 2024-11-06T03:35:25Z- handleDepth()- Depth_Keller msg: 1730864125.65 epoch sec; seqNo:6845; depth 50.16 2024-11-06T03:35:25Z- handleDVL() - DVL msg epochMillisec: 1730864125.65 2024-11-06T03:35:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:25Z- handleDvl() - RDI_Pathfinder msg: 1730864125.65 sec; seqNo:2356; #beams:0 2024-11-06T03:35:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.05 2024-11-06T03:35:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864125.66 epoch sec; seqNo:6845; 4061712.17 north; 594320.49 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:35:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864124.07 2024-11-06T03:35:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:25Z- waiting for 1730864128.07, time is 1730864125.66 2024-11-06T03:35:26Z- AHRS_M2 msg: 1730864126.05 epoch sec; seqNo:6427 2024-11-06T03:35:26Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.16 psi 2024-11-06T03:35:26Z- handleDepth()- Depth_Keller msg: 1730864126.06 epoch sec; seqNo:6846; depth 50.14 2024-11-06T03:35:26Z- handleDVL() - DVL msg epochMillisec: 1730864126.10 2024-11-06T03:35:26Z- handleDvl() - RDI_Pathfinder msg: 1730864126.10 sec; seqNo:2357; #beams:4 2024-11-06T03:35:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.88 , 38.95 , 42.82 , 40.78 2024-11-06T03:35:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.01 2024-11-06T03:35:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864126.11 epoch sec; seqNo:6846; 4061712.15 north; 594319.27 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:35:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864124.07 2024-11-06T03:35:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:26Z- waiting for 1730864128.07, time is 1730864126.11 2024-11-06T03:35:26Z- AHRS_M2 msg: 1730864126.46 epoch sec; seqNo:6428 2024-11-06T03:35:26Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.17 psi 2024-11-06T03:35:26Z- handleDepth()- Depth_Keller msg: 1730864126.47 epoch sec; seqNo:6847; depth 50.12 2024-11-06T03:35:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864126.48 epoch sec; seqNo:6847; 4061711.77 north; 594319.27 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:35:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864124.07 2024-11-06T03:35:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:26Z- waiting for 1730864128.07, time is 1730864126.48 2024-11-06T03:35:26Z- handleDepth()- Depth_Keller msg: 1730864126.87 epoch sec; seqNo:6848; depth 50.12 2024-11-06T03:35:26Z- handleDVL() - DVL msg epochMillisec: 1730864126.88 2024-11-06T03:35:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:26Z- handleDvl() - RDI_Pathfinder msg: 1730864126.88 sec; seqNo:2358; #beams:0 2024-11-06T03:35:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.01 2024-11-06T03:35:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864126.88 epoch sec; seqNo:6848; 4061711.77 north; 594319.27 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:35:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864124.07 2024-11-06T03:35:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:27Z- waiting for 1730864128.07, time is 1730864126.88 2024-11-06T03:35:27Z- AHRS_M2 msg: 1730864127.26 epoch sec; seqNo:6429 2024-11-06T03:35:27Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.17 psi 2024-11-06T03:35:27Z- handleDepth()- Depth_Keller msg: 1730864127.27 epoch sec; seqNo:6849; depth 50.09 2024-11-06T03:35:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864127.28 epoch sec; seqNo:6849; 4061711.40 north; 594319.27 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:35:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864124.07 2024-11-06T03:35:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:27Z- waiting for 1730864128.07, time is 1730864127.28 2024-11-06T03:35:27Z- AHRS_M2 msg: 1730864127.66 epoch sec; seqNo:6430 2024-11-06T03:35:27Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.17 psi 2024-11-06T03:35:27Z- handleDepth()- Depth_Keller msg: 1730864127.67 epoch sec; seqNo:6850; depth 50.07 2024-11-06T03:35:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864127.68 epoch sec; seqNo:6850; 4061711.38 north; 594318.05 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:35:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864124.07 2024-11-06T03:35:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:27Z- waiting for 1730864128.07, time is 1730864127.68 2024-11-06T03:35:28Z- AHRS_M2 msg: 1730864128.08 epoch sec; seqNo:6431 2024-11-06T03:35:28Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.18 psi 2024-11-06T03:35:28Z- handleDepth()- Depth_Keller msg: 1730864128.09 epoch sec; seqNo:6851; depth 50.05 2024-11-06T03:35:28Z- handleDVL() - DVL msg epochMillisec: 1730864128.11 2024-11-06T03:35:28Z- handleDvl() - RDI_Pathfinder msg: 1730864128.11 sec; seqNo:2359; #beams:4 2024-11-06T03:35:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.29 , 39.35 , 42.62 , 41.19 2024-11-06T03:35:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , -0.04 2024-11-06T03:35:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864128.13 epoch sec; seqNo:6851; 4061711.00 north; 594318.06 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:35:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864124.07 2024-11-06T03:35:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:28Z- waiting for 1730864128.07, time is 1730864128.13 2024-11-06T03:35:28Z- LcmTrn::updateTrn() >>>> pose time 1730864128.13, meas time 1730864128.11 <<<< 2024-11-06T03:35:28Z- LcmTrn::updateTrn() equating pose 1730864128.11 and meas times 1730864128.11 2024-11-06T03:35:28Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730864128.11 2024-11-06T03:35:28Z- TerrainNav::motionUpdate() - currEstimate = (4061711, 594318.06, 50.05) 2024-11-06T03:35:28Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:35:28Z- TerrainNav:: SubcloudNIS is 0.96, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:35:28Z- TNavPF::Projecting Measurements 2024-11-06T03:35:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:35:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:35:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:35:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:35:28Z- TNavFilter::4 good beams. 2024-11-06T03:35:28Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:35:28Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:35:28Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:35:28Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:35:28Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:35:28Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:35:28Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:35:28Z- TerrainMapOctree: (vehN, vehE ) = (4061711.00, 594318.06). 2024-11-06T03:35:28Z- TerrainMapOctree: Min Distance = 78.44. 2024-11-06T03:35:28Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:35:28Z- Forcing Subcloud Comparison 2024-11-06T03:35:28Z- Weighting particles with subcloud comparison 2024-11-06T03:35:28Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:28Z- meanExpectedMeasDiff: 2.900202 delta_rms_squared: 0.854691 alpha: 0.977135 2024-11-06T03:35:28Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:28Z- meanExpectedMeasDiff: -6.126470 delta_rms_squared: 0.622545 alpha: 0.968874 2024-11-06T03:35:28Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:28Z- meanExpectedMeasDiff: -5.067683 delta_rms_squared: 0.719291 alpha: 0.972947 2024-11-06T03:35:28Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:28Z- meanExpectedMeasDiff: 3.466395 delta_rms_squared: 0.687655 alpha: 0.971738 2024-11-06T03:35:28Z- Subcloud NIS: 0.967959 2024-11-06T03:35:28Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:35:28Z- TNavPF:: sumWeights = 0.000155 2024-11-06T03:35:28Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:35:28Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:35:28Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:35:28Z- TerrainNav::Resampling particle filter... 2024-11-06T03:35:28Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730864128.11 sec, ping # 0. 2024-11-06T03:35:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:35:28Z- variances([0], [2], and psi_berg) = 0.101831 0.061388 0.000000 2024-11-06T03:35:28Z- publishEstimatesDecoder() - at 1730864128.11 sec 2024-11-06T03:35:28Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:35:28Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061738.92 594301.37 49.61 2024-11-06T03:35:28Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061738.74 594301.40 49.79 lat/lon: 36.70 -121.94 2024-11-06T03:35:28Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.10 0.06 0.24 0.00 2024-11-06T03:35:28Z- handleDepth()- Depth_Keller msg: 1730864128.49 epoch sec; seqNo:6852; depth 50.05 2024-11-06T03:35:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864128.51 epoch sec; seqNo:6852; 4061711.00 north; 594318.06 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:35:28Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:35:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864128.11 2024-11-06T03:35:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:28Z- waiting for 1730864132.11, time is 1730864128.51 2024-11-06T03:35:28Z- AHRS_M2 msg: 1730864128.87 epoch sec; seqNo:6432 2024-11-06T03:35:28Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 4.18 psi 2024-11-06T03:35:28Z- handleDepth()- Depth_Keller msg: 1730864128.88 epoch sec; seqNo:6853; depth 50.04 2024-11-06T03:35:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864128.89 epoch sec; seqNo:6853; 4061710.62 north; 594318.06 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:35:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864128.11 2024-11-06T03:35:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:29Z- waiting for 1730864132.11, time is 1730864128.89 2024-11-06T03:35:29Z- AHRS_M2 msg: 1730864129.28 epoch sec; seqNo:6433 2024-11-06T03:35:29Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.18 psi 2024-11-06T03:35:29Z- handleDepth()- Depth_Keller msg: 1730864129.29 epoch sec; seqNo:6854; depth 50.04 2024-11-06T03:35:29Z- handleDVL() - DVL msg epochMillisec: 1730864129.30 2024-11-06T03:35:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:29Z- handleDvl() - RDI_Pathfinder msg: 1730864129.30 sec; seqNo:2360; #beams:0 2024-11-06T03:35:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.01 , -0.04 2024-11-06T03:35:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864129.30 epoch sec; seqNo:6854; 4061710.61 north; 594316.84 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:35:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864128.11 2024-11-06T03:35:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:29Z- waiting for 1730864132.11, time is 1730864129.30 2024-11-06T03:35:29Z- AHRS_M2 msg: 1730864129.68 epoch sec; seqNo:6434 2024-11-06T03:35:29Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.18 psi 2024-11-06T03:35:29Z- handleDepth()- Depth_Keller msg: 1730864129.69 epoch sec; seqNo:6855; depth 50.02 2024-11-06T03:35:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864129.70 epoch sec; seqNo:6855; 4061710.23 north; 594316.85 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:35:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864128.11 2024-11-06T03:35:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:29Z- waiting for 1730864132.11, time is 1730864129.70 2024-11-06T03:35:30Z- AHRS_M2 msg: 1730864130.10 epoch sec; seqNo:6435 2024-11-06T03:35:30Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.18 psi 2024-11-06T03:35:30Z- handleDepth()- Depth_Keller msg: 1730864130.10 epoch sec; seqNo:6856; depth 50.01 2024-11-06T03:35:30Z- handleDVL() - DVL msg epochMillisec: 1730864130.13 2024-11-06T03:35:30Z- handleDvl() - RDI_Pathfinder msg: 1730864130.13 sec; seqNo:2361; #beams:4 2024-11-06T03:35:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.88 , 39.15 , 42.41 , 41.60 2024-11-06T03:35:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.07 , -0.05 2024-11-06T03:35:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864130.14 epoch sec; seqNo:6856; 4061710.23 north; 594316.85 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:35:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864128.11 2024-11-06T03:35:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:30Z- waiting for 1730864132.11, time is 1730864130.14 2024-11-06T03:35:30Z- AHRS_M2 msg: 1730864130.50 epoch sec; seqNo:6436 2024-11-06T03:35:30Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.18 psi 2024-11-06T03:35:30Z- handleDepth()- Depth_Keller msg: 1730864130.50 epoch sec; seqNo:6857; depth 50.00 2024-11-06T03:35:30Z- handleDVL() - DVL msg epochMillisec: 1730864130.51 2024-11-06T03:35:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:30Z- handleDvl() - RDI_Pathfinder msg: 1730864130.51 sec; seqNo:2362; #beams:0 2024-11-06T03:35:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.07 , -0.05 2024-11-06T03:35:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864130.52 epoch sec; seqNo:6857; 4061710.22 north; 594315.63 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:35:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864128.11 2024-11-06T03:35:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:30Z- waiting for 1730864132.11, time is 1730864130.52 2024-11-06T03:35:30Z- handleDepth()- Depth_Keller msg: 1730864130.91 epoch sec; seqNo:6858; depth 49.99 2024-11-06T03:35:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864130.93 epoch sec; seqNo:6858; 4061709.84 north; 594315.63 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:35:31Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:35:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864128.11 2024-11-06T03:35:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:31Z- waiting for 1730864132.11, time is 1730864130.93 2024-11-06T03:35:31Z- AHRS_M2 msg: 1730864131.30 epoch sec; seqNo:6437 2024-11-06T03:35:31Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.18 psi 2024-11-06T03:35:31Z- handleDepth()- Depth_Keller msg: 1730864131.30 epoch sec; seqNo:6859; depth 50.00 2024-11-06T03:35:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864131.31 epoch sec; seqNo:6859; 4061709.84 north; 594315.63 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:35:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864128.11 2024-11-06T03:35:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:31Z- waiting for 1730864132.11, time is 1730864131.31 2024-11-06T03:35:31Z- AHRS_M2 msg: 1730864131.70 epoch sec; seqNo:6438 2024-11-06T03:35:31Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.18 psi 2024-11-06T03:35:31Z- handleDepth()- Depth_Keller msg: 1730864131.71 epoch sec; seqNo:6860; depth 49.99 2024-11-06T03:35:31Z- handleDVL() - DVL msg epochMillisec: 1730864131.71 2024-11-06T03:35:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:31Z- handleDvl() - RDI_Pathfinder msg: 1730864131.71 sec; seqNo:2363; #beams:0 2024-11-06T03:35:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.07 , -0.05 2024-11-06T03:35:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864131.72 epoch sec; seqNo:6860; 4061709.46 north; 594315.63 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T03:35:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864128.11 2024-11-06T03:35:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:31Z- waiting for 1730864132.11, time is 1730864131.72 2024-11-06T03:35:32Z- AHRS_M2 msg: 1730864132.12 epoch sec; seqNo:6439 2024-11-06T03:35:32Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.18 psi 2024-11-06T03:35:32Z- handleDepth()- Depth_Keller msg: 1730864132.13 epoch sec; seqNo:6861; depth 50.00 2024-11-06T03:35:32Z- handleDVL() - DVL msg epochMillisec: 1730864132.15 2024-11-06T03:35:32Z- handleDvl() - RDI_Pathfinder msg: 1730864132.15 sec; seqNo:2364; #beams:4 2024-11-06T03:35:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.29 , 38.95 , 42.41 , 41.60 2024-11-06T03:35:32Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.10 , 0.02 2024-11-06T03:35:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864132.18 epoch sec; seqNo:6861; 4061709.45 north; 594314.41 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:35:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864128.11 2024-11-06T03:35:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:32Z- waiting for 1730864132.11, time is 1730864132.18 2024-11-06T03:35:32Z- LcmTrn::updateTrn() >>>> pose time 1730864132.18, meas time 1730864132.15 <<<< 2024-11-06T03:35:32Z- LcmTrn::updateTrn() equating pose 1730864132.15 and meas times 1730864132.15 2024-11-06T03:35:32Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730864132.15 2024-11-06T03:35:32Z- TerrainNav::motionUpdate() - currEstimate = (4061709, 594314.41, 50.00) 2024-11-06T03:35:32Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:35:32Z- TerrainNav:: SubcloudNIS is 0.97, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:35:32Z- TNavPF::Projecting Measurements 2024-11-06T03:35:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:35:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:35:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:35:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:35:32Z- TNavFilter::4 good beams. 2024-11-06T03:35:32Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:35:32Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:35:32Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:35:32Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:35:32Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:35:32Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:35:32Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:35:32Z- TerrainMapOctree: (vehN, vehE ) = (4061709.45, 594314.41). 2024-11-06T03:35:32Z- TerrainMapOctree: Min Distance = 76.12. 2024-11-06T03:35:32Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:35:32Z- Forcing Subcloud Comparison 2024-11-06T03:35:32Z- Weighting particles with subcloud comparison 2024-11-06T03:35:32Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:32Z- meanExpectedMeasDiff: 1.348563 delta_rms_squared: 0.857200 alpha: 0.977200 2024-11-06T03:35:32Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:32Z- meanExpectedMeasDiff: -6.815696 delta_rms_squared: 1.066327 alpha: 0.981589 2024-11-06T03:35:32Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:32Z- meanExpectedMeasDiff: -4.667821 delta_rms_squared: 0.642096 alpha: 0.969793 2024-11-06T03:35:32Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:32Z- meanExpectedMeasDiff: 3.560851 delta_rms_squared: 0.562209 alpha: 0.965648 2024-11-06T03:35:32Z- Subcloud NIS: 0.996296 2024-11-06T03:35:32Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:35:32Z- TNavPF:: sumWeights = 0.000189 2024-11-06T03:35:32Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:35:32Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T03:35:32Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:35:32Z- TerrainNav::Resampling particle filter... 2024-11-06T03:35:32Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730864132.15 sec, ping # 0. 2024-11-06T03:35:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:35:32Z- variances([0], [2], and psi_berg) = 0.102402 0.072744 0.000000 2024-11-06T03:35:32Z- publishEstimatesDecoder() - at 1730864132.15 sec 2024-11-06T03:35:32Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:35:32Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061737.34 594297.72 49.33 2024-11-06T03:35:32Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061737.12 594297.80 50.48 lat/lon: 36.70 -121.94 2024-11-06T03:35:32Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.10 0.07 0.34 0.00 2024-11-06T03:35:32Z- AHRS_M2 msg: 1730864132.51 epoch sec; seqNo:6440 2024-11-06T03:35:32Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.18 psi 2024-11-06T03:35:32Z- handleDepth()- Depth_Keller msg: 1730864132.55 epoch sec; seqNo:6862; depth 50.01 2024-11-06T03:35:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864132.57 epoch sec; seqNo:6862; 4061709.07 north; 594314.42 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:35:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864132.15 2024-11-06T03:35:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:32Z- waiting for 1730864136.15, time is 1730864132.57 2024-11-06T03:35:32Z- AHRS_M2 msg: 1730864132.91 epoch sec; seqNo:6441 2024-11-06T03:35:32Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.17 psi 2024-11-06T03:35:32Z- handleDepth()- Depth_Keller msg: 1730864132.92 epoch sec; seqNo:6863; depth 50.02 2024-11-06T03:35:32Z- handleDVL() - DVL msg epochMillisec: 1730864132.92 2024-11-06T03:35:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:32Z- handleDvl() - RDI_Pathfinder msg: 1730864132.92 sec; seqNo:2365; #beams:0 2024-11-06T03:35:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:32Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.10 , 0.02 2024-11-06T03:35:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864132.93 epoch sec; seqNo:6863; 4061709.07 north; 594314.42 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:35:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864132.15 2024-11-06T03:35:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:33Z- waiting for 1730864136.15, time is 1730864132.93 2024-11-06T03:35:33Z- AHRS_M2 msg: 1730864133.32 epoch sec; seqNo:6442 2024-11-06T03:35:33Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.17 psi 2024-11-06T03:35:33Z- handleDepth()- Depth_Keller msg: 1730864133.32 epoch sec; seqNo:6864; depth 50.03 2024-11-06T03:35:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864133.34 epoch sec; seqNo:6864; 4061708.68 north; 594313.20 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:35:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864132.15 2024-11-06T03:35:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:33Z- waiting for 1730864136.15, time is 1730864133.34 2024-11-06T03:35:33Z- AHRS_M2 msg: 1730864133.72 epoch sec; seqNo:6443 2024-11-06T03:35:33Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.17 psi 2024-11-06T03:35:33Z- handleDepth()- Depth_Keller msg: 1730864133.73 epoch sec; seqNo:6865; depth 50.03 2024-11-06T03:35:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864133.75 epoch sec; seqNo:6865; 4061708.68 north; 594313.20 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:35:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864132.15 2024-11-06T03:35:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:33Z- waiting for 1730864136.15, time is 1730864133.75 2024-11-06T03:35:34Z- handleDepth()- Depth_Keller msg: 1730864134.15 epoch sec; seqNo:6866; depth 50.05 2024-11-06T03:35:34Z- handleDVL() - DVL msg epochMillisec: 1730864134.18 2024-11-06T03:35:34Z- handleDvl() - RDI_Pathfinder msg: 1730864134.18 sec; seqNo:2366; #beams:4 2024-11-06T03:35:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.08 , 39.15 , 42.62 , 41.80 2024-11-06T03:35:34Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.07 , 0.09 2024-11-06T03:35:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864134.19 epoch sec; seqNo:6866; 4061708.30 north; 594313.21 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:35:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864132.15 2024-11-06T03:35:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:34Z- waiting for 1730864136.15, time is 1730864134.19 2024-11-06T03:35:34Z- handleDepth()- Depth_Keller msg: 1730864134.55 epoch sec; seqNo:6867; depth 50.07 2024-11-06T03:35:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864134.60 epoch sec; seqNo:6867; 4061708.28 north; 594311.99 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:35:34Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:35:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864132.15 2024-11-06T03:35:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:34Z- waiting for 1730864136.15, time is 1730864134.60 2024-11-06T03:35:34Z- AHRS_M2 msg: 1730864134.94 epoch sec; seqNo:6444 2024-11-06T03:35:34Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.17 psi 2024-11-06T03:35:34Z- handleDepth()- Depth_Keller msg: 1730864134.95 epoch sec; seqNo:6868; depth 50.08 2024-11-06T03:35:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864134.96 epoch sec; seqNo:6868; 4061707.91 north; 594311.99 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:35:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864132.15 2024-11-06T03:35:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:35Z- waiting for 1730864136.15, time is 1730864134.96 2024-11-06T03:35:35Z- AHRS_M2 msg: 1730864135.34 epoch sec; seqNo:6445 2024-11-06T03:35:35Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.18 psi 2024-11-06T03:35:35Z- handleDepth()- Depth_Keller msg: 1730864135.35 epoch sec; seqNo:6869; depth 50.08 2024-11-06T03:35:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864135.35 epoch sec; seqNo:6869; 4061707.91 north; 594311.99 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:35:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864132.15 2024-11-06T03:35:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:35Z- waiting for 1730864136.15, time is 1730864135.35 2024-11-06T03:35:35Z- AHRS_M2 msg: 1730864135.74 epoch sec; seqNo:6446 2024-11-06T03:35:35Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.18 psi 2024-11-06T03:35:35Z- handleDepth()- Depth_Keller msg: 1730864135.75 epoch sec; seqNo:6870; depth 50.12 2024-11-06T03:35:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864135.76 epoch sec; seqNo:6870; 4061707.53 north; 594311.99 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:35:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864132.15 2024-11-06T03:35:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:35Z- waiting for 1730864136.15, time is 1730864135.76 2024-11-06T03:35:36Z- AHRS_M2 msg: 1730864136.15 epoch sec; seqNo:6447 2024-11-06T03:35:36Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.18 psi 2024-11-06T03:35:36Z- handleDepth()- Depth_Keller msg: 1730864136.16 epoch sec; seqNo:6871; depth 50.13 2024-11-06T03:35:36Z- handleDVL() - DVL msg epochMillisec: 1730864136.18 2024-11-06T03:35:36Z- handleDvl() - RDI_Pathfinder msg: 1730864136.18 sec; seqNo:2367; #beams:4 2024-11-06T03:35:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.69 , 39.35 , 42.62 , 41.39 2024-11-06T03:35:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , 0.09 2024-11-06T03:35:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864136.19 epoch sec; seqNo:6871; 4061707.51 north; 594310.77 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:35:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864132.15 2024-11-06T03:35:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:36Z- waiting for 1730864136.15, time is 1730864136.19 2024-11-06T03:35:36Z- LcmTrn::updateTrn() >>>> pose time 1730864136.19, meas time 1730864136.18 <<<< 2024-11-06T03:35:36Z- LcmTrn::updateTrn() equating pose 1730864136.18 and meas times 1730864136.18 2024-11-06T03:35:36Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730864136.18 2024-11-06T03:35:36Z- TerrainNav::motionUpdate() - currEstimate = (4061708, 594310.77, 50.13) 2024-11-06T03:35:36Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:35:36Z- TerrainNav:: SubcloudNIS is 1.00, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:35:36Z- TNavPF::Projecting Measurements 2024-11-06T03:35:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:35:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:35:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:35:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:35:36Z- TNavFilter::4 good beams. 2024-11-06T03:35:36Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:35:36Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:35:36Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:35:36Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:35:36Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:35:36Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:35:36Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:35:36Z- TerrainMapOctree: (vehN, vehE ) = (4061707.51, 594310.77). 2024-11-06T03:35:36Z- TerrainMapOctree: Min Distance = 74.15. 2024-11-06T03:35:36Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:35:36Z- Forcing Subcloud Comparison 2024-11-06T03:35:36Z- Weighting particles with subcloud comparison 2024-11-06T03:35:36Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:36Z- meanExpectedMeasDiff: 1.457066 delta_rms_squared: 0.965315 alpha: 0.979702 2024-11-06T03:35:36Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:36Z- meanExpectedMeasDiff: -5.682423 delta_rms_squared: 0.661709 alpha: 0.970662 2024-11-06T03:35:36Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:36Z- meanExpectedMeasDiff: -4.374323 delta_rms_squared: 1.354252 alpha: 0.985447 2024-11-06T03:35:36Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:36Z- meanExpectedMeasDiff: 1.805704 delta_rms_squared: 0.906934 alpha: 0.978423 2024-11-06T03:35:36Z- Subcloud NIS: 1.050496 2024-11-06T03:35:36Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:35:36Z- TNavPF:: sumWeights = 0.002662 2024-11-06T03:35:36Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:35:36Z- TNavPF::First Term of Map Covariance Matrix is 0.02 2024-11-06T03:35:36Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:35:36Z- TerrainNav::Resampling particle filter... 2024-11-06T03:35:36Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730864136.18 sec, ping # 0. 2024-11-06T03:35:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:35:36Z- variances([0], [2], and psi_berg) = 0.116487 0.082862 0.000000 2024-11-06T03:35:36Z- publishEstimatesDecoder() - at 1730864136.18 sec 2024-11-06T03:35:36Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:35:36Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061735.27 594293.83 50.72 2024-11-06T03:35:36Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061735.25 594294.17 51.32 lat/lon: 36.70 -121.94 2024-11-06T03:35:36Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.12 0.08 0.37 0.00 2024-11-06T03:35:36Z- AHRS_M2 msg: 1730864136.55 epoch sec; seqNo:6448 2024-11-06T03:35:36Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.18 psi 2024-11-06T03:35:36Z- handleDepth()- Depth_Keller msg: 1730864136.56 epoch sec; seqNo:6872; depth 50.15 2024-11-06T03:35:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864136.58 epoch sec; seqNo:6872; 4061707.13 north; 594310.78 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:35:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864136.18 2024-11-06T03:35:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:36Z- waiting for 1730864140.18, time is 1730864136.58 2024-11-06T03:35:36Z- AHRS_M2 msg: 1730864136.96 epoch sec; seqNo:6449 2024-11-06T03:35:36Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.18 psi 2024-11-06T03:35:36Z- handleDepth()- Depth_Keller msg: 1730864136.96 epoch sec; seqNo:6873; depth 50.15 2024-11-06T03:35:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864136.97 epoch sec; seqNo:6873; 4061707.13 north; 594310.78 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:35:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864136.18 2024-11-06T03:35:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:37Z- waiting for 1730864140.18, time is 1730864136.97 2024-11-06T03:35:37Z- AHRS_M2 msg: 1730864137.36 epoch sec; seqNo:6450 2024-11-06T03:35:37Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.19 psi 2024-11-06T03:35:37Z- handleDepth()- Depth_Keller msg: 1730864137.36 epoch sec; seqNo:6874; depth 50.17 2024-11-06T03:35:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864137.38 epoch sec; seqNo:6874; 4061706.76 north; 594310.78 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:35:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864136.18 2024-11-06T03:35:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:37Z- waiting for 1730864140.18, time is 1730864137.38 2024-11-06T03:35:37Z- AHRS_M2 msg: 1730864137.76 epoch sec; seqNo:6451 2024-11-06T03:35:37Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.19 psi 2024-11-06T03:35:37Z- handleDepth()- Depth_Keller msg: 1730864137.77 epoch sec; seqNo:6875; depth 50.19 2024-11-06T03:35:37Z- handleDVL() - DVL msg epochMillisec: 1730864137.78 2024-11-06T03:35:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:37Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:37Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:37Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:37Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:37Z- handleDvl() - RDI_Pathfinder msg: 1730864137.78 sec; seqNo:2368; #beams:0 2024-11-06T03:35:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , 0.09 2024-11-06T03:35:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864137.78 epoch sec; seqNo:6875; 4061706.74 north; 594309.56 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:35:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864136.18 2024-11-06T03:35:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:37Z- waiting for 1730864140.18, time is 1730864137.78 2024-11-06T03:35:38Z- AHRS_M2 msg: 1730864138.17 epoch sec; seqNo:6452 2024-11-06T03:35:38Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.19 psi 2024-11-06T03:35:38Z- handleDepth()- Depth_Keller msg: 1730864138.18 epoch sec; seqNo:6876; depth 50.18 2024-11-06T03:35:38Z- handleDVL() - DVL msg epochMillisec: 1730864138.21 2024-11-06T03:35:38Z- handleDvl() - RDI_Pathfinder msg: 1730864138.21 sec; seqNo:2369; #beams:4 2024-11-06T03:35:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.31 , 39.56 , 43.02 , 41.39 2024-11-06T03:35:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.03 2024-11-06T03:35:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864138.22 epoch sec; seqNo:6876; 4061706.36 north; 594309.57 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:35:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864136.18 2024-11-06T03:35:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:38Z- waiting for 1730864140.18, time is 1730864138.22 2024-11-06T03:35:38Z- AHRS_M2 msg: 1730864138.57 epoch sec; seqNo:6453 2024-11-06T03:35:38Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.20 psi 2024-11-06T03:35:38Z- handleDepth()- Depth_Keller msg: 1730864138.58 epoch sec; seqNo:6877; depth 50.18 2024-11-06T03:35:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864138.59 epoch sec; seqNo:6877; 4061706.36 north; 594309.57 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T03:35:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864136.18 2024-11-06T03:35:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:38Z- waiting for 1730864140.18, time is 1730864138.59 2024-11-06T03:35:38Z- AHRS_M2 msg: 1730864138.98 epoch sec; seqNo:6454 2024-11-06T03:35:38Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.20 psi 2024-11-06T03:35:38Z- handleDepth()- Depth_Keller msg: 1730864138.98 epoch sec; seqNo:6878; depth 50.20 2024-11-06T03:35:38Z- handleDVL() - DVL msg epochMillisec: 1730864138.99 2024-11-06T03:35:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:39Z- handleDvl() - RDI_Pathfinder msg: 1730864138.99 sec; seqNo:2370; #beams:0 2024-11-06T03:35:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.03 2024-11-06T03:35:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864139.00 epoch sec; seqNo:6878; 4061705.97 north; 594308.35 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:35:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864136.18 2024-11-06T03:35:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:39Z- waiting for 1730864140.18, time is 1730864139.00 2024-11-06T03:35:39Z- AHRS_M2 msg: 1730864139.38 epoch sec; seqNo:6455 2024-11-06T03:35:39Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.20 psi 2024-11-06T03:35:39Z- handleDepth()- Depth_Keller msg: 1730864139.39 epoch sec; seqNo:6879; depth 50.19 2024-11-06T03:35:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864139.40 epoch sec; seqNo:6879; 4061705.97 north; 594308.35 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:35:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864136.18 2024-11-06T03:35:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:39Z- waiting for 1730864140.18, time is 1730864139.40 2024-11-06T03:35:39Z- handleDepth()- Depth_Keller msg: 1730864139.80 epoch sec; seqNo:6880; depth 50.17 2024-11-06T03:35:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864139.81 epoch sec; seqNo:6880; 4061705.59 north; 594308.35 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:35:39Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:35:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864136.18 2024-11-06T03:35:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:39Z- waiting for 1730864140.18, time is 1730864139.81 2024-11-06T03:35:40Z- AHRS_M2 msg: 1730864140.19 epoch sec; seqNo:6456 2024-11-06T03:35:40Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.20 psi 2024-11-06T03:35:40Z- handleDepth()- Depth_Keller msg: 1730864140.19 epoch sec; seqNo:6881; depth 50.19 2024-11-06T03:35:40Z- handleDVL() - DVL msg epochMillisec: 1730864140.21 2024-11-06T03:35:40Z- handleDvl() - RDI_Pathfinder msg: 1730864140.21 sec; seqNo:2371; #beams:4 2024-11-06T03:35:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 39.35 , 43.02 , 42.00 2024-11-06T03:35:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , -0.02 2024-11-06T03:35:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864140.23 epoch sec; seqNo:6881; 4061705.59 north; 594308.35 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:35:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864136.18 2024-11-06T03:35:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:40Z- waiting for 1730864140.18, time is 1730864140.23 2024-11-06T03:35:40Z- LcmTrn::updateTrn() >>>> pose time 1730864140.23, meas time 1730864140.21 <<<< 2024-11-06T03:35:40Z- LcmTrn::updateTrn() equating pose 1730864140.21 and meas times 1730864140.21 2024-11-06T03:35:40Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730864140.21 2024-11-06T03:35:40Z- TerrainNav::motionUpdate() - currEstimate = (4061706, 594308.35, 50.19) 2024-11-06T03:35:40Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:35:40Z- TerrainNav:: SubcloudNIS is 1.05, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:35:40Z- TNavPF::Projecting Measurements 2024-11-06T03:35:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:35:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:35:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:35:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:35:40Z- TNavFilter::4 good beams. 2024-11-06T03:35:40Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:35:40Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:35:40Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:35:40Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:35:40Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:35:40Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:35:40Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:35:40Z- TerrainMapOctree: (vehN, vehE ) = (4061705.59, 594308.35). 2024-11-06T03:35:40Z- TerrainMapOctree: Min Distance = 73.33. 2024-11-06T03:35:40Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:35:40Z- Forcing Subcloud Comparison 2024-11-06T03:35:40Z- Weighting particles with subcloud comparison 2024-11-06T03:35:40Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:40Z- meanExpectedMeasDiff: 1.721127 delta_rms_squared: 1.001734 alpha: 0.980425 2024-11-06T03:35:40Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:40Z- meanExpectedMeasDiff: -4.821369 delta_rms_squared: 0.684585 alpha: 0.971615 2024-11-06T03:35:40Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:40Z- meanExpectedMeasDiff: -3.688320 delta_rms_squared: 0.805687 alpha: 0.975778 2024-11-06T03:35:40Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:40Z- meanExpectedMeasDiff: 2.142245 delta_rms_squared: 0.488858 alpha: 0.960696 2024-11-06T03:35:40Z- Subcloud NIS: 1.053547 2024-11-06T03:35:40Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:35:40Z- TNavPF:: sumWeights = 0.008777 2024-11-06T03:35:40Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:35:40Z- TNavPF::First Term of Map Covariance Matrix is 0.05 2024-11-06T03:35:40Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:35:40Z- TerrainNav::Resampling particle filter... 2024-11-06T03:35:40Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730864140.21 sec, ping # 0. 2024-11-06T03:35:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:35:40Z- variances([0], [2], and psi_berg) = 0.117825 0.081464 0.000000 2024-11-06T03:35:40Z- publishEstimatesDecoder() - at 1730864140.21 sec 2024-11-06T03:35:40Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:35:40Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061733.37 594291.35 51.13 2024-11-06T03:35:40Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061733.31 594291.69 51.89 lat/lon: 36.70 -121.94 2024-11-06T03:35:40Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.12 0.08 0.37 0.00 2024-11-06T03:35:40Z- AHRS_M2 msg: 1730864140.59 epoch sec; seqNo:6457 2024-11-06T03:35:40Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.20 psi 2024-11-06T03:35:40Z- handleDepth()- Depth_Keller msg: 1730864140.61 epoch sec; seqNo:6882; depth 50.17 2024-11-06T03:35:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864140.62 epoch sec; seqNo:6882; 4061705.20 north; 594307.14 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:35:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864140.21 2024-11-06T03:35:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:40Z- waiting for 1730864144.21, time is 1730864140.62 2024-11-06T03:35:41Z- AHRS_M2 msg: 1730864140.99 epoch sec; seqNo:6458 2024-11-06T03:35:41Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.20 psi 2024-11-06T03:35:41Z- handleDepth()- Depth_Keller msg: 1730864141.00 epoch sec; seqNo:6883; depth 50.16 2024-11-06T03:35:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864141.02 epoch sec; seqNo:6883; 4061705.20 north; 594307.14 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:35:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864140.21 2024-11-06T03:35:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:41Z- waiting for 1730864144.21, time is 1730864141.02 2024-11-06T03:35:41Z- AHRS_M2 msg: 1730864141.40 epoch sec; seqNo:6459 2024-11-06T03:35:41Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 4.20 psi 2024-11-06T03:35:41Z- handleDepth()- Depth_Keller msg: 1730864141.41 epoch sec; seqNo:6884; depth 50.16 2024-11-06T03:35:41Z- handleDVL() - DVL msg epochMillisec: 1730864141.41 2024-11-06T03:35:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:41Z- handleDvl() - RDI_Pathfinder msg: 1730864141.41 sec; seqNo:2372; #beams:0 2024-11-06T03:35:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , -0.02 2024-11-06T03:35:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864141.42 epoch sec; seqNo:6884; 4061705.20 north; 594307.14 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:35:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864140.21 2024-11-06T03:35:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:41Z- waiting for 1730864144.21, time is 1730864141.42 2024-11-06T03:35:41Z- AHRS_M2 msg: 1730864141.80 epoch sec; seqNo:6460 2024-11-06T03:35:41Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 4.21 psi 2024-11-06T03:35:41Z- handleDepth()- Depth_Keller msg: 1730864141.81 epoch sec; seqNo:6885; depth 50.13 2024-11-06T03:35:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864141.82 epoch sec; seqNo:6885; 4061704.82 north; 594307.14 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:35:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864140.21 2024-11-06T03:35:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:41Z- waiting for 1730864144.21, time is 1730864141.82 2024-11-06T03:35:42Z- AHRS_M2 msg: 1730864142.21 epoch sec; seqNo:6461 2024-11-06T03:35:42Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 4.21 psi 2024-11-06T03:35:42Z- handleDepth()- Depth_Keller msg: 1730864142.21 epoch sec; seqNo:6886; depth 50.14 2024-11-06T03:35:42Z- handleDVL() - DVL msg epochMillisec: 1730864142.25 2024-11-06T03:35:42Z- handleDvl() - RDI_Pathfinder msg: 1730864142.25 sec; seqNo:2373; #beams:4 2024-11-06T03:35:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 39.55 , 43.22 , 42.21 2024-11-06T03:35:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , -0.05 2024-11-06T03:35:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864142.28 epoch sec; seqNo:6886; 4061704.81 north; 594305.92 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T03:35:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864140.21 2024-11-06T03:35:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:42Z- waiting for 1730864144.21, time is 1730864142.28 2024-11-06T03:35:42Z- AHRS_M2 msg: 1730864142.61 epoch sec; seqNo:6462 2024-11-06T03:35:42Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 4.21 psi 2024-11-06T03:35:42Z- handleDepth()- Depth_Keller msg: 1730864142.63 epoch sec; seqNo:6887; depth 50.11 2024-11-06T03:35:42Z- handleDVL() - DVL msg epochMillisec: 1730864142.63 2024-11-06T03:35:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:42Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:42Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:42Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:42Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:42Z- handleDvl() - RDI_Pathfinder msg: 1730864142.63 sec; seqNo:2374; #beams:0 2024-11-06T03:35:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , -0.05 2024-11-06T03:35:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864142.65 epoch sec; seqNo:6887; 4061704.43 north; 594305.93 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T03:35:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864140.21 2024-11-06T03:35:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:42Z- waiting for 1730864144.21, time is 1730864142.65 2024-11-06T03:35:43Z- AHRS_M2 msg: 1730864143.02 epoch sec; seqNo:6463 2024-11-06T03:35:43Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 4.21 psi 2024-11-06T03:35:43Z- handleDepth()- Depth_Keller msg: 1730864143.02 epoch sec; seqNo:6888; depth 50.10 2024-11-06T03:35:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864143.05 epoch sec; seqNo:6888; 4061704.43 north; 594305.93 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:35:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864140.21 2024-11-06T03:35:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:43Z- waiting for 1730864144.21, time is 1730864143.05 2024-11-06T03:35:43Z- handleDepth()- Depth_Keller msg: 1730864143.44 epoch sec; seqNo:6889; depth 50.09 2024-11-06T03:35:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864143.46 epoch sec; seqNo:6889; 4061704.05 north; 594305.93 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:35:43Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:35:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864140.21 2024-11-06T03:35:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:43Z- waiting for 1730864144.21, time is 1730864143.46 2024-11-06T03:35:43Z- AHRS_M2 msg: 1730864143.84 epoch sec; seqNo:6464 2024-11-06T03:35:43Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 4.21 psi 2024-11-06T03:35:43Z- handleDepth()- Depth_Keller msg: 1730864143.84 epoch sec; seqNo:6890; depth 50.09 2024-11-06T03:35:43Z- handleDVL() - DVL msg epochMillisec: 1730864143.85 2024-11-06T03:35:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:43Z- handleDvl() - RDI_Pathfinder msg: 1730864143.85 sec; seqNo:2375; #beams:0 2024-11-06T03:35:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.02 , -0.05 2024-11-06T03:35:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864143.85 epoch sec; seqNo:6890; 4061704.04 north; 594304.71 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T03:35:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864140.21 2024-11-06T03:35:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:44Z- waiting for 1730864144.21, time is 1730864143.85 2024-11-06T03:35:44Z- AHRS_M2 msg: 1730864144.23 epoch sec; seqNo:6465 2024-11-06T03:35:44Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 4.21 psi 2024-11-06T03:35:44Z- handleDepth()- Depth_Keller msg: 1730864144.24 epoch sec; seqNo:6891; depth 50.07 2024-11-06T03:35:44Z- handleDVL() - DVL msg epochMillisec: 1730864144.26 2024-11-06T03:35:44Z- handleDvl() - RDI_Pathfinder msg: 1730864144.26 sec; seqNo:2376; #beams:4 2024-11-06T03:35:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 39.55 , 43.02 , 42.00 2024-11-06T03:35:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.05 , -0.05 2024-11-06T03:35:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864144.28 epoch sec; seqNo:6891; 4061703.66 north; 594304.71 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:35:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864140.21 2024-11-06T03:35:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:44Z- waiting for 1730864144.21, time is 1730864144.28 2024-11-06T03:35:44Z- LcmTrn::updateTrn() >>>> pose time 1730864144.28, meas time 1730864144.26 <<<< 2024-11-06T03:35:44Z- LcmTrn::updateTrn() equating pose 1730864144.26 and meas times 1730864144.26 2024-11-06T03:35:44Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730864144.26 2024-11-06T03:35:44Z- TerrainNav::motionUpdate() - currEstimate = (4061704, 594304.71, 50.07) 2024-11-06T03:35:44Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:35:44Z- TerrainNav:: SubcloudNIS is 1.05, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:35:44Z- TNavPF::Projecting Measurements 2024-11-06T03:35:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:35:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:35:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:35:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:35:44Z- TNavFilter::4 good beams. 2024-11-06T03:35:44Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:35:44Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:35:44Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:35:44Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:35:44Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:35:44Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:35:44Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:35:44Z- TerrainMapOctree: (vehN, vehE ) = (4061703.66, 594304.71). 2024-11-06T03:35:44Z- TerrainMapOctree: Min Distance = 71.70. 2024-11-06T03:35:44Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:35:44Z- Forcing Subcloud Comparison 2024-11-06T03:35:44Z- Weighting particles with subcloud comparison 2024-11-06T03:35:44Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:44Z- meanExpectedMeasDiff: 1.163049 delta_rms_squared: 0.998307 alpha: 0.980360 2024-11-06T03:35:44Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:44Z- meanExpectedMeasDiff: -4.368110 delta_rms_squared: 0.700236 alpha: 0.972231 2024-11-06T03:35:44Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:44Z- meanExpectedMeasDiff: -4.685991 delta_rms_squared: 0.833860 alpha: 0.976577 2024-11-06T03:35:44Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:44Z- meanExpectedMeasDiff: 2.039023 delta_rms_squared: 0.434515 alpha: 0.955997 2024-11-06T03:35:44Z- Subcloud NIS: 1.076826 2024-11-06T03:35:44Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:35:44Z- TNavPF:: sumWeights = 0.008783 2024-11-06T03:35:44Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:35:44Z- TNavPF::First Term of Map Covariance Matrix is 0.04 2024-11-06T03:35:44Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T03:35:44Z- TerrainNav::Resampling particle filter... 2024-11-06T03:35:44Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730864144.26 sec, ping # 0. 2024-11-06T03:35:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:35:44Z- variances([0], [2], and psi_berg) = 0.118973 0.077326 0.000000 2024-11-06T03:35:44Z- publishEstimatesDecoder() - at 1730864144.26 sec 2024-11-06T03:35:44Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:35:44Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061732.00 594287.76 52.08 2024-11-06T03:35:44Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061731.39 594287.99 52.43 lat/lon: 36.70 -121.94 2024-11-06T03:35:44Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.12 0.08 0.37 0.00 2024-11-06T03:35:44Z- AHRS_M2 msg: 1730864144.66 epoch sec; seqNo:6466 2024-11-06T03:35:44Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 4.21 psi 2024-11-06T03:35:44Z- handleDepth()- Depth_Keller msg: 1730864144.68 epoch sec; seqNo:6892; depth 50.06 2024-11-06T03:35:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864144.70 epoch sec; seqNo:6892; 4061703.66 north; 594304.71 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:35:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864144.26 2024-11-06T03:35:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:44Z- waiting for 1730864148.26, time is 1730864144.70 2024-11-06T03:35:45Z- handleDepth()- Depth_Keller msg: 1730864145.06 epoch sec; seqNo:6893; depth 50.06 2024-11-06T03:35:45Z- handleDVL() - DVL msg epochMillisec: 1730864145.06 2024-11-06T03:35:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:45Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:45Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:45Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:45Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:45Z- handleDvl() - RDI_Pathfinder msg: 1730864145.06 sec; seqNo:2377; #beams:0 2024-11-06T03:35:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.05 , -0.05 2024-11-06T03:35:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864145.07 epoch sec; seqNo:6893; 4061703.66 north; 594304.71 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:35:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864144.26 2024-11-06T03:35:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:45Z- waiting for 1730864148.26, time is 1730864145.07 2024-11-06T03:35:45Z- AHRS_M2 msg: 1730864145.44 epoch sec; seqNo:6467 2024-11-06T03:35:45Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 4.21 psi 2024-11-06T03:35:45Z- handleDepth()- Depth_Keller msg: 1730864145.45 epoch sec; seqNo:6894; depth 50.05 2024-11-06T03:35:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864145.46 epoch sec; seqNo:6894; 4061703.27 north; 594303.50 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:35:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864144.26 2024-11-06T03:35:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:45Z- waiting for 1730864148.26, time is 1730864145.46 2024-11-06T03:35:45Z- AHRS_M2 msg: 1730864145.85 epoch sec; seqNo:6468 2024-11-06T03:35:45Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 4.21 psi 2024-11-06T03:35:45Z- handleDepth()- Depth_Keller msg: 1730864145.86 epoch sec; seqNo:6895; depth 50.07 2024-11-06T03:35:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864145.87 epoch sec; seqNo:6895; 4061703.27 north; 594303.50 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:35:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864144.26 2024-11-06T03:35:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:46Z- waiting for 1730864148.26, time is 1730864145.87 2024-11-06T03:35:46Z- AHRS_M2 msg: 1730864146.25 epoch sec; seqNo:6469 2024-11-06T03:35:46Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 4.21 psi 2024-11-06T03:35:46Z- handleDepth()- Depth_Keller msg: 1730864146.25 epoch sec; seqNo:6896; depth 50.04 2024-11-06T03:35:46Z- handleDVL() - DVL msg epochMillisec: 1730864146.28 2024-11-06T03:35:46Z- handleDvl() - RDI_Pathfinder msg: 1730864146.28 sec; seqNo:2378; #beams:4 2024-11-06T03:35:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.49 , 39.55 , 43.22 , 41.59 2024-11-06T03:35:46Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.08 , -0.01 2024-11-06T03:35:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864146.30 epoch sec; seqNo:6896; 4061702.89 north; 594303.50 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:35:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864144.26 2024-11-06T03:35:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:46Z- waiting for 1730864148.26, time is 1730864146.30 2024-11-06T03:35:46Z- AHRS_M2 msg: 1730864146.65 epoch sec; seqNo:6470 2024-11-06T03:35:46Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.21 psi 2024-11-06T03:35:46Z- handleDepth()- Depth_Keller msg: 1730864146.66 epoch sec; seqNo:6897; depth 50.05 2024-11-06T03:35:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864146.67 epoch sec; seqNo:6897; 4061702.87 north; 594302.28 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:35:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864144.26 2024-11-06T03:35:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:46Z- waiting for 1730864148.26, time is 1730864146.67 2024-11-06T03:35:47Z- AHRS_M2 msg: 1730864147.06 epoch sec; seqNo:6471 2024-11-06T03:35:47Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.21 psi 2024-11-06T03:35:47Z- handleDepth()- Depth_Keller msg: 1730864147.06 epoch sec; seqNo:6898; depth 50.06 2024-11-06T03:35:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864147.07 epoch sec; seqNo:6898; 4061702.49 north; 594302.29 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:35:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864144.26 2024-11-06T03:35:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:47Z- waiting for 1730864148.26, time is 1730864147.07 2024-11-06T03:35:47Z- AHRS_M2 msg: 1730864147.46 epoch sec; seqNo:6472 2024-11-06T03:35:47Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.21 psi 2024-11-06T03:35:47Z- handleDepth()- Depth_Keller msg: 1730864147.46 epoch sec; seqNo:6899; depth 50.05 2024-11-06T03:35:47Z- handleDVL() - DVL msg epochMillisec: 1730864147.47 2024-11-06T03:35:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:47Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:47Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:47Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:47Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:47Z- handleDvl() - RDI_Pathfinder msg: 1730864147.47 sec; seqNo:2379; #beams:0 2024-11-06T03:35:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:47Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.08 , -0.01 2024-11-06T03:35:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864147.47 epoch sec; seqNo:6899; 4061702.49 north; 594302.29 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:35:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864144.26 2024-11-06T03:35:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:47Z- waiting for 1730864148.26, time is 1730864147.47 2024-11-06T03:35:47Z- AHRS_M2 msg: 1730864147.87 epoch sec; seqNo:6473 2024-11-06T03:35:47Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.21 psi 2024-11-06T03:35:47Z- handleDepth()- Depth_Keller msg: 1730864147.88 epoch sec; seqNo:6900; depth 50.05 2024-11-06T03:35:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864147.90 epoch sec; seqNo:6900; 4061702.48 north; 594301.07 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:35:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864144.26 2024-11-06T03:35:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:48Z- waiting for 1730864148.26, time is 1730864147.90 2024-11-06T03:35:48Z- AHRS_M2 msg: 1730864148.27 epoch sec; seqNo:6474 2024-11-06T03:35:48Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.21 psi 2024-11-06T03:35:48Z- handleDepth()- Depth_Keller msg: 1730864148.28 epoch sec; seqNo:6901; depth 50.06 2024-11-06T03:35:48Z- handleDVL() - DVL msg epochMillisec: 1730864148.30 2024-11-06T03:35:48Z- handleDvl() - RDI_Pathfinder msg: 1730864148.30 sec; seqNo:2380; #beams:4 2024-11-06T03:35:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.49 , 39.76 , 43.84 , 41.59 2024-11-06T03:35:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.08 , 0.06 2024-11-06T03:35:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864148.32 epoch sec; seqNo:6901; 4061702.10 north; 594301.07 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:35:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864144.26 2024-11-06T03:35:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:48Z- waiting for 1730864148.26, time is 1730864148.32 2024-11-06T03:35:48Z- LcmTrn::updateTrn() >>>> pose time 1730864148.32, meas time 1730864148.30 <<<< 2024-11-06T03:35:48Z- LcmTrn::updateTrn() equating pose 1730864148.30 and meas times 1730864148.30 2024-11-06T03:35:48Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730864148.30 2024-11-06T03:35:48Z- TerrainNav::motionUpdate() - currEstimate = (4061702, 594301.07, 50.06) 2024-11-06T03:35:48Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:35:48Z- TerrainNav:: SubcloudNIS is 1.08, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:35:48Z- TNavPF::Projecting Measurements 2024-11-06T03:35:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:35:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:35:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:35:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:35:48Z- TNavFilter::4 good beams. 2024-11-06T03:35:48Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:35:48Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:35:48Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:35:48Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:35:48Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:35:48Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:35:48Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:35:48Z- TerrainMapOctree: (vehN, vehE ) = (4061702.10, 594301.07). 2024-11-06T03:35:48Z- TerrainMapOctree: Min Distance = 70.02. 2024-11-06T03:35:48Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:35:48Z- Forcing Subcloud Comparison 2024-11-06T03:35:48Z- Weighting particles with subcloud comparison 2024-11-06T03:35:48Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:48Z- meanExpectedMeasDiff: 1.586140 delta_rms_squared: 1.031399 alpha: 0.980978 2024-11-06T03:35:48Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:48Z- meanExpectedMeasDiff: -3.617032 delta_rms_squared: 0.735533 alpha: 0.973529 2024-11-06T03:35:48Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:48Z- meanExpectedMeasDiff: -3.526503 delta_rms_squared: 0.909531 alpha: 0.978484 2024-11-06T03:35:48Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:48Z- meanExpectedMeasDiff: 0.869214 delta_rms_squared: 0.793480 alpha: 0.975414 2024-11-06T03:35:48Z- Subcloud NIS: 1.092021 2024-11-06T03:35:48Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:35:48Z- TNavPF:: sumWeights = 0.045127 2024-11-06T03:35:48Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:35:48Z- TNavPF::First Term of Map Covariance Matrix is 0.21 2024-11-06T03:35:48Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:35:48Z- TerrainNav::Resampling particle filter... 2024-11-06T03:35:48Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730864148.30 sec, ping # 0. 2024-11-06T03:35:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:35:48Z- variances([0], [2], and psi_berg) = 0.112025 0.072789 0.000000 2024-11-06T03:35:48Z- publishEstimatesDecoder() - at 1730864148.30 sec 2024-11-06T03:35:48Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:35:48Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061730.47 594284.12 52.01 2024-11-06T03:35:48Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061729.86 594284.32 52.83 lat/lon: 36.70 -121.94 2024-11-06T03:35:48Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.11 0.07 0.36 0.00 2024-11-06T03:35:48Z- AHRS_M2 msg: 1730864148.69 epoch sec; seqNo:6475 2024-11-06T03:35:48Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.21 psi 2024-11-06T03:35:48Z- handleDepth()- Depth_Keller msg: 1730864148.69 epoch sec; seqNo:6902; depth 50.06 2024-11-06T03:35:48Z- handleDVL() - DVL msg epochMillisec: 1730864148.69 2024-11-06T03:35:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:48Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:48Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:48Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:48Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:48Z- handleDvl() - RDI_Pathfinder msg: 1730864148.69 sec; seqNo:2381; #beams:0 2024-11-06T03:35:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.08 , 0.06 2024-11-06T03:35:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864148.72 epoch sec; seqNo:6902; 4061702.10 north; 594301.07 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:35:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864148.30 2024-11-06T03:35:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:48Z- waiting for 1730864152.30, time is 1730864148.72 2024-11-06T03:35:49Z- AHRS_M2 msg: 1730864149.08 epoch sec; seqNo:6476 2024-11-06T03:35:49Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:35:49Z- handleDepth()- Depth_Keller msg: 1730864149.08 epoch sec; seqNo:6903; depth 50.06 2024-11-06T03:35:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864149.09 epoch sec; seqNo:6903; 4061701.72 north; 594301.07 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:35:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864148.30 2024-11-06T03:35:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:49Z- waiting for 1730864152.30, time is 1730864149.09 2024-11-06T03:35:49Z- AHRS_M2 msg: 1730864149.48 epoch sec; seqNo:6477 2024-11-06T03:35:49Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:35:49Z- handleDepth()- Depth_Keller msg: 1730864149.49 epoch sec; seqNo:6904; depth 50.07 2024-11-06T03:35:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864149.49 epoch sec; seqNo:6904; 4061701.71 north; 594299.85 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:35:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864148.30 2024-11-06T03:35:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:49Z- waiting for 1730864152.30, time is 1730864149.49 2024-11-06T03:35:49Z- AHRS_M2 msg: 1730864149.88 epoch sec; seqNo:6478 2024-11-06T03:35:49Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:35:49Z- handleDepth()- Depth_Keller msg: 1730864149.89 epoch sec; seqNo:6905; depth 50.07 2024-11-06T03:35:49Z- handleDVL() - DVL msg epochMillisec: 1730864149.89 2024-11-06T03:35:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:49Z- handleDvl() - RDI_Pathfinder msg: 1730864149.89 sec; seqNo:2382; #beams:0 2024-11-06T03:35:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.08 , 0.06 2024-11-06T03:35:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864149.90 epoch sec; seqNo:6905; 4061701.33 north; 594299.86 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:35:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864148.30 2024-11-06T03:35:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:50Z- waiting for 1730864152.30, time is 1730864149.90 2024-11-06T03:35:50Z- AHRS_M2 msg: 1730864150.29 epoch sec; seqNo:6479 2024-11-06T03:35:50Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:35:50Z- handleDepth()- Depth_Keller msg: 1730864150.30 epoch sec; seqNo:6906; depth 50.07 2024-11-06T03:35:50Z- handleDVL() - DVL msg epochMillisec: 1730864150.32 2024-11-06T03:35:50Z- handleDvl() - RDI_Pathfinder msg: 1730864150.32 sec; seqNo:2383; #beams:4 2024-11-06T03:35:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.69 , 39.96 , 44.04 , 41.39 2024-11-06T03:35:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , 0.08 2024-11-06T03:35:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864150.34 epoch sec; seqNo:6906; 4061701.33 north; 594299.86 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:35:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864148.30 2024-11-06T03:35:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:50Z- waiting for 1730864152.30, time is 1730864150.34 2024-11-06T03:35:50Z- AHRS_M2 msg: 1730864150.69 epoch sec; seqNo:6480 2024-11-06T03:35:50Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:35:50Z- handleDepth()- Depth_Keller msg: 1730864150.70 epoch sec; seqNo:6907; depth 50.07 2024-11-06T03:35:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864150.71 epoch sec; seqNo:6907; 4061700.94 north; 594298.64 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:35:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864148.30 2024-11-06T03:35:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:50Z- waiting for 1730864152.30, time is 1730864150.71 2024-11-06T03:35:51Z- AHRS_M2 msg: 1730864151.10 epoch sec; seqNo:6481 2024-11-06T03:35:51Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:35:51Z- handleDepth()- Depth_Keller msg: 1730864151.10 epoch sec; seqNo:6908; depth 50.07 2024-11-06T03:35:51Z- handleDVL() - DVL msg epochMillisec: 1730864151.10 2024-11-06T03:35:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:51Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:51Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:51Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:51Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:51Z- handleDvl() - RDI_Pathfinder msg: 1730864151.10 sec; seqNo:2384; #beams:0 2024-11-06T03:35:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , 0.08 2024-11-06T03:35:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864151.11 epoch sec; seqNo:6908; 4061700.94 north; 594298.64 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:35:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864148.30 2024-11-06T03:35:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:51Z- waiting for 1730864152.30, time is 1730864151.11 2024-11-06T03:35:51Z- AHRS_M2 msg: 1730864151.50 epoch sec; seqNo:6482 2024-11-06T03:35:51Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:35:51Z- handleDepth()- Depth_Keller msg: 1730864151.52 epoch sec; seqNo:6909; depth 50.05 2024-11-06T03:35:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864151.52 epoch sec; seqNo:6909; 4061700.56 north; 594298.65 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:35:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864148.30 2024-11-06T03:35:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:51Z- waiting for 1730864152.30, time is 1730864151.52 2024-11-06T03:35:51Z- AHRS_M2 msg: 1730864151.90 epoch sec; seqNo:6483 2024-11-06T03:35:51Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:35:51Z- handleDepth()- Depth_Keller msg: 1730864151.91 epoch sec; seqNo:6910; depth 50.07 2024-11-06T03:35:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864151.92 epoch sec; seqNo:6910; 4061700.56 north; 594298.65 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T03:35:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864148.30 2024-11-06T03:35:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:52Z- waiting for 1730864152.30, time is 1730864151.92 2024-11-06T03:35:52Z- AHRS_M2 msg: 1730864152.31 epoch sec; seqNo:6484 2024-11-06T03:35:52Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 4.21 psi 2024-11-06T03:35:52Z- handleDepth()- Depth_Keller msg: 1730864152.31 epoch sec; seqNo:6911; depth 50.06 2024-11-06T03:35:52Z- handleDVL() - DVL msg epochMillisec: 1730864152.33 2024-11-06T03:35:52Z- handleDvl() - RDI_Pathfinder msg: 1730864152.33 sec; seqNo:2385; #beams:4 2024-11-06T03:35:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.49 , 40.17 , 44.45 , 41.80 2024-11-06T03:35:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.04 2024-11-06T03:35:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864152.35 epoch sec; seqNo:6911; 4061700.55 north; 594297.43 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:35:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864148.30 2024-11-06T03:35:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:52Z- waiting for 1730864152.30, time is 1730864152.35 2024-11-06T03:35:52Z- LcmTrn::updateTrn() >>>> pose time 1730864152.35, meas time 1730864152.33 <<<< 2024-11-06T03:35:52Z- LcmTrn::updateTrn() equating pose 1730864152.33 and meas times 1730864152.33 2024-11-06T03:35:52Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730864152.33 2024-11-06T03:35:52Z- TerrainNav::motionUpdate() - currEstimate = (4061701, 594297.43, 50.06) 2024-11-06T03:35:52Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:35:52Z- TerrainNav:: SubcloudNIS is 1.09, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:35:52Z- TNavPF::Projecting Measurements 2024-11-06T03:35:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:35:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:35:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:35:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:35:52Z- TNavFilter::4 good beams. 2024-11-06T03:35:52Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:35:52Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:35:52Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:35:52Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:35:52Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:35:52Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:35:52Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:35:52Z- TerrainMapOctree: (vehN, vehE ) = (4061700.55, 594297.43). 2024-11-06T03:35:52Z- TerrainMapOctree: Min Distance = 68.52. 2024-11-06T03:35:52Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:35:52Z- Forcing Subcloud Comparison 2024-11-06T03:35:52Z- Weighting particles with subcloud comparison 2024-11-06T03:35:52Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:52Z- meanExpectedMeasDiff: 2.612846 delta_rms_squared: 0.898098 alpha: 0.978216 2024-11-06T03:35:52Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:52Z- meanExpectedMeasDiff: -2.846113 delta_rms_squared: 0.721460 alpha: 0.973026 2024-11-06T03:35:52Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:52Z- meanExpectedMeasDiff: -3.910986 delta_rms_squared: 0.831586 alpha: 0.976514 2024-11-06T03:35:52Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:52Z- meanExpectedMeasDiff: 0.681480 delta_rms_squared: 0.662124 alpha: 0.970680 2024-11-06T03:35:52Z- Subcloud NIS: 1.101895 2024-11-06T03:35:52Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:35:52Z- TNavPF:: sumWeights = 0.040871 2024-11-06T03:35:52Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:35:52Z- TNavPF::First Term of Map Covariance Matrix is 0.34 2024-11-06T03:35:52Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:35:52Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730864152.33 sec, ping # 0. 2024-11-06T03:35:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:35:52Z- variances([0], [2], and psi_berg) = 0.109269 0.077058 0.000000 2024-11-06T03:35:52Z- publishEstimatesDecoder() - at 1730864152.33 sec 2024-11-06T03:35:52Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:35:52Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061727.99 594281.41 53.36 2024-11-06T03:35:52Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061728.34 594280.68 53.20 lat/lon: 36.70 -121.94 2024-11-06T03:35:52Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.11 0.08 0.36 0.00 2024-11-06T03:35:52Z- AHRS_M2 msg: 1730864152.71 epoch sec; seqNo:6485 2024-11-06T03:35:52Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 4.21 psi 2024-11-06T03:35:52Z- handleDepth()- Depth_Keller msg: 1730864152.72 epoch sec; seqNo:6912; depth 50.05 2024-11-06T03:35:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864152.74 epoch sec; seqNo:6912; 4061700.17 north; 594297.43 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T03:35:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864152.33 2024-11-06T03:35:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:52Z- waiting for 1730864156.33, time is 1730864152.74 2024-11-06T03:35:53Z- AHRS_M2 msg: 1730864153.11 epoch sec; seqNo:6486 2024-11-06T03:35:53Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 4.21 psi 2024-11-06T03:35:53Z- handleDepth()- Depth_Keller msg: 1730864153.13 epoch sec; seqNo:6913; depth 50.03 2024-11-06T03:35:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864153.14 epoch sec; seqNo:6913; 4061700.17 north; 594297.43 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T03:35:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864152.33 2024-11-06T03:35:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:53Z- waiting for 1730864156.33, time is 1730864153.14 2024-11-06T03:35:53Z- AHRS_M2 msg: 1730864153.52 epoch sec; seqNo:6487 2024-11-06T03:35:53Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.21 psi 2024-11-06T03:35:53Z- handleDepth()- Depth_Keller msg: 1730864153.53 epoch sec; seqNo:6914; depth 50.02 2024-11-06T03:35:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864153.54 epoch sec; seqNo:6914; 4061699.79 north; 594297.43 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:35:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864152.33 2024-11-06T03:35:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:53Z- waiting for 1730864156.33, time is 1730864153.54 2024-11-06T03:35:53Z- AHRS_M2 msg: 1730864153.93 epoch sec; seqNo:6488 2024-11-06T03:35:53Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.20 psi 2024-11-06T03:35:53Z- handleDepth()- Depth_Keller msg: 1730864153.94 epoch sec; seqNo:6915; depth 50.00 2024-11-06T03:35:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864153.97 epoch sec; seqNo:6915; 4061699.78 north; 594296.21 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T03:35:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864152.33 2024-11-06T03:35:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:54Z- waiting for 1730864156.33, time is 1730864153.97 2024-11-06T03:35:54Z- AHRS_M2 msg: 1730864154.33 epoch sec; seqNo:6489 2024-11-06T03:35:54Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.20 psi 2024-11-06T03:35:54Z- handleDepth()- Depth_Keller msg: 1730864154.33 epoch sec; seqNo:6916; depth 49.98 2024-11-06T03:35:54Z- handleDVL() - DVL msg epochMillisec: 1730864154.36 2024-11-06T03:35:54Z- handleDvl() - RDI_Pathfinder msg: 1730864154.36 sec; seqNo:2386; #beams:4 2024-11-06T03:35:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.69 , 40.57 , 44.65 , 41.80 2024-11-06T03:35:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.02 2024-11-06T03:35:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864154.37 epoch sec; seqNo:6916; 4061699.40 north; 594296.22 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:35:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864152.33 2024-11-06T03:35:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:54Z- waiting for 1730864156.33, time is 1730864154.37 2024-11-06T03:35:54Z- AHRS_M2 msg: 1730864154.73 epoch sec; seqNo:6490 2024-11-06T03:35:54Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.20 psi 2024-11-06T03:35:54Z- handleDepth()- Depth_Keller msg: 1730864154.74 epoch sec; seqNo:6917; depth 49.96 2024-11-06T03:35:54Z- handleDVL() - DVL msg epochMillisec: 1730864154.74 2024-11-06T03:35:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:54Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:54Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:54Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:54Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:54Z- handleDvl() - RDI_Pathfinder msg: 1730864154.74 sec; seqNo:2387; #beams:0 2024-11-06T03:35:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.02 2024-11-06T03:35:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864154.76 epoch sec; seqNo:6917; 4061699.40 north; 594296.22 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T03:35:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864152.33 2024-11-06T03:35:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:54Z- waiting for 1730864156.33, time is 1730864154.76 2024-11-06T03:35:55Z- AHRS_M2 msg: 1730864155.13 epoch sec; seqNo:6491 2024-11-06T03:35:55Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.20 psi 2024-11-06T03:35:55Z- handleDepth()- Depth_Keller msg: 1730864155.15 epoch sec; seqNo:6918; depth 49.93 2024-11-06T03:35:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864155.15 epoch sec; seqNo:6918; 4061699.02 north; 594296.22 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T03:35:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864152.33 2024-11-06T03:35:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:55Z- waiting for 1730864156.33, time is 1730864155.15 2024-11-06T03:35:55Z- AHRS_M2 msg: 1730864155.54 epoch sec; seqNo:6492 2024-11-06T03:35:55Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.20 psi 2024-11-06T03:35:55Z- handleDepth()- Depth_Keller msg: 1730864155.54 epoch sec; seqNo:6919; depth 49.92 2024-11-06T03:35:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864155.55 epoch sec; seqNo:6919; 4061699.00 north; 594295.00 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T03:35:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864152.33 2024-11-06T03:35:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:55Z- waiting for 1730864156.33, time is 1730864155.55 2024-11-06T03:35:55Z- AHRS_M2 msg: 1730864155.97 epoch sec; seqNo:6493 2024-11-06T03:35:55Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.20 psi 2024-11-06T03:35:55Z- handleDepth()- Depth_Keller msg: 1730864155.99 epoch sec; seqNo:6920; depth 49.90 2024-11-06T03:35:56Z- handleDVL() - DVL msg epochMillisec: 1730864156.03 2024-11-06T03:35:56Z- handleDvl() - RDI_Pathfinder msg: 1730864156.03 sec; seqNo:2388; #beams:4 2024-11-06T03:35:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.49 , 40.78 , 44.45 , 41.59 2024-11-06T03:35:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , -0.05 2024-11-06T03:35:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864156.04 epoch sec; seqNo:6920; 4061698.63 north; 594295.01 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:35:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864152.33 2024-11-06T03:35:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:56Z- waiting for 1730864156.33, time is 1730864156.04 2024-11-06T03:35:56Z- AHRS_M2 msg: 1730864156.35 epoch sec; seqNo:6494 2024-11-06T03:35:56Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.20 psi 2024-11-06T03:35:56Z- handleDepth()- Depth_Keller msg: 1730864156.35 epoch sec; seqNo:6921; depth 49.87 2024-11-06T03:35:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864156.38 epoch sec; seqNo:6921; 4061698.63 north; 594295.01 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T03:35:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864152.33 2024-11-06T03:35:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:56Z- waiting for 1730864156.33, time is 1730864156.38 2024-11-06T03:35:56Z- LcmTrn::updateTrn() >>>> pose time 1730864156.38, meas time 1730864156.03 <<<< 2024-11-06T03:35:56Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864156.38 2024-11-06T03:35:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864156.03 sec, ping # 0; waiting for INS data more recent than 1730864152.33... 2024-11-06T03:35:56Z- TerrainNav::motionUpdate() - currEstimate = (4061699, 594295.01, 49.87) 2024-11-06T03:35:56Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:35:56Z- TerrainNav:: SubcloudNIS is 1.10, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:35:56Z- TNavPF::Projecting Measurements 2024-11-06T03:35:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:35:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:35:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:35:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:35:56Z- TNavFilter::4 good beams. 2024-11-06T03:35:56Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:35:56Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:35:56Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:35:56Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:35:56Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:35:56Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:35:56Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:35:56Z- TerrainMapOctree: (vehN, vehE ) = (4061698.79, 594295.22). 2024-11-06T03:35:56Z- TerrainMapOctree: Min Distance = 68.31. 2024-11-06T03:35:56Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:35:56Z- Forcing Subcloud Comparison 2024-11-06T03:35:56Z- Weighting particles with subcloud comparison 2024-11-06T03:35:56Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:56Z- meanExpectedMeasDiff: 3.964748 delta_rms_squared: 0.546182 alpha: 0.964676 2024-11-06T03:35:56Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:56Z- meanExpectedMeasDiff: -2.336752 delta_rms_squared: 0.704823 alpha: 0.972407 2024-11-06T03:35:56Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:56Z- meanExpectedMeasDiff: -3.716155 delta_rms_squared: 0.837800 alpha: 0.976685 2024-11-06T03:35:56Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:35:56Z- meanExpectedMeasDiff: 0.052707 delta_rms_squared: 0.407128 alpha: 0.953176 2024-11-06T03:35:56Z- Subcloud NIS: 1.108872 2024-11-06T03:35:56Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:35:56Z- TNavPF:: sumWeights = 0.031243 2024-11-06T03:35:56Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:35:56Z- TNavPF::First Term of Map Covariance Matrix is 0.52 2024-11-06T03:35:56Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:35:56Z- TerrainNav::Resampling particle filter... 2024-11-06T03:35:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864156.03 sec. 2024-11-06T03:35:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:35:56Z- variances([0], [2], and psi_berg) = 0.114057 0.078800 0.000000 2024-11-06T03:35:56Z- publishEstimatesDecoder() - at 1730864156.38 sec 2024-11-06T03:35:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:35:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061726.81 594277.89 52.66 2024-11-06T03:35:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061726.45 594278.20 53.37 lat/lon: 36.70 -121.94 2024-11-06T03:35:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.11 0.08 0.66 0.00 2024-11-06T03:35:56Z- AHRS_M2 msg: 1730864156.77 epoch sec; seqNo:6495 2024-11-06T03:35:56Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.20 psi 2024-11-06T03:35:56Z- handleDepth()- Depth_Keller msg: 1730864156.77 epoch sec; seqNo:6922; depth 49.85 2024-11-06T03:35:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864156.79 epoch sec; seqNo:6922; 4061698.25 north; 594295.01 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:35:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864156.38 2024-11-06T03:35:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:56Z- waiting for 1730864160.38, time is 1730864156.79 2024-11-06T03:35:57Z- AHRS_M2 msg: 1730864157.15 epoch sec; seqNo:6496 2024-11-06T03:35:57Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.20 psi 2024-11-06T03:35:57Z- handleDepth()- Depth_Keller msg: 1730864157.17 epoch sec; seqNo:6923; depth 49.82 2024-11-06T03:35:57Z- handleDVL() - DVL msg epochMillisec: 1730864157.18 2024-11-06T03:35:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:35:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:35:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:35:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:35:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:35:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:35:57Z- handleDvl() - RDI_Pathfinder msg: 1730864157.18 sec; seqNo:2389; #beams:0 2024-11-06T03:35:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:35:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , -0.05 2024-11-06T03:35:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864157.19 epoch sec; seqNo:6923; 4061698.23 north; 594293.79 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:35:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864156.38 2024-11-06T03:35:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:57Z- waiting for 1730864160.38, time is 1730864157.19 2024-11-06T03:35:57Z- AHRS_M2 msg: 1730864157.56 epoch sec; seqNo:6497 2024-11-06T03:35:57Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.20 psi 2024-11-06T03:35:57Z- handleDepth()- Depth_Keller msg: 1730864157.57 epoch sec; seqNo:6924; depth 49.80 2024-11-06T03:35:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864157.59 epoch sec; seqNo:6924; 4061698.23 north; 594293.79 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:35:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864156.38 2024-11-06T03:35:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:57Z- waiting for 1730864160.38, time is 1730864157.59 2024-11-06T03:35:58Z- AHRS_M2 msg: 1730864158.00 epoch sec; seqNo:6498 2024-11-06T03:35:58Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.19 psi 2024-11-06T03:35:58Z- handleDepth()- Depth_Keller msg: 1730864158.01 epoch sec; seqNo:6925; depth 49.79 2024-11-06T03:35:58Z- handleDVL() - DVL msg epochMillisec: 1730864158.04 2024-11-06T03:35:58Z- handleDvl() - RDI_Pathfinder msg: 1730864158.04 sec; seqNo:2390; #beams:4 2024-11-06T03:35:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.08 , 40.78 , 44.45 , 41.59 2024-11-06T03:35:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , -0.05 2024-11-06T03:35:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864158.07 epoch sec; seqNo:6925; 4061697.85 north; 594293.79 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T03:35:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:35:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864156.38 2024-11-06T03:35:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:58Z- waiting for 1730864160.38, time is 1730864158.07 2024-11-06T03:35:58Z- AHRS_M2 msg: 1730864158.37 epoch sec; seqNo:6499 2024-11-06T03:35:58Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.19 psi 2024-11-06T03:35:58Z- handleDepth()- Depth_Keller msg: 1730864158.38 epoch sec; seqNo:6926; depth 49.75 2024-11-06T03:35:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864158.40 epoch sec; seqNo:6926; 4061697.84 north; 594292.57 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:35:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864156.38 2024-11-06T03:35:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:58Z- waiting for 1730864160.38, time is 1730864158.40 2024-11-06T03:35:58Z- AHRS_M2 msg: 1730864158.77 epoch sec; seqNo:6500 2024-11-06T03:35:58Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.19 psi 2024-11-06T03:35:58Z- handleDepth()- Depth_Keller msg: 1730864158.78 epoch sec; seqNo:6927; depth 49.74 2024-11-06T03:35:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864158.79 epoch sec; seqNo:6927; 4061697.46 north; 594292.58 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:35:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864156.38 2024-11-06T03:35:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:58Z- waiting for 1730864160.38, time is 1730864158.79 2024-11-06T03:35:59Z- AHRS_M2 msg: 1730864159.18 epoch sec; seqNo:6501 2024-11-06T03:35:59Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.19 psi 2024-11-06T03:35:59Z- handleDepth()- Depth_Keller msg: 1730864159.20 epoch sec; seqNo:6928; depth 49.71 2024-11-06T03:35:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864159.21 epoch sec; seqNo:6928; 4061697.46 north; 594292.58 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:35:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864156.38 2024-11-06T03:35:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:59Z- waiting for 1730864160.38, time is 1730864159.21 2024-11-06T03:35:59Z- AHRS_M2 msg: 1730864159.58 epoch sec; seqNo:6502 2024-11-06T03:35:59Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.19 psi 2024-11-06T03:35:59Z- handleDepth()- Depth_Keller msg: 1730864159.59 epoch sec; seqNo:6929; depth 49.70 2024-11-06T03:35:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864159.60 epoch sec; seqNo:6929; 4061697.08 north; 594292.58 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:35:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:35:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864156.38 2024-11-06T03:35:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:35:59Z- waiting for 1730864160.38, time is 1730864159.60 2024-11-06T03:36:00Z- AHRS_M2 msg: 1730864160.01 epoch sec; seqNo:6503 2024-11-06T03:36:00Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.18 psi 2024-11-06T03:36:00Z- handleDepth()- Depth_Keller msg: 1730864160.01 epoch sec; seqNo:6930; depth 49.69 2024-11-06T03:36:00Z- handleDVL() - DVL msg epochMillisec: 1730864160.04 2024-11-06T03:36:00Z- handleDvl() - RDI_Pathfinder msg: 1730864160.04 sec; seqNo:2391; #beams:4 2024-11-06T03:36:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 40.78 , 44.65 , 41.80 2024-11-06T03:36:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.07 , 0.00 2024-11-06T03:36:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864160.06 epoch sec; seqNo:6930; 4061697.07 north; 594291.36 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:36:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864156.38 2024-11-06T03:36:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:00Z- waiting for 1730864160.38, time is 1730864160.06 2024-11-06T03:36:00Z- AHRS_M2 msg: 1730864160.39 epoch sec; seqNo:6504 2024-11-06T03:36:00Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 4.18 psi 2024-11-06T03:36:00Z- handleDepth()- Depth_Keller msg: 1730864160.40 epoch sec; seqNo:6931; depth 49.68 2024-11-06T03:36:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864160.42 epoch sec; seqNo:6931; 4061696.69 north; 594291.37 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:36:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864156.38 2024-11-06T03:36:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:00Z- waiting for 1730864160.38, time is 1730864160.42 2024-11-06T03:36:00Z- LcmTrn::updateTrn() >>>> pose time 1730864160.42, meas time 1730864160.04 <<<< 2024-11-06T03:36:00Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864160.42 2024-11-06T03:36:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864160.04 sec, ping # 0; waiting for INS data more recent than 1730864156.38... 2024-11-06T03:36:00Z- TerrainNav::motionUpdate() - currEstimate = (4061697, 594291.37, 49.68) 2024-11-06T03:36:00Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:36:00Z- TerrainNav:: SubcloudNIS is 1.11, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:36:00Z- TNavPF::Projecting Measurements 2024-11-06T03:36:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:36:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:36:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:36:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:36:00Z- TNavFilter::4 good beams. 2024-11-06T03:36:00Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:36:00Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:36:00Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:36:00Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:36:00Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:36:00Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:36:00Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:36:00Z- TerrainMapOctree: (vehN, vehE ) = (4061696.87, 594291.70). 2024-11-06T03:36:00Z- TerrainMapOctree: Min Distance = 67.54. 2024-11-06T03:36:00Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:36:00Z- Forcing Subcloud Comparison 2024-11-06T03:36:00Z- Weighting particles with subcloud comparison 2024-11-06T03:36:00Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:00Z- meanExpectedMeasDiff: 2.993986 delta_rms_squared: 1.206895 alpha: 0.983699 2024-11-06T03:36:00Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:00Z- meanExpectedMeasDiff: -3.196477 delta_rms_squared: 1.064314 alpha: 0.981555 2024-11-06T03:36:00Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:00Z- meanExpectedMeasDiff: -3.214196 delta_rms_squared: 1.262886 alpha: 0.984410 2024-11-06T03:36:00Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:00Z- meanExpectedMeasDiff: -0.010029 delta_rms_squared: 1.179492 alpha: 0.983326 2024-11-06T03:36:00Z- Subcloud NIS: 1.112697 2024-11-06T03:36:00Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:36:00Z- TNavPF:: sumWeights = 0.041185 2024-11-06T03:36:00Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:36:00Z- TNavPF::First Term of Map Covariance Matrix is 0.49 2024-11-06T03:36:00Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T03:36:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864160.04 sec. 2024-11-06T03:36:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:36:00Z- variances([0], [2], and psi_berg) = 0.115546 0.079318 0.000000 2024-11-06T03:36:00Z- publishEstimatesDecoder() - at 1730864160.42 sec 2024-11-06T03:36:00Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:36:00Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061725.40 594273.55 54.71 2024-11-06T03:36:00Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061724.54 594274.53 53.64 lat/lon: 36.70 -121.94 2024-11-06T03:36:00Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.12 0.08 0.77 0.00 2024-11-06T03:36:00Z- AHRS_M2 msg: 1730864160.80 epoch sec; seqNo:6505 2024-11-06T03:36:00Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 4.18 psi 2024-11-06T03:36:00Z- handleDepth()- Depth_Keller msg: 1730864160.82 epoch sec; seqNo:6932; depth 49.68 2024-11-06T03:36:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864160.83 epoch sec; seqNo:6932; 4061696.69 north; 594291.37 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:36:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864160.42 2024-11-06T03:36:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:00Z- waiting for 1730864164.42, time is 1730864160.83 2024-11-06T03:36:01Z- AHRS_M2 msg: 1730864161.20 epoch sec; seqNo:6506 2024-11-06T03:36:01Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 4.17 psi 2024-11-06T03:36:01Z- handleDepth()- Depth_Keller msg: 1730864161.21 epoch sec; seqNo:6933; depth 49.66 2024-11-06T03:36:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864161.26 epoch sec; seqNo:6933; 4061696.30 north; 594290.15 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T03:36:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864160.42 2024-11-06T03:36:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:01Z- waiting for 1730864164.42, time is 1730864161.26 2024-11-06T03:36:01Z- AHRS_M2 msg: 1730864161.61 epoch sec; seqNo:6507 2024-11-06T03:36:01Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 4.17 psi 2024-11-06T03:36:01Z- handleDepth()- Depth_Keller msg: 1730864161.61 epoch sec; seqNo:6934; depth 49.64 2024-11-06T03:36:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864161.63 epoch sec; seqNo:6934; 4061696.30 north; 594290.15 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:36:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864160.42 2024-11-06T03:36:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:01Z- waiting for 1730864164.42, time is 1730864161.63 2024-11-06T03:36:02Z- handleDepth()- Depth_Keller msg: 1730864162.04 epoch sec; seqNo:6935; depth 49.64 2024-11-06T03:36:02Z- handleDVL() - DVL msg epochMillisec: 1730864162.06 2024-11-06T03:36:02Z- handleDvl() - RDI_Pathfinder msg: 1730864162.06 sec; seqNo:2392; #beams:4 2024-11-06T03:36:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 40.98 , 44.65 , 42.21 2024-11-06T03:36:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.07 , 0.06 2024-11-06T03:36:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864162.08 epoch sec; seqNo:6935; 4061695.92 north; 594290.15 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:36:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864160.42 2024-11-06T03:36:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:02Z- waiting for 1730864164.42, time is 1730864162.08 2024-11-06T03:36:02Z- AHRS_M2 msg: 1730864162.41 epoch sec; seqNo:6508 2024-11-06T03:36:02Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 4.16 psi 2024-11-06T03:36:02Z- handleDepth()- Depth_Keller msg: 1730864162.41 epoch sec; seqNo:6936; depth 49.63 2024-11-06T03:36:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864162.47 epoch sec; seqNo:6936; 4061695.92 north; 594290.15 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:36:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864160.42 2024-11-06T03:36:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:02Z- waiting for 1730864164.42, time is 1730864162.47 2024-11-06T03:36:02Z- AHRS_M2 msg: 1730864162.81 epoch sec; seqNo:6509 2024-11-06T03:36:02Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 4.16 psi 2024-11-06T03:36:02Z- handleDepth()- Depth_Keller msg: 1730864162.82 epoch sec; seqNo:6937; depth 49.63 2024-11-06T03:36:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864162.83 epoch sec; seqNo:6937; 4061695.53 north; 594288.94 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:36:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864160.42 2024-11-06T03:36:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:02Z- waiting for 1730864164.42, time is 1730864162.83 2024-11-06T03:36:03Z- AHRS_M2 msg: 1730864163.22 epoch sec; seqNo:6510 2024-11-06T03:36:03Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 4.16 psi 2024-11-06T03:36:03Z- handleDepth()- Depth_Keller msg: 1730864163.23 epoch sec; seqNo:6938; depth 49.63 2024-11-06T03:36:03Z- handleDVL() - DVL msg epochMillisec: 1730864163.24 2024-11-06T03:36:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:03Z- handleDvl() - RDI_Pathfinder msg: 1730864163.24 sec; seqNo:2393; #beams:0 2024-11-06T03:36:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.07 , 0.06 2024-11-06T03:36:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864163.25 epoch sec; seqNo:6938; 4061695.53 north; 594288.94 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T03:36:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864160.42 2024-11-06T03:36:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:03Z- waiting for 1730864164.42, time is 1730864163.25 2024-11-06T03:36:03Z- AHRS_M2 msg: 1730864163.62 epoch sec; seqNo:6511 2024-11-06T03:36:03Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 4.15 psi 2024-11-06T03:36:03Z- handleDepth()- Depth_Keller msg: 1730864163.63 epoch sec; seqNo:6939; depth 49.62 2024-11-06T03:36:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864163.65 epoch sec; seqNo:6939; 4061695.15 north; 594288.94 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:36:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864160.42 2024-11-06T03:36:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:03Z- waiting for 1730864164.42, time is 1730864163.65 2024-11-06T03:36:04Z- AHRS_M2 msg: 1730864164.02 epoch sec; seqNo:6512 2024-11-06T03:36:04Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 4.15 psi 2024-11-06T03:36:04Z- handleDepth()- Depth_Keller msg: 1730864164.03 epoch sec; seqNo:6940; depth 49.61 2024-11-06T03:36:04Z- handleDVL() - DVL msg epochMillisec: 1730864164.06 2024-11-06T03:36:04Z- handleDvl() - RDI_Pathfinder msg: 1730864164.06 sec; seqNo:2394; #beams:4 2024-11-06T03:36:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 41.19 , 44.45 , 42.61 2024-11-06T03:36:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.03 , 0.05 2024-11-06T03:36:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864164.07 epoch sec; seqNo:6940; 4061695.13 north; 594287.72 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:36:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864160.42 2024-11-06T03:36:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:04Z- waiting for 1730864164.42, time is 1730864164.07 2024-11-06T03:36:04Z- AHRS_M2 msg: 1730864164.43 epoch sec; seqNo:6513 2024-11-06T03:36:04Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 4.15 psi 2024-11-06T03:36:04Z- handleDepth()- Depth_Keller msg: 1730864164.43 epoch sec; seqNo:6941; depth 49.62 2024-11-06T03:36:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864164.45 epoch sec; seqNo:6941; 4061694.76 north; 594287.73 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:36:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864160.42 2024-11-06T03:36:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:04Z- waiting for 1730864164.42, time is 1730864164.45 2024-11-06T03:36:04Z- LcmTrn::updateTrn() >>>> pose time 1730864164.45, meas time 1730864164.06 <<<< 2024-11-06T03:36:04Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864164.45 2024-11-06T03:36:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864164.06 sec, ping # 0; waiting for INS data more recent than 1730864160.42... 2024-11-06T03:36:04Z- TerrainNav::motionUpdate() - currEstimate = (4061695, 594287.73, 49.62) 2024-11-06T03:36:04Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:36:04Z- TerrainNav:: SubcloudNIS is 1.11, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:36:04Z- TNavPF::Projecting Measurements 2024-11-06T03:36:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:36:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:36:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:36:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:36:04Z- TNavFilter::4 good beams. 2024-11-06T03:36:04Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:36:04Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:36:04Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:36:04Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:36:04Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:36:04Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:36:04Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:36:04Z- TerrainMapOctree: (vehN, vehE ) = (4061694.94, 594288.07). 2024-11-06T03:36:04Z- TerrainMapOctree: Min Distance = 66.94. 2024-11-06T03:36:04Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:36:04Z- Forcing Subcloud Comparison 2024-11-06T03:36:04Z- Weighting particles with subcloud comparison 2024-11-06T03:36:04Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:04Z- meanExpectedMeasDiff: 2.402591 delta_rms_squared: 1.604318 alpha: 0.987687 2024-11-06T03:36:04Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:04Z- meanExpectedMeasDiff: -2.412443 delta_rms_squared: 1.212856 alpha: 0.983778 2024-11-06T03:36:04Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:04Z- meanExpectedMeasDiff: -2.343510 delta_rms_squared: 1.398539 alpha: 0.985901 2024-11-06T03:36:04Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:04Z- meanExpectedMeasDiff: -0.221105 delta_rms_squared: 1.114707 alpha: 0.982374 2024-11-06T03:36:04Z- Subcloud NIS: 1.086070 2024-11-06T03:36:04Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:36:04Z- TNavPF:: sumWeights = 0.129832 2024-11-06T03:36:04Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:36:04Z- TNavPF::First Term of Map Covariance Matrix is 0.86 2024-11-06T03:36:04Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.01 2024-11-06T03:36:04Z- TerrainNav::Resampling particle filter... 2024-11-06T03:36:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864164.06 sec. 2024-11-06T03:36:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:36:04Z- variances([0], [2], and psi_berg) = 0.115517 0.082138 0.000000 2024-11-06T03:36:04Z- publishEstimatesDecoder() - at 1730864164.45 sec 2024-11-06T03:36:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:36:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061722.91 594270.66 54.07 2024-11-06T03:36:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061722.61 594270.89 53.88 lat/lon: 36.70 -121.94 2024-11-06T03:36:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.12 0.08 0.73 0.00 2024-11-06T03:36:04Z- AHRS_M2 msg: 1730864164.83 epoch sec; seqNo:6514 2024-11-06T03:36:04Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 4.15 psi 2024-11-06T03:36:04Z- handleDepth()- Depth_Keller msg: 1730864164.84 epoch sec; seqNo:6942; depth 49.62 2024-11-06T03:36:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864164.87 epoch sec; seqNo:6942; 4061694.76 north; 594287.73 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:36:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864164.45 2024-11-06T03:36:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:04Z- waiting for 1730864168.45, time is 1730864164.87 2024-11-06T03:36:05Z- AHRS_M2 msg: 1730864165.24 epoch sec; seqNo:6515 2024-11-06T03:36:05Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 4.15 psi 2024-11-06T03:36:05Z- handleDepth()- Depth_Keller msg: 1730864165.24 epoch sec; seqNo:6943; depth 49.60 2024-11-06T03:36:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864165.25 epoch sec; seqNo:6943; 4061694.38 north; 594287.73 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:36:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864164.45 2024-11-06T03:36:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:05Z- waiting for 1730864168.45, time is 1730864165.25 2024-11-06T03:36:05Z- AHRS_M2 msg: 1730864165.64 epoch sec; seqNo:6516 2024-11-06T03:36:05Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 4.15 psi 2024-11-06T03:36:05Z- handleDepth()- Depth_Keller msg: 1730864165.65 epoch sec; seqNo:6944; depth 49.60 2024-11-06T03:36:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864165.66 epoch sec; seqNo:6944; 4061694.36 north; 594286.51 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:36:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864164.45 2024-11-06T03:36:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:05Z- waiting for 1730864168.45, time is 1730864165.66 2024-11-06T03:36:06Z- AHRS_M2 msg: 1730864166.04 epoch sec; seqNo:6517 2024-11-06T03:36:06Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 4.15 psi 2024-11-06T03:36:06Z- handleDepth()- Depth_Keller msg: 1730864166.05 epoch sec; seqNo:6945; depth 49.61 2024-11-06T03:36:06Z- handleDVL() - DVL msg epochMillisec: 1730864166.08 2024-11-06T03:36:06Z- handleDvl() - RDI_Pathfinder msg: 1730864166.08 sec; seqNo:2395; #beams:4 2024-11-06T03:36:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.49 , 41.19 , 44.24 , 42.82 2024-11-06T03:36:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.01 2024-11-06T03:36:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864166.09 epoch sec; seqNo:6945; 4061693.98 north; 594286.51 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:36:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864164.45 2024-11-06T03:36:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:06Z- waiting for 1730864168.45, time is 1730864166.09 2024-11-06T03:36:06Z- AHRS_M2 msg: 1730864166.45 epoch sec; seqNo:6518 2024-11-06T03:36:06Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 4.15 psi 2024-11-06T03:36:06Z- handleDepth()- Depth_Keller msg: 1730864166.45 epoch sec; seqNo:6946; depth 49.60 2024-11-06T03:36:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864166.47 epoch sec; seqNo:6946; 4061693.98 north; 594286.51 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:36:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864164.45 2024-11-06T03:36:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:06Z- waiting for 1730864168.45, time is 1730864166.47 2024-11-06T03:36:06Z- AHRS_M2 msg: 1730864166.85 epoch sec; seqNo:6519 2024-11-06T03:36:06Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 4.15 psi 2024-11-06T03:36:06Z- handleDepth()- Depth_Keller msg: 1730864166.86 epoch sec; seqNo:6947; depth 49.59 2024-11-06T03:36:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864166.87 epoch sec; seqNo:6947; 4061693.61 north; 594286.52 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:36:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864164.45 2024-11-06T03:36:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:07Z- waiting for 1730864168.45, time is 1730864166.87 2024-11-06T03:36:07Z- AHRS_M2 msg: 1730864167.26 epoch sec; seqNo:6520 2024-11-06T03:36:07Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 4.15 psi 2024-11-06T03:36:07Z- handleDepth()- Depth_Keller msg: 1730864167.26 epoch sec; seqNo:6948; depth 49.59 2024-11-06T03:36:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864167.28 epoch sec; seqNo:6948; 4061693.59 north; 594285.30 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:36:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864164.45 2024-11-06T03:36:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:07Z- waiting for 1730864168.45, time is 1730864167.28 2024-11-06T03:36:07Z- AHRS_M2 msg: 1730864167.66 epoch sec; seqNo:6521 2024-11-06T03:36:07Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 4.15 psi 2024-11-06T03:36:07Z- handleDepth()- Depth_Keller msg: 1730864167.67 epoch sec; seqNo:6949; depth 49.59 2024-11-06T03:36:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864167.68 epoch sec; seqNo:6949; 4061693.21 north; 594285.30 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:36:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864164.45 2024-11-06T03:36:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:07Z- waiting for 1730864168.45, time is 1730864167.68 2024-11-06T03:36:08Z- AHRS_M2 msg: 1730864168.06 epoch sec; seqNo:6522 2024-11-06T03:36:08Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 4.15 psi 2024-11-06T03:36:08Z- handleDepth()- Depth_Keller msg: 1730864168.07 epoch sec; seqNo:6950; depth 49.59 2024-11-06T03:36:08Z- handleDVL() - DVL msg epochMillisec: 1730864168.10 2024-11-06T03:36:08Z- handleDvl() - RDI_Pathfinder msg: 1730864168.10 sec; seqNo:2396; #beams:4 2024-11-06T03:36:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 41.19 , 44.04 , 43.43 2024-11-06T03:36:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , -0.03 2024-11-06T03:36:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864168.12 epoch sec; seqNo:6950; 4061693.21 north; 594285.30 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:36:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864164.45 2024-11-06T03:36:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:08Z- waiting for 1730864168.45, time is 1730864168.12 2024-11-06T03:36:08Z- AHRS_M2 msg: 1730864168.47 epoch sec; seqNo:6523 2024-11-06T03:36:08Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 4.15 psi 2024-11-06T03:36:08Z- handleDepth()- Depth_Keller msg: 1730864168.49 epoch sec; seqNo:6951; depth 49.57 2024-11-06T03:36:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864168.50 epoch sec; seqNo:6951; 4061692.83 north; 594285.31 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:36:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864164.45 2024-11-06T03:36:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:08Z- waiting for 1730864168.45, time is 1730864168.50 2024-11-06T03:36:08Z- LcmTrn::updateTrn() >>>> pose time 1730864168.50, meas time 1730864168.10 <<<< 2024-11-06T03:36:08Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864168.50 2024-11-06T03:36:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864168.10 sec, ping # 0; waiting for INS data more recent than 1730864164.45... 2024-11-06T03:36:08Z- TerrainNav::motionUpdate() - currEstimate = (4061693, 594285.31, 49.57) 2024-11-06T03:36:08Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:36:08Z- TerrainNav:: SubcloudNIS is 1.09, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:36:08Z- TNavPF::Projecting Measurements 2024-11-06T03:36:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:36:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:36:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:36:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:36:08Z- TNavFilter::4 good beams. 2024-11-06T03:36:08Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:36:08Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:36:08Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:36:08Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:36:08Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:36:08Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:36:08Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:36:08Z- TerrainMapOctree: (vehN, vehE ) = (4061693.03, 594285.55). 2024-11-06T03:36:08Z- TerrainMapOctree: Min Distance = 67.15. 2024-11-06T03:36:08Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:36:08Z- Forcing Subcloud Comparison 2024-11-06T03:36:08Z- Weighting particles with subcloud comparison 2024-11-06T03:36:08Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:08Z- meanExpectedMeasDiff: 2.941522 delta_rms_squared: 1.535597 alpha: 0.987143 2024-11-06T03:36:08Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:08Z- meanExpectedMeasDiff: -1.958047 delta_rms_squared: 1.136753 alpha: 0.982710 2024-11-06T03:36:08Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:08Z- meanExpectedMeasDiff: -2.140888 delta_rms_squared: 1.303932 alpha: 0.984893 2024-11-06T03:36:08Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:08Z- meanExpectedMeasDiff: 0.416681 delta_rms_squared: 1.124334 alpha: 0.982523 2024-11-06T03:36:08Z- Subcloud NIS: 1.070314 2024-11-06T03:36:08Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:36:08Z- TNavPF:: sumWeights = 0.125775 2024-11-06T03:36:08Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:36:08Z- TNavPF::First Term of Map Covariance Matrix is 1.00 2024-11-06T03:36:08Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.01 2024-11-06T03:36:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864168.10 sec. 2024-11-06T03:36:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:36:08Z- variances([0], [2], and psi_berg) = 0.116337 0.082895 0.000000 2024-11-06T03:36:08Z- publishEstimatesDecoder() - at 1730864168.50 sec 2024-11-06T03:36:08Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:36:08Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061720.81 594269.26 54.56 2024-11-06T03:36:08Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061720.69 594268.47 53.91 lat/lon: 36.70 -121.94 2024-11-06T03:36:08Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.12 0.08 0.73 0.00 2024-11-06T03:36:08Z- AHRS_M2 msg: 1730864168.87 epoch sec; seqNo:6524 2024-11-06T03:36:08Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 4.15 psi 2024-11-06T03:36:08Z- handleDepth()- Depth_Keller msg: 1730864168.88 epoch sec; seqNo:6952; depth 49.59 2024-11-06T03:36:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864168.91 epoch sec; seqNo:6952; 4061692.82 north; 594284.09 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:36:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864168.50 2024-11-06T03:36:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:09Z- waiting for 1730864172.50, time is 1730864168.91 2024-11-06T03:36:09Z- AHRS_M2 msg: 1730864169.27 epoch sec; seqNo:6525 2024-11-06T03:36:09Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 4.14 psi 2024-11-06T03:36:09Z- handleDepth()- Depth_Keller msg: 1730864169.28 epoch sec; seqNo:6953; depth 49.57 2024-11-06T03:36:09Z- handleDVL() - DVL msg epochMillisec: 1730864169.29 2024-11-06T03:36:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:09Z- handleDvl() - RDI_Pathfinder msg: 1730864169.29 sec; seqNo:2397; #beams:0 2024-11-06T03:36:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , -0.03 2024-11-06T03:36:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864169.30 epoch sec; seqNo:6953; 4061692.44 north; 594284.09 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:36:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864168.50 2024-11-06T03:36:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:09Z- waiting for 1730864172.50, time is 1730864169.30 2024-11-06T03:36:09Z- AHRS_M2 msg: 1730864169.68 epoch sec; seqNo:6526 2024-11-06T03:36:09Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 4.14 psi 2024-11-06T03:36:09Z- handleDepth()- Depth_Keller msg: 1730864169.68 epoch sec; seqNo:6954; depth 49.58 2024-11-06T03:36:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864169.69 epoch sec; seqNo:6954; 4061692.44 north; 594284.09 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:36:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864168.50 2024-11-06T03:36:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:09Z- waiting for 1730864172.50, time is 1730864169.69 2024-11-06T03:36:10Z- AHRS_M2 msg: 1730864170.09 epoch sec; seqNo:6527 2024-11-06T03:36:10Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.14 psi 2024-11-06T03:36:10Z- handleDepth()- Depth_Keller msg: 1730864170.09 epoch sec; seqNo:6955; depth 49.58 2024-11-06T03:36:10Z- handleDVL() - DVL msg epochMillisec: 1730864170.11 2024-11-06T03:36:10Z- handleDvl() - RDI_Pathfinder msg: 1730864170.11 sec; seqNo:2398; #beams:4 2024-11-06T03:36:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 40.98 , 43.84 , 43.43 2024-11-06T03:36:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , -0.03 2024-11-06T03:36:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864170.12 epoch sec; seqNo:6955; 4061692.06 north; 594284.09 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:36:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864168.50 2024-11-06T03:36:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:10Z- waiting for 1730864172.50, time is 1730864170.12 2024-11-06T03:36:10Z- AHRS_M2 msg: 1730864170.49 epoch sec; seqNo:6528 2024-11-06T03:36:10Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 4.14 psi 2024-11-06T03:36:10Z- handleDepth()- Depth_Keller msg: 1730864170.49 epoch sec; seqNo:6956; depth 49.58 2024-11-06T03:36:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864170.52 epoch sec; seqNo:6956; 4061692.05 north; 594282.87 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:36:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864168.50 2024-11-06T03:36:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:10Z- waiting for 1730864172.50, time is 1730864170.52 2024-11-06T03:36:10Z- AHRS_M2 msg: 1730864170.89 epoch sec; seqNo:6529 2024-11-06T03:36:10Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 4.14 psi 2024-11-06T03:36:10Z- handleDepth()- Depth_Keller msg: 1730864170.89 epoch sec; seqNo:6957; depth 49.57 2024-11-06T03:36:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864170.91 epoch sec; seqNo:6957; 4061691.67 north; 594282.88 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T03:36:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864168.50 2024-11-06T03:36:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:11Z- waiting for 1730864172.50, time is 1730864170.91 2024-11-06T03:36:11Z- AHRS_M2 msg: 1730864171.30 epoch sec; seqNo:6530 2024-11-06T03:36:11Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 4.14 psi 2024-11-06T03:36:11Z- handleDepth()- Depth_Keller msg: 1730864171.31 epoch sec; seqNo:6958; depth 49.60 2024-11-06T03:36:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864171.31 epoch sec; seqNo:6958; 4061691.67 north; 594282.88 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:36:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864168.50 2024-11-06T03:36:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:11Z- waiting for 1730864172.50, time is 1730864171.31 2024-11-06T03:36:11Z- AHRS_M2 msg: 1730864171.70 epoch sec; seqNo:6531 2024-11-06T03:36:11Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 4.13 psi 2024-11-06T03:36:11Z- handleDepth()- Depth_Keller msg: 1730864171.70 epoch sec; seqNo:6959; depth 49.58 2024-11-06T03:36:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864171.71 epoch sec; seqNo:6959; 4061691.29 north; 594282.88 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:36:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864168.50 2024-11-06T03:36:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:11Z- waiting for 1730864172.50, time is 1730864171.71 2024-11-06T03:36:12Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:36:12Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:36:12Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:36:12Z- AHRS_M2 msg: 1730864172.10 epoch sec; seqNo:6532 2024-11-06T03:36:12Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 4.13 psi 2024-11-06T03:36:12Z- handleDepth()- Depth_Keller msg: 1730864172.13 epoch sec; seqNo:6960; depth 49.58 2024-11-06T03:36:12Z- handleDVL() - DVL msg epochMillisec: 1730864172.15 2024-11-06T03:36:12Z- handleDvl() - RDI_Pathfinder msg: 1730864172.15 sec; seqNo:2399; #beams:4 2024-11-06T03:36:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 40.98 , 43.63 , 43.43 2024-11-06T03:36:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.05 , 0.00 2024-11-06T03:36:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864172.16 epoch sec; seqNo:6960; 4061690.90 north; 594281.67 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T03:36:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864168.50 2024-11-06T03:36:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:12Z- waiting for 1730864172.50, time is 1730864172.16 2024-11-06T03:36:12Z- AHRS_M2 msg: 1730864172.51 epoch sec; seqNo:6533 2024-11-06T03:36:12Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 4.13 psi 2024-11-06T03:36:12Z- handleDepth()- Depth_Keller msg: 1730864172.52 epoch sec; seqNo:6961; depth 49.59 2024-11-06T03:36:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864172.55 epoch sec; seqNo:6961; 4061690.90 north; 594281.67 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:36:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864168.50 2024-11-06T03:36:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:12Z- waiting for 1730864172.50, time is 1730864172.55 2024-11-06T03:36:12Z- LcmTrn::updateTrn() >>>> pose time 1730864172.55, meas time 1730864172.15 <<<< 2024-11-06T03:36:12Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864172.55 2024-11-06T03:36:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864172.15 sec, ping # 0; waiting for INS data more recent than 1730864168.50... 2024-11-06T03:36:12Z- TerrainNav::motionUpdate() - currEstimate = (4061691, 594281.67, 49.59) 2024-11-06T03:36:12Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:36:12Z- TerrainNav:: SubcloudNIS is 1.07, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:36:12Z- TNavPF::Projecting Measurements 2024-11-06T03:36:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:36:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:36:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:36:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:36:12Z- TNavFilter::4 good beams. 2024-11-06T03:36:12Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:36:12Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:36:12Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:36:12Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:36:12Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:36:12Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:36:12Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:36:12Z- TerrainMapOctree: (vehN, vehE ) = (4061691.09, 594282.02). 2024-11-06T03:36:12Z- TerrainMapOctree: Min Distance = 67.05. 2024-11-06T03:36:12Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:36:12Z- Forcing Subcloud Comparison 2024-11-06T03:36:12Z- Weighting particles with subcloud comparison 2024-11-06T03:36:12Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:12Z- meanExpectedMeasDiff: 2.169855 delta_rms_squared: 0.865193 alpha: 0.977406 2024-11-06T03:36:12Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:12Z- meanExpectedMeasDiff: -1.961521 delta_rms_squared: 1.139013 alpha: 0.982744 2024-11-06T03:36:12Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:12Z- meanExpectedMeasDiff: -2.133762 delta_rms_squared: 1.295062 alpha: 0.984792 2024-11-06T03:36:12Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:12Z- meanExpectedMeasDiff: 0.016366 delta_rms_squared: 1.363682 alpha: 0.985546 2024-11-06T03:36:12Z- Subcloud NIS: 1.008241 2024-11-06T03:36:12Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:36:12Z- TNavPF:: sumWeights = 0.212366 2024-11-06T03:36:12Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:36:12Z- TNavPF::First Term of Map Covariance Matrix is 0.82 2024-11-06T03:36:12Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T03:36:12Z- TerrainNav::Resampling particle filter... 2024-11-06T03:36:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864172.15 sec. 2024-11-06T03:36:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:36:12Z- variances([0], [2], and psi_berg) = 0.116253 0.077155 0.000000 2024-11-06T03:36:12Z- publishEstimatesDecoder() - at 1730864172.55 sec 2024-11-06T03:36:12Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:36:12Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061719.14 594264.56 53.64 2024-11-06T03:36:12Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061718.76 594264.79 54.23 lat/lon: 36.70 -121.94 2024-11-06T03:36:12Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.12 0.08 0.73 0.00 2024-11-06T03:36:12Z- handleDepth()- Depth_Keller msg: 1730864172.93 epoch sec; seqNo:6962; depth 49.60 2024-11-06T03:36:12Z- handleDVL() - DVL msg epochMillisec: 1730864172.93 2024-11-06T03:36:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:12Z- handleDvl() - RDI_Pathfinder msg: 1730864172.93 sec; seqNo:2400; #beams:0 2024-11-06T03:36:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.05 , 0.00 2024-11-06T03:36:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864172.94 epoch sec; seqNo:6962; 4061690.52 north; 594281.67 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T03:36:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864172.55 2024-11-06T03:36:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:13Z- waiting for 1730864176.55, time is 1730864172.94 2024-11-06T03:36:13Z- AHRS_M2 msg: 1730864173.32 epoch sec; seqNo:6534 2024-11-06T03:36:13Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 4.13 psi 2024-11-06T03:36:13Z- handleDepth()- Depth_Keller msg: 1730864173.33 epoch sec; seqNo:6963; depth 49.59 2024-11-06T03:36:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864173.34 epoch sec; seqNo:6963; 4061690.52 north; 594281.67 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T03:36:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864172.55 2024-11-06T03:36:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:13Z- waiting for 1730864176.55, time is 1730864173.34 2024-11-06T03:36:13Z- AHRS_M2 msg: 1730864173.72 epoch sec; seqNo:6535 2024-11-06T03:36:13Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 4.12 psi 2024-11-06T03:36:13Z- handleDepth()- Depth_Keller msg: 1730864173.72 epoch sec; seqNo:6964; depth 49.61 2024-11-06T03:36:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864173.74 epoch sec; seqNo:6964; 4061690.13 north; 594280.45 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:36:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864172.55 2024-11-06T03:36:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:13Z- waiting for 1730864176.55, time is 1730864173.74 2024-11-06T03:36:14Z- AHRS_M2 msg: 1730864174.16 epoch sec; seqNo:6536 2024-11-06T03:36:14Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 4.12 psi 2024-11-06T03:36:14Z- handleDepth()- Depth_Keller msg: 1730864174.16 epoch sec; seqNo:6965; depth 49.61 2024-11-06T03:36:14Z- handleDVL() - DVL msg epochMillisec: 1730864174.18 2024-11-06T03:36:14Z- handleDvl() - RDI_Pathfinder msg: 1730864174.18 sec; seqNo:2401; #beams:4 2024-11-06T03:36:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 40.78 , 43.84 , 43.63 2024-11-06T03:36:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , 0.03 2024-11-06T03:36:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864174.19 epoch sec; seqNo:6965; 4061690.13 north; 594280.45 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T03:36:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864172.55 2024-11-06T03:36:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:14Z- waiting for 1730864176.55, time is 1730864174.19 2024-11-06T03:36:14Z- AHRS_M2 msg: 1730864174.53 epoch sec; seqNo:6537 2024-11-06T03:36:14Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 4.12 psi 2024-11-06T03:36:14Z- handleDepth()- Depth_Keller msg: 1730864174.53 epoch sec; seqNo:6966; depth 49.62 2024-11-06T03:36:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864174.55 epoch sec; seqNo:6966; 4061689.75 north; 594280.46 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T03:36:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864172.55 2024-11-06T03:36:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:14Z- waiting for 1730864176.55, time is 1730864174.55 2024-11-06T03:36:14Z- AHRS_M2 msg: 1730864174.95 epoch sec; seqNo:6538 2024-11-06T03:36:14Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 4.12 psi 2024-11-06T03:36:14Z- handleDepth()- Depth_Keller msg: 1730864174.95 epoch sec; seqNo:6967; depth 49.65 2024-11-06T03:36:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864174.96 epoch sec; seqNo:6967; 4061689.74 north; 594279.24 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:36:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864172.55 2024-11-06T03:36:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:15Z- waiting for 1730864176.55, time is 1730864174.96 2024-11-06T03:36:15Z- AHRS_M2 msg: 1730864175.35 epoch sec; seqNo:6539 2024-11-06T03:36:15Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 4.12 psi 2024-11-06T03:36:15Z- handleDepth()- Depth_Keller msg: 1730864175.35 epoch sec; seqNo:6968; depth 49.64 2024-11-06T03:36:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864175.36 epoch sec; seqNo:6968; 4061689.36 north; 594279.24 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T03:36:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864172.55 2024-11-06T03:36:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:15Z- waiting for 1730864176.55, time is 1730864175.36 2024-11-06T03:36:15Z- AHRS_M2 msg: 1730864175.74 epoch sec; seqNo:6540 2024-11-06T03:36:15Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 4.12 psi 2024-11-06T03:36:15Z- handleDepth()- Depth_Keller msg: 1730864175.75 epoch sec; seqNo:6969; depth 49.65 2024-11-06T03:36:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864175.76 epoch sec; seqNo:6969; 4061689.36 north; 594279.24 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T03:36:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864172.55 2024-11-06T03:36:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:15Z- waiting for 1730864176.55, time is 1730864175.76 2024-11-06T03:36:16Z- AHRS_M2 msg: 1730864176.14 epoch sec; seqNo:6541 2024-11-06T03:36:16Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 4.12 psi 2024-11-06T03:36:16Z- handleDepth()- Depth_Keller msg: 1730864176.15 epoch sec; seqNo:6970; depth 49.68 2024-11-06T03:36:16Z- handleDVL() - DVL msg epochMillisec: 1730864176.17 2024-11-06T03:36:16Z- handleDvl() - RDI_Pathfinder msg: 1730864176.17 sec; seqNo:2402; #beams:4 2024-11-06T03:36:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 40.98 , 43.63 , 43.84 2024-11-06T03:36:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , 0.01 2024-11-06T03:36:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864176.18 epoch sec; seqNo:6970; 4061688.98 north; 594279.25 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T03:36:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864172.55 2024-11-06T03:36:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:16Z- waiting for 1730864176.55, time is 1730864176.18 2024-11-06T03:36:16Z- AHRS_M2 msg: 1730864176.55 epoch sec; seqNo:6542 2024-11-06T03:36:16Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 4.12 psi 2024-11-06T03:36:16Z- handleDepth()- Depth_Keller msg: 1730864176.55 epoch sec; seqNo:6971; depth 49.67 2024-11-06T03:36:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864176.57 epoch sec; seqNo:6971; 4061688.97 north; 594278.03 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T03:36:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864172.55 2024-11-06T03:36:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:16Z- waiting for 1730864176.55, time is 1730864176.57 2024-11-06T03:36:16Z- LcmTrn::updateTrn() >>>> pose time 1730864176.57, meas time 1730864176.17 <<<< 2024-11-06T03:36:16Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864176.57 2024-11-06T03:36:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864176.17 sec, ping # 0; waiting for INS data more recent than 1730864172.55... 2024-11-06T03:36:16Z- TerrainNav::motionUpdate() - currEstimate = (4061689, 594278.03, 49.67) 2024-11-06T03:36:16Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:36:16Z- TerrainNav:: SubcloudNIS is 1.01, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:36:16Z- TNavPF::Projecting Measurements 2024-11-06T03:36:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:36:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:36:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:36:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:36:16Z- TNavFilter::4 good beams. 2024-11-06T03:36:16Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:36:16Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:36:16Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:36:16Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:36:16Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:36:16Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:36:16Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:36:16Z- TerrainMapOctree: (vehN, vehE ) = (4061689.16, 594278.39). 2024-11-06T03:36:16Z- TerrainMapOctree: Min Distance = 67.14. 2024-11-06T03:36:16Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:36:16Z- Forcing Subcloud Comparison 2024-11-06T03:36:16Z- Weighting particles with subcloud comparison 2024-11-06T03:36:16Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:16Z- meanExpectedMeasDiff: 1.854853 delta_rms_squared: 0.913529 alpha: 0.978576 2024-11-06T03:36:16Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:16Z- meanExpectedMeasDiff: -1.462452 delta_rms_squared: 1.151256 alpha: 0.982924 2024-11-06T03:36:16Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:16Z- meanExpectedMeasDiff: -1.523884 delta_rms_squared: 1.304424 alpha: 0.984899 2024-11-06T03:36:16Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:16Z- meanExpectedMeasDiff: 0.584619 delta_rms_squared: 1.402926 alpha: 0.985944 2024-11-06T03:36:16Z- Subcloud NIS: 0.947792 2024-11-06T03:36:16Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:36:16Z- TNavPF:: sumWeights = 0.314636 2024-11-06T03:36:16Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:36:16Z- TNavPF::First Term of Map Covariance Matrix is 0.72 2024-11-06T03:36:16Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.04 2024-11-06T03:36:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864176.17 sec. 2024-11-06T03:36:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:36:16Z- variances([0], [2], and psi_berg) = 0.119207 0.077591 0.000000 2024-11-06T03:36:16Z- publishEstimatesDecoder() - at 1730864176.57 sec 2024-11-06T03:36:16Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:36:16Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061716.91 594260.52 54.13 2024-11-06T03:36:16Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061716.83 594261.12 54.46 lat/lon: 36.70 -121.94 2024-11-06T03:36:16Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.12 0.08 0.78 0.00 2024-11-06T03:36:16Z- handleDepth()- Depth_Keller msg: 1730864176.97 epoch sec; seqNo:6972; depth 49.69 2024-11-06T03:36:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864176.99 epoch sec; seqNo:6972; 4061688.59 north; 594278.03 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T03:36:17Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:36:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864176.57 2024-11-06T03:36:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:17Z- waiting for 1730864180.57, time is 1730864176.99 2024-11-06T03:36:17Z- AHRS_M2 msg: 1730864177.36 epoch sec; seqNo:6543 2024-11-06T03:36:17Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 4.12 psi 2024-11-06T03:36:17Z- handleDepth()- Depth_Keller msg: 1730864177.37 epoch sec; seqNo:6973; depth 49.70 2024-11-06T03:36:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864177.38 epoch sec; seqNo:6973; 4061688.59 north; 594278.03 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T03:36:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864176.57 2024-11-06T03:36:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:17Z- waiting for 1730864180.57, time is 1730864177.38 2024-11-06T03:36:17Z- AHRS_M2 msg: 1730864177.76 epoch sec; seqNo:6544 2024-11-06T03:36:17Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.12 psi 2024-11-06T03:36:17Z- handleDepth()- Depth_Keller msg: 1730864177.76 epoch sec; seqNo:6974; depth 49.72 2024-11-06T03:36:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864177.77 epoch sec; seqNo:6974; 4061688.21 north; 594278.03 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T03:36:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864176.57 2024-11-06T03:36:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:17Z- waiting for 1730864180.57, time is 1730864177.77 2024-11-06T03:36:18Z- AHRS_M2 msg: 1730864178.16 epoch sec; seqNo:6545 2024-11-06T03:36:18Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.12 psi 2024-11-06T03:36:18Z- handleDepth()- Depth_Keller msg: 1730864178.18 epoch sec; seqNo:6975; depth 49.71 2024-11-06T03:36:18Z- handleDVL() - DVL msg epochMillisec: 1730864178.20 2024-11-06T03:36:18Z- handleDvl() - RDI_Pathfinder msg: 1730864178.20 sec; seqNo:2403; #beams:4 2024-11-06T03:36:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 40.78 , 43.63 , 44.04 2024-11-06T03:36:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.04 , -0.02 2024-11-06T03:36:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864178.21 epoch sec; seqNo:6975; 4061688.19 north; 594276.81 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T03:36:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864176.57 2024-11-06T03:36:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:18Z- waiting for 1730864180.57, time is 1730864178.21 2024-11-06T03:36:18Z- AHRS_M2 msg: 1730864178.57 epoch sec; seqNo:6546 2024-11-06T03:36:18Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.12 psi 2024-11-06T03:36:18Z- handleDepth()- Depth_Keller msg: 1730864178.58 epoch sec; seqNo:6976; depth 49.75 2024-11-06T03:36:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864178.59 epoch sec; seqNo:6976; 4061687.82 north; 594276.82 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T03:36:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864176.57 2024-11-06T03:36:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:18Z- waiting for 1730864180.57, time is 1730864178.59 2024-11-06T03:36:18Z- AHRS_M2 msg: 1730864178.98 epoch sec; seqNo:6547 2024-11-06T03:36:18Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.12 psi 2024-11-06T03:36:18Z- handleDepth()- Depth_Keller msg: 1730864178.99 epoch sec; seqNo:6977; depth 49.74 2024-11-06T03:36:18Z- handleDVL() - DVL msg epochMillisec: 1730864178.99 2024-11-06T03:36:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:18Z- handleDvl() - RDI_Pathfinder msg: 1730864178.99 sec; seqNo:2404; #beams:0 2024-11-06T03:36:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.04 , -0.02 2024-11-06T03:36:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864179.00 epoch sec; seqNo:6977; 4061687.82 north; 594276.82 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T03:36:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864176.57 2024-11-06T03:36:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:19Z- waiting for 1730864180.57, time is 1730864179.00 2024-11-06T03:36:19Z- handleDepth()- Depth_Keller msg: 1730864179.40 epoch sec; seqNo:6978; depth 49.78 2024-11-06T03:36:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864179.41 epoch sec; seqNo:6978; 4061687.44 north; 594276.82 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T03:36:19Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:36:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864176.57 2024-11-06T03:36:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:19Z- waiting for 1730864180.57, time is 1730864179.41 2024-11-06T03:36:19Z- AHRS_M2 msg: 1730864179.78 epoch sec; seqNo:6548 2024-11-06T03:36:19Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.12 psi 2024-11-06T03:36:19Z- handleDepth()- Depth_Keller msg: 1730864179.79 epoch sec; seqNo:6979; depth 49.77 2024-11-06T03:36:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864179.81 epoch sec; seqNo:6979; 4061687.04 north; 594275.61 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T03:36:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864176.57 2024-11-06T03:36:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:19Z- waiting for 1730864180.57, time is 1730864179.81 2024-11-06T03:36:20Z- AHRS_M2 msg: 1730864180.18 epoch sec; seqNo:6549 2024-11-06T03:36:20Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 4.12 psi 2024-11-06T03:36:20Z- handleDepth()- Depth_Keller msg: 1730864180.19 epoch sec; seqNo:6980; depth 49.80 2024-11-06T03:36:20Z- handleDVL() - DVL msg epochMillisec: 1730864180.22 2024-11-06T03:36:20Z- handleDvl() - RDI_Pathfinder msg: 1730864180.22 sec; seqNo:2405; #beams:4 2024-11-06T03:36:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 40.57 , 43.63 , 44.04 2024-11-06T03:36:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , -0.00 2024-11-06T03:36:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864180.24 epoch sec; seqNo:6980; 4061687.04 north; 594275.61 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:36:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864176.57 2024-11-06T03:36:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:20Z- waiting for 1730864180.57, time is 1730864180.24 2024-11-06T03:36:20Z- AHRS_M2 msg: 1730864180.59 epoch sec; seqNo:6550 2024-11-06T03:36:20Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.12 psi 2024-11-06T03:36:20Z- handleDepth()- Depth_Keller msg: 1730864180.59 epoch sec; seqNo:6981; depth 49.80 2024-11-06T03:36:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864180.61 epoch sec; seqNo:6981; 4061686.67 north; 594275.61 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T03:36:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864176.57 2024-11-06T03:36:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:20Z- waiting for 1730864180.57, time is 1730864180.61 2024-11-06T03:36:20Z- LcmTrn::updateTrn() >>>> pose time 1730864180.61, meas time 1730864180.22 <<<< 2024-11-06T03:36:20Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864180.61 2024-11-06T03:36:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864180.22 sec, ping # 0; waiting for INS data more recent than 1730864176.57... 2024-11-06T03:36:20Z- TerrainNav::motionUpdate() - currEstimate = (4061687, 594275.61, 49.80) 2024-11-06T03:36:20Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:36:20Z- TerrainNav:: SubcloudNIS is 0.95, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:36:20Z- TNavPF::Projecting Measurements 2024-11-06T03:36:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:36:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:36:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:36:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:36:20Z- TNavFilter::4 good beams. 2024-11-06T03:36:20Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:36:20Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:36:20Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:36:20Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:36:20Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:36:20Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:36:20Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:36:20Z- TerrainMapOctree: (vehN, vehE ) = (4061686.89, 594275.84). 2024-11-06T03:36:20Z- TerrainMapOctree: Min Distance = 68.20. 2024-11-06T03:36:20Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:36:20Z- Forcing Subcloud Comparison 2024-11-06T03:36:20Z- Weighting particles with subcloud comparison 2024-11-06T03:36:20Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:20Z- meanExpectedMeasDiff: 1.224273 delta_rms_squared: 1.494745 alpha: 0.986796 2024-11-06T03:36:20Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:20Z- meanExpectedMeasDiff: -2.430057 delta_rms_squared: 1.320844 alpha: 0.985084 2024-11-06T03:36:20Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:20Z- meanExpectedMeasDiff: -1.073564 delta_rms_squared: 1.341266 alpha: 0.985308 2024-11-06T03:36:20Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:20Z- meanExpectedMeasDiff: 0.814307 delta_rms_squared: 1.470793 alpha: 0.986584 2024-11-06T03:36:20Z- Subcloud NIS: 0.910934 2024-11-06T03:36:20Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:36:20Z- TNavPF:: sumWeights = 0.261273 2024-11-06T03:36:20Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:36:20Z- TNavPF::First Term of Map Covariance Matrix is 0.48 2024-11-06T03:36:20Z- TNavPF::Calculated NIS Value : 0.14 numBeams normalized NIS: 0.04 2024-11-06T03:36:20Z- TerrainNav::Resampling particle filter... 2024-11-06T03:36:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864180.22 sec. 2024-11-06T03:36:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:36:20Z- variances([0], [2], and psi_berg) = 0.120839 0.080184 0.000000 2024-11-06T03:36:20Z- publishEstimatesDecoder() - at 1730864180.61 sec 2024-11-06T03:36:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:36:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061714.84 594258.57 53.04 2024-11-06T03:36:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061714.53 594258.69 54.79 lat/lon: 36.70 -121.94 2024-11-06T03:36:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.12 0.08 0.76 0.00 2024-11-06T03:36:21Z- AHRS_M2 msg: 1730864180.99 epoch sec; seqNo:6551 2024-11-06T03:36:21Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.12 psi 2024-11-06T03:36:21Z- handleDepth()- Depth_Keller msg: 1730864181.01 epoch sec; seqNo:6982; depth 49.82 2024-11-06T03:36:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864181.02 epoch sec; seqNo:6982; 4061686.65 north; 594274.39 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T03:36:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864180.61 2024-11-06T03:36:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:21Z- waiting for 1730864184.61, time is 1730864181.02 2024-11-06T03:36:21Z- AHRS_M2 msg: 1730864181.40 epoch sec; seqNo:6552 2024-11-06T03:36:21Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.12 psi 2024-11-06T03:36:21Z- handleDepth()- Depth_Keller msg: 1730864181.40 epoch sec; seqNo:6983; depth 49.84 2024-11-06T03:36:21Z- handleDVL() - DVL msg epochMillisec: 1730864181.40 2024-11-06T03:36:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:21Z- handleDvl() - RDI_Pathfinder msg: 1730864181.40 sec; seqNo:2406; #beams:0 2024-11-06T03:36:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , -0.00 2024-11-06T03:36:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864181.42 epoch sec; seqNo:6983; 4061686.27 north; 594274.39 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T03:36:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864180.61 2024-11-06T03:36:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:21Z- waiting for 1730864184.61, time is 1730864181.42 2024-11-06T03:36:21Z- AHRS_M2 msg: 1730864181.80 epoch sec; seqNo:6553 2024-11-06T03:36:21Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.12 psi 2024-11-06T03:36:21Z- handleDepth()- Depth_Keller msg: 1730864181.81 epoch sec; seqNo:6984; depth 49.85 2024-11-06T03:36:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864181.82 epoch sec; seqNo:6984; 4061686.27 north; 594274.39 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T03:36:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864180.61 2024-11-06T03:36:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:21Z- waiting for 1730864184.61, time is 1730864181.82 2024-11-06T03:36:22Z- AHRS_M2 msg: 1730864182.20 epoch sec; seqNo:6554 2024-11-06T03:36:22Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.12 psi 2024-11-06T03:36:22Z- handleDepth()- Depth_Keller msg: 1730864182.21 epoch sec; seqNo:6985; depth 49.88 2024-11-06T03:36:22Z- handleDVL() - DVL msg epochMillisec: 1730864182.23 2024-11-06T03:36:22Z- handleDvl() - RDI_Pathfinder msg: 1730864182.23 sec; seqNo:2407; #beams:4 2024-11-06T03:36:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 40.78 , 43.43 , 44.24 2024-11-06T03:36:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , 0.02 2024-11-06T03:36:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864182.24 epoch sec; seqNo:6985; 4061685.89 north; 594274.40 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T03:36:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864180.61 2024-11-06T03:36:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:22Z- waiting for 1730864184.61, time is 1730864182.24 2024-11-06T03:36:22Z- AHRS_M2 msg: 1730864182.61 epoch sec; seqNo:6555 2024-11-06T03:36:22Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.12 psi 2024-11-06T03:36:22Z- handleDepth()- Depth_Keller msg: 1730864182.62 epoch sec; seqNo:6986; depth 49.90 2024-11-06T03:36:22Z- handleDVL() - DVL msg epochMillisec: 1730864182.63 2024-11-06T03:36:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:22Z- handleDvl() - RDI_Pathfinder msg: 1730864182.63 sec; seqNo:2408; #beams:0 2024-11-06T03:36:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , 0.02 2024-11-06T03:36:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864182.65 epoch sec; seqNo:6986; 4061685.88 north; 594273.18 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:36:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864180.61 2024-11-06T03:36:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:22Z- waiting for 1730864184.61, time is 1730864182.65 2024-11-06T03:36:23Z- AHRS_M2 msg: 1730864183.01 epoch sec; seqNo:6556 2024-11-06T03:36:23Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.13 psi 2024-11-06T03:36:23Z- handleDepth()- Depth_Keller msg: 1730864183.02 epoch sec; seqNo:6987; depth 49.91 2024-11-06T03:36:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864183.03 epoch sec; seqNo:6987; 4061685.50 north; 594273.18 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T03:36:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864180.61 2024-11-06T03:36:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:23Z- waiting for 1730864184.61, time is 1730864183.03 2024-11-06T03:36:23Z- AHRS_M2 msg: 1730864183.41 epoch sec; seqNo:6557 2024-11-06T03:36:23Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.13 psi 2024-11-06T03:36:23Z- handleDepth()- Depth_Keller msg: 1730864183.42 epoch sec; seqNo:6988; depth 49.90 2024-11-06T03:36:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864183.43 epoch sec; seqNo:6988; 4061685.50 north; 594273.18 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T03:36:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864180.61 2024-11-06T03:36:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:23Z- waiting for 1730864184.61, time is 1730864183.43 2024-11-06T03:36:23Z- AHRS_M2 msg: 1730864183.82 epoch sec; seqNo:6558 2024-11-06T03:36:23Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.13 psi 2024-11-06T03:36:23Z- handleDepth()- Depth_Keller msg: 1730864183.82 epoch sec; seqNo:6989; depth 49.94 2024-11-06T03:36:23Z- handleDVL() - DVL msg epochMillisec: 1730864183.83 2024-11-06T03:36:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:23Z- handleDvl() - RDI_Pathfinder msg: 1730864183.83 sec; seqNo:2409; #beams:0 2024-11-06T03:36:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , 0.02 2024-11-06T03:36:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864183.84 epoch sec; seqNo:6989; 4061685.12 north; 594273.19 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T03:36:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864180.61 2024-11-06T03:36:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:23Z- waiting for 1730864184.61, time is 1730864183.84 2024-11-06T03:36:24Z- AHRS_M2 msg: 1730864184.22 epoch sec; seqNo:6559 2024-11-06T03:36:24Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 4.13 psi 2024-11-06T03:36:24Z- handleDepth()- Depth_Keller msg: 1730864184.24 epoch sec; seqNo:6990; depth 49.97 2024-11-06T03:36:24Z- handleDVL() - DVL msg epochMillisec: 1730864184.26 2024-11-06T03:36:24Z- handleDvl() - RDI_Pathfinder msg: 1730864184.26 sec; seqNo:2410; #beams:4 2024-11-06T03:36:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 40.78 , 43.63 , 44.24 2024-11-06T03:36:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , 0.01 2024-11-06T03:36:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864184.27 epoch sec; seqNo:6990; 4061684.73 north; 594271.97 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T03:36:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864180.61 2024-11-06T03:36:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:24Z- waiting for 1730864184.61, time is 1730864184.27 2024-11-06T03:36:24Z- AHRS_M2 msg: 1730864184.63 epoch sec; seqNo:6560 2024-11-06T03:36:24Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.13 psi 2024-11-06T03:36:24Z- handleDepth()- Depth_Keller msg: 1730864184.64 epoch sec; seqNo:6991; depth 49.98 2024-11-06T03:36:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864184.66 epoch sec; seqNo:6991; 4061684.73 north; 594271.97 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:36:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864180.61 2024-11-06T03:36:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:24Z- waiting for 1730864184.61, time is 1730864184.66 2024-11-06T03:36:24Z- LcmTrn::updateTrn() >>>> pose time 1730864184.66, meas time 1730864184.26 <<<< 2024-11-06T03:36:24Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864184.66 2024-11-06T03:36:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864184.26 sec, ping # 0; waiting for INS data more recent than 1730864180.61... 2024-11-06T03:36:24Z- TerrainNav::motionUpdate() - currEstimate = (4061685, 594271.97, 49.98) 2024-11-06T03:36:24Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:36:24Z- TerrainNav:: SubcloudNIS is 0.91, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:36:24Z- TNavPF::Projecting Measurements 2024-11-06T03:36:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:36:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:36:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:36:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:36:24Z- TNavFilter::4 good beams. 2024-11-06T03:36:24Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:36:24Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:36:24Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:36:24Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:36:24Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:36:24Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:36:24Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:36:24Z- TerrainMapOctree: (vehN, vehE ) = (4061684.92, 594272.33). 2024-11-06T03:36:24Z- TerrainMapOctree: Min Distance = 68.80. 2024-11-06T03:36:24Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:36:24Z- Forcing Subcloud Comparison 2024-11-06T03:36:24Z- Weighting particles with subcloud comparison 2024-11-06T03:36:24Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:24Z- meanExpectedMeasDiff: -0.279161 delta_rms_squared: 1.072861 alpha: 0.981699 2024-11-06T03:36:24Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:24Z- meanExpectedMeasDiff: -1.793502 delta_rms_squared: 1.143105 alpha: 0.982805 2024-11-06T03:36:24Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:24Z- meanExpectedMeasDiff: -0.539238 delta_rms_squared: 1.296411 alpha: 0.984807 2024-11-06T03:36:24Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:24Z- meanExpectedMeasDiff: 0.324721 delta_rms_squared: 0.953942 alpha: 0.979465 2024-11-06T03:36:24Z- Subcloud NIS: 0.891290 2024-11-06T03:36:24Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:36:24Z- TNavPF:: sumWeights = 0.520363 2024-11-06T03:36:24Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:36:24Z- TNavPF::First Term of Map Covariance Matrix is 0.26 2024-11-06T03:36:24Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.05 2024-11-06T03:36:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864184.26 sec. 2024-11-06T03:36:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:36:24Z- variances([0], [2], and psi_berg) = 0.122185 0.082156 0.000000 2024-11-06T03:36:24Z- publishEstimatesDecoder() - at 1730864184.66 sec 2024-11-06T03:36:24Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:36:24Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061713.24 594254.79 55.82 2024-11-06T03:36:24Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061712.60 594255.05 55.27 lat/lon: 36.70 -121.94 2024-11-06T03:36:24Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.12 0.08 0.78 0.00 2024-11-06T03:36:25Z- AHRS_M2 msg: 1730864185.03 epoch sec; seqNo:6561 2024-11-06T03:36:25Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.13 psi 2024-11-06T03:36:25Z- handleDepth()- Depth_Keller msg: 1730864185.04 epoch sec; seqNo:6992; depth 49.98 2024-11-06T03:36:25Z- handleDVL() - DVL msg epochMillisec: 1730864185.04 2024-11-06T03:36:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:25Z- handleDvl() - RDI_Pathfinder msg: 1730864185.04 sec; seqNo:2411; #beams:0 2024-11-06T03:36:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , 0.01 2024-11-06T03:36:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864185.05 epoch sec; seqNo:6992; 4061684.35 north; 594271.97 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T03:36:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864184.66 2024-11-06T03:36:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:25Z- waiting for 1730864188.66, time is 1730864185.05 2024-11-06T03:36:25Z- AHRS_M2 msg: 1730864185.44 epoch sec; seqNo:6562 2024-11-06T03:36:25Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.13 psi 2024-11-06T03:36:25Z- handleDepth()- Depth_Keller msg: 1730864185.44 epoch sec; seqNo:6993; depth 50.01 2024-11-06T03:36:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864185.45 epoch sec; seqNo:6993; 4061684.35 north; 594271.97 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T03:36:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864184.66 2024-11-06T03:36:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:25Z- waiting for 1730864188.66, time is 1730864185.45 2024-11-06T03:36:25Z- AHRS_M2 msg: 1730864185.84 epoch sec; seqNo:6563 2024-11-06T03:36:25Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.13 psi 2024-11-06T03:36:25Z- handleDepth()- Depth_Keller msg: 1730864185.84 epoch sec; seqNo:6994; depth 50.02 2024-11-06T03:36:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864185.86 epoch sec; seqNo:6994; 4061683.96 north; 594270.76 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T03:36:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864184.66 2024-11-06T03:36:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:26Z- waiting for 1730864188.66, time is 1730864185.86 2024-11-06T03:36:26Z- AHRS_M2 msg: 1730864186.24 epoch sec; seqNo:6564 2024-11-06T03:36:26Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.14 psi 2024-11-06T03:36:26Z- handleDepth()- Depth_Keller msg: 1730864186.25 epoch sec; seqNo:6995; depth 50.04 2024-11-06T03:36:26Z- handleDVL() - DVL msg epochMillisec: 1730864186.28 2024-11-06T03:36:26Z- handleDvl() - RDI_Pathfinder msg: 1730864186.28 sec; seqNo:2412; #beams:4 2024-11-06T03:36:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.51 , 40.57 , 43.43 , 44.24 2024-11-06T03:36:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , -0.01 2024-11-06T03:36:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864186.29 epoch sec; seqNo:6995; 4061683.96 north; 594270.76 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T03:36:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864184.66 2024-11-06T03:36:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:26Z- waiting for 1730864188.66, time is 1730864186.29 2024-11-06T03:36:26Z- AHRS_M2 msg: 1730864186.65 epoch sec; seqNo:6565 2024-11-06T03:36:26Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.14 psi 2024-11-06T03:36:26Z- handleDepth()- Depth_Keller msg: 1730864186.66 epoch sec; seqNo:6996; depth 50.06 2024-11-06T03:36:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864186.67 epoch sec; seqNo:6996; 4061683.58 north; 594270.76 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:36:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864184.66 2024-11-06T03:36:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:26Z- waiting for 1730864188.66, time is 1730864186.67 2024-11-06T03:36:27Z- AHRS_M2 msg: 1730864187.05 epoch sec; seqNo:6566 2024-11-06T03:36:27Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.14 psi 2024-11-06T03:36:27Z- handleDepth()- Depth_Keller msg: 1730864187.06 epoch sec; seqNo:6997; depth 50.06 2024-11-06T03:36:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864187.07 epoch sec; seqNo:6997; 4061683.58 north; 594270.76 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T03:36:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864184.66 2024-11-06T03:36:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:27Z- waiting for 1730864188.66, time is 1730864187.07 2024-11-06T03:36:27Z- AHRS_M2 msg: 1730864187.45 epoch sec; seqNo:6567 2024-11-06T03:36:27Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.14 psi 2024-11-06T03:36:27Z- handleDepth()- Depth_Keller msg: 1730864187.46 epoch sec; seqNo:6998; depth 50.08 2024-11-06T03:36:27Z- handleDVL() - DVL msg epochMillisec: 1730864187.46 2024-11-06T03:36:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:27Z- handleDvl() - RDI_Pathfinder msg: 1730864187.46 sec; seqNo:2413; #beams:0 2024-11-06T03:36:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , -0.01 2024-11-06T03:36:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864187.47 epoch sec; seqNo:6998; 4061683.19 north; 594269.55 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T03:36:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864184.66 2024-11-06T03:36:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:27Z- waiting for 1730864188.66, time is 1730864187.47 2024-11-06T03:36:27Z- AHRS_M2 msg: 1730864187.86 epoch sec; seqNo:6568 2024-11-06T03:36:27Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.14 psi 2024-11-06T03:36:27Z- handleDepth()- Depth_Keller msg: 1730864187.87 epoch sec; seqNo:6999; depth 50.10 2024-11-06T03:36:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864187.88 epoch sec; seqNo:6999; 4061683.19 north; 594269.55 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T03:36:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864184.66 2024-11-06T03:36:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:28Z- waiting for 1730864188.66, time is 1730864187.88 2024-11-06T03:36:28Z- AHRS_M2 msg: 1730864188.26 epoch sec; seqNo:6569 2024-11-06T03:36:28Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.14 psi 2024-11-06T03:36:28Z- handleDepth()- Depth_Keller msg: 1730864188.27 epoch sec; seqNo:7000; depth 50.12 2024-11-06T03:36:28Z- handleDVL() - DVL msg epochMillisec: 1730864188.29 2024-11-06T03:36:28Z- handleDvl() - RDI_Pathfinder msg: 1730864188.29 sec; seqNo:2414; #beams:4 2024-11-06T03:36:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.51 , 40.57 , 43.22 , 44.24 2024-11-06T03:36:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , -0.02 2024-11-06T03:36:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864188.30 epoch sec; seqNo:7000; 4061682.81 north; 594269.55 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T03:36:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864184.66 2024-11-06T03:36:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:28Z- waiting for 1730864188.66, time is 1730864188.30 2024-11-06T03:36:28Z- AHRS_M2 msg: 1730864188.67 epoch sec; seqNo:6570 2024-11-06T03:36:28Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.14 psi 2024-11-06T03:36:28Z- handleDepth()- Depth_Keller msg: 1730864188.67 epoch sec; seqNo:7001; depth 50.13 2024-11-06T03:36:28Z- handleDVL() - DVL msg epochMillisec: 1730864188.67 2024-11-06T03:36:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:28Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:28Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:28Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:28Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:28Z- handleDvl() - RDI_Pathfinder msg: 1730864188.67 sec; seqNo:2415; #beams:0 2024-11-06T03:36:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , -0.02 2024-11-06T03:36:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864188.69 epoch sec; seqNo:7001; 4061682.43 north; 594269.55 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T03:36:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864184.66 2024-11-06T03:36:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:28Z- waiting for 1730864188.66, time is 1730864188.69 2024-11-06T03:36:28Z- LcmTrn::updateTrn() >>>> pose time 1730864188.69, meas time 1730864188.67 <<<< 2024-11-06T03:36:28Z- LcmTrn::updateTrn() equating pose 1730864188.67 and meas times 1730864188.67 2024-11-06T03:36:28Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730864188.67 2024-11-06T03:36:28Z- TerrainNav::motionUpdate() - currEstimate = (4061682, 594269.55, 50.13) 2024-11-06T03:36:28Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:36:28Z- TerrainNav:: SubcloudNIS is 0.89, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:36:28Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:36:28Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:36:28Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:36:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:36:28Z- variances([0], [2], and psi_berg) = 0.123508 0.084029 0.000000 2024-11-06T03:36:28Z- publishEstimatesDecoder() - at 1730864188.67 sec 2024-11-06T03:36:28Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:36:28Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061711.00 594252.34 56.19 2024-11-06T03:36:28Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061710.30 594252.63 55.43 lat/lon: 36.70 -121.94 2024-11-06T03:36:28Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.12 0.08 1.02 0.00 2024-11-06T03:36:29Z- AHRS_M2 msg: 1730864189.07 epoch sec; seqNo:6571 2024-11-06T03:36:29Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.14 psi 2024-11-06T03:36:29Z- handleDepth()- Depth_Keller msg: 1730864189.08 epoch sec; seqNo:7002; depth 50.15 2024-11-06T03:36:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864189.09 epoch sec; seqNo:7002; 4061682.42 north; 594268.33 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T03:36:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864188.67 2024-11-06T03:36:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:29Z- waiting for 1730864192.67, time is 1730864189.09 2024-11-06T03:36:29Z- AHRS_M2 msg: 1730864189.48 epoch sec; seqNo:6572 2024-11-06T03:36:29Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.14 psi 2024-11-06T03:36:29Z- handleDepth()- Depth_Keller msg: 1730864189.49 epoch sec; seqNo:7003; depth 50.17 2024-11-06T03:36:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864189.49 epoch sec; seqNo:7003; 4061682.04 north; 594268.34 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T03:36:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864188.67 2024-11-06T03:36:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:29Z- waiting for 1730864192.67, time is 1730864189.49 2024-11-06T03:36:29Z- AHRS_M2 msg: 1730864189.88 epoch sec; seqNo:6573 2024-11-06T03:36:29Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.15 psi 2024-11-06T03:36:29Z- handleDepth()- Depth_Keller msg: 1730864189.89 epoch sec; seqNo:7004; depth 50.16 2024-11-06T03:36:29Z- handleDVL() - DVL msg epochMillisec: 1730864189.89 2024-11-06T03:36:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:29Z- handleDvl() - RDI_Pathfinder msg: 1730864189.89 sec; seqNo:2416; #beams:0 2024-11-06T03:36:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , -0.02 2024-11-06T03:36:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864189.90 epoch sec; seqNo:7004; 4061682.04 north; 594268.34 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T03:36:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864188.67 2024-11-06T03:36:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:30Z- waiting for 1730864192.67, time is 1730864189.90 2024-11-06T03:36:30Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:36:30Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:36:30Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:36:30Z- AHRS_M2 msg: 1730864190.27 epoch sec; seqNo:6574 2024-11-06T03:36:30Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.15 psi 2024-11-06T03:36:30Z- handleDepth()- Depth_Keller msg: 1730864190.31 epoch sec; seqNo:7005; depth 50.19 2024-11-06T03:36:30Z- handleDVL() - DVL msg epochMillisec: 1730864190.33 2024-11-06T03:36:30Z- handleDvl() - RDI_Pathfinder msg: 1730864190.33 sec; seqNo:2417; #beams:4 2024-11-06T03:36:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 40.57 , 43.02 , 44.04 2024-11-06T03:36:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , -0.03 2024-11-06T03:36:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864190.34 epoch sec; seqNo:7005; 4061681.65 north; 594267.12 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T03:36:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864188.67 2024-11-06T03:36:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:30Z- waiting for 1730864192.67, time is 1730864190.34 2024-11-06T03:36:30Z- AHRS_M2 msg: 1730864190.69 epoch sec; seqNo:6575 2024-11-06T03:36:30Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.15 psi 2024-11-06T03:36:30Z- handleDepth()- Depth_Keller msg: 1730864190.69 epoch sec; seqNo:7006; depth 50.20 2024-11-06T03:36:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864190.73 epoch sec; seqNo:7006; 4061681.65 north; 594267.12 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T03:36:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864188.67 2024-11-06T03:36:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:30Z- waiting for 1730864192.67, time is 1730864190.73 2024-11-06T03:36:31Z- AHRS_M2 msg: 1730864191.09 epoch sec; seqNo:6576 2024-11-06T03:36:31Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.15 psi 2024-11-06T03:36:31Z- handleDepth()- Depth_Keller msg: 1730864191.10 epoch sec; seqNo:7007; depth 50.22 2024-11-06T03:36:31Z- handleDVL() - DVL msg epochMillisec: 1730864191.10 2024-11-06T03:36:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:31Z- handleDvl() - RDI_Pathfinder msg: 1730864191.10 sec; seqNo:2418; #beams:0 2024-11-06T03:36:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , -0.03 2024-11-06T03:36:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864191.11 epoch sec; seqNo:7007; 4061681.27 north; 594267.13 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T03:36:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864188.67 2024-11-06T03:36:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:31Z- waiting for 1730864192.67, time is 1730864191.11 2024-11-06T03:36:31Z- AHRS_M2 msg: 1730864191.50 epoch sec; seqNo:6577 2024-11-06T03:36:31Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.15 psi 2024-11-06T03:36:31Z- handleDepth()- Depth_Keller msg: 1730864191.51 epoch sec; seqNo:7008; depth 50.24 2024-11-06T03:36:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864191.52 epoch sec; seqNo:7008; 4061681.27 north; 594267.13 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T03:36:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864188.67 2024-11-06T03:36:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:31Z- waiting for 1730864192.67, time is 1730864191.52 2024-11-06T03:36:31Z- handleDepth()- Depth_Keller msg: 1730864191.92 epoch sec; seqNo:7009; depth 50.26 2024-11-06T03:36:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864191.93 epoch sec; seqNo:7009; 4061680.88 north; 594265.91 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T03:36:32Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:36:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864188.67 2024-11-06T03:36:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:32Z- waiting for 1730864192.67, time is 1730864191.93 2024-11-06T03:36:32Z- handleDepth()- Depth_Keller msg: 1730864192.32 epoch sec; seqNo:7010; depth 50.27 2024-11-06T03:36:32Z- handleDVL() - DVL msg epochMillisec: 1730864192.35 2024-11-06T03:36:32Z- handleDvl() - RDI_Pathfinder msg: 1730864192.35 sec; seqNo:2419; #beams:4 2024-11-06T03:36:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.51 , 40.37 , 43.02 , 43.63 2024-11-06T03:36:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.05 , -0.02 2024-11-06T03:36:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864192.36 epoch sec; seqNo:7010; 4061680.88 north; 594265.91 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T03:36:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864188.67 2024-11-06T03:36:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:32Z- waiting for 1730864192.67, time is 1730864192.36 2024-11-06T03:36:32Z- AHRS_M2 msg: 1730864192.71 epoch sec; seqNo:6578 2024-11-06T03:36:32Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.15 psi 2024-11-06T03:36:32Z- handleDepth()- Depth_Keller msg: 1730864192.72 epoch sec; seqNo:7011; depth 50.29 2024-11-06T03:36:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864192.75 epoch sec; seqNo:7011; 4061680.50 north; 594265.91 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T03:36:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864188.67 2024-11-06T03:36:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:32Z- waiting for 1730864192.67, time is 1730864192.75 2024-11-06T03:36:32Z- LcmTrn::updateTrn() >>>> pose time 1730864192.75, meas time 1730864192.35 <<<< 2024-11-06T03:36:32Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864192.75 2024-11-06T03:36:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864192.35 sec, ping # 0; waiting for INS data more recent than 1730864188.67... 2024-11-06T03:36:32Z- TerrainNav::motionUpdate() - currEstimate = (4061680, 594265.91, 50.29) 2024-11-06T03:36:32Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:36:32Z- TerrainNav:: SubcloudNIS is 0.89, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:36:32Z- TNavPF::Projecting Measurements 2024-11-06T03:36:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:36:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:36:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:36:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:36:32Z- TNavFilter::4 good beams. 2024-11-06T03:36:32Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:36:32Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:36:32Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:36:32Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:36:32Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:36:32Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:36:32Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:36:32Z- TerrainMapOctree: (vehN, vehE ) = (4061680.69, 594266.27). 2024-11-06T03:36:32Z- TerrainMapOctree: Min Distance = 71.20. 2024-11-06T03:36:32Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:36:33Z- Forcing Subcloud Comparison 2024-11-06T03:36:33Z- Weighting particles with subcloud comparison 2024-11-06T03:36:33Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:33Z- meanExpectedMeasDiff: -0.354370 delta_rms_squared: 2.097418 alpha: 0.990555 2024-11-06T03:36:33Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:33Z- meanExpectedMeasDiff: -1.514953 delta_rms_squared: 1.447371 alpha: 0.986370 2024-11-06T03:36:33Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:33Z- meanExpectedMeasDiff: -0.541002 delta_rms_squared: 1.649753 alpha: 0.988022 2024-11-06T03:36:33Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:33Z- meanExpectedMeasDiff: -0.533142 delta_rms_squared: 1.072195 alpha: 0.981688 2024-11-06T03:36:33Z- Subcloud NIS: 0.863693 2024-11-06T03:36:33Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:36:33Z- TNavPF:: sumWeights = 0.533856 2024-11-06T03:36:33Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:36:33Z- TNavPF::First Term of Map Covariance Matrix is 0.49 2024-11-06T03:36:33Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T03:36:33Z- TerrainNav::Resampling particle filter... 2024-11-06T03:36:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864192.35 sec. 2024-11-06T03:36:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:36:33Z- variances([0], [2], and psi_berg) = 0.126738 0.086833 0.000000 2024-11-06T03:36:33Z- publishEstimatesDecoder() - at 1730864192.75 sec 2024-11-06T03:36:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:36:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061708.77 594249.11 55.34 2024-11-06T03:36:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061708.36 594248.99 55.98 lat/lon: 36.70 -121.94 2024-11-06T03:36:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.13 0.09 0.81 0.00 2024-11-06T03:36:33Z- AHRS_M2 msg: 1730864193.14 epoch sec; seqNo:6579 2024-11-06T03:36:33Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.15 psi 2024-11-06T03:36:33Z- handleDepth()- Depth_Keller msg: 1730864193.15 epoch sec; seqNo:7012; depth 50.33 2024-11-06T03:36:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864193.17 epoch sec; seqNo:7012; 4061680.48 north; 594264.69 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T03:36:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864192.75 2024-11-06T03:36:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:33Z- waiting for 1730864196.75, time is 1730864193.17 2024-11-06T03:36:33Z- AHRS_M2 msg: 1730864193.52 epoch sec; seqNo:6580 2024-11-06T03:36:33Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.15 psi 2024-11-06T03:36:33Z- handleDepth()- Depth_Keller msg: 1730864193.53 epoch sec; seqNo:7013; depth 50.34 2024-11-06T03:36:33Z- handleDVL() - DVL msg epochMillisec: 1730864193.53 2024-11-06T03:36:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:33Z- handleDvl() - RDI_Pathfinder msg: 1730864193.53 sec; seqNo:2420; #beams:0 2024-11-06T03:36:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.05 , -0.02 2024-11-06T03:36:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864193.54 epoch sec; seqNo:7013; 4061680.10 north; 594264.70 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T03:36:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864192.75 2024-11-06T03:36:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:33Z- waiting for 1730864196.75, time is 1730864193.54 2024-11-06T03:36:33Z- handleDepth()- Depth_Keller msg: 1730864193.96 epoch sec; seqNo:7014; depth 50.35 2024-11-06T03:36:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864194.01 epoch sec; seqNo:7014; 4061680.10 north; 594264.70 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T03:36:34Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:36:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864192.75 2024-11-06T03:36:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:34Z- waiting for 1730864196.75, time is 1730864194.01 2024-11-06T03:36:34Z- handleDepth()- Depth_Keller msg: 1730864194.34 epoch sec; seqNo:7015; depth 50.39 2024-11-06T03:36:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864194.35 epoch sec; seqNo:7015; 4061679.73 north; 594264.70 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T03:36:34Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:36:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864192.75 2024-11-06T03:36:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:34Z- waiting for 1730864196.75, time is 1730864194.35 2024-11-06T03:36:34Z- AHRS_M2 msg: 1730864194.73 epoch sec; seqNo:6581 2024-11-06T03:36:34Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 4.15 psi 2024-11-06T03:36:34Z- handleDepth()- Depth_Keller msg: 1730864194.73 epoch sec; seqNo:7016; depth 50.40 2024-11-06T03:36:34Z- handleDVL() - DVL msg epochMillisec: 1730864194.74 2024-11-06T03:36:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:34Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:34Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:34Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:34Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:34Z- handleDvl() - RDI_Pathfinder msg: 1730864194.74 sec; seqNo:2421; #beams:0 2024-11-06T03:36:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.05 , -0.02 2024-11-06T03:36:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864194.74 epoch sec; seqNo:7016; 4061679.71 north; 594263.48 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T03:36:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864192.75 2024-11-06T03:36:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:34Z- waiting for 1730864196.75, time is 1730864194.74 2024-11-06T03:36:35Z- AHRS_M2 msg: 1730864195.13 epoch sec; seqNo:6582 2024-11-06T03:36:35Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.15 psi 2024-11-06T03:36:35Z- handleDepth()- Depth_Keller msg: 1730864195.15 epoch sec; seqNo:7017; depth 50.41 2024-11-06T03:36:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864195.16 epoch sec; seqNo:7017; 4061679.33 north; 594263.49 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T03:36:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864192.75 2024-11-06T03:36:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:35Z- waiting for 1730864196.75, time is 1730864195.16 2024-11-06T03:36:35Z- AHRS_M2 msg: 1730864195.53 epoch sec; seqNo:6583 2024-11-06T03:36:35Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:36:35Z- handleDepth()- Depth_Keller msg: 1730864195.55 epoch sec; seqNo:7018; depth 50.44 2024-11-06T03:36:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864195.58 epoch sec; seqNo:7018; 4061679.33 north; 594263.49 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T03:36:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864192.75 2024-11-06T03:36:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:35Z- waiting for 1730864196.75, time is 1730864195.58 2024-11-06T03:36:35Z- AHRS_M2 msg: 1730864195.97 epoch sec; seqNo:6584 2024-11-06T03:36:36Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:36:36Z- handleDepth()- Depth_Keller msg: 1730864196.00 epoch sec; seqNo:7019; depth 50.46 2024-11-06T03:36:36Z- handleDVL() - DVL msg epochMillisec: 1730864196.01 2024-11-06T03:36:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:36Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:36Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:36Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:36Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:36Z- handleDvl() - RDI_Pathfinder msg: 1730864196.01 sec; seqNo:2422; #beams:0 2024-11-06T03:36:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.05 , -0.02 2024-11-06T03:36:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864196.05 epoch sec; seqNo:7019; 4061678.94 north; 594262.27 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T03:36:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864192.75 2024-11-06T03:36:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:36Z- waiting for 1730864196.75, time is 1730864196.05 2024-11-06T03:36:36Z- AHRS_M2 msg: 1730864196.36 epoch sec; seqNo:6585 2024-11-06T03:36:36Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 4.16 psi 2024-11-06T03:36:36Z- handleDepth()- Depth_Keller msg: 1730864196.36 epoch sec; seqNo:7020; depth 50.48 2024-11-06T03:36:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864196.38 epoch sec; seqNo:7020; 4061678.94 north; 594262.27 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T03:36:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864192.75 2024-11-06T03:36:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:36Z- waiting for 1730864196.75, time is 1730864196.38 2024-11-06T03:36:36Z- AHRS_M2 msg: 1730864196.75 epoch sec; seqNo:6586 2024-11-06T03:36:36Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.16 psi 2024-11-06T03:36:36Z- handleDepth()- Depth_Keller msg: 1730864196.76 epoch sec; seqNo:7021; depth 50.51 2024-11-06T03:36:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864196.77 epoch sec; seqNo:7021; 4061678.56 north; 594262.27 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:36:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864192.75 2024-11-06T03:36:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:36Z- waiting for 1730864196.75, time is 1730864196.77 2024-11-06T03:36:36Z- LcmTrn::updateTrn() >>>> pose time 1730864196.77, meas time 1730864196.01 <<<< 2024-11-06T03:36:36Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864196.77 2024-11-06T03:36:36Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:36:36Z- TerrainNav::motionUpdate() - currEstimate = (4061679, 594262.27, 50.51) 2024-11-06T03:36:36Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:36:36Z- TerrainNav:: SubcloudNIS is 0.86, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:36:36Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:36:36Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:36:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:36:36Z- variances([0], [2], and psi_berg) = 0.129834 0.089826 0.000000 2024-11-06T03:36:36Z- publishEstimatesDecoder() - at 1730864196.77 sec 2024-11-06T03:36:36Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:36:36Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061706.78 594245.47 55.19 2024-11-06T03:36:36Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061706.43 594245.35 56.20 lat/lon: 36.70 -121.94 2024-11-06T03:36:36Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.13 0.09 1.06 0.00 2024-11-06T03:36:37Z- AHRS_M2 msg: 1730864197.15 epoch sec; seqNo:6587 2024-11-06T03:36:37Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.17 psi 2024-11-06T03:36:37Z- handleDepth()- Depth_Keller msg: 1730864197.16 epoch sec; seqNo:7022; depth 50.52 2024-11-06T03:36:37Z- handleDVL() - DVL msg epochMillisec: 1730864197.16 2024-11-06T03:36:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:37Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:37Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:37Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:37Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:37Z- handleDvl() - RDI_Pathfinder msg: 1730864197.16 sec; seqNo:2423; #beams:0 2024-11-06T03:36:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.05 , -0.02 2024-11-06T03:36:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864197.19 epoch sec; seqNo:7022; 4061678.56 north; 594262.27 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:36:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864196.77 2024-11-06T03:36:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:37Z- waiting for 1730864200.77, time is 1730864197.19 2024-11-06T03:36:37Z- AHRS_M2 msg: 1730864197.55 epoch sec; seqNo:6588 2024-11-06T03:36:37Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.17 psi 2024-11-06T03:36:37Z- handleDepth()- Depth_Keller msg: 1730864197.56 epoch sec; seqNo:7023; depth 50.51 2024-11-06T03:36:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864197.57 epoch sec; seqNo:7023; 4061678.17 north; 594261.06 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:36:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864196.77 2024-11-06T03:36:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:37Z- waiting for 1730864200.77, time is 1730864197.57 2024-11-06T03:36:38Z- AHRS_M2 msg: 1730864198.00 epoch sec; seqNo:6589 2024-11-06T03:36:38Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.17 psi 2024-11-06T03:36:38Z- handleDepth()- Depth_Keller msg: 1730864198.01 epoch sec; seqNo:7024; depth 50.54 2024-11-06T03:36:38Z- handleDVL() - DVL msg epochMillisec: 1730864198.03 2024-11-06T03:36:38Z- handleDvl() - RDI_Pathfinder msg: 1730864198.03 sec; seqNo:2424; #beams:4 2024-11-06T03:36:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 40.57 , 43.43 , 43.43 2024-11-06T03:36:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , 0.01 2024-11-06T03:36:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864198.05 epoch sec; seqNo:7024; 4061678.17 north; 594261.06 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T03:36:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864196.77 2024-11-06T03:36:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:38Z- waiting for 1730864200.77, time is 1730864198.05 2024-11-06T03:36:38Z- AHRS_M2 msg: 1730864198.36 epoch sec; seqNo:6590 2024-11-06T03:36:38Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.17 psi 2024-11-06T03:36:38Z- handleDepth()- Depth_Keller msg: 1730864198.37 epoch sec; seqNo:7025; depth 50.56 2024-11-06T03:36:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864198.39 epoch sec; seqNo:7025; 4061677.79 north; 594261.06 east; 36.70 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T03:36:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864196.77 2024-11-06T03:36:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:38Z- waiting for 1730864200.77, time is 1730864198.39 2024-11-06T03:36:38Z- AHRS_M2 msg: 1730864198.77 epoch sec; seqNo:6591 2024-11-06T03:36:38Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.18 psi 2024-11-06T03:36:38Z- handleDepth()- Depth_Keller msg: 1730864198.78 epoch sec; seqNo:7026; depth 50.56 2024-11-06T03:36:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864198.79 epoch sec; seqNo:7026; 4061677.79 north; 594261.06 east; 36.70 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T03:36:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864196.77 2024-11-06T03:36:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:38Z- waiting for 1730864200.77, time is 1730864198.79 2024-11-06T03:36:39Z- AHRS_M2 msg: 1730864199.17 epoch sec; seqNo:6592 2024-11-06T03:36:39Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 4.18 psi 2024-11-06T03:36:39Z- handleDepth()- Depth_Keller msg: 1730864199.18 epoch sec; seqNo:7027; depth 50.57 2024-11-06T03:36:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864199.19 epoch sec; seqNo:7027; 4061677.40 north; 594259.85 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T03:36:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864196.77 2024-11-06T03:36:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:39Z- waiting for 1730864200.77, time is 1730864199.19 2024-11-06T03:36:39Z- AHRS_M2 msg: 1730864199.58 epoch sec; seqNo:6593 2024-11-06T03:36:39Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.18 psi 2024-11-06T03:36:39Z- handleDepth()- Depth_Keller msg: 1730864199.58 epoch sec; seqNo:7028; depth 50.59 2024-11-06T03:36:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864199.59 epoch sec; seqNo:7028; 4061677.40 north; 594259.85 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:36:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864196.77 2024-11-06T03:36:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:39Z- waiting for 1730864200.77, time is 1730864199.59 2024-11-06T03:36:40Z- AHRS_M2 msg: 1730864200.01 epoch sec; seqNo:6594 2024-11-06T03:36:40Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.19 psi 2024-11-06T03:36:40Z- handleDepth()- Depth_Keller msg: 1730864200.01 epoch sec; seqNo:7029; depth 50.59 2024-11-06T03:36:40Z- handleDVL() - DVL msg epochMillisec: 1730864200.03 2024-11-06T03:36:40Z- handleDvl() - RDI_Pathfinder msg: 1730864200.03 sec; seqNo:2425; #beams:4 2024-11-06T03:36:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 40.17 , 43.63 , 43.22 2024-11-06T03:36:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , -0.04 2024-11-06T03:36:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864200.06 epoch sec; seqNo:7029; 4061677.02 north; 594259.85 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:36:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864196.77 2024-11-06T03:36:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:40Z- waiting for 1730864200.77, time is 1730864200.06 2024-11-06T03:36:40Z- AHRS_M2 msg: 1730864200.38 epoch sec; seqNo:6595 2024-11-06T03:36:40Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.19 psi 2024-11-06T03:36:40Z- handleDepth()- Depth_Keller msg: 1730864200.39 epoch sec; seqNo:7030; depth 50.60 2024-11-06T03:36:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864200.42 epoch sec; seqNo:7030; 4061677.02 north; 594259.85 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:36:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864196.77 2024-11-06T03:36:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:40Z- waiting for 1730864200.77, time is 1730864200.42 2024-11-06T03:36:40Z- AHRS_M2 msg: 1730864200.79 epoch sec; seqNo:6596 2024-11-06T03:36:40Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.19 psi 2024-11-06T03:36:40Z- handleDepth()- Depth_Keller msg: 1730864200.79 epoch sec; seqNo:7031; depth 50.61 2024-11-06T03:36:40Z- handleDVL() - DVL msg epochMillisec: 1730864200.80 2024-11-06T03:36:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:40Z- handleDvl() - RDI_Pathfinder msg: 1730864200.80 sec; seqNo:2426; #beams:0 2024-11-06T03:36:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , -0.04 2024-11-06T03:36:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864200.80 epoch sec; seqNo:7031; 4061676.63 north; 594258.63 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:36:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864196.77 2024-11-06T03:36:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:40Z- waiting for 1730864200.77, time is 1730864200.80 2024-11-06T03:36:40Z- LcmTrn::updateTrn() >>>> pose time 1730864200.80, meas time 1730864200.80 <<<< 2024-11-06T03:36:40Z- LcmTrn::updateTrn() equating pose 1730864200.80 and meas times 1730864200.80 2024-11-06T03:36:40Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730864200.80 2024-11-06T03:36:40Z- TerrainNav::motionUpdate() - currEstimate = (4061677, 594258.63, 50.61) 2024-11-06T03:36:40Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:36:40Z- TerrainNav:: SubcloudNIS is 0.86, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:36:40Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:36:40Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:36:40Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:36:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:36:40Z- variances([0], [2], and psi_berg) = 0.133248 0.092846 0.000000 2024-11-06T03:36:40Z- publishEstimatesDecoder() - at 1730864200.80 sec 2024-11-06T03:36:40Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:36:40Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061704.80 594241.84 55.21 2024-11-06T03:36:40Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061704.49 594241.71 56.30 lat/lon: 36.70 -121.95 2024-11-06T03:36:40Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.13 0.09 1.32 0.00 2024-11-06T03:36:41Z- AHRS_M2 msg: 1730864201.20 epoch sec; seqNo:6597 2024-11-06T03:36:41Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.20 psi 2024-11-06T03:36:41Z- handleDepth()- Depth_Keller msg: 1730864201.21 epoch sec; seqNo:7032; depth 50.58 2024-11-06T03:36:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864201.22 epoch sec; seqNo:7032; 4061676.63 north; 594258.63 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T03:36:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864200.80 2024-11-06T03:36:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:41Z- waiting for 1730864204.80, time is 1730864201.22 2024-11-06T03:36:41Z- AHRS_M2 msg: 1730864201.60 epoch sec; seqNo:6598 2024-11-06T03:36:41Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 4.20 psi 2024-11-06T03:36:41Z- handleDepth()- Depth_Keller msg: 1730864201.60 epoch sec; seqNo:7033; depth 50.59 2024-11-06T03:36:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864201.63 epoch sec; seqNo:7033; 4061676.25 north; 594258.64 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:36:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864200.80 2024-11-06T03:36:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:41Z- waiting for 1730864204.80, time is 1730864201.63 2024-11-06T03:36:42Z- AHRS_M2 msg: 1730864202.02 epoch sec; seqNo:6599 2024-11-06T03:36:42Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 4.20 psi 2024-11-06T03:36:42Z- handleDepth()- Depth_Keller msg: 1730864202.02 epoch sec; seqNo:7034; depth 50.61 2024-11-06T03:36:42Z- handleDVL() - DVL msg epochMillisec: 1730864202.04 2024-11-06T03:36:42Z- handleDvl() - RDI_Pathfinder msg: 1730864202.04 sec; seqNo:2427; #beams:4 2024-11-06T03:36:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 40.17 , 43.63 , 42.82 2024-11-06T03:36:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , -0.05 2024-11-06T03:36:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864202.07 epoch sec; seqNo:7034; 4061676.24 north; 594257.42 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:36:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864200.80 2024-11-06T03:36:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:42Z- waiting for 1730864204.80, time is 1730864202.07 2024-11-06T03:36:42Z- AHRS_M2 msg: 1730864202.41 epoch sec; seqNo:6600 2024-11-06T03:36:42Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 4.20 psi 2024-11-06T03:36:42Z- handleDepth()- Depth_Keller msg: 1730864202.41 epoch sec; seqNo:7035; depth 50.62 2024-11-06T03:36:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864202.44 epoch sec; seqNo:7035; 4061675.86 north; 594257.42 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:36:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864200.80 2024-11-06T03:36:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:42Z- waiting for 1730864204.80, time is 1730864202.44 2024-11-06T03:36:42Z- AHRS_M2 msg: 1730864202.81 epoch sec; seqNo:6601 2024-11-06T03:36:42Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 4.20 psi 2024-11-06T03:36:42Z- handleDepth()- Depth_Keller msg: 1730864202.82 epoch sec; seqNo:7036; depth 50.61 2024-11-06T03:36:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864202.83 epoch sec; seqNo:7036; 4061675.86 north; 594257.42 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:36:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864200.80 2024-11-06T03:36:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:42Z- waiting for 1730864204.80, time is 1730864202.83 2024-11-06T03:36:43Z- AHRS_M2 msg: 1730864203.21 epoch sec; seqNo:6602 2024-11-06T03:36:43Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 4.20 psi 2024-11-06T03:36:43Z- handleDepth()- Depth_Keller msg: 1730864203.22 epoch sec; seqNo:7037; depth 50.61 2024-11-06T03:36:43Z- handleDVL() - DVL msg epochMillisec: 1730864203.22 2024-11-06T03:36:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:43Z- handleDvl() - RDI_Pathfinder msg: 1730864203.22 sec; seqNo:2428; #beams:0 2024-11-06T03:36:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.04 , -0.05 2024-11-06T03:36:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864203.23 epoch sec; seqNo:7037; 4061675.48 north; 594257.43 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:36:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864200.80 2024-11-06T03:36:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:43Z- waiting for 1730864204.80, time is 1730864203.23 2024-11-06T03:36:43Z- AHRS_M2 msg: 1730864203.62 epoch sec; seqNo:6603 2024-11-06T03:36:43Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 4.20 psi 2024-11-06T03:36:43Z- handleDepth()- Depth_Keller msg: 1730864203.63 epoch sec; seqNo:7038; depth 50.62 2024-11-06T03:36:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864203.64 epoch sec; seqNo:7038; 4061675.46 north; 594256.21 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:36:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864200.80 2024-11-06T03:36:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:43Z- waiting for 1730864204.80, time is 1730864203.64 2024-11-06T03:36:44Z- AHRS_M2 msg: 1730864204.02 epoch sec; seqNo:6604 2024-11-06T03:36:44Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 4.20 psi 2024-11-06T03:36:44Z- handleDepth()- Depth_Keller msg: 1730864204.03 epoch sec; seqNo:7039; depth 50.61 2024-11-06T03:36:44Z- handleDVL() - DVL msg epochMillisec: 1730864204.06 2024-11-06T03:36:44Z- handleDvl() - RDI_Pathfinder msg: 1730864204.06 sec; seqNo:2429; #beams:4 2024-11-06T03:36:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 40.37 , 43.63 , 42.41 2024-11-06T03:36:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.06 , -0.03 2024-11-06T03:36:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864204.07 epoch sec; seqNo:7039; 4061675.09 north; 594256.21 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:36:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864200.80 2024-11-06T03:36:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:44Z- waiting for 1730864204.80, time is 1730864204.07 2024-11-06T03:36:44Z- AHRS_M2 msg: 1730864204.42 epoch sec; seqNo:6605 2024-11-06T03:36:44Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 4.20 psi 2024-11-06T03:36:44Z- handleDepth()- Depth_Keller msg: 1730864204.43 epoch sec; seqNo:7040; depth 50.62 2024-11-06T03:36:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864204.46 epoch sec; seqNo:7040; 4061675.09 north; 594256.21 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:36:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864200.80 2024-11-06T03:36:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:44Z- waiting for 1730864204.80, time is 1730864204.46 2024-11-06T03:36:44Z- AHRS_M2 msg: 1730864204.83 epoch sec; seqNo:6606 2024-11-06T03:36:44Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 4.20 psi 2024-11-06T03:36:44Z- handleDepth()- Depth_Keller msg: 1730864204.84 epoch sec; seqNo:7041; depth 50.62 2024-11-06T03:36:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864204.85 epoch sec; seqNo:7041; 4061675.07 north; 594254.99 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:36:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864200.80 2024-11-06T03:36:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:44Z- waiting for 1730864204.80, time is 1730864204.85 2024-11-06T03:36:44Z- LcmTrn::updateTrn() >>>> pose time 1730864204.85, meas time 1730864204.06 <<<< 2024-11-06T03:36:44Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864204.85 2024-11-06T03:36:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864204.06 sec, ping # 0; waiting for INS data more recent than 1730864200.80... 2024-11-06T03:36:44Z- TerrainNav::motionUpdate() - currEstimate = (4061675, 594254.99, 50.62) 2024-11-06T03:36:44Z- TerrainNav:: Windowed NIS is -nan, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:36:44Z- TerrainNav:: SubcloudNIS is 0.86, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:36:45Z- TNavPF::Projecting Measurements 2024-11-06T03:36:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:36:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:36:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:36:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:36:45Z- TNavFilter::4 good beams. 2024-11-06T03:36:45Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:36:45Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:36:45Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:36:45Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:36:45Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:36:45Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:36:45Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:36:45Z- TerrainMapOctree: (vehN, vehE ) = (4061675.37, 594255.70). 2024-11-06T03:36:45Z- TerrainMapOctree: Min Distance = 74.84. 2024-11-06T03:36:45Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:36:45Z- Forcing Subcloud Comparison 2024-11-06T03:36:45Z- Weighting particles with subcloud comparison 2024-11-06T03:36:45Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:45Z- meanExpectedMeasDiff: -0.802627 delta_rms_squared: 2.575890 alpha: 0.992296 2024-11-06T03:36:45Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:45Z- meanExpectedMeasDiff: -0.718295 delta_rms_squared: 1.779508 alpha: 0.988886 2024-11-06T03:36:45Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:45Z- meanExpectedMeasDiff: -1.286098 delta_rms_squared: 2.128705 alpha: 0.990692 2024-11-06T03:36:45Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:45Z- meanExpectedMeasDiff: -1.224565 delta_rms_squared: 2.117443 alpha: 0.990643 2024-11-06T03:36:45Z- Subcloud NIS: 0.857605 2024-11-06T03:36:45Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:36:45Z- TNavPF:: sumWeights = 0.499238 2024-11-06T03:36:45Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:36:45Z- TNavPF::First Term of Map Covariance Matrix is 0.47 2024-11-06T03:36:45Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T03:36:45Z- TerrainNav::Resampling particle filter... 2024-11-06T03:36:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864204.06 sec. 2024-11-06T03:36:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:36:45Z- variances([0], [2], and psi_berg) = 0.136530 0.094780 0.000000 2024-11-06T03:36:45Z- publishEstimatesDecoder() - at 1730864204.85 sec 2024-11-06T03:36:45Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:36:45Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061703.29 594238.19 54.91 2024-11-06T03:36:45Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061702.94 594238.06 56.88 lat/lon: 36.70 -121.95 2024-11-06T03:36:45Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.09 0.82 0.00 2024-11-06T03:36:45Z- AHRS_M2 msg: 1730864205.25 epoch sec; seqNo:6607 2024-11-06T03:36:45Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 4.20 psi 2024-11-06T03:36:45Z- handleDepth()- Depth_Keller msg: 1730864205.26 epoch sec; seqNo:7042; depth 50.63 2024-11-06T03:36:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864205.29 epoch sec; seqNo:7042; 4061674.69 north; 594254.99 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:36:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864204.85 2024-11-06T03:36:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:45Z- waiting for 1730864208.85, time is 1730864205.29 2024-11-06T03:36:45Z- AHRS_M2 msg: 1730864205.64 epoch sec; seqNo:6608 2024-11-06T03:36:45Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 4.20 psi 2024-11-06T03:36:45Z- handleDepth()- Depth_Keller msg: 1730864205.64 epoch sec; seqNo:7043; depth 50.63 2024-11-06T03:36:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864205.66 epoch sec; seqNo:7043; 4061674.69 north; 594254.99 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:36:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864204.85 2024-11-06T03:36:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:45Z- waiting for 1730864208.85, time is 1730864205.66 2024-11-06T03:36:46Z- AHRS_M2 msg: 1730864206.04 epoch sec; seqNo:6609 2024-11-06T03:36:46Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 4.20 psi 2024-11-06T03:36:46Z- handleDepth()- Depth_Keller msg: 1730864206.05 epoch sec; seqNo:7044; depth 50.63 2024-11-06T03:36:46Z- handleDVL() - DVL msg epochMillisec: 1730864206.07 2024-11-06T03:36:46Z- handleDvl() - RDI_Pathfinder msg: 1730864206.07 sec; seqNo:2430; #beams:4 2024-11-06T03:36:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 40.37 , 44.03 , 42.40 2024-11-06T03:36:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.04 , 0.02 2024-11-06T03:36:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864206.09 epoch sec; seqNo:7044; 4061674.31 north; 594255.00 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:36:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864204.85 2024-11-06T03:36:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:46Z- waiting for 1730864208.85, time is 1730864206.09 2024-11-06T03:36:46Z- AHRS_M2 msg: 1730864206.44 epoch sec; seqNo:6610 2024-11-06T03:36:46Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 4.20 psi 2024-11-06T03:36:46Z- handleDepth()- Depth_Keller msg: 1730864206.45 epoch sec; seqNo:7045; depth 50.63 2024-11-06T03:36:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864206.46 epoch sec; seqNo:7045; 4061674.30 north; 594253.78 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:36:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864204.85 2024-11-06T03:36:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:46Z- waiting for 1730864208.85, time is 1730864206.46 2024-11-06T03:36:46Z- handleDepth()- Depth_Keller msg: 1730864206.87 epoch sec; seqNo:7046; depth 50.63 2024-11-06T03:36:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864206.88 epoch sec; seqNo:7046; 4061673.92 north; 594253.78 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:36:47Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:36:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864204.85 2024-11-06T03:36:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:47Z- waiting for 1730864208.85, time is 1730864206.88 2024-11-06T03:36:47Z- AHRS_M2 msg: 1730864207.25 epoch sec; seqNo:6611 2024-11-06T03:36:47Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 4.20 psi 2024-11-06T03:36:47Z- handleDepth()- Depth_Keller msg: 1730864207.26 epoch sec; seqNo:7047; depth 50.63 2024-11-06T03:36:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864207.27 epoch sec; seqNo:7047; 4061673.92 north; 594253.78 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:36:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864204.85 2024-11-06T03:36:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:47Z- waiting for 1730864208.85, time is 1730864207.27 2024-11-06T03:36:47Z- AHRS_M2 msg: 1730864207.65 epoch sec; seqNo:6612 2024-11-06T03:36:47Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.20 psi 2024-11-06T03:36:47Z- handleDepth()- Depth_Keller msg: 1730864207.67 epoch sec; seqNo:7048; depth 50.65 2024-11-06T03:36:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864207.69 epoch sec; seqNo:7048; 4061673.53 north; 594252.57 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:36:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864204.85 2024-11-06T03:36:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:47Z- waiting for 1730864208.85, time is 1730864207.69 2024-11-06T03:36:48Z- AHRS_M2 msg: 1730864208.06 epoch sec; seqNo:6613 2024-11-06T03:36:48Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.20 psi 2024-11-06T03:36:48Z- handleDepth()- Depth_Keller msg: 1730864208.06 epoch sec; seqNo:7049; depth 50.64 2024-11-06T03:36:48Z- handleDVL() - DVL msg epochMillisec: 1730864208.08 2024-11-06T03:36:48Z- handleDvl() - RDI_Pathfinder msg: 1730864208.08 sec; seqNo:2431; #beams:4 2024-11-06T03:36:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 40.57 , 44.24 , 42.41 2024-11-06T03:36:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.00 , 0.02 2024-11-06T03:36:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864208.11 epoch sec; seqNo:7049; 4061673.53 north; 594252.57 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:36:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864204.85 2024-11-06T03:36:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:48Z- waiting for 1730864208.85, time is 1730864208.11 2024-11-06T03:36:48Z- AHRS_M2 msg: 1730864208.47 epoch sec; seqNo:6614 2024-11-06T03:36:48Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.20 psi 2024-11-06T03:36:48Z- handleDepth()- Depth_Keller msg: 1730864208.47 epoch sec; seqNo:7050; depth 50.65 2024-11-06T03:36:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864208.49 epoch sec; seqNo:7050; 4061673.15 north; 594252.57 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:36:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864204.85 2024-11-06T03:36:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:48Z- waiting for 1730864208.85, time is 1730864208.49 2024-11-06T03:36:48Z- AHRS_M2 msg: 1730864208.87 epoch sec; seqNo:6615 2024-11-06T03:36:48Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.20 psi 2024-11-06T03:36:48Z- handleDepth()- Depth_Keller msg: 1730864208.88 epoch sec; seqNo:7051; depth 50.65 2024-11-06T03:36:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864208.90 epoch sec; seqNo:7051; 4061673.15 north; 594252.57 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:36:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864204.85 2024-11-06T03:36:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:49Z- waiting for 1730864208.85, time is 1730864208.90 2024-11-06T03:36:49Z- LcmTrn::updateTrn() >>>> pose time 1730864208.90, meas time 1730864208.08 <<<< 2024-11-06T03:36:49Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864208.90 2024-11-06T03:36:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864208.08 sec, ping # 0; waiting for INS data more recent than 1730864204.85... 2024-11-06T03:36:49Z- TerrainNav::motionUpdate() - currEstimate = (4061673, 594252.57, 50.65) 2024-11-06T03:36:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:36:49Z- TerrainNav:: SubcloudNIS is 0.86, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:36:49Z- TNavPF::Projecting Measurements 2024-11-06T03:36:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:36:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:36:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:36:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:36:49Z- TNavFilter::4 good beams. 2024-11-06T03:36:49Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:36:49Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:36:49Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:36:49Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:36:49Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:36:49Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:36:49Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:36:49Z- TerrainMapOctree: (vehN, vehE ) = (4061673.54, 594253.06). 2024-11-06T03:36:49Z- TerrainMapOctree: Min Distance = 76.52. 2024-11-06T03:36:49Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:36:49Z- Forcing Subcloud Comparison 2024-11-06T03:36:49Z- Weighting particles with subcloud comparison 2024-11-06T03:36:49Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:49Z- meanExpectedMeasDiff: 0.211978 delta_rms_squared: 1.697774 alpha: 0.988357 2024-11-06T03:36:49Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:49Z- meanExpectedMeasDiff: -0.158002 delta_rms_squared: 1.222182 alpha: 0.983899 2024-11-06T03:36:49Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:49Z- meanExpectedMeasDiff: -0.075628 delta_rms_squared: 1.448805 alpha: 0.986383 2024-11-06T03:36:49Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:49Z- meanExpectedMeasDiff: -0.212259 delta_rms_squared: 1.413519 alpha: 0.986048 2024-11-06T03:36:49Z- Subcloud NIS: 0.769196 2024-11-06T03:36:49Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:36:49Z- TNavPF:: sumWeights = 0.667203 2024-11-06T03:36:49Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:36:49Z- TNavPF::First Term of Map Covariance Matrix is 0.52 2024-11-06T03:36:49Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T03:36:49Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864208.08 sec. 2024-11-06T03:36:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:36:49Z- variances([0], [2], and psi_berg) = 0.138906 0.095285 0.000000 2024-11-06T03:36:49Z- publishEstimatesDecoder() - at 1730864208.90 sec 2024-11-06T03:36:49Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:36:49Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061700.73 594235.52 56.72 2024-11-06T03:36:49Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061701.02 594235.64 56.95 lat/lon: 36.70 -121.95 2024-11-06T03:36:49Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.10 0.74 0.00 2024-11-06T03:36:49Z- AHRS_M2 msg: 1730864209.28 epoch sec; seqNo:6616 2024-11-06T03:36:49Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 4.20 psi 2024-11-06T03:36:49Z- handleDepth()- Depth_Keller msg: 1730864209.29 epoch sec; seqNo:7052; depth 50.66 2024-11-06T03:36:49Z- handleDVL() - DVL msg epochMillisec: 1730864209.29 2024-11-06T03:36:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:49Z- handleDvl() - RDI_Pathfinder msg: 1730864209.29 sec; seqNo:2432; #beams:0 2024-11-06T03:36:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.00 , 0.02 2024-11-06T03:36:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864209.30 epoch sec; seqNo:7052; 4061672.76 north; 594251.35 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T03:36:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864208.90 2024-11-06T03:36:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:49Z- waiting for 1730864212.90, time is 1730864209.30 2024-11-06T03:36:49Z- handleDepth()- Depth_Keller msg: 1730864209.70 epoch sec; seqNo:7053; depth 50.64 2024-11-06T03:36:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864209.71 epoch sec; seqNo:7053; 4061672.76 north; 594251.35 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:36:49Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:36:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864208.90 2024-11-06T03:36:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:49Z- waiting for 1730864212.90, time is 1730864209.71 2024-11-06T03:36:50Z- handleDepth()- Depth_Keller msg: 1730864210.10 epoch sec; seqNo:7054; depth 50.65 2024-11-06T03:36:50Z- handleDVL() - DVL msg epochMillisec: 1730864210.12 2024-11-06T03:36:50Z- handleDvl() - RDI_Pathfinder msg: 1730864210.12 sec; seqNo:2433; #beams:4 2024-11-06T03:36:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 40.78 , 44.04 , 42.00 2024-11-06T03:36:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , -0.02 2024-11-06T03:36:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864210.13 epoch sec; seqNo:7054; 4061672.38 north; 594251.36 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:36:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864208.90 2024-11-06T03:36:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:50Z- waiting for 1730864212.90, time is 1730864210.13 2024-11-06T03:36:50Z- AHRS_M2 msg: 1730864210.48 epoch sec; seqNo:6617 2024-11-06T03:36:50Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.20 psi 2024-11-06T03:36:50Z- handleDepth()- Depth_Keller msg: 1730864210.49 epoch sec; seqNo:7055; depth 50.65 2024-11-06T03:36:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864210.50 epoch sec; seqNo:7055; 4061672.38 north; 594251.36 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:36:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864208.90 2024-11-06T03:36:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:50Z- waiting for 1730864212.90, time is 1730864210.50 2024-11-06T03:36:50Z- AHRS_M2 msg: 1730864210.89 epoch sec; seqNo:6618 2024-11-06T03:36:50Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:36:50Z- handleDepth()- Depth_Keller msg: 1730864210.89 epoch sec; seqNo:7056; depth 50.64 2024-11-06T03:36:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864210.90 epoch sec; seqNo:7056; 4061671.99 north; 594250.14 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:36:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864208.90 2024-11-06T03:36:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:51Z- waiting for 1730864212.90, time is 1730864210.90 2024-11-06T03:36:51Z- AHRS_M2 msg: 1730864211.29 epoch sec; seqNo:6619 2024-11-06T03:36:51Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:36:51Z- handleDepth()- Depth_Keller msg: 1730864211.30 epoch sec; seqNo:7057; depth 50.65 2024-11-06T03:36:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864211.31 epoch sec; seqNo:7057; 4061671.99 north; 594250.14 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T03:36:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864208.90 2024-11-06T03:36:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:51Z- waiting for 1730864212.90, time is 1730864211.31 2024-11-06T03:36:51Z- AHRS_M2 msg: 1730864211.69 epoch sec; seqNo:6620 2024-11-06T03:36:51Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:36:51Z- handleDepth()- Depth_Keller msg: 1730864211.70 epoch sec; seqNo:7058; depth 50.64 2024-11-06T03:36:51Z- handleDVL() - DVL msg epochMillisec: 1730864211.70 2024-11-06T03:36:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:51Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:51Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:51Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:51Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:51Z- handleDvl() - RDI_Pathfinder msg: 1730864211.70 sec; seqNo:2434; #beams:0 2024-11-06T03:36:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , -0.02 2024-11-06T03:36:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864211.71 epoch sec; seqNo:7058; 4061671.61 north; 594250.15 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T03:36:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864208.90 2024-11-06T03:36:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:51Z- waiting for 1730864212.90, time is 1730864211.71 2024-11-06T03:36:52Z- AHRS_M2 msg: 1730864212.10 epoch sec; seqNo:6621 2024-11-06T03:36:52Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:36:52Z- handleDepth()- Depth_Keller msg: 1730864212.10 epoch sec; seqNo:7059; depth 50.63 2024-11-06T03:36:52Z- handleDVL() - DVL msg epochMillisec: 1730864212.14 2024-11-06T03:36:52Z- handleDvl() - RDI_Pathfinder msg: 1730864212.14 sec; seqNo:2435; #beams:4 2024-11-06T03:36:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 40.78 , 44.04 , 42.21 2024-11-06T03:36:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , -0.04 2024-11-06T03:36:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864212.16 epoch sec; seqNo:7059; 4061671.60 north; 594248.93 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:36:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864208.90 2024-11-06T03:36:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:52Z- waiting for 1730864212.90, time is 1730864212.16 2024-11-06T03:36:52Z- handleDepth()- Depth_Keller msg: 1730864212.52 epoch sec; seqNo:7060; depth 50.63 2024-11-06T03:36:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864212.54 epoch sec; seqNo:7060; 4061671.22 north; 594248.93 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T03:36:52Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:36:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864208.90 2024-11-06T03:36:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:52Z- waiting for 1730864212.90, time is 1730864212.54 2024-11-06T03:36:52Z- AHRS_M2 msg: 1730864212.91 epoch sec; seqNo:6622 2024-11-06T03:36:52Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:36:52Z- handleDepth()- Depth_Keller msg: 1730864212.92 epoch sec; seqNo:7061; depth 50.62 2024-11-06T03:36:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864212.93 epoch sec; seqNo:7061; 4061671.22 north; 594248.93 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T03:36:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864208.90 2024-11-06T03:36:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:53Z- waiting for 1730864212.90, time is 1730864212.93 2024-11-06T03:36:53Z- LcmTrn::updateTrn() >>>> pose time 1730864212.93, meas time 1730864212.14 <<<< 2024-11-06T03:36:53Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864212.93 2024-11-06T03:36:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864212.14 sec, ping # 0; waiting for INS data more recent than 1730864208.90... 2024-11-06T03:36:53Z- TerrainNav::motionUpdate() - currEstimate = (4061671, 594248.93, 50.62) 2024-11-06T03:36:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:36:53Z- TerrainNav:: SubcloudNIS is 0.77, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:36:53Z- TNavPF::Projecting Measurements 2024-11-06T03:36:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:36:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:36:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:36:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:36:53Z- TNavFilter::4 good beams. 2024-11-06T03:36:53Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:36:53Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:36:53Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:36:53Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:36:53Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:36:53Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:36:53Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:36:53Z- TerrainMapOctree: (vehN, vehE ) = (4061671.59, 594249.64). 2024-11-06T03:36:53Z- TerrainMapOctree: Min Distance = 78.41. 2024-11-06T03:36:53Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:36:53Z- Forcing Subcloud Comparison 2024-11-06T03:36:53Z- Weighting particles with subcloud comparison 2024-11-06T03:36:53Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:53Z- meanExpectedMeasDiff: 0.178337 delta_rms_squared: 1.595013 alpha: 0.987616 2024-11-06T03:36:53Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:53Z- meanExpectedMeasDiff: 0.054380 delta_rms_squared: 1.145892 alpha: 0.982846 2024-11-06T03:36:53Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:53Z- meanExpectedMeasDiff: -0.557195 delta_rms_squared: 1.376312 alpha: 0.985677 2024-11-06T03:36:53Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:36:53Z- meanExpectedMeasDiff: -0.172256 delta_rms_squared: 1.310746 alpha: 0.984971 2024-11-06T03:36:53Z- Subcloud NIS: 0.707352 2024-11-06T03:36:53Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:36:53Z- TNavPF:: sumWeights = 0.660277 2024-11-06T03:36:53Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:36:53Z- TNavPF::First Term of Map Covariance Matrix is 0.51 2024-11-06T03:36:53Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T03:36:53Z- TerrainNav::Resampling particle filter... 2024-11-06T03:36:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864212.14 sec. 2024-11-06T03:36:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:36:53Z- variances([0], [2], and psi_berg) = 0.141314 0.098519 0.000000 2024-11-06T03:36:53Z- publishEstimatesDecoder() - at 1730864212.93 sec 2024-11-06T03:36:53Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:36:53Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061699.66 594232.08 55.72 2024-11-06T03:36:53Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061699.09 594232.00 56.96 lat/lon: 36.70 -121.95 2024-11-06T03:36:53Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.10 0.73 0.00 2024-11-06T03:36:53Z- AHRS_M2 msg: 1730864213.32 epoch sec; seqNo:6623 2024-11-06T03:36:53Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:36:53Z- handleDepth()- Depth_Keller msg: 1730864213.32 epoch sec; seqNo:7062; depth 50.60 2024-11-06T03:36:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864213.33 epoch sec; seqNo:7062; 4061670.84 north; 594248.93 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:36:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864212.93 2024-11-06T03:36:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:53Z- waiting for 1730864216.93, time is 1730864213.33 2024-11-06T03:36:53Z- AHRS_M2 msg: 1730864213.72 epoch sec; seqNo:6624 2024-11-06T03:36:53Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 4.21 psi 2024-11-06T03:36:53Z- handleDepth()- Depth_Keller msg: 1730864213.72 epoch sec; seqNo:7063; depth 50.61 2024-11-06T03:36:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864213.73 epoch sec; seqNo:7063; 4061670.82 north; 594247.71 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T03:36:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864212.93 2024-11-06T03:36:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:53Z- waiting for 1730864216.93, time is 1730864213.73 2024-11-06T03:36:54Z- AHRS_M2 msg: 1730864214.12 epoch sec; seqNo:6625 2024-11-06T03:36:54Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 4.21 psi 2024-11-06T03:36:54Z- handleDepth()- Depth_Keller msg: 1730864214.13 epoch sec; seqNo:7064; depth 50.59 2024-11-06T03:36:54Z- handleDVL() - DVL msg epochMillisec: 1730864214.17 2024-11-06T03:36:54Z- handleDvl() - RDI_Pathfinder msg: 1730864214.17 sec; seqNo:2436; #beams:4 2024-11-06T03:36:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 40.57 , 44.24 , 42.21 2024-11-06T03:36:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.05 , -0.03 2024-11-06T03:36:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864214.20 epoch sec; seqNo:7064; 4061670.45 north; 594247.72 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T03:36:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864212.93 2024-11-06T03:36:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:54Z- waiting for 1730864216.93, time is 1730864214.20 2024-11-06T03:36:54Z- AHRS_M2 msg: 1730864214.52 epoch sec; seqNo:6626 2024-11-06T03:36:54Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 4.21 psi 2024-11-06T03:36:54Z- handleDepth()- Depth_Keller msg: 1730864214.53 epoch sec; seqNo:7065; depth 50.60 2024-11-06T03:36:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864214.54 epoch sec; seqNo:7065; 4061670.45 north; 594247.72 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T03:36:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864212.93 2024-11-06T03:36:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:54Z- waiting for 1730864216.93, time is 1730864214.54 2024-11-06T03:36:54Z- handleDepth()- Depth_Keller msg: 1730864214.95 epoch sec; seqNo:7066; depth 50.57 2024-11-06T03:36:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864214.97 epoch sec; seqNo:7066; 4061670.45 north; 594247.72 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T03:36:55Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:36:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864212.93 2024-11-06T03:36:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:55Z- waiting for 1730864216.93, time is 1730864214.97 2024-11-06T03:36:55Z- AHRS_M2 msg: 1730864215.33 epoch sec; seqNo:6627 2024-11-06T03:36:55Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 4.21 psi 2024-11-06T03:36:55Z- handleDepth()- Depth_Keller msg: 1730864215.34 epoch sec; seqNo:7067; depth 50.58 2024-11-06T03:36:55Z- handleDVL() - DVL msg epochMillisec: 1730864215.34 2024-11-06T03:36:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:55Z- handleDvl() - RDI_Pathfinder msg: 1730864215.34 sec; seqNo:2437; #beams:0 2024-11-06T03:36:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.05 , -0.03 2024-11-06T03:36:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864215.35 epoch sec; seqNo:7067; 4061670.05 north; 594246.50 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T03:36:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864212.93 2024-11-06T03:36:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:55Z- waiting for 1730864216.93, time is 1730864215.35 2024-11-06T03:36:55Z- AHRS_M2 msg: 1730864215.73 epoch sec; seqNo:6628 2024-11-06T03:36:55Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.21 psi 2024-11-06T03:36:55Z- handleDepth()- Depth_Keller msg: 1730864215.74 epoch sec; seqNo:7068; depth 50.57 2024-11-06T03:36:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864215.76 epoch sec; seqNo:7068; 4061670.05 north; 594246.50 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T03:36:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864212.93 2024-11-06T03:36:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:55Z- waiting for 1730864216.93, time is 1730864215.76 2024-11-06T03:36:56Z- AHRS_M2 msg: 1730864216.14 epoch sec; seqNo:6629 2024-11-06T03:36:56Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 4.21 psi 2024-11-06T03:36:56Z- handleDepth()- Depth_Keller msg: 1730864216.15 epoch sec; seqNo:7069; depth 50.56 2024-11-06T03:36:56Z- handleDVL() - DVL msg epochMillisec: 1730864216.17 2024-11-06T03:36:56Z- handleDvl() - RDI_Pathfinder msg: 1730864216.17 sec; seqNo:2438; #beams:4 2024-11-06T03:36:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.69 , 40.78 , 44.45 , 42.21 2024-11-06T03:36:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.04 , -0.01 2024-11-06T03:36:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864216.19 epoch sec; seqNo:7069; 4061669.67 north; 594246.51 east; 36.70 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T03:36:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864212.93 2024-11-06T03:36:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:56Z- waiting for 1730864216.93, time is 1730864216.19 2024-11-06T03:36:56Z- AHRS_M2 msg: 1730864216.54 epoch sec; seqNo:6630 2024-11-06T03:36:56Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 4.21 psi 2024-11-06T03:36:56Z- handleDepth()- Depth_Keller msg: 1730864216.55 epoch sec; seqNo:7070; depth 50.55 2024-11-06T03:36:56Z- handleDVL() - DVL msg epochMillisec: 1730864216.55 2024-11-06T03:36:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:56Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:56Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:56Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:56Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:56Z- handleDvl() - RDI_Pathfinder msg: 1730864216.55 sec; seqNo:2439; #beams:0 2024-11-06T03:36:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.04 , -0.01 2024-11-06T03:36:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864216.57 epoch sec; seqNo:7070; 4061669.66 north; 594245.29 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:36:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864212.93 2024-11-06T03:36:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:56Z- waiting for 1730864216.93, time is 1730864216.57 2024-11-06T03:36:56Z- AHRS_M2 msg: 1730864216.95 epoch sec; seqNo:6631 2024-11-06T03:36:56Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 4.21 psi 2024-11-06T03:36:56Z- handleDepth()- Depth_Keller msg: 1730864216.96 epoch sec; seqNo:7071; depth 50.55 2024-11-06T03:36:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864216.97 epoch sec; seqNo:7071; 4061669.28 north; 594245.29 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T03:36:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864212.93 2024-11-06T03:36:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:57Z- waiting for 1730864216.93, time is 1730864216.97 2024-11-06T03:36:57Z- LcmTrn::updateTrn() >>>> pose time 1730864216.97, meas time 1730864216.55 <<<< 2024-11-06T03:36:57Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864216.97 2024-11-06T03:36:57Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T03:36:57Z- TerrainNav::motionUpdate() - currEstimate = (4061669, 594245.29, 50.55) 2024-11-06T03:36:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:36:57Z- TerrainNav:: SubcloudNIS is 0.71, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:36:57Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T03:36:57Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T03:36:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:36:57Z- variances([0], [2], and psi_berg) = 0.143642 0.100980 0.000000 2024-11-06T03:36:57Z- publishEstimatesDecoder() - at 1730864216.97 sec 2024-11-06T03:36:57Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:36:57Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061697.73 594228.43 56.12 2024-11-06T03:36:57Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061697.15 594228.36 56.90 lat/lon: 36.70 -121.95 2024-11-06T03:36:57Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.10 0.97 0.00 2024-11-06T03:36:57Z- AHRS_M2 msg: 1730864217.35 epoch sec; seqNo:6632 2024-11-06T03:36:57Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 4.21 psi 2024-11-06T03:36:57Z- handleDepth()- Depth_Keller msg: 1730864217.36 epoch sec; seqNo:7072; depth 50.53 2024-11-06T03:36:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864217.37 epoch sec; seqNo:7072; 4061669.28 north; 594245.29 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:36:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864216.97 2024-11-06T03:36:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:57Z- waiting for 1730864220.97, time is 1730864217.37 2024-11-06T03:36:57Z- AHRS_M2 msg: 1730864217.76 epoch sec; seqNo:6633 2024-11-06T03:36:57Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 4.21 psi 2024-11-06T03:36:57Z- handleDepth()- Depth_Keller msg: 1730864217.77 epoch sec; seqNo:7073; depth 50.53 2024-11-06T03:36:57Z- handleDVL() - DVL msg epochMillisec: 1730864217.77 2024-11-06T03:36:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:36:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:36:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:36:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:36:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:36:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:36:57Z- handleDvl() - RDI_Pathfinder msg: 1730864217.77 sec; seqNo:2440; #beams:0 2024-11-06T03:36:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:36:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.04 , -0.01 2024-11-06T03:36:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864217.79 epoch sec; seqNo:7073; 4061668.90 north; 594245.29 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T03:36:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864216.97 2024-11-06T03:36:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:57Z- waiting for 1730864220.97, time is 1730864217.79 2024-11-06T03:36:58Z- AHRS_M2 msg: 1730864218.16 epoch sec; seqNo:6634 2024-11-06T03:36:58Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 4.20 psi 2024-11-06T03:36:58Z- handleDepth()- Depth_Keller msg: 1730864218.16 epoch sec; seqNo:7074; depth 50.52 2024-11-06T03:36:58Z- handleDVL() - DVL msg epochMillisec: 1730864218.18 2024-11-06T03:36:58Z- handleDvl() - RDI_Pathfinder msg: 1730864218.18 sec; seqNo:2441; #beams:4 2024-11-06T03:36:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 40.77 , 44.44 , 42.20 2024-11-06T03:36:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , 0.03 , -0.00 2024-11-06T03:36:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864218.20 epoch sec; seqNo:7074; 4061668.89 north; 594244.07 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T03:36:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:36:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864216.97 2024-11-06T03:36:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:58Z- waiting for 1730864220.97, time is 1730864218.20 2024-11-06T03:36:58Z- AHRS_M2 msg: 1730864218.56 epoch sec; seqNo:6635 2024-11-06T03:36:58Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 4.20 psi 2024-11-06T03:36:58Z- handleDepth()- Depth_Keller msg: 1730864218.57 epoch sec; seqNo:7075; depth 50.50 2024-11-06T03:36:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864218.59 epoch sec; seqNo:7075; 4061668.89 north; 594244.07 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T03:36:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864216.97 2024-11-06T03:36:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:58Z- waiting for 1730864220.97, time is 1730864218.59 2024-11-06T03:36:58Z- AHRS_M2 msg: 1730864218.97 epoch sec; seqNo:6636 2024-11-06T03:36:58Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 4.20 psi 2024-11-06T03:36:58Z- handleDepth()- Depth_Keller msg: 1730864218.97 epoch sec; seqNo:7076; depth 50.51 2024-11-06T03:36:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864218.98 epoch sec; seqNo:7076; 4061668.51 north; 594244.08 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T03:36:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864216.97 2024-11-06T03:36:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:59Z- waiting for 1730864220.97, time is 1730864218.98 2024-11-06T03:36:59Z- AHRS_M2 msg: 1730864219.37 epoch sec; seqNo:6637 2024-11-06T03:36:59Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.20 psi 2024-11-06T03:36:59Z- handleDepth()- Depth_Keller msg: 1730864219.38 epoch sec; seqNo:7077; depth 50.50 2024-11-06T03:36:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864219.38 epoch sec; seqNo:7077; 4061668.50 north; 594242.86 east; 36.70 lat; -121.95 lon; 50.50 depth; utmZone: 10 2024-11-06T03:36:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864216.97 2024-11-06T03:36:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:59Z- waiting for 1730864220.97, time is 1730864219.38 2024-11-06T03:36:59Z- AHRS_M2 msg: 1730864219.78 epoch sec; seqNo:6638 2024-11-06T03:36:59Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.20 psi 2024-11-06T03:36:59Z- handleDepth()- Depth_Keller msg: 1730864219.78 epoch sec; seqNo:7078; depth 50.48 2024-11-06T03:36:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864219.79 epoch sec; seqNo:7078; 4061668.12 north; 594242.86 east; 36.70 lat; -121.95 lon; 50.48 depth; utmZone: 10 2024-11-06T03:36:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:36:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864216.97 2024-11-06T03:36:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:36:59Z- waiting for 1730864220.97, time is 1730864219.79 2024-11-06T03:37:00Z- AHRS_M2 msg: 1730864220.18 epoch sec; seqNo:6639 2024-11-06T03:37:00Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.20 psi 2024-11-06T03:37:00Z- handleDepth()- Depth_Keller msg: 1730864220.19 epoch sec; seqNo:7079; depth 50.47 2024-11-06T03:37:00Z- handleDVL() - DVL msg epochMillisec: 1730864220.21 2024-11-06T03:37:00Z- handleDvl() - RDI_Pathfinder msg: 1730864220.21 sec; seqNo:2442; #beams:4 2024-11-06T03:37:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 40.98 , 44.65 , 42.40 2024-11-06T03:37:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , -0.00 2024-11-06T03:37:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864220.22 epoch sec; seqNo:7079; 4061668.12 north; 594242.86 east; 36.70 lat; -121.95 lon; 50.47 depth; utmZone: 10 2024-11-06T03:37:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:37:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864216.97 2024-11-06T03:37:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:00Z- waiting for 1730864220.97, time is 1730864220.22 2024-11-06T03:37:00Z- AHRS_M2 msg: 1730864220.58 epoch sec; seqNo:6640 2024-11-06T03:37:00Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.20 psi 2024-11-06T03:37:00Z- handleDepth()- Depth_Keller msg: 1730864220.59 epoch sec; seqNo:7080; depth 50.47 2024-11-06T03:37:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864220.62 epoch sec; seqNo:7080; 4061667.74 north; 594242.87 east; 36.70 lat; -121.95 lon; 50.47 depth; utmZone: 10 2024-11-06T03:37:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864216.97 2024-11-06T03:37:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:00Z- waiting for 1730864220.97, time is 1730864220.62 2024-11-06T03:37:00Z- AHRS_M2 msg: 1730864220.99 epoch sec; seqNo:6641 2024-11-06T03:37:00Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.19 psi 2024-11-06T03:37:00Z- handleDepth()- Depth_Keller msg: 1730864220.99 epoch sec; seqNo:7081; depth 50.48 2024-11-06T03:37:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864221.00 epoch sec; seqNo:7081; 4061667.73 north; 594241.64 east; 36.70 lat; -121.95 lon; 50.48 depth; utmZone: 10 2024-11-06T03:37:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864216.97 2024-11-06T03:37:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:01Z- waiting for 1730864220.97, time is 1730864221.00 2024-11-06T03:37:01Z- LcmTrn::updateTrn() >>>> pose time 1730864221.00, meas time 1730864220.21 <<<< 2024-11-06T03:37:01Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864221.00 2024-11-06T03:37:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864220.21 sec, ping # 0; waiting for INS data more recent than 1730864216.97... 2024-11-06T03:37:01Z- TerrainNav::motionUpdate() - currEstimate = (4061668, 594241.64, 50.48) 2024-11-06T03:37:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:37:01Z- TerrainNav:: SubcloudNIS is 0.71, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:37:01Z- TNavPF::Projecting Measurements 2024-11-06T03:37:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:37:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:37:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:37:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:37:01Z- TNavFilter::4 good beams. 2024-11-06T03:37:01Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:37:01Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:37:01Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:37:01Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:37:01Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:37:01Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:37:01Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:37:01Z- TerrainMapOctree: (vehN, vehE ) = (4061668.03, 594242.36). 2024-11-06T03:37:01Z- TerrainMapOctree: Min Distance = 82.32. 2024-11-06T03:37:01Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:37:01Z- Forcing Subcloud Comparison 2024-11-06T03:37:01Z- Weighting particles with subcloud comparison 2024-11-06T03:37:01Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:37:01Z- meanExpectedMeasDiff: 0.520419 delta_rms_squared: 1.896016 alpha: 0.989562 2024-11-06T03:37:01Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:37:01Z- meanExpectedMeasDiff: -0.064858 delta_rms_squared: 1.405384 alpha: 0.985969 2024-11-06T03:37:01Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:37:01Z- meanExpectedMeasDiff: -1.562957 delta_rms_squared: 1.731535 alpha: 0.988581 2024-11-06T03:37:01Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:37:01Z- meanExpectedMeasDiff: 0.352448 delta_rms_squared: 1.557628 alpha: 0.987323 2024-11-06T03:37:01Z- Subcloud NIS: 0.637810 2024-11-06T03:37:01Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:37:01Z- TNavPF:: sumWeights = 0.495276 2024-11-06T03:37:01Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:37:01Z- TNavPF::First Term of Map Covariance Matrix is 0.45 2024-11-06T03:37:01Z- TNavPF::Calculated NIS Value : 0.19 numBeams normalized NIS: 0.05 2024-11-06T03:37:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864220.21 sec. 2024-11-06T03:37:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:37:01Z- variances([0], [2], and psi_berg) = 0.146434 0.104196 0.000000 2024-11-06T03:37:01Z- publishEstimatesDecoder() - at 1730864221.00 sec 2024-11-06T03:37:01Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:37:01Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061695.32 594225.58 57.26 2024-11-06T03:37:01Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061695.60 594224.72 56.91 lat/lon: 36.70 -121.95 2024-11-06T03:37:01Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.10 0.74 0.00 2024-11-06T03:37:01Z- AHRS_M2 msg: 1730864221.39 epoch sec; seqNo:6642 2024-11-06T03:37:01Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 4.19 psi 2024-11-06T03:37:01Z- handleDepth()- Depth_Keller msg: 1730864221.40 epoch sec; seqNo:7082; depth 50.47 2024-11-06T03:37:01Z- handleDVL() - DVL msg epochMillisec: 1730864221.41 2024-11-06T03:37:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:37:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:37:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:37:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:37:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:37:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:37:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:37:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:37:01Z- handleDvl() - RDI_Pathfinder msg: 1730864221.41 sec; seqNo:2443; #beams:0 2024-11-06T03:37:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:37:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , -0.00 2024-11-06T03:37:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864221.42 epoch sec; seqNo:7082; 4061667.35 north; 594241.65 east; 36.70 lat; -121.95 lon; 50.47 depth; utmZone: 10 2024-11-06T03:37:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:37:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864221.00 2024-11-06T03:37:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:01Z- waiting for 1730864225.00, time is 1730864221.42 2024-11-06T03:37:01Z- AHRS_M2 msg: 1730864221.79 epoch sec; seqNo:6643 2024-11-06T03:37:01Z- AHRS_M2 msg: 0.12 phi; 0.04 theta; 4.19 psi 2024-11-06T03:37:01Z- handleDepth()- Depth_Keller msg: 1730864221.81 epoch sec; seqNo:7083; depth 50.46 2024-11-06T03:37:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864221.82 epoch sec; seqNo:7083; 4061667.35 north; 594241.65 east; 36.70 lat; -121.95 lon; 50.46 depth; utmZone: 10 2024-11-06T03:37:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864221.00 2024-11-06T03:37:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:01Z- waiting for 1730864225.00, time is 1730864221.82 2024-11-06T03:37:02Z- handleDepth()- Depth_Keller msg: 1730864222.22 epoch sec; seqNo:7084; depth 50.45 2024-11-06T03:37:02Z- handleDVL() - DVL msg epochMillisec: 1730864222.24 2024-11-06T03:37:02Z- handleDvl() - RDI_Pathfinder msg: 1730864222.24 sec; seqNo:2444; #beams:4 2024-11-06T03:37:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.49 , 41.19 , 44.85 , 42.21 2024-11-06T03:37:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , -0.00 2024-11-06T03:37:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864222.25 epoch sec; seqNo:7084; 4061666.96 north; 594240.43 east; 36.70 lat; -121.95 lon; 50.45 depth; utmZone: 10 2024-11-06T03:37:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864221.00 2024-11-06T03:37:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:02Z- waiting for 1730864225.00, time is 1730864222.25 2024-11-06T03:37:02Z- AHRS_M2 msg: 1730864222.60 epoch sec; seqNo:6644 2024-11-06T03:37:02Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.19 psi 2024-11-06T03:37:02Z- handleDepth()- Depth_Keller msg: 1730864222.62 epoch sec; seqNo:7085; depth 50.43 2024-11-06T03:37:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864222.64 epoch sec; seqNo:7085; 4061666.96 north; 594240.43 east; 36.70 lat; -121.95 lon; 50.43 depth; utmZone: 10 2024-11-06T03:37:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864221.00 2024-11-06T03:37:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:02Z- waiting for 1730864225.00, time is 1730864222.64 2024-11-06T03:37:03Z- AHRS_M2 msg: 1730864223.01 epoch sec; seqNo:6645 2024-11-06T03:37:03Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.19 psi 2024-11-06T03:37:03Z- handleDepth()- Depth_Keller msg: 1730864223.02 epoch sec; seqNo:7086; depth 50.42 2024-11-06T03:37:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864223.04 epoch sec; seqNo:7086; 4061666.58 north; 594240.44 east; 36.70 lat; -121.95 lon; 50.42 depth; utmZone: 10 2024-11-06T03:37:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864221.00 2024-11-06T03:37:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:03Z- waiting for 1730864225.00, time is 1730864223.04 2024-11-06T03:37:03Z- AHRS_M2 msg: 1730864223.42 epoch sec; seqNo:6646 2024-11-06T03:37:03Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.18 psi 2024-11-06T03:37:03Z- handleDepth()- Depth_Keller msg: 1730864223.42 epoch sec; seqNo:7087; depth 50.42 2024-11-06T03:37:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864223.43 epoch sec; seqNo:7087; 4061666.58 north; 594240.44 east; 36.70 lat; -121.95 lon; 50.42 depth; utmZone: 10 2024-11-06T03:37:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864221.00 2024-11-06T03:37:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:03Z- waiting for 1730864225.00, time is 1730864223.43 2024-11-06T03:37:03Z- AHRS_M2 msg: 1730864223.82 epoch sec; seqNo:6647 2024-11-06T03:37:03Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.18 psi 2024-11-06T03:37:03Z- handleDepth()- Depth_Keller msg: 1730864223.82 epoch sec; seqNo:7088; depth 50.40 2024-11-06T03:37:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864223.83 epoch sec; seqNo:7088; 4061666.18 north; 594239.22 east; 36.70 lat; -121.95 lon; 50.40 depth; utmZone: 10 2024-11-06T03:37:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864221.00 2024-11-06T03:37:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:03Z- waiting for 1730864225.00, time is 1730864223.83 2024-11-06T03:37:04Z- AHRS_M2 msg: 1730864224.22 epoch sec; seqNo:6648 2024-11-06T03:37:04Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.18 psi 2024-11-06T03:37:04Z- handleDepth()- Depth_Keller msg: 1730864224.24 epoch sec; seqNo:7089; depth 50.40 2024-11-06T03:37:04Z- handleDVL() - DVL msg epochMillisec: 1730864224.26 2024-11-06T03:37:04Z- handleDvl() - RDI_Pathfinder msg: 1730864224.26 sec; seqNo:2445; #beams:4 2024-11-06T03:37:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 40.98 , 45.05 , 42.61 2024-11-06T03:37:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , -0.01 2024-11-06T03:37:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864224.27 epoch sec; seqNo:7089; 4061666.18 north; 594239.22 east; 36.70 lat; -121.95 lon; 50.40 depth; utmZone: 10 2024-11-06T03:37:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:37:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864221.00 2024-11-06T03:37:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:04Z- waiting for 1730864225.00, time is 1730864224.27 2024-11-06T03:37:04Z- AHRS_M2 msg: 1730864224.63 epoch sec; seqNo:6649 2024-11-06T03:37:04Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.18 psi 2024-11-06T03:37:04Z- handleDepth()- Depth_Keller msg: 1730864224.63 epoch sec; seqNo:7090; depth 50.37 2024-11-06T03:37:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864224.65 epoch sec; seqNo:7090; 4061665.81 north; 594239.23 east; 36.70 lat; -121.95 lon; 50.37 depth; utmZone: 10 2024-11-06T03:37:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864221.00 2024-11-06T03:37:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:04Z- waiting for 1730864225.00, time is 1730864224.65 2024-11-06T03:37:05Z- AHRS_M2 msg: 1730864225.03 epoch sec; seqNo:6650 2024-11-06T03:37:05Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.18 psi 2024-11-06T03:37:05Z- handleDepth()- Depth_Keller msg: 1730864225.05 epoch sec; seqNo:7091; depth 50.36 2024-11-06T03:37:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864225.06 epoch sec; seqNo:7091; 4061665.81 north; 594239.23 east; 36.70 lat; -121.95 lon; 50.36 depth; utmZone: 10 2024-11-06T03:37:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864221.00 2024-11-06T03:37:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:05Z- waiting for 1730864225.00, time is 1730864225.06 2024-11-06T03:37:05Z- LcmTrn::updateTrn() >>>> pose time 1730864225.06, meas time 1730864224.26 <<<< 2024-11-06T03:37:05Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864225.06 2024-11-06T03:37:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864224.26 sec, ping # 0; waiting for INS data more recent than 1730864221.00... 2024-11-06T03:37:05Z- TerrainNav::motionUpdate() - currEstimate = (4061666, 594239.23, 50.36) 2024-11-06T03:37:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:37:05Z- TerrainNav:: SubcloudNIS is 0.64, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:37:05Z- TNavPF::Projecting Measurements 2024-11-06T03:37:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:37:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:37:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:37:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:37:05Z- TNavFilter::4 good beams. 2024-11-06T03:37:05Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:37:05Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:37:05Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:37:05Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:37:05Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:37:05Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:37:05Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:37:05Z- TerrainMapOctree: (vehN, vehE ) = (4061666.18, 594239.70). 2024-11-06T03:37:05Z- TerrainMapOctree: Min Distance = 84.45. 2024-11-06T03:37:05Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:37:05Z- Forcing Subcloud Comparison 2024-11-06T03:37:05Z- Weighting particles with subcloud comparison 2024-11-06T03:37:05Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:37:05Z- meanExpectedMeasDiff: 0.334160 delta_rms_squared: 1.587972 alpha: 0.987562 2024-11-06T03:37:05Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:37:05Z- meanExpectedMeasDiff: -0.014643 delta_rms_squared: 1.164304 alpha: 0.983112 2024-11-06T03:37:05Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:37:05Z- meanExpectedMeasDiff: -1.379774 delta_rms_squared: 1.420257 alpha: 0.986114 2024-11-06T03:37:05Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:37:05Z- meanExpectedMeasDiff: 0.648548 delta_rms_squared: 1.288073 alpha: 0.984710 2024-11-06T03:37:05Z- Subcloud NIS: 0.559806 2024-11-06T03:37:05Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:37:05Z- TNavPF:: sumWeights = 0.537731 2024-11-06T03:37:05Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:37:05Z- TNavPF::First Term of Map Covariance Matrix is 0.43 2024-11-06T03:37:05Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T03:37:05Z- TerrainNav::Resampling particle filter... 2024-11-06T03:37:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864224.26 sec. 2024-11-06T03:37:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:37:05Z- variances([0], [2], and psi_berg) = 0.147725 0.105491 0.000000 2024-11-06T03:37:05Z- publishEstimatesDecoder() - at 1730864225.06 sec 2024-11-06T03:37:05Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:37:05Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061694.20 594222.46 56.34 2024-11-06T03:37:05Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061693.67 594222.30 56.83 lat/lon: 36.70 -121.95 2024-11-06T03:37:05Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.11 0.73 0.00 2024-11-06T03:37:05Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:37:05Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:37:05Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:37:05Z- AHRS_M2 msg: 1730864225.42 epoch sec; seqNo:6651 2024-11-06T03:37:05Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 4.18 psi 2024-11-06T03:37:05Z- handleDepth()- Depth_Keller msg: 1730864225.46 epoch sec; seqNo:7092; depth 50.35 2024-11-06T03:37:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864225.47 epoch sec; seqNo:7092; 4061665.41 north; 594238.01 east; 36.70 lat; -121.95 lon; 50.35 depth; utmZone: 10 2024-11-06T03:37:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864225.06 2024-11-06T03:37:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:05Z- waiting for 1730864229.06, time is 1730864225.47 2024-11-06T03:37:05Z- AHRS_M2 msg: 1730864225.84 epoch sec; seqNo:6652 2024-11-06T03:37:05Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 4.18 psi 2024-11-06T03:37:05Z- handleDepth()- Depth_Keller msg: 1730864225.84 epoch sec; seqNo:7093; depth 50.34 2024-11-06T03:37:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864225.86 epoch sec; seqNo:7093; 4061665.41 north; 594238.01 east; 36.70 lat; -121.95 lon; 50.34 depth; utmZone: 10 2024-11-06T03:37:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864225.06 2024-11-06T03:37:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:06Z- waiting for 1730864229.06, time is 1730864225.86 2024-11-06T03:37:06Z- AHRS_M2 msg: 1730864226.24 epoch sec; seqNo:6653 2024-11-06T03:37:06Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 4.18 psi 2024-11-06T03:37:06Z- handleDepth()- Depth_Keller msg: 1730864226.25 epoch sec; seqNo:7094; depth 50.31 2024-11-06T03:37:06Z- handleDVL() - DVL msg epochMillisec: 1730864226.26 2024-11-06T03:37:06Z- handleDvl() - RDI_Pathfinder msg: 1730864226.26 sec; seqNo:2446; #beams:4 2024-11-06T03:37:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 40.98 , 45.47 , 42.82 2024-11-06T03:37:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , -0.02 2024-11-06T03:37:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864226.30 epoch sec; seqNo:7094; 4061665.03 north; 594238.01 east; 36.70 lat; -121.95 lon; 50.31 depth; utmZone: 10 2024-11-06T03:37:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:37:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864225.06 2024-11-06T03:37:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:06Z- waiting for 1730864229.06, time is 1730864226.30 2024-11-06T03:37:06Z- AHRS_M2 msg: 1730864226.65 epoch sec; seqNo:6654 2024-11-06T03:37:06Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 4.18 psi 2024-11-06T03:37:06Z- handleDepth()- Depth_Keller msg: 1730864226.65 epoch sec; seqNo:7095; depth 50.29 2024-11-06T03:37:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864226.67 epoch sec; seqNo:7095; 4061665.02 north; 594236.79 east; 36.70 lat; -121.95 lon; 50.29 depth; utmZone: 10 2024-11-06T03:37:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864225.06 2024-11-06T03:37:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:06Z- waiting for 1730864229.06, time is 1730864226.67 2024-11-06T03:37:07Z- AHRS_M2 msg: 1730864227.05 epoch sec; seqNo:6655 2024-11-06T03:37:07Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 4.18 psi 2024-11-06T03:37:07Z- handleDepth()- Depth_Keller msg: 1730864227.05 epoch sec; seqNo:7096; depth 50.28 2024-11-06T03:37:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864227.08 epoch sec; seqNo:7096; 4061664.64 north; 594236.80 east; 36.70 lat; -121.95 lon; 50.28 depth; utmZone: 10 2024-11-06T03:37:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864225.06 2024-11-06T03:37:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:07Z- waiting for 1730864229.06, time is 1730864227.08 2024-11-06T03:37:07Z- AHRS_M2 msg: 1730864227.45 epoch sec; seqNo:6656 2024-11-06T03:37:07Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 4.18 psi 2024-11-06T03:37:07Z- handleDepth()- Depth_Keller msg: 1730864227.46 epoch sec; seqNo:7097; depth 50.26 2024-11-06T03:37:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864227.47 epoch sec; seqNo:7097; 4061664.64 north; 594236.80 east; 36.70 lat; -121.95 lon; 50.26 depth; utmZone: 10 2024-11-06T03:37:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864225.06 2024-11-06T03:37:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:07Z- waiting for 1730864229.06, time is 1730864227.47 2024-11-06T03:37:07Z- AHRS_M2 msg: 1730864227.86 epoch sec; seqNo:6657 2024-11-06T03:37:07Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 4.18 psi 2024-11-06T03:37:07Z- handleDepth()- Depth_Keller msg: 1730864227.86 epoch sec; seqNo:7098; depth 50.24 2024-11-06T03:37:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864227.87 epoch sec; seqNo:7098; 4061664.26 north; 594236.80 east; 36.70 lat; -121.95 lon; 50.24 depth; utmZone: 10 2024-11-06T03:37:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864225.06 2024-11-06T03:37:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:08Z- waiting for 1730864229.06, time is 1730864227.87 2024-11-06T03:37:08Z- AHRS_M2 msg: 1730864228.26 epoch sec; seqNo:6658 2024-11-06T03:37:08Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 4.18 psi 2024-11-06T03:37:08Z- handleDepth()- Depth_Keller msg: 1730864228.27 epoch sec; seqNo:7099; depth 50.23 2024-11-06T03:37:08Z- handleDVL() - DVL msg epochMillisec: 1730864228.30 2024-11-06T03:37:08Z- handleDvl() - RDI_Pathfinder msg: 1730864228.30 sec; seqNo:2447; #beams:4 2024-11-06T03:37:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 41.19 , 45.26 , 42.61 2024-11-06T03:37:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.03 , -0.03 2024-11-06T03:37:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864228.32 epoch sec; seqNo:7099; 4061664.25 north; 594235.58 east; 36.70 lat; -121.95 lon; 50.23 depth; utmZone: 10 2024-11-06T03:37:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:37:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864225.06 2024-11-06T03:37:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:08Z- waiting for 1730864229.06, time is 1730864228.32 2024-11-06T03:37:08Z- AHRS_M2 msg: 1730864228.66 epoch sec; seqNo:6659 2024-11-06T03:37:08Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 4.18 psi 2024-11-06T03:37:08Z- handleDepth()- Depth_Keller msg: 1730864228.67 epoch sec; seqNo:7100; depth 50.20 2024-11-06T03:37:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864228.69 epoch sec; seqNo:7100; 4061663.87 north; 594235.59 east; 36.70 lat; -121.95 lon; 50.20 depth; utmZone: 10 2024-11-06T03:37:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864225.06 2024-11-06T03:37:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:08Z- waiting for 1730864229.06, time is 1730864228.69 2024-11-06T03:37:09Z- AHRS_M2 msg: 1730864229.07 epoch sec; seqNo:6660 2024-11-06T03:37:09Z- AHRS_M2 msg: 0.13 phi; 0.05 theta; 4.18 psi 2024-11-06T03:37:09Z- handleDepth()- Depth_Keller msg: 1730864229.07 epoch sec; seqNo:7101; depth 50.20 2024-11-06T03:37:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864229.08 epoch sec; seqNo:7101; 4061663.87 north; 594235.59 east; 36.70 lat; -121.95 lon; 50.20 depth; utmZone: 10 2024-11-06T03:37:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864225.06 2024-11-06T03:37:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:09Z- waiting for 1730864229.06, time is 1730864229.08 2024-11-06T03:37:09Z- LcmTrn::updateTrn() >>>> pose time 1730864229.08, meas time 1730864228.30 <<<< 2024-11-06T03:37:09Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864229.08 2024-11-06T03:37:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864228.30 sec, ping # 0; waiting for INS data more recent than 1730864225.06... 2024-11-06T03:37:09Z- TerrainNav::motionUpdate() - currEstimate = (4061664, 594235.59, 50.20) 2024-11-06T03:37:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:37:09Z- TerrainNav:: SubcloudNIS is 0.56, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:37:09Z- TNavPF::Projecting Measurements 2024-11-06T03:37:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:37:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:37:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:37:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:37:09Z- TNavFilter::4 good beams. 2024-11-06T03:37:09Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:37:09Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:37:09Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:37:09Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:37:09Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:37:09Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:37:09Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:37:09Z- TerrainMapOctree: (vehN, vehE ) = (4061664.24, 594236.29). 2024-11-06T03:37:09Z- TerrainMapOctree: Min Distance = 86.84. 2024-11-06T03:37:09Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:37:09Z- Forcing Subcloud Comparison 2024-11-06T03:37:09Z- Weighting particles with subcloud comparison 2024-11-06T03:37:09Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:37:09Z- meanExpectedMeasDiff: 0.344074 delta_rms_squared: 1.532359 alpha: 0.987116 2024-11-06T03:37:09Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:37:09Z- meanExpectedMeasDiff: 0.069627 delta_rms_squared: 1.120786 alpha: 0.982468 2024-11-06T03:37:09Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:37:09Z- meanExpectedMeasDiff: -1.547295 delta_rms_squared: 1.381489 alpha: 0.985729 2024-11-06T03:37:09Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:37:09Z- meanExpectedMeasDiff: 0.683032 delta_rms_squared: 1.230446 alpha: 0.984006 2024-11-06T03:37:09Z- Subcloud NIS: 0.499433 2024-11-06T03:37:09Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:37:09Z- TNavPF:: sumWeights = 0.517576 2024-11-06T03:37:09Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:37:09Z- TNavPF::First Term of Map Covariance Matrix is 0.41 2024-11-06T03:37:09Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.06 2024-11-06T03:37:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864228.30 sec. 2024-11-06T03:37:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:37:09Z- variances([0], [2], and psi_berg) = 0.151046 0.109447 0.000000 2024-11-06T03:37:09Z- publishEstimatesDecoder() - at 1730864229.08 sec 2024-11-06T03:37:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:37:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061691.83 594218.21 57.42 2024-11-06T03:37:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061691.74 594218.66 56.73 lat/lon: 36.70 -121.95 2024-11-06T03:37:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.11 0.70 0.00 2024-11-06T03:37:09Z- AHRS_M2 msg: 1730864229.48 epoch sec; seqNo:6661 2024-11-06T03:37:09Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 4.18 psi 2024-11-06T03:37:09Z- handleDepth()- Depth_Keller msg: 1730864229.50 epoch sec; seqNo:7102; depth 50.17 2024-11-06T03:37:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864229.53 epoch sec; seqNo:7102; 4061663.49 north; 594235.59 east; 36.70 lat; -121.95 lon; 50.17 depth; utmZone: 10 2024-11-06T03:37:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864229.08 2024-11-06T03:37:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:09Z- waiting for 1730864233.08, time is 1730864229.53 2024-11-06T03:37:09Z- AHRS_M2 msg: 1730864229.87 epoch sec; seqNo:6662 2024-11-06T03:37:09Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 4.18 psi 2024-11-06T03:37:09Z- handleDepth()- Depth_Keller msg: 1730864229.88 epoch sec; seqNo:7103; depth 50.15 2024-11-06T03:37:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864229.90 epoch sec; seqNo:7103; 4061663.48 north; 594234.37 east; 36.70 lat; -121.95 lon; 50.15 depth; utmZone: 10 2024-11-06T03:37:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864229.08 2024-11-06T03:37:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:10Z- waiting for 1730864233.08, time is 1730864229.90 2024-11-06T03:37:10Z- AHRS_M2 msg: 1730864230.29 epoch sec; seqNo:6663 2024-11-06T03:37:10Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 4.17 psi 2024-11-06T03:37:10Z- handleDepth()- Depth_Keller msg: 1730864230.30 epoch sec; seqNo:7104; depth 50.14 2024-11-06T03:37:10Z- handleDVL() - DVL msg epochMillisec: 1730864230.33 2024-11-06T03:37:10Z- handleDvl() - RDI_Pathfinder msg: 1730864230.33 sec; seqNo:2448; #beams:4 2024-11-06T03:37:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 41.19 , 45.26 , 42.82 2024-11-06T03:37:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.07 , -0.02 2024-11-06T03:37:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864230.34 epoch sec; seqNo:7104; 4061663.10 north; 594234.37 east; 36.70 lat; -121.95 lon; 50.14 depth; utmZone: 10 2024-11-06T03:37:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:37:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864229.08 2024-11-06T03:37:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:10Z- waiting for 1730864233.08, time is 1730864230.34 2024-11-06T03:37:10Z- AHRS_M2 msg: 1730864230.68 epoch sec; seqNo:6664 2024-11-06T03:37:10Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 4.17 psi 2024-11-06T03:37:10Z- handleDepth()- Depth_Keller msg: 1730864230.69 epoch sec; seqNo:7105; depth 50.11 2024-11-06T03:37:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864230.71 epoch sec; seqNo:7105; 4061663.10 north; 594234.37 east; 36.70 lat; -121.95 lon; 50.11 depth; utmZone: 10 2024-11-06T03:37:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864229.08 2024-11-06T03:37:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:10Z- waiting for 1730864233.08, time is 1730864230.71 2024-11-06T03:37:11Z- AHRS_M2 msg: 1730864231.09 epoch sec; seqNo:6665 2024-11-06T03:37:11Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 4.17 psi 2024-11-06T03:37:11Z- handleDepth()- Depth_Keller msg: 1730864231.09 epoch sec; seqNo:7106; depth 50.10 2024-11-06T03:37:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864231.10 epoch sec; seqNo:7106; 4061663.09 north; 594233.15 east; 36.70 lat; -121.95 lon; 50.10 depth; utmZone: 10 2024-11-06T03:37:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864229.08 2024-11-06T03:37:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:11Z- waiting for 1730864233.08, time is 1730864231.10 2024-11-06T03:37:11Z- AHRS_M2 msg: 1730864231.50 epoch sec; seqNo:6666 2024-11-06T03:37:11Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 4.17 psi 2024-11-06T03:37:11Z- handleDepth()- Depth_Keller msg: 1730864231.50 epoch sec; seqNo:7107; depth 50.08 2024-11-06T03:37:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864231.51 epoch sec; seqNo:7107; 4061662.71 north; 594233.16 east; 36.70 lat; -121.95 lon; 50.08 depth; utmZone: 10 2024-11-06T03:37:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864229.08 2024-11-06T03:37:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:11Z- waiting for 1730864233.08, time is 1730864231.51 2024-11-06T03:37:11Z- handleDepth()- Depth_Keller msg: 1730864231.92 epoch sec; seqNo:7108; depth 50.07 2024-11-06T03:37:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864231.93 epoch sec; seqNo:7108; 4061662.71 north; 594233.16 east; 36.70 lat; -121.95 lon; 50.07 depth; utmZone: 10 2024-11-06T03:37:12Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:37:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864229.08 2024-11-06T03:37:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:12Z- waiting for 1730864233.08, time is 1730864231.93 2024-11-06T03:37:12Z- AHRS_M2 msg: 1730864232.30 epoch sec; seqNo:6667 2024-11-06T03:37:12Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 4.16 psi 2024-11-06T03:37:12Z- handleDepth()- Depth_Keller msg: 1730864232.31 epoch sec; seqNo:7109; depth 50.04 2024-11-06T03:37:12Z- handleDVL() - DVL msg epochMillisec: 1730864232.34 2024-11-06T03:37:12Z- handleDvl() - RDI_Pathfinder msg: 1730864232.34 sec; seqNo:2449; #beams:4 2024-11-06T03:37:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 41.18 , 45.67 , 42.81 2024-11-06T03:37:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.06 , 0.01 2024-11-06T03:37:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864232.35 epoch sec; seqNo:7109; 4061662.33 north; 594233.16 east; 36.70 lat; -121.95 lon; 50.04 depth; utmZone: 10 2024-11-06T03:37:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T03:37:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864229.08 2024-11-06T03:37:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:12Z- waiting for 1730864233.08, time is 1730864232.35 2024-11-06T03:37:12Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T03:37:12Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T03:37:12Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T03:37:12Z- AHRS_M2 msg: 1730864232.69 epoch sec; seqNo:6668 2024-11-06T03:37:12Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 4.16 psi 2024-11-06T03:37:12Z- handleDepth()- Depth_Keller msg: 1730864232.73 epoch sec; seqNo:7110; depth 50.03 2024-11-06T03:37:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864232.75 epoch sec; seqNo:7110; 4061662.32 north; 594231.94 east; 36.70 lat; -121.95 lon; 50.03 depth; utmZone: 10 2024-11-06T03:37:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T03:37:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864229.08 2024-11-06T03:37:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:12Z- waiting for 1730864233.08, time is 1730864232.75 2024-11-06T03:37:13Z- handleDepth()- Depth_Keller msg: 1730864233.14 epoch sec; seqNo:7111; depth 50.03 2024-11-06T03:37:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730864233.15 epoch sec; seqNo:7111; 4061661.94 north; 594231.94 east; 36.70 lat; -121.95 lon; 50.03 depth; utmZone: 10 2024-11-06T03:37:13Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T03:37:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730864229.08 2024-11-06T03:37:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T03:37:13Z- waiting for 1730864233.08, time is 1730864233.15 2024-11-06T03:37:13Z- LcmTrn::updateTrn() >>>> pose time 1730864233.15, meas time 1730864232.34 <<<< 2024-11-06T03:37:13Z- LcmTrn::updateTrn() measUpdate first last update time: 1730864233.15 2024-11-06T03:37:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730864232.34 sec, ping # 0; waiting for INS data more recent than 1730864229.08... 2024-11-06T03:37:13Z- TerrainNav::motionUpdate() - currEstimate = (4061662, 594231.94, 50.03) 2024-11-06T03:37:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T03:37:13Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T03:37:13Z- TNavPF::Projecting Measurements 2024-11-06T03:37:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T03:37:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T03:37:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T03:37:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T03:37:13Z- TNavFilter::4 good beams. 2024-11-06T03:37:13Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T03:37:13Z- TNavPF:: beamIndex[0] = 0 2024-11-06T03:37:13Z- TNavPF:: beamIndex[1] = 1 2024-11-06T03:37:13Z- TNavPF:: beamIndex[2] = 2 2024-11-06T03:37:13Z- TNavPF:: beamIndex[3] = 3 2024-11-06T03:37:13Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T03:37:13Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T03:37:13Z- TerrainMapOctree: (vehN, vehE ) = (4061662.32, 594232.67). 2024-11-06T03:37:13Z- TerrainMapOctree: Min Distance = 89.37. 2024-11-06T03:37:13Z- TerrainMapOctree: Using tile 4. 2024-11-06T03:37:13Z- Forcing Subcloud Comparison 2024-11-06T03:37:13Z- Weighting particles with subcloud comparison 2024-11-06T03:37:13Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:37:13Z- meanExpectedMeasDiff: 0.870718 delta_rms_squared: 1.519721 alpha: 0.987011 2024-11-06T03:37:13Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:37:13Z- meanExpectedMeasDiff: -0.098659 delta_rms_squared: 1.111989 alpha: 0.982332 2024-11-06T03:37:13Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:37:13Z- meanExpectedMeasDiff: -1.440830 delta_rms_squared: 1.377308 alpha: 0.985687 2024-11-06T03:37:13Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T03:37:13Z- meanExpectedMeasDiff: 0.147673 delta_rms_squared: 0.699609 alpha: 0.972207 2024-11-06T03:37:13Z- Subcloud NIS: 0.448644 2024-11-06T03:37:13Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T03:37:13Z- TNavPF:: sumWeights = 0.547805 2024-11-06T03:37:13Z- TNavPF::Calculating NIS Matrices 2024-11-06T03:37:13Z- TNavPF::First Term of Map Covariance Matrix is 0.53 2024-11-06T03:37:13Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.06 2024-11-06T03:37:13Z- TerrainNav::Resampling particle filter... 2024-11-06T03:37:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730864232.34 sec. 2024-11-06T03:37:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T03:37:13Z- variances([0], [2], and psi_berg) = 0.156003 0.110723 0.000000 2024-11-06T03:37:13Z- publishEstimatesDecoder() - at 1730864233.15 sec 2024-11-06T03:37:13Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T03:37:13Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061690.40 594215.22 55.75 2024-11-06T03:37:13Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061689.80 594215.02 56.63 lat/lon: 36.70 -121.95 2024-11-06T03:37:13Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.11 0.74 0.00 2024-11-06T03:37:13Z- AHRS_M2 msg: 1730864233.54 epoch sec; seqNo:6669 2024-11-06T03:37:13Z- AHRS_M2 msg: 0.12 phi; 0.06 theta; 4.16 psi 2024-11-06T03:37:13Z- handleDepth()- Depth_Keller msg: 1730864233.55 epoch sec; seqNo:7112; depth 49.99 2024-11-06T03:37:13Z- handleDVL() - DVL msg epochMillisec: 1730864233.55 2024-11-06T03:37:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:37:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:37:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T03:37:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T03:37:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T03:37:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T03:37:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T03:37:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T03:37:13Z- handleDvl() - RDI_Pathfinder msg: 1730864233.55 sec; seqNo:2450; #beams:0 2024-11-06T03:37:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T03:37:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.06 , 0.01