nav2_mppi_controller: add optional LP-MPPI-style low-pass perturbation sampling (paper-based, unofficial)

[mohamedsamirx]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	N/A
Primary OS tested on	Ubuntu (dev container)
Robotic platform tested on	Unit tests only (no hardware run in this PR)
Does this PR contain AI generated software?	Yes and it is marked inline in the code
Was this PR description generated by AI software?	Out of respect for maintainers, AI for human-to-human communications are banned

---

Description of contribution in a few bullet points

• This PR adds an unofficial Nav2 implementation of the sampling idea from the paper: LP-MPPI: Low-Pass Filtering for Efficient Model Predictive Path Integral Control (accepted at ICRA 2026).
• I've designed this feature to be strictly opt-in, so it won't affect any existing MPPI configurations or change the current default behavior of the controller.
• New parameters:
  • use_low_pass_filter (bool, default false)
  • filter_cutoff_frequency (double, Hz, default 2.0)
  • filter_order (int, default 2)
• Nyquist safety guard added:
  • validates cutoff against Nyquist frequency f_N = 1 / (2 * model_dt)
  • clamps invalid cutoff and logs warning

Algorithm details implemented in this PR

• Baseline MPPI sampling in Nav2 uses Gaussian perturbations:
  • epsilon ~ N(0, Sigma)
  • u_sampled = u_nominal + epsilon
• This PR adds LP-MPPI-style perturbation shaping:
  • epsilon_lp = LPF(epsilon, cutoff, order)
  • u_sampled = u_nominal + epsilon_lp
• Filtering is applied:
  • on perturbations (not on final command output)
  • per rollout trajectory
  • along the time_steps axis
  • for each control dimension (vx, wz, and vy when holonomic)
• Filter implementation:
  • digital Butterworth low-pass
  • runtime coefficient design
  • direct-form recursive application

Files changed

• nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp
  • Added LP filter config/data members and helper method declarations.
• nav2_mppi_controller/src/noise_generator.cpp
  • Added parameter loading for LP filter controls.
  • Added Nyquist validation/clamping.
  • Added Butterworth coefficient design and perturbation filtering.
  • Integrated filtering into the perturbation generation path.
• nav2_mppi_controller/test/noise_generator_test.cpp
  • Added LowPassFilterSmoothsPerturbations test.
  • Added holonomic vy smoothing assertion.
  • Added CutoffClampedToNyquist test.
• nav2_mppi_controller/README.md
  • Added parameter docs and example config.
• nav2_bringup/params/nav2_params.yaml
  • Added sample/default LP filter parameters under FollowPath.

Usage

Example configuration:

controller_server:
  ros__parameters:
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      model_dt: 0.05
      regenerate_noises: true
      use_low_pass_filter: true
      filter_cutoff_frequency: 2.0
      filter_order: 2

Tuning notes:

• Increase filter_cutoff_frequency for more responsiveness.
• Decrease filter_cutoff_frequency for smoother perturbations and less high-frequency chatter.
• Higher filter_order increases attenuation above cutoff but can make exploration more conservative.
• Keep regenerate_noises: true for behavior closest to LP-MPPI paper sampling assumptions.

Description of documentation updates required from your changes

• Add use_low_pass_filter, filter_cutoff_frequency, and filter_order to docs.nav2.org MPPI parameter reference.
• Add tuning guidance for cutoff/order vs responsiveness/smoothness tradeoff.
• No migration guide entry required for breaking changes (feature is opt-in), but could be listed under new controller capabilities.

Description of how this change was tested

• Focused new-test validation:
  • GTEST_FILTER=NoiseGeneratorTest.LowPassFilterSmoothsPerturbations colcon
    test --packages-select nav2_mppi_controller --ctest-args -R
    noise_generator_test --event-handlers console_direct+
• Full package test run:
  • colcon test --packages-select nav2_mppi_controller
  • colcon test-result --verbose
• Observed result in environment:
  • 0 errors, 0 failures (skips present, expected for Nav2 CI/dev
    environments)

Future work that may be required in bullet points

• Add noise_filter_type mode selector when additional filter families are introduced.
• Add comparative benchmark plots for tracking error/control smoothness / compute-time impact vs baseline MPPI across robot classes.
• Evaluate explicit synchronization strategy after reset() to avoid potential first-cycle zero-perturbation edge case with async regeneration.
• Add documentation examples for differential, omni, and Ackermann recommended

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org and 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 are 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 98.92473% with 1 line in your changes missing coverage. Please review.

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

Files with missing lines	Coverage Δ
...ude/nav2_mppi_controller/tools/noise_generator.hpp	100.00% <ø> (ø)
nav2_mppi_controller/src/noise_generator.cpp	99.30% <98.92%> (-0.70%)	⬇️

... and 19 files with indirect coverage changes

🚀 New features to boost your workflow:

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

[mini-1235]

@mohamedsamirx before we start reviewing, have you had a chance to test this in a simulation environment in addition to the unit tests? I am curious whether you observed any noticeable differences before / after this PR, and in which situations you would recommend enabling / disabling this

[mohamedsamirx]

@mini-1235 Sorry, I haven't had time to get to it to be tested on simulation yet, but it's on my list. I may prioritize it soon.

[SteveMacenski]

Please do and provide some thoughts on the improvements!

[mohamedsamirx]

I tested LP-MPPI in Gazebo on two Nav2 simulation environments: the TurtleBot3 sandbox map and the warehouse map. The final benchmark used 10 candidate routes in total; 9 were valid for controller comparison, and 1 route was excluded because the global planner failed for all 4 configurations before controller execution. Each valid route was run 5 times under these 4 configurations:

• nav2_smooth_on: Nav2 smoothing on, LP-MPPI off
• nav2_smooth_off: Nav2 smoothing off, LP-MPPI off
• lp_only_default: Nav2 smoothing off, LP-MPPI on
• lp_default: Nav2 smoothing on, LP-MPPI on

My honest conclusion from the simulation results is that I would not present this as a clear “smoothness improvement” over Nav2 in its current form. The practical Nav2 winner was lp_default: it won 7 of 9 valid routes by success rate and then mean duration. Compared against nav2_smooth_on, it improved mean success rate from 0.844 to 0.978 (+13.3 percentage points), reduced mean duration from 32.69s to 30.20s (+7.6%), improved MSGFD by 21.4%, and reduced linear variation (TV vx) by 22.3%. However, it also worsened MSSD by 104.4% and worsened angular variation (TV wz) by 30.9%.

I also compared the paper-faithful case, lp_only_default, against raw Nav2 without Savitzky-Golay smoothing (nav2_smooth_off). In that comparison, success improved from 0.756 to 0.867 (+11.1 percentage points), duration improved from 33.19s to 29.99s (+9.6%), and TV vx improved by 26.6%. But MSGFD was essentially flat to slightly worse (-0.5%), and both MSSD and TV wz got worse.

So the honest takeaway is: LP-MPPI seems beneficial in Nav2 mainly as a success-rate and completion-time improvement, and in some cases as a linear-command smoothing improvement, but not as a consistent overall smoothness improvement. In particular, angular smoothness and MSSD regress often enough that I do not think the current evidence supports claiming it is uniformly better than the existing Nav2 behavior.

@SteveMacenski

[SteveMacenski]

How did you evaluate trajectory smoothness? I think what we’re talking about here has little to do with Path Smoothing (is that what you mean by Nav2 Smoothing? I’m really not sure)

That’s to mean the smoothness in terms of the velocities in the optimal trajectory + inter-iteration, ie the amount of jitter that could cause small jerking.

That is the intention behind the ticket: #5973. That method was recommended to me by the MPPI developers in the GT lab so I take that recommendation particularly seriously. Have you looked into that?

Maybe its good for you to start at the beginning:

• What is the stated benefit / rationale behind the LP variant you’ve implemented?
• Why have you chosen this very new variant to implement? What need is this filling for your applications?
• How do you plan to use this?

[mohamedsamirx]

That is a fair point, and I should clarify.
By “Nav2 smoothing” I did not mean path smoothing. I meant the existing Savitzky-Golay smoothing inside the MPPI controller’s control update. My benchmark was based on the executed velocity-command sequence, using metrics like MSSD, MSGFD, and total variation in vx / wz, so I was evaluating control-output smoothness / jitter, not path smoothing.

Also, I should note that I had not seen #5973 before implementing this, so I was not developing directly against that ticket or its exact evaluation intent. I implemented LP-MPPI because I was interested in the paper’s idea of low-pass filtering sampled perturbations before rollout, with the expected benefit of reducing high-frequency sampling noise and, hopefully, reducing small jerking in the resulting control commands.

Also, I'm not planning to use it soon. I wanted to test it on our autonomous golf car in the lab later on, but I mainly implemented it thinking it might be a good addition to the library.

But if this is coming from MPPI developers, then definitely I Wil read that paper and compare both and check the difference.

From your comment, I agree the ticket is really about smoothness in the optimal trajectory / inter-iteration velocity behavior, not just output-level control metrics. So I do not want to overclaim that my current benchmark fully answers #5973.
What I can say is that, in my current Gazebo testing, LP-MPPI looked beneficial mainly in success rate and completion time, and in some linear-command smoothness metrics, but I did not get clear evidence yet that it consistently improves the exact jitter behavior the ticket is targeting.

So I think the right next step is to reframe the evaluation around that specific intent.
So, to keep it simple, if we agree on this, next I will evaluate my implementation over ticket intent, check the mentioned paper, and probably implement it to compare both.

If it performs better, I may open a new pull request with it.

[SteveMacenski]

I wanted to test it on our autonomous golf car in the lab later on, but I mainly implemented it thinking it might be a good addition to the library.

Always appreciated and thanks for thinking of contributing!

Please do give that paper a glance and let me know what you think.

What I can say is that, in my current Gazebo testing, LP-MPPI looked beneficial mainly in success rate and completion time

These I'm a little confused about since I never see a 15%+ failure rate of MPPI on missions. I'm also not sure how these changes would have such an impact. Completion time, I could see in some situation if its smoother and/or biases samples to higher speeds (metrics support that?).

A metric like "32.69s to 30.20s" from 5x runs also is close enough that I'd also want to understand the standard deviation on those metrics and if that's reproducable if you run this 5x re-run experiment a couple of more times (i.e. run 5x a second time, then a third). That small of a deviation may not be actually real improvements but just experimental variance.

So, to keep it simple, if we agree on this, next I will evaluate my implementation over ticket intent, check the mentioned paper, and probably implement it to compare both.

Please! As you progress on that, commenting on that ticket on progress or questions would be great 😄

I appreciate this work and especially help in smoothness and more research-oriented improvements in MPPI :-) Its often difficult for me to find contributors with the technical chops to contribute in such matters so its highly appreciated

[SteveMacenski]

Any word? :-)

[mohamedsamirx]

Any word? :-)

I'm sorry for taking so long, I got distracted with work.

I've implemented the algorithm, but I'm unsure if I'm validating it correctly since the simulation isn't fully set up yet.

If possible, I'd like to push the implementation (based on the paper) and get a review before continuing with testing. That would be very helpful.

[SteveMacenski]

I think you should test it before I review in detail (since not worth much to review the code if the code doesn't help), but feel free to push :-)

I'm doing something related right now so maybe I will be able to test the changes on my end (but no promises on timeline)