Proposal: FFT analysis for Notch filter for Gyro

Daniel_Frenzel · July 26, 2018, 11:33am

Do you think it is worth to get this implemented? Theoretically, this could replace the need to set a fixed frequency…

khancyr · July 26, 2018, 12:11pm

Hello,

How do you want to do this ? With an external program or during a calibration phase like autotune ?

Daniel_Frenzel · July 26, 2018, 12:31pm

I have the feel, Betaflight runs a FFT while copter is in the air. ATM I do not think that a FFT would be very costly. You anyway have to take just a couple of samples and average the results of the FFT a bit to get the frequencies you want…

khancyr · July 26, 2018, 12:54pm

I think it could be valuable as a new autotune phase… doing it continuously doesn’t make sense for me … but maybe I am wrong.

Daniel_Frenzel · July 26, 2018, 1:51pm

No this was not the intention.

Daniel_Frenzel · August 2, 2018, 11:27am

Well, I hoped for a little discussion, but no discussion is an answer too

bnsgeyer · August 2, 2018, 11:58am

Daniel,
So what you are saying is that the FFT would run continuously in the background and update the notch frequency every so many seconds. I have used the new notch filter but I’m not sure if it requires a reboot for a new center frequency. The other question remains as to how to pick the frequency. Would it always be used and choose the highest peak? Would you put a floor on it so below a certain db the notch would not be used?

Do you consider the notch a way to filter noise or are you more concerned with feedback instabilities? … All of the above?

Regards,
Bill

Daniel_Frenzel · August 3, 2018, 11:33am

What I would expect would be this:

Control signals must be <= 50 Hz
Motor noise will be at around 200 to 500 Hz
Beyond 500 Hz noise decreases to ± nothing

Consequently, we have to just find the maximum frequency (for motor noise) in a range of ~200 to 500 Hz. The FFT algorithm seems not so super intense and it is not necessary to let it run in every cycle. I think this could maybe make sense, because the Motor noise frequency is certainly dependent on the motor class, propeller and voltage.
What is btw the current sampling and loop rate in copter?

Q/A)

Would it always be used and choose the highest peak?

Yes, for the ~ 200 to 500 Hz range

Would you put a floor on it so below a certain db the notch would not be used

I think we currently use a notch already?! I further think it makes no sense to disable the filter, because in this range there will be noise as long as the motors run.

Do you consider the notch a way to filter noise or are you more concerned with feedback instabilities? … All of the above?

I just think it will improve the gyro data if motor noise is cancelled out as good as possible by dynamically adjusting the frequency. Furthermore the user will not be in need to manually tuning the filter.

bnsgeyer · August 3, 2018, 1:23pm

I’m pretty sure that a notch is NOT used by default for gyro signals. A low pass filter is defaulted for 20 hz for both gyros and accels.

I believe the gyro data is available at 1K hz. the control loops are being run at 400hz.

Daniel_Frenzel · August 3, 2018, 2:48pm

I was just briefly checked, and thought because of that line there is a notch filter constantly running.

github.com

ArduPilot/ardupilot/blob/master/libraries/AP_InertialSensor/AP_InertialSensor.cpp#L1334


      }
      for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
          if (_accel_healthy[i] && _use[i]) {
              _primary_accel = i;
              break;
          }
      }
  }


  // apply notch filter to primary gyro
  _gyro[_primary_gyro] = _notch_filter.apply(_gyro[_primary_gyro]);
  
  _last_update_usec = AP_HAL::micros();
  
  _have_sample = false;
}


/*
wait for a sample to be available. This is the function that
determines the timing of the main loop in ardupilot.

Is there still work going on to enhance the sample rates to maybe 1k/1k or 1k/2k?

bnsgeyer · August 3, 2018, 4:07pm

only if you enable it using the INS_Notch_enable parameter. It is not running by default.

Not sure what you mean by 1K/1K and 1k/2k. Is that the data logging rate or control loop rate/gyro sample rate? I think they are trying to make it so the control loop rates can be upwards of 1khz. I’m not involved in this part of the code. My focus is traditional heli flight controls. However I have interest in the filtering because many times traditional heli sees feedback instabilities. So the notch filter is used to attenuate the offending frequency band in the hopes of achieving a better tuned aircraft.