Add custom window size and poly order for SG filter by mini-1235 · Pull Request #5489 · ros-navigation/navigation2

mini-1235 · 2025-08-27T17:19:49Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5483
Primary OS tested on	Ubuntu
Robotic platform tested on	My robot + tb4 simulation
Does this PR contain AI generated software?	No
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

Added window size and poly order for SG filter
Fix smoother benchmark tools

Description of documentation updates required from your changes

Added new parameter, so need to add that to default configs and documentation page

Description of how this change was tested

Tested in real world + sim + benchmark

Future work that may be required in bullet points

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-*.

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

codecov · 2025-08-27T17:44:50Z

Codecov Report

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

Files with missing lines	Patch %	Lines
nav2_smoother/src/savitzky_golay_smoother.cpp	96.29%	1 Missing ⚠️

Files with missing lines	Coverage Δ
.../include/nav2_smoother/savitzky_golay_smoother.hpp	`100.00% <ø> (ø)`
nav2_smoother/src/savitzky_golay_smoother.cpp	`98.50% <96.29%> (-1.50%)`	⬇️

... and 6 files with indirect coverage changes

🚀 New features to boost your workflow:

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

SteveMacenski

It generally looks good to me. Did you verify the output of the coefficients for a few examples of input order and window size & make sure that the changes are outputting the same values?

nav2_smoother/src/savitzky_golay_smoother.cpp

SteveMacenski · 2025-08-27T22:01:30Z

nav2_smoother/src/savitzky_golay_smoother.cpp

+  calculateCoefficients();
+}
+
+void SavitzkyGolaySmoother::calculateCoefficients()


A citation for this equation would be good so I can validate against that & for future readers

Is citing a blog possible? I think this blog https://www.colmryan.org/posts/savitsky_golay/ explains my approach here

I also added some comments in the latest commit

Yeah, that's OK! Just something someone can later look at and reason about why this was done the way it is in case they think its incorrect or want to learn

mini-1235 · 2025-08-28T15:23:42Z

It generally looks good to me. Did you verify the output of the coefficients for a few examples of input order and window size & make sure that the changes are outputting the same values?

I did. For example, in the original code we have window_size 7 and polynomial order of 3

navigation2/nav2_smoother/src/savitzky_golay_smoother.cpp

Lines 107 to 114 in 2b8cc4d

    
           const std::array<double, 7> filter = { 
        
             -2.0 / 21.0, 
        
             3.0 / 21.0, 
        
             6.0 / 21.0, 
        
             7.0 / 21.0, 
        
             6.0 / 21.0, 
        
             3.0 / 21.0, 
        
             -2.0 / 21.0};

I threw a log in my code

[component_container_isolated-7] sg coeffs -0.0952381
[component_container_isolated-7]   0.142857
[component_container_isolated-7]   0.285714
[component_container_isolated-7]   0.333333
[component_container_isolated-7]   0.285714
[component_container_isolated-7]   0.142857
[component_container_isolated-7] -0.0952381

Another example is mppi's sg filter, we have window size 9 and an order of 2

navigation2/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

Lines 516 to 519 in 2b8cc4d

    
           // Savitzky-Golay Quadratic, 9-point Coefficients 
        
           Eigen::Array<float, 9, 1> filter = {-21.0f, 14.0f, 39.0f, 54.0f, 59.0f, 54.0f, 39.0f, 14.0f, 
        
             -21.0f}; 
        
           filter /= 231.0f;

[component_container_isolated-7] sg coeffs -0.0909091
[component_container_isolated-7]  0.0606061
[component_container_isolated-7]   0.168831
[component_container_isolated-7]   0.233766
[component_container_isolated-7]   0.255411
[component_container_isolated-7]   0.233766
[component_container_isolated-7]   0.168831
[component_container_isolated-7]  0.0606061
[component_container_isolated-7] -0.0909091

I think they are identical

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

SteveMacenski · 2025-08-28T17:11:50Z

LGTM - just add that blog link please!

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

) * Add custom window size and poly order in SG filter Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix linting and benchmark tool Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Cmakelist Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Adding comments Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add blog Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> --------- Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

) * Add custom window size and poly order in SG filter Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix linting and benchmark tool Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Cmakelist Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Adding comments Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add blog Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> --------- Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> Signed-off-by: Abhishekh Reddy <helloarm@pm.me>

) * Add custom window size and poly order in SG filter Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix linting and benchmark tool Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Cmakelist Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Adding comments Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add blog Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> --------- Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

mini-1235 added 3 commits August 24, 2025 19:57

Add custom window size and poly order in SG filter

6fdafec

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

Fix linting and benchmark tool

903548a

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

Cmakelist

bd924ea

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

SteveMacenski requested changes Aug 27, 2025

View reviewed changes

SteveMacenski linked an issue Aug 27, 2025 that may be closed by this pull request

Custom window size and poly order for SG filter #5483

Closed

mini-1235 mentioned this pull request Aug 28, 2025

Add custom window size and poly order for SG filter ros-navigation/docs.nav2.org#769

Merged

Adding comments

13246e2

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

SteveMacenski approved these changes Aug 28, 2025

View reviewed changes

Add blog

85389bf

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

SteveMacenski merged commit 3c081db into ros-navigation:main Aug 28, 2025
10 of 12 checks passed

mini-1235 deleted the sg_filter branch August 28, 2025 17:31

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add custom window size and poly order for SG filter#5489

Add custom window size and poly order for SG filter#5489
SteveMacenski merged 5 commits intoros-navigation:mainfrom
mini-1235:sg_filter

mini-1235 commented Aug 27, 2025

Uh oh!

codecov bot commented Aug 27, 2025 •

edited

Loading

Uh oh!

SteveMacenski left a comment

Uh oh!

Uh oh!

SteveMacenski Aug 27, 2025

Uh oh!

mini-1235 Aug 28, 2025 •

edited

Loading

Uh oh!

SteveMacenski Aug 28, 2025 •

edited

Loading

Uh oh!

mini-1235 commented Aug 28, 2025

Uh oh!

SteveMacenski commented Aug 28, 2025

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Conversation

mini-1235 commented Aug 27, 2025

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

Future work that may be required in bullet points

For Maintainers:

Uh oh!

codecov bot commented Aug 27, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

SteveMacenski Aug 27, 2025

Choose a reason for hiding this comment

Uh oh!

mini-1235 Aug 28, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Aug 28, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

mini-1235 commented Aug 28, 2025

Uh oh!

SteveMacenski commented Aug 28, 2025

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

codecov bot commented Aug 27, 2025 •

edited

Loading

mini-1235 Aug 28, 2025 •

edited

Loading

SteveMacenski Aug 28, 2025 •

edited

Loading