Add custom window size and poly order for SG filter

[mini-1235]

---

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

[codecov]

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?

[SteveMacenski]

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

[mini-1235]

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

[SteveMacenski]

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]

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

[SteveMacenski]

LGTM - just add that blog link please!