Phone-based Visual–Inertial Odometry + GPS anchoring on ROS 2 Humble.

Goal: produce a clean, SLAM-ready odometry stream:

• Local, smooth motion: VO + IMU → /vio_fused/odom
• Globally bounded drift: GPS motion-gated anchor → /vio_gps/odom

Demonstrate IMU-only dead reckoning using a smartphone as a sensor platform, and explicitly observe drift.

• linear_acceleration (gravity removed)
• angular_velocity
• orientation quaternion

Phone IMU → phone_imu_bridge → /imu/data → imu_deadreckoning → /imu/odom

• Axis alignment + unit normalization
• Bias calibration
• Low-pass filtering
• Integrate accel → velocity → position
• ZUPT during stationarity

Bound IMU drift using GPS as a slow anchor signal (no EKF).

• IMU odom: /imu/odom
• GPS fix: /gps/fix (sensor_msgs/NavSatFix)

• Fused: /fused/odom, /fused/path

• Convert lat/lon to local XY (tangent plane approximation)
• Low-gain offset correction (anchor) + jump rejection + accuracy gating

Create a stable, SLAM-ready odometry stream from:

• Position: Visual Odometry (/vio/odom)
• Orientation: IMU quaternion (/imu/data.orientation)
• Velocity: accel integration (world frame)
• Stationarity constraint: ZUPT, gated by VO motion

DroidCam MJPEG → ipcam_bridge → /camera/image_raw → feature_tracker → vo_estimator → /vio/odom

/vio/odom + /imu/data → vio_fuser → /vio_fused/odom, /vio_fused/path

• Normalize IMU quaternion and replace VO orientation
• Rotate IMU linear_acceleration into world frame and integrate to velocity
• ZUPT: damp/clamp velocity when stationary
• VO-gated ZUPT: prevents false stationarity during smooth constant-velocity motion

zupt_enable: true
zupt_mode: damp
zupt_damp: 0.3
zupt_accel_thresh: 0.08      # m/s^2
zupt_gyro_thresh: 0.06       # rad/s
zupt_count: 6
zupt_use_vo_gate: true
zupt_vo_speed_thresh: 0.03   # m/s

Anchor VIO globally without letting GPS jitter walk the pose when the phone is stationary.

• VIO fused odom: /vio_fused/odom
• GPS fix: /gps/fix

• SLAM-ready anchored odom: /vio_gps/odom
• /vio_gps/path

GPS has meters of noise; if you apply it while stationary, your pose will drift even when the phone doesn’t move.

Fix: only update the GPS correction offset when motion is confidently detected.

gps_alpha: 0.03              # anchor strength (0..1). smaller = gentler
max_hacc_m: 15.0             # reject GPS if covariance implies poor accuracy
max_gps_step_m: 12.0         # reject GPS teleports/jumps
min_speed_for_gps_update: 0.25  # motion gate: only apply GPS corrections when moving

Behavior check:

• Phone still → /vio_gps/odom position stays stable (no jitter-walk)
• Phone moving → GPS slowly pulls trajectory toward global consistency

cd ~/phonefusion_nav/ros2_ws
colcon build --merge-install
source install/setup.bash

ros2 launch phonefusion_nav_bringup vio_full_stack.launch.py

/camera/image_raw
/vio/odom
/imu/data
/gps/fix
/vio_fused/odom
/vio_gps/odom

ros2 topic hz /vio_fused/odom
ros2 topic hz /vio_gps/odom
ros2 topic echo /gps/fix --once
ros2 topic echo /vio_gps/odom --field pose.pose.position --once

• Phase 2: IMU dead reckoning — ✅ Complete
• Phase 2.5: GPS anchoring (IMU) — ✅ Complete
• Phase 3: VIO fusion (manual) — ✅ Complete
• Phase 4A: GPS anchor for VIO (motion-gated) — ✅ Complete
• Phase 4B: SLAM integration / loop closure (future) — Planned
• EKF refactor (optional future) — Planned