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`. When set, predicted velocity is multiplied by `exp(-k * dt)` each step: `vel2 = (vel1 + accdt) * exp(-kdt)`. At `k=1.0` (20 Hz), velocity halves in ~0.7s. Default `0.0` preserves existing behavior.

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, 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):