Add velocity_decay to Omnidirectional3D to prevent drift when odom goes silent

[bkanator]

Summary

When a nav2 controller deactivates, wheel odom stops publishing. Without a correcting velocity source, `Omnidirectional3D` propagates the last known velocity indefinitely, causing unbounded position drift in the fuse estimate.

Adds a `velocity_decay` parameter (1/s, default `0.0`) to `Omnidirectional3D`. Predicted velocity and acceleration are multiplied by `exp(-k * dt)` each step. At `k=1.0` (20 Hz), velocity and acceleration decay to ~zero in ~3s. Default `0.0` preserves existing behavior.

Decaying acceleration alongside velocity is necessary: a non-zero acceleration state maintains a non-zero steady-state velocity through `vel2 = (vel1 + acc*dt) * decay_factor`, causing sustained drift even after velocity appears to decay.

Updates analytical Jacobians and Boost serialization (versioned, backward compatible with v0 archives).

Claude agent checks

• code-reviewer
• test-runner
• platform-architect-bot

[Copilot]

Pull request overview

Adds configurable exponential velocity decay to the Omnidirectional3D motion model to reduce drift when velocity observations stop, while preserving existing behavior by default.

Changes:

• Adds velocity_decay parameter parsing, validation, storage, and propagation through Omnidirectional3D constraints.
• Updates prediction functions and analytical Jacobians to apply decay to linear/angular velocity.
• Adds prediction tests for zero decay, velocity reduction, and decay-factor Jacobians.

Reviewed changes

Copilot reviewed 7 out of 7 changed files in this pull request and generated 3 comments.

Show a summary per file

File	Description
fuse_models/src/omnidirectional_3d.cpp	Reads and forwards velocity_decay into prediction and constraints.
fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp	Stores decay on the constraint and passes it to the cost function.
fuse_models/include/fuse_models/omnidirectional_3d.hpp	Documents and stores the new model parameter.
fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp	Extends constructor/serialization with decay.
fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp	Extends cost function with decay propagation.
fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp	Applies decay in prediction and Jacobians.
fuse_models/test/test_omnidirectional_3d_predict.cpp	Adds tests for decay behavior and Jacobian scaling.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[bkanator]

Steps to ship after merge (from Josh)

Both Humble and Jazzy are built from the `humble` branch, so this single PR covers both distros.

1. Update `apt_build_farm` — update the SHA (and optionally bump the package version) in packages/fuse/package.json. Open a PR to `main` of `apt_build_farm`.
2. Wait for `apt_build_farm` CI — once that PR merges to `main` and the CI job on `main` finishes, the new apt packages are published.
3. Bump `moveit_pro` Dockerfile — open a PR updating both the Humble and Jazzy pinned fuse versions (currently `ros-humble-fuse=1.2.0-99jammy` and `ros-jazzy-fuse=1.2.0-99noble`).
4. `moveit_pro_example_ws` config — feat(hangar_sim): enable fuse velocity_decay to prevent drift after nav2 stops moveit_pro_example_ws#674 can merge any time; the `velocity_decay: 1.0` key is ignored until the new fuse binary is in the image.

[bkanator]

tested by

• cloning fuse into the example_ws on this branch : git clone --branch feat/omnidirectional-3d-velocity-decay \ https://github.com/PickNikRobotics/fuse.git,

• locally build colcon build --packages-select \ fuse_msgs fuse_core fuse_graphs fuse_variables \ fuse_constraints fuse_publishers fuse_optimizers fuse_models \ --allow-overriding fuse_msgs fuse_core fuse_graphs fuse_variables \ fuse_constraints fuse_publishers fuse_optimizers fuse_models, and

• set publish_odom is false when use_fuse:=true in example_ws.

Step 2 — Install deps and build:
cd ~/user_ws
install_ros_dependencies.bash
colcon build --packages-select fuse_models fuse_optimizers

Step 3 — Launch (source user_ws FIRST so it overrides the
source /etc/skel/.moveit-pro-bashrc
source ~/user_ws/install/setup.bash
agent_robot.app use_fuse:=true

Step 4 — RViz (new terminal, same sourcing order):

[bkanator]

here is the video from testing that verified both fixing velocity and acceleration drift does not make the yellow arrow move after robot is stopped and nav2 controller is deactivated.

fuse_no_drift-2026-06-03_10.46.45.mp4

[bkanator]

@henrygerardmoore griz recommended you get eyes on this, it is for closing https://github.com/PickNikRobotics/moveit_pro/issues/18070 and that issue shows the initial video where the bug is, and then the video after the fix