Add low-frequency MPPI sampling

[mohamedsamirx]

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	Addresses #5973
Primary OS tested on	Ubuntu 24.04 with ROS Rolling in Docker / overlay workspace
Robotic platform tested on	TurtleBot3-style differential-drive loopback simulation using Nav2 tb3_loopback_simulation_launch.py; headless benchmark traces with RViz/plot replay inspection
Does this PR contain AI generated software?	Yes
Was this PR description generated by AI software?	No

---

Description of contribution in a few bullet points

• Adds an optional low-frequency / colored-noise sampling mode to the MPPI controller.
• This addresses MPPI: Low-frequency distribution sampling to create smoother trajectories #5973 by adding a sampling distribution that generates temporally correlated perturbations instead of independent white Gaussian noise at every time step.
• The feature is disabled by default, so existing MPPI behavior remains unchanged unless the new parameters are enabled.
• The new colored-noise parameters are:
  • colored_noise.enabled
  • colored_noise.exponent
  • colored_noise.offset_t
  • colored_noise.offset_decay_rate
  • colored_noise.fmin
• The implementation is based on the low-frequency sampling approach described in:
  • Vlahov et al., “Low Frequency Sampling in Model Predictive Path Integral Control”
  • https://arxiv.org/pdf/2404.03094
• I also referenced the implementation examples mentioned in MPPI: Low-frequency distribution sampling to create smoother trajectories #5973:
  • https://github.com/ACDSLab/MPPI-Generic/tree/main/include/mppi/sampling_distributions
  • https://github.com/felixpatzelt/colorednoise/blob/master/colorednoise.py
• Colored-noise perturbations are generated in the frequency domain and transformed back to the time domain with an inverse FFT.
• The generator uses:
  • doubled-horizon sampling, 2 * time_steps
  • power-law frequency scaling using frequency^(-exponent / 2)
  • Hermitian symmetry so the inverse FFT produces real-valued control perturbations
  • normalization so the time-domain perturbation variance matches the requested sampling standard deviation
  • crop/offset handling using colored_noise.offset_t
  • optional decayed offset correction using colored_noise.offset_decay_rate
  • a minimum frequency clamp using colored_noise.fmin
• The colored-noise generator supports differential and holonomic control dimensions:
  • vx
  • wz
  • vy when holonomic
• Adds sampling_std.reduction_factor so multi-iteration MPPI runs can optionally reduce the sampling standard deviation on later iterations.
• The refinement path keeps the default behavior unchanged:
  • default iteration_count remains 1
  • default sampling_std.reduction_factor is 1.0
• Adds unit coverage for:
  • colored-noise disabled mode
  • colored-noise parameter clamping
  • colored-noise high-frequency energy reduction compared to white Gaussian samples
  • sampling_std.reduction_factor parameter loading
  • invalid sampling_std.reduction_factor fallback

Implementation notes and scope

This PR adds the colored-noise sampling distribution to Nav2’s existing MPPI implementation. It is not intended to replace the whole MPPI controller derivation with a full paper-replica implementation.

The sampler itself follows the paper/reference structure closely:

• sample Gaussian values in the frequency domain
• apply power-law frequency scaling
• enforce Hermitian symmetry
• use inverse FFT to generate real-valued time-domain samples
• normalize the samples
• use doubled-horizon crop/offset handling

One important theoretical difference is that Nav2 still uses its existing MPPI update and control-cost correction structure. The existing correction uses the configured per-axis sampling standard deviations. Colored noise introduces temporal correlation across the horizon, so this PR should be understood as adding a low-frequency sampling distribution to Nav2 MPPI rather than adding a full covariance-aware paper clone.

The multi-iteration refinement path works best with regenerate_noises: true, which is already the default in Nav2 bringup params. If users disable noise regeneration, repeated iterations can reuse the same noise batch depending on configuration, which limits the usefulness of refinement.

Description of documentation updates.

• Updated nav2_mppi_controller/README.md with the new colored-noise parameters.
• Updated default bringup params with disabled-by-default values:
  • colored_noise.enabled: false
  • colored_noise.exponent: 2.0
  • colored_noise.offset_t: 1
  • colored_noise.offset_decay_rate: 0.97
  • colored_noise.fmin: 0.0
  • sampling_std.reduction_factor: 1.0
• docs.nav2.org should add the new MPPI parameters.
• The MPPI tuning guide should document the main tuning behavior:
  • larger colored_noise.exponent shifts more sample energy into lower frequencies
  • lower-frequency samples can help produce longer, smoother control perturbations
  • tuning is scenario-dependent
  • this should be treated as an optional sampling mode, not a new default behavior
  • in my benchmark, exponent 2.0 was the best tested value
  • lower tested exponents with SGF, 0.5 and 1.0, were not favorable in this matrix
• The tuning guide should also mention that this is different from SGF:
  • SGF smooths the selected control sequence after optimization
  • colored-noise sampling changes the perturbation distribution before rollout/evaluation
  • these can be used independently or together

Description of how this change was tested

Build and unit tests

I built the MPPI package sequentially:

colcon build --packages-select nav2_mppi_controller --executor sequential --parallel-workers 1

The build completed successfully.

I then ran the targeted unit tests:

ctest --test-dir /opt/overlay_ws/build/nav2_mppi_controller -R "noise_generator_test|optimizer_unit_tests" --output-on-failure

Result:

100% tests passed, 0 tests failed out of 2

The passing tests were:

• noise_generator_test
• optimizer_unit_tests

Benchmark setup

I ran a repeated headless loopback simulation matrix using Nav2’s TurtleBot3 loopback simulation.

The benchmark runner launched a fresh Nav2 stack per trial, waited for lifecycle activation, sent a NavigateToPose goal, collected /cmd_vel_nav, odom, and controller_server/optimal_trajectory, then shut the stack down before the next trial. This was done to keep each trial isolated and avoid reusing stale state or artifacts.

The benchmark also saved:

• result JSONs
• launch logs
• evaluation logs
• trace JSONs
• replay metadata
• summary CSV/JSON/Markdown
• plots for visual inspection

The matrix had:

• 4 scenarios
• 9 profiles
• 2 repeats per scenario/profile
• 72 final result runs

Artifact integrity from the final matrix:

• 72 final result JSONs
• 72 localization prime JSONs
• 72 trace files
• 72 replay metadata files
• 0 infrastructure failures
• 0 data-quality warning runs
• 48 successful navigation runs
• 24 behavioral failures

Scenarios tested

The scenarios were selected to include easy, moderate, and hard cases:

• tb3_open_straight: basic straight navigation sanity check
• tb3_turning_route: moderate route with turning behavior
• tb3_tight_s_turn: hard route requiring tight maneuvering
• tb3_near_obstacle_crossing: hard obstacle interaction case

Profiles compared

The benchmark compared current baselines and low-frequency variants:

Profile	Meaning
vanilla_raw	White Gaussian sampling, no SGF
vanilla_sgf	Current default SGF order 2 behavior
vanilla_sgf_o1	SGF order 1 baseline
lowfreq_raw	Colored noise, exponent 2.0, no SGF
lowfreq_sgf_g0p5	Colored noise, exponent 0.5, SGF enabled
lowfreq_sgf_g1p0	Colored noise, exponent 1.0, SGF enabled
lowfreq_sgf_g2p0	Colored noise, exponent 2.0, SGF enabled
lowfreq_refined_i2_s1p0	Colored noise, exponent 2.0, no SGF, iteration_count=2, sampling_std.reduction_factor=1.0
lowfreq_refined_i3_s1p0	Colored noise, exponent 2.0, no SGF, iteration_count=3, sampling_std.reduction_factor=1.0

Aggregate benchmark results

Overall success counts:

Profile	Successes
Colored noise exponent 2.0 + SGF	7/8
SGF order 1	6/8
Colored noise raw, exponent 2.0	6/8
Colored noise refined, iteration_count=2	6/8
Colored noise refined, iteration_count=3	6/8
SGF order 2 default	5/8
White Gaussian raw	4/8
Colored noise exponent 0.5 + SGF	4/8
Colored noise exponent 1.0 + SGF	4/8

The strongest positive case was tb3_near_obstacle_crossing:

Profile	Result
Colored noise exponent 2.0 + SGF	2/2
Colored noise raw, exponent 2.0	1/2
Colored noise refined, iteration_count=2	1/2
Colored noise refined, iteration_count=3	1/2
White Gaussian raw	0/2
SGF order 2 default	0/2
SGF order 1	0/2
Colored noise exponent 0.5 + SGF	0/2
Colored noise exponent 1.0 + SGF	0/2

The easier scenarios were passed by all tested profiles:

• tb3_open_straight: all profiles 2/2
• tb3_turning_route: all profiles 2/2

The hard tight S-turn remained mixed:

Profile	Result
SGF order 1	2/2
SGF order 2 default	1/2
Colored noise raw, exponent 2.0	1/2
Colored noise exponent 2.0 + SGF	1/2
Colored noise refined, iteration_count=2	1/2
Colored noise refined, iteration_count=3	1/2
White Gaussian raw	0/2
Colored noise exponent 0.5 + SGF	0/2
Colored noise exponent 1.0 + SGF	0/2

Smoothness, jitter, and frequency metrics

The benchmark also computed the smoothness and jitter metrics requested in #5973. Lower values are smoother for the delta/jitter/high-frequency metrics below.

Metric definitions:

• Opt mean abs dvx/dwz: mean absolute step-to-step velocity change inside each published optimal trajectory.
• Inter first mean abs dvx/dwz: mean absolute change of the first optimal-trajectory command between controller iterations. This is the closest “inter-iteration jitter” metric.
• Inter overlap mean abs dvx/dwz: mean absolute change between overlapping shifted horizon commands from consecutive optimal trajectories.
• Cmd mean abs dvx/dwz: mean absolute command-to-command change in executed /cmd_vel_nav.
• Cmd HF vx/wz: high-frequency energy ratio in executed commands above the benchmark cutoff.

Aggregate values from the final working matrix:

Profile	Success	Opt mean abs dvx	Opt mean abs dwz	Inter first mean abs dvx	Inter first mean abs dwz	Inter overlap mean abs dvx	Inter overlap mean abs dwz	Cmd mean abs dvx	Cmd mean abs dwz	Cmd HF vx	Cmd HF wz
White Gaussian raw	4/8	0.00689	0.00843	0.00590	0.01576	0.00764	0.00951	0.00615	0.01609	0.00190	0.00133
SGF order 2 default	5/8	0.00668	0.00826	0.00568	0.01479	0.00733	0.00924	0.00594	0.01531	0.00195	0.00204
SGF order 1	6/8	0.00631	0.00660	0.00495	0.01423	0.00731	0.00869	0.00517	0.01469	0.00111	0.00087
Colored noise raw, exponent 2.0	6/8	0.00503	0.01391	0.00675	0.02592	0.02441	0.02968	0.00702	0.02652	0.00171	0.00119
Colored noise exponent 2.0 + SGF	7/8	0.00498	0.01359	0.00665	0.02394	0.02426	0.02674	0.00694	0.02453	0.00196	0.00097
Colored noise refined, iteration_count=2	6/8	0.00487	0.01321	0.00650	0.02370	0.02245	0.02689	0.00680	0.02431	0.00176	0.00106
Colored noise refined, iteration_count=3	6/8	0.00490	0.01328	0.00662	0.02389	0.02386	0.02760	0.00691	0.02452	0.00235	0.00105

The metric takeaway is mixed and should be stated that way:

• Colored-noise sampling reduced optimal-trajectory vx step changes compared with the white Gaussian and SGF baselines.
• SGF order 1 remained best on executed command smoothness and inter-iteration jitter in this matrix.
• The best colored-noise reliability profile, exponent 2.0 + SGF, had higher wz and inter-overlap deltas than SGF order 1, so it is not a blanket smoothness win.
• The strongest positive signal for this PR is reliability and trajectory feasibility in the hard near-obstacle case, not universal improvement of every smoothness metric.

Runtime / controller-rate observations

All tested low-frequency profiles maintained approximately the expected 20 Hz optimal trajectory publication rate in this matrix.

Mean values from the final matrix:

Profile	Successes	Mean duration	Mean optimal trajectory rate
Colored noise exponent 2.0 + SGF	7/8	19.41 s	19.99 Hz
Colored noise refined, iteration_count=3	6/8	19.89 s	19.98 Hz
Colored noise refined, iteration_count=2	6/8	20.91 s	20.01 Hz
Colored noise raw, exponent 2.0	6/8	21.04 s	19.98 Hz
Colored noise exponent 1.0 + SGF	4/8	22.93 s	19.99 Hz
Colored noise exponent 0.5 + SGF	4/8	24.20 s	19.99 Hz

I interpret this as “no observed controller-rate degradation in this benchmark.” This is not a CPU profiler result; it only shows that the controller maintained the expected rate under these test conditions.

Smoothness / velocity inspection

I generated before/after-style plots for visual inspection of the velocity signals and trajectories. The plots include:

• profile summary
• scenario success heatmap
• velocity inspection for tb3_turning_route
• velocity inspection for tb3_tight_s_turn
• velocity inspection for tb3_near_obstacle_crossing

The plots are intended to make the velocity/noise behavior inspectable rather than relying only on scalar metrics.

The main visual/metric finding is that colored-noise sampling changes the perturbation distribution before rollout/evaluation, while SGF smooths after the optimizer selects the trajectory. The best tested result came from combining colored noise exponent 2.0 with SGF.

Recommended plot attachments:

Benchmark interpretation

My conclusion from this matrix is:

• Low-frequency colored-noise sampling is a promising optional MPPI sampling mode.
• The best tested configuration was colored noise exponent 2.0 with SGF enabled.
• The main benefit showed up in the hard near-obstacle crossing scenario, where the best colored-noise profile succeeded 2/2 while the tested white Gaussian / SGF baselines failed 0/2.
• The result is not a universal win across every metric.
• SGF order 1 remains very competitive, especially on executed-command smoothness metrics.
• Lower tested colored-noise exponents with SGF, 0.5 and 1.0, were not favorable in this matrix.
• Multi-iteration refinement worked and maintained controller rate, but did not clearly outperform colored noise exponent 2.0 + SGF in this test set.
• I do not think this should become a default behavior from this PR. It is better introduced as an optional sampling mode with documentation and tuning guidance.

Future work that may be required.

• Add docs.nav2.org documentation for:
  • colored_noise.enabled
  • colored_noise.exponent
  • colored_noise.offset_t
  • colored_noise.offset_decay_rate
  • colored_noise.fmin
  • sampling_std.reduction_factor
• Add MPPI tuning-guide notes for:
  • exponent selection
  • interaction with SGF
  • when to use colored-noise sampling
  • when not to use it
• Add broader validation on:
  • physical robots
  • omnidirectional platforms
  • robots with slower actuator dynamics
  • longer and more cluttered routes
• Add CPU profiling focused on:
  • Eigen FFT overhead
  • colored-noise generation cost
  • multi-iteration refinement cost
• Consider future investigation into a covariance-aware update/control-cost correction for a closer theoretical match to the paper.
• Add more distribution-level tests:
  • output variance close to requested vx_std, vy_std, and wz_std
  • exponent=0.0 approximating white-noise spectral behavior
  • offset_t and offset_decay_rate behavior
  • multi-iteration regeneration behavior
• Continue tuning before considering any default-enabled configuration.

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists
• Should this be backported to current distributions? If so, tag with backport-*.

[codecov]

Codecov Report

❌ Patch coverage is 99.18699% with 1 line in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_mppi_controller/src/noise_generator.cpp	99.04%	1 Missing ⚠️

Files with missing lines	Coverage Δ
...nav2_mppi_controller/models/optimizer_settings.hpp	100.00% <ø> (ø)
...ude/nav2_mppi_controller/tools/noise_generator.hpp	100.00% <100.00%> (ø)
nav2_mppi_controller/src/optimizer.cpp	96.52% <100.00%> (-0.40%)	⬇️
nav2_mppi_controller/src/noise_generator.cpp	99.31% <99.04%> (-0.69%)	⬇️

... and 25 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[mohamedsamirx]

@SteveMacenski, any news on this?