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. At k=1.0 (20 Hz), velocity halves in ~0.7s. Default 0.0 preserves existing behavior.

Updates analytical Jacobians: d(vel2)/d(vel1) = exp(-k*dt) instead of 1.0.

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.