Sub-issue of #54.
Background
FusionCore uses one noise model for all operating conditions. ZUPT is a hardcoded exception for stationary detection, and coast mode is a hardcoded exception for GPS outage. These are special cases of a more general idea: the robot is always in one of several regimes, each with its own optimal noise model.
Hidden Markov Models (HMMs) are used in quantitative finance (Goldman Sachs, Renaissance Technologies) for market regime detection and in genomics for finding gene boundaries in DNA sequences. The math is identical to what is needed here.
The idea
Define K operating regimes, each with its own Q and R matrices:
- Stationary: very low Q, ZUPT active, encoder fully trusted
- Straight driving: standard Q and R
- Turning: higher angular process noise, tighter encoder weight
- Aggressive maneuver: higher Q across all states
- GPS outage: coast mode, GPS R inflated to infinity
- High vibration: inflated IMU accel R at vibration frequencies
- Wheel slip: inflated encoder R
The HMM has:
- Hidden state: current regime (K states)
- Observations: innovation vector from each sensor
- Transition matrix: learned from rosbag data, or set by hand with sensible priors
- Emission model: Gaussian per regime (or GMM for harder cases)
At each timestep, a Viterbi or forward pass gives the posterior regime probability. The noise matrices used by the UKF are a weighted mixture of the regime-specific matrices.
Why this is novel and unifying
This subsumes ZUPT, coast mode, adaptive noise, and slip detection into one probabilistic framework. Instead of four separate mechanisms with four sets of thresholds, there is one model with a learned transition structure. The finance and genomics connection makes it a genuine cross-industry contribution.
Why it matters practically
The transition matrix learns that GPS tends to degrade gradually (canopy entry), that wheel slip follows high acceleration commands, and that vibration correlates with motor current. These patterns cannot be captured by threshold-based switching.
References
- Rabiner: A Tutorial on Hidden Markov Models (1989, the foundational paper)
- Hamilton: Analysis of Time Series Subject to Changes in Regime (1990, finance application)
- Durbin et al.: Biological Sequence Analysis (genomics application)
Sub-issue of #54.
Background
FusionCore uses one noise model for all operating conditions. ZUPT is a hardcoded exception for stationary detection, and coast mode is a hardcoded exception for GPS outage. These are special cases of a more general idea: the robot is always in one of several regimes, each with its own optimal noise model.
Hidden Markov Models (HMMs) are used in quantitative finance (Goldman Sachs, Renaissance Technologies) for market regime detection and in genomics for finding gene boundaries in DNA sequences. The math is identical to what is needed here.
The idea
Define K operating regimes, each with its own Q and R matrices:
The HMM has:
At each timestep, a Viterbi or forward pass gives the posterior regime probability. The noise matrices used by the UKF are a weighted mixture of the regime-specific matrices.
Why this is novel and unifying
This subsumes ZUPT, coast mode, adaptive noise, and slip detection into one probabilistic framework. Instead of four separate mechanisms with four sets of thresholds, there is one model with a learned transition structure. The finance and genomics connection makes it a genuine cross-industry contribution.
Why it matters practically
The transition matrix learns that GPS tends to degrade gradually (canopy entry), that wheel slip follows high acceleration commands, and that vibration correlates with motor current. These patterns cannot be captured by threshold-based switching.
References