Sub-issue of #51.
Background
FusionCore's predict step uses Euler integration. Euler does not conserve energy: over long runs it injects or drains energy from the state estimate, producing slow drift that no noise tuning can fix. This is a systematic error from the integration scheme itself.
Symplectic integrators (used in orbital mechanics and molecular dynamics) conserve energy exactly by construction. NASA uses them for spacecraft trajectory propagation. Molecular dynamics uses them for protein simulation. No ROS navigation package does.
The idea
Replace Euler with leapfrog integration in the motion model predict step:
# Euler (current):
x(t+dt) = x(t) + v(t) * dt # uses old velocity
# Leapfrog (symplectic):
x(t+dt) = x(t) + v(t+dt) * dt # uses new velocity
One line change. Second-order accurate (O(dt²) vs Euler's O(dt)). Identical computational cost. Energy conservation follows from the mathematical structure of the update.
Why novel
Every ROS EKF/UKF uses Euler. The academic Kalman filter literature uses Euler. This is standard in orbital mechanics and molecular dynamics but has never appeared in a ROS navigation package. The justification is airtight and the implementation is ~10 lines localized to the motion model predict step in fusioncore_core.
References
- Leimkuhler, Reich: Simulating Hamiltonian Dynamics (Cambridge, 2004)
- Verlet: Computer Experiments on Classical Fluids (1967)
Sub-issue of #51.
Background
FusionCore's predict step uses Euler integration. Euler does not conserve energy: over long runs it injects or drains energy from the state estimate, producing slow drift that no noise tuning can fix. This is a systematic error from the integration scheme itself.
Symplectic integrators (used in orbital mechanics and molecular dynamics) conserve energy exactly by construction. NASA uses them for spacecraft trajectory propagation. Molecular dynamics uses them for protein simulation. No ROS navigation package does.
The idea
Replace Euler with leapfrog integration in the motion model predict step:
One line change. Second-order accurate (O(dt²) vs Euler's O(dt)). Identical computational cost. Energy conservation follows from the mathematical structure of the update.
Why novel
Every ROS EKF/UKF uses Euler. The academic Kalman filter literature uses Euler. This is standard in orbital mechanics and molecular dynamics but has never appeared in a ROS navigation package. The justification is airtight and the implementation is ~10 lines localized to the motion model predict step in fusioncore_core.
References