Custom window size and poly order for SG filter #5483
Description
Feature request
Feature description
In sg filter we have fixed window size 7 and a polynomial order of 2/3, we can instead calculate the coefficient in the configure stage so that we can have custom window size and polynomial order
Implementation considerations
I have tried to implement this and test with the smoother benchmark, I am using nav2's default config for planner, smoother and costmap, result:
Before(latest commit)
-------------------- --------------------- ------------------ ----------------- -------- ---------------------- -----------------------
Planner Time (s) Path length (m) Average cost Max cost Path smoothness (x100) Average turning rad (m)
GridBased 0.0033331181099999995 10.616600969059586 9.734400645098237 103.12 52.538887623390416 0.10565008775456532
sg_smoother 0.00032982396 10.58656618445626 9.663083701020799 103.1 25.648998614435047 nan
-------------------- --------------------- ------------------ ----------------- -------- ---------------------- -----------------------
After
(window_size x poly_order)
5x3
sg_smoother 0.00014089780999999998 10.594502602562615 9.683048599989485 103.11 31.540321079714044 nan
7x3(default window size and poly order)
sg_smoother 0.00031711681 10.586566184456258 9.733282376517487 103.1 25.648998614435015 nan
11x3
sg_smoother 0.0003834335 10.57650925067801 9.79461170709219 103.05 23.664865830675694 nan
13x3
sg_smoother 0.00039271377 10.572616927729385 9.885466729220324 102.95 23.559227155651158 nan
7x4
sg_smoother 0.00014826532 10.599816094198873 9.728279924347085 103.11 36.55368509664171 0.15594886625035922
11x4
sg_smoother 0.00015034915 10.588676680921791 9.664317744871411 103.1 26.929345261725746 nan
13x4
sg_smoother 0.00014891237000000003 10.585086935443575 9.69634468372192 103.1 25.890158815954038 nan
I think this is also applicable to MPPI's SG filter, I will test this when I have time