sl-perception
c76d5b0dd7
New package saltybot_pose_fusion — EKF fusing UWB+IMU absolute pose,
visual odometry velocity, and raw IMU into a single authoritative pose.
pose_fusion_ekf.py (pure Python, no ROS2 deps):
PoseFusionEKF — state [x, y, θ, vx, vy, ω], 6-state EKF.
- predict_imu(ax_body, ay_body, omega, dt): body-frame IMU predict step
with Jacobian F, bias-compensated accel, process noise Q.
- update_uwb_position(x, y, sigma_m): absolute position measurement
(H=[1,0,0,0,0,0; 0,1,0,0,0,0]) from UWB+IMU fused stream.
- update_uwb_heading(heading_rad, sigma_rad): heading measurement.
- update_vo_velocity(vx_body, omega, ...): VO velocity measurement —
body-frame vx rotated to world via cos/sin(θ), updates [vx,vy,ω] state.
- Joseph-form covariance update for numerical stability.
- Dual dropout clocks: uwb_dropout_s, vo_dropout_s (reset on each update).
- Velocity damping when uwb_dropout_s > 2s.
- Sensor weight parameters: sigma_uwb_pos_m, sigma_uwb_head_rad,
sigma_vo_vel_m_s, sigma_vo_omega_r_s, sigma_imu_accel/gyro,
sigma_vel_drift, dropout_vel_damp.
pose_fusion_node.py (ROS2 node 'pose_fusion'):
- Subscribes: /imu/data (Imu, 200Hz → predict), /saltybot/pose/fused_cov
(PoseWithCovarianceStamped, 10Hz → position+heading update, σ extracted
from message covariance when use_uwb_covariance=true), /saltybot/visual_odom
(Odometry, 30Hz → velocity update, σ from twist covariance).
- Publishes: /saltybot/pose/authoritative (PoseWithCovarianceStamped),
/saltybot/pose/status (String JSON, 10Hz).
- TF2: map→base_link broadcast at IMU rate.
- Suppresses output when uwb_dropout_s > uwb_dropout_max_s (10s).
- Warns (throttled) on UWB/VO dropout.
config/pose_fusion_params.yaml: sensor weights + dropout thresholds.
launch/pose_fusion.launch.py: single node launch with params_file arg.
test/test_pose_fusion_ekf.py: 13 unit tests — init, predict, UWB/VO
updates, dropout reset, covariance shape/convergence, sigma override.
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
Jetson Nano — AI/SLAM Platform Setup
Self-balancing robot: Jetson Nano dev environment for ROS2 Humble + SLAM stack.
Stack
| Component | Version / Part |
|---|---|
| Platform | Jetson Nano 4GB |
| JetPack | 4.6 (L4T R32.6.1, CUDA 10.2) |
| ROS2 | Humble Hawksbill |
| DDS | CycloneDDS |
| SLAM | slam_toolbox |
| Nav | Nav2 |
| Depth camera | Intel RealSense D435i |
| LiDAR | RPLIDAR A1M8 |
| MCU bridge | STM32F722 (USB CDC @ 921600) |
Quick Start
# 1. Host setup (once, on fresh JetPack 4.6)
sudo bash scripts/setup-jetson.sh
# 2. Build Docker image
bash scripts/build-and-run.sh build
# 3. Start full stack
bash scripts/build-and-run.sh up
# 4. Open ROS2 shell
bash scripts/build-and-run.sh shell
Docs
docs/pinout.md— GPIO/I2C/UART pinout for all peripheralsdocs/power-budget.md— 10W power envelope analysis
Files
jetson/
├── Dockerfile # L4T base + ROS2 Humble + SLAM packages
├── docker-compose.yml # Multi-service stack (ROS2, RPLIDAR, D435i, STM32)
├── README.md # This file
├── docs/
│ ├── pinout.md # GPIO/I2C/UART pinout reference
│ └── power-budget.md # Power budget analysis (10W envelope)
└── scripts/
├── entrypoint.sh # Docker container entrypoint
├── setup-jetson.sh # Host setup (udev, Docker, nvpmodel)
└── build-and-run.sh # Build/run helper
Power Budget (Summary)
| Scenario | Total |
|---|---|
| Idle | 2.9W |
| Nominal (SLAM active) | ~10.2W |
| Peak | 15.4W |
Target: 10W (MAXN nvpmodel). Use RPLIDAR standby + 640p D435i for compliance.
See docs/power-budget.md for full analysis.