Change 2nd order Butterworth low pass filter to 1st order #1483

SansoneG · 2019-05-27T11:32:51Z

Description

Fixes Issue #1471 about filter overshooting by changing the 2nd order Butterworth low pass filter to 1st order.

welcome · 2019-05-27T11:32:54Z

Thanks for helping in improving MoveIt!

moveit_experimental/jog_arm/src/jog_arm/low_pass_filter.cpp

AndyZe

I'd like to check this equation in Matlab before it gets merged.

AndyZe · 2019-05-27T16:34:16Z

moveit_experimental/jog_arm/include/jog_arm/low_pass_filter.h

-  double previous_measurements_[3] = { 0., 0., 0. };
-  double previous_filtered_measurements_[2] = { 0., 0. };
+  double previous_measurements_[2] = { 0., 0. };
+  double previous_filtered_measurement = 0.;


Should be previous_filtered_measurement_ (the underscore indicates it's a member of the class)

SansoneG · 2019-05-28T05:45:51Z

I'd like to check this equation in Matlab before it gets merged.

Once that is done, I can also change the comment in the header file to say that it is indeed a first order Butterworth filter.

felixvd · 2019-06-21T07:21:50Z

Why was this originally a 2nd order filter? Has anyone looked into the history and can explain?

AndyZe · 2019-06-21T14:24:33Z

It was just re-used code from an older project. I haven't forgotten to test this...

AndyZe · 2019-06-26T20:08:47Z

moveit_experimental/jog_arm/src/jog_arm/low_pass_filter.cpp

-      (previous_measurements_[2] + 2. * previous_measurements_[1] + previous_measurements_[0] -
-       (filter_coeff_ * filter_coeff_ - 1.414 * filter_coeff_ + 1.) * previous_filtered_measurements_[1] -
-       (-2. * filter_coeff_ * filter_coeff_ + 2.) * previous_filtered_measurements_[0]);
+      (1. / (1. + filter_coeff_)) * (previous_measurements_[1] + previous_measurements_[0] -


Should be (Matlab syntax):

new_filtered_msrmt = (1 + filter_coeff_ / (2pi))^-1 * (previous_measurements_[1] + previous_measurements_[0] - (1 - filter_coeff_/(2pi)) * previous_filtered_measurement_);

AndyZe · 2019-06-26T20:10:31Z

OK, I tested a 1st-order Butterworth filter with Matlab. Made a suggestion on the equations.

gavanderhoorn · 2019-06-26T20:11:28Z

Would it perhaps be an idea to use the filters package here?

AndyZe · 2019-06-26T20:16:52Z

Would it perhaps be an idea to use the filters package here?

I see that a TransferFunctionFilter is available. That could work. Making it configurable to different cutoff frequencies might be tricky.

AndyZe · 2019-07-05T20:32:25Z

@SansoneG don't forget to do this:

Once that is done, I can also change the comment in the header file to say that it is indeed a first order Butterworth filter.

AndyZe

Tests well with the equation in my comment. I'll approve if you make those small changes to the equation and update the description in the header. Thanks!

AndyZe · 2019-07-05T20:40:31Z

moveit_experimental/jog_arm/src/jog_arm/low_pass_filter.cpp

-       (filter_coeff_ * filter_coeff_ - 1.414 * filter_coeff_ + 1.) * previous_filtered_measurements_[1] -
-       (-2. * filter_coeff_ * filter_coeff_ + 2.) * previous_filtered_measurements_[0]);
+      (1. / (1. + filter_coeff_ / (2 * M_PI))) * (previous_measurements_[1] + previous_measurements_[0] -
+                                     (-1. * filter_coeff_ / (2 * M_PI) + 1.) * previous_filtered_measurement_);


No need to multiply by negative 1, just put the negative sign:

(1. / (1. + filter_coeff_ / (2 * M_PI))) * (previous_measurements_[1] + previous_measurements_[0] -
(-filter_coeff_ / (2 * M_PI) + 1.) * previous_filtered_measurement_);

I guess we might as well combine filter_coeff_ with the 2*M_PI constant, right? Save a couple extra computations (like you had it originally - my bad):

(1. / (1. + filter_coeff_)) * (previous_measurements_[1] + previous_measurements_[0] -
(-filter_coeff_ + 1.) * previous_filtered_measurement_);

I tested carefully with this equation. Did not see any issues:

double new_filtered_msrmt = (1. / (1. + filter_coeff_)) * (previous_measurements_[1] + previous_measurements_[0] - (-filter_coeff_ + 1.) * previous_filtered_measurement_);

Top graph is the input to the jogger from my joystick, bottom is the outgoing command to the robot.

^Note, different timescales on the 2 plots

…ilter

SansoneG · 2019-07-08T06:05:02Z

I was not sure if you meant i should move the constant into the constructor (so the computation is done only once instead of each iteration) or remove it completely (maybe it serves the purpose of giving the filter_coeff_ a certain unit). For now I decided to remove it completely.

AndyZe

I tested it locally again, seems fine. Thanks a lot!

(I'm just a "core contributor" though, a maintainer needs to approve it before merging.)

AndyZe · 2019-07-20T19:07:42Z

@SansoneG Can you assign a few code reviewers so we can finally get this merged? It's a button in the top right that looks like a gear. Just pick a few MoveIt maintainers at random - I'm not aware of any who have experience with digital filtering. Here's a list to pick from.

To summarize, the benefits of this PR are:

Slightly more efficient (in computation and memory)
No overshoot
There's a slower "roll off" near the cutoff frequency, which is good because we don't want a drastic change in behavior.

SansoneG · 2019-07-22T07:08:40Z

Unfortunately that button is not present on my end. I think the reason for that is stated in this StackOverflow answer.
I will just have to mention them here:
@gavanderhoorn, @BryceStevenWilley, @davetcoleman Could one of you review this pull request please?

moveit_experimental/jog_arm/include/jog_arm/low_pass_filter.h

jschleicher · 2019-08-06T11:31:54Z

@BryceStevenWilley, @felixvd awaits2ndreview please.

I'd appreciate, if this finally gets merged.

welcome · 2019-08-07T12:12:41Z

Congrats on getting your first MoveIt pull request merged and improving open source robotics!

AndyZe · 2022-04-27T14:51:45Z

I still get asked about this from time to time. Here's the MATLAB code for the filter:

function output = first_order_filter(c, input)
  % c: filter parameter related to cutoff frequency and sampling rate. Smaller c -->
  % higher cutoff frequency, less filtering
  % input: a vector of input data
  % returns output: a vector of output data

  % Preallocate an array for filter output
  output = zeros(size(input));

  for i=2 : length(input)
      output(i) = (1 + c / (2*pi))^-1 * (input(i) + input(i-1) - (1 - c/(2*pi)) * output(i-1));
  end

Add jog_arm with 1st order Butterworth low pass filter

a0c3595

SansoneG mentioned this pull request May 27, 2019

jog_arm first order low-pass filter #1471

Closed

jschleicher reviewed May 27, 2019

View reviewed changes

moveit_experimental/jog_arm/src/jog_arm/low_pass_filter.cpp Show resolved Hide resolved

SansoneG added 2 commits May 27, 2019 15:20

reduce preallocation requiredfor buffers

855b28a

Clang format

ec253a0

AndyZe reviewed May 27, 2019

View reviewed changes

Fix missing underscore for member variable

1d2a20b

AndyZe requested changes Jun 26, 2019

View reviewed changes

Introduce new factor for filter coefficient.

b1fcc16

AndyZe reviewed Jul 5, 2019

View reviewed changes

SansoneG added 2 commits July 8, 2019 08:00

Edit comment about the class being a 1st order Butterworth low pass f…

8298d38

…ilter

Remove constant 2*pi from filter_coeff_

86a0065

AndyZe approved these changes Jul 15, 2019

View reviewed changes

davetcoleman approved these changes Jul 22, 2019

View reviewed changes

moveit_experimental/jog_arm/include/jog_arm/low_pass_filter.h Show resolved Hide resolved

SansoneG added 2 commits July 24, 2019 08:30

Add TODO suggesting to use ROS filters package

5c6d843

Clang format

01b07a9

rhaschke approved these changes Aug 7, 2019

View reviewed changes

rhaschke merged commit bd3a239 into moveit:master Aug 7, 2019

tylerjw mentioned this pull request May 8, 2020

July 2020 Melodic Release #2066

Closed

20 tasks

tylerjw pushed a commit to PickNikRobotics/moveit that referenced this pull request May 12, 2020

Change 2nd order Butterworth low pass filter to 1st order (moveit#1483)

83af13e

tylerjw pushed a commit to PickNikRobotics/moveit that referenced this pull request May 20, 2020

Change 2nd order Butterworth low pass filter to 1st order (moveit#1483)

9b6bfc1

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Change 2nd order Butterworth low pass filter to 1st order #1483

Change 2nd order Butterworth low pass filter to 1st order #1483

SansoneG commented May 27, 2019

welcome bot commented May 27, 2019

AndyZe left a comment

AndyZe May 27, 2019 •

edited

Loading

SansoneG commented May 28, 2019

felixvd commented Jun 21, 2019

AndyZe commented Jun 21, 2019

AndyZe Jun 26, 2019 •

edited

Loading

AndyZe commented Jun 26, 2019

gavanderhoorn commented Jun 26, 2019

AndyZe commented Jun 26, 2019

AndyZe commented Jul 5, 2019

AndyZe left a comment

AndyZe Jul 5, 2019 •

edited

Loading

AndyZe Jul 5, 2019

AndyZe Jul 5, 2019

SansoneG commented Jul 8, 2019

AndyZe left a comment

AndyZe commented Jul 20, 2019

SansoneG commented Jul 22, 2019

jschleicher commented Aug 6, 2019

welcome bot commented Aug 7, 2019

AndyZe commented Apr 27, 2022

Change 2nd order Butterworth low pass filter to 1st order #1483

Change 2nd order Butterworth low pass filter to 1st order #1483

Conversation

SansoneG commented May 27, 2019

Description

welcome bot commented May 27, 2019

AndyZe left a comment

Choose a reason for hiding this comment

AndyZe May 27, 2019 • edited Loading

Choose a reason for hiding this comment

SansoneG commented May 28, 2019

felixvd commented Jun 21, 2019

AndyZe commented Jun 21, 2019

AndyZe Jun 26, 2019 • edited Loading

Choose a reason for hiding this comment

AndyZe commented Jun 26, 2019

gavanderhoorn commented Jun 26, 2019

AndyZe commented Jun 26, 2019

AndyZe commented Jul 5, 2019

AndyZe left a comment

Choose a reason for hiding this comment

AndyZe Jul 5, 2019 • edited Loading

Choose a reason for hiding this comment

AndyZe Jul 5, 2019

Choose a reason for hiding this comment

AndyZe Jul 5, 2019

Choose a reason for hiding this comment

SansoneG commented Jul 8, 2019

AndyZe left a comment

Choose a reason for hiding this comment

AndyZe commented Jul 20, 2019

SansoneG commented Jul 22, 2019

jschleicher commented Aug 6, 2019

welcome bot commented Aug 7, 2019

AndyZe commented Apr 27, 2022

AndyZe May 27, 2019 •

edited

Loading

AndyZe Jun 26, 2019 •

edited

Loading

AndyZe Jul 5, 2019 •

edited

Loading