Hello,
I’m still wondering why there is no low pass filter on the gyroscope and accelerometer before the EKF. There is this old topic for which I’m not convinced.
Specifically, I see many accelerometer vibrations since v1.14 which get fed by the position controller D term into the roll and pitch setpoint. Like this log for instance. The roll setpoint has +/-1.5 deg of high frequency oscillations.
I tried implementing a low pass filter in the accelerometer driver itself, before publishing sensor_accel, and this reduced considerably this high frequency oscillations on the roll and pitch setpoints.
My questions are
- Is it dangerous / not recommended to low pass filter the accelerometer before the EKF, and why?
- if it is improving and does not mess up the EKF, why not adding this filter? (I’m happy to create a PR)
- which other solutions do we have to use a clean MPC_D term?
1 Like
Adding a pre-filter introduces phase delay and eliminates information. This will reduce the effectiveness and accuracy of the EKF.
The EKF should receive the information from the sensors without pre-processing. If there is excessive high-frequency noise in the output of the EKF due to high frequency noise on the measurement, this would indicate poor tuning and likely a too-small variance estimate on the measurement leading to overconfidence on that sensor’s accuracy.
How is this related to the existing filters (see docs)?
First, thanks a lot for your replies.
@oursland are you suggesting to increase EKF2_ACC_NOISE ?
I understand the problem of the phase delay.
In my understanding, for a given vehicle with a step response of about 0.1s on the roll rate (𝜏=0.02), any frequency above f=1/(2.𝜋.𝜏)=8Hz is not wanted in the controller. We take 5 times this frequency to ensure a small enough phase delay: anything above 40Hz can be filtered. This information is not wanted in the controller. Everyone agree so far?
- Is there any danger of low pass filtering the acceleration before the EKF? For instance, is there a risk of the EKF diverging the attitude estimation?
I made a diagram of the IMU signal pipeline from looking directly into the code in v1.15.4, (Please point me the code line if I’m wrong with anything).
A few comments
The cutoff frequency for the 2nd order butterworth filter on the primary accelerometer. This only affects the signal sent to the controllers, not the estimators. 0 disables the filter.
- As you see in the diagram, the filtered signal vehicle_acceleration_s is not sent to the controller (to the Rover controller yes, not to the multicopter controller). So, if we cannot filter the EKF for stability reasons, can we send the vehicle_acceleration_s to the position controller instead of doing a discrete differentiation on the velocity? Taking the derivative of a high frequency noise signal will amplify this noise. Why not taking (after proper rotation) the filtered acceleration?
Digging in the code, it seems the discrete differentiation of the local inertial velocity into local inertial acceleration is equipped with a first order low pass filter. On which when I call getLP() returns me 5… 5Hz I imagine. Any idea where this is initialized, and how I can change this cutoff frequency?
@oursland are you suggesting to increase EKF2_ACC_NOISE ?
Yes.
In general a KF is itself a low-pass filter. We can observe this if reduce system to a single-variable that is being filtered, in which the KF simplifies to a sort of IIR low-pass filter. In that case we have the two tunable values: the process noise in which the lower bound is derived from time-delay but may be increased it to account for other sources of process noise (a common approach to ensure positive definiteness of the covariance matrix), and the measurement noise which is an estimate the noise present in the sensor.
By increasing the measurement noise estimate, we’re effectively reducing the co-efficient on the current measurement and increasing the co-efficient on the prior measurement, or increasing the filter cutoff frequency. Reducing the measurement noise estimate reduces the filter cutoff frequency.
There’s a little more discussion on this with the math here: Is the Kalman filter a low-pass filter? Sometimes!
Naturally the EKF is model-based multivariate adaptive low-pass filter and suitable for applications that a standard single variable low-pass filter cannot be. But you can (and must) tune these parameters to get the desired low-pass functionality out of a KF.
Maybe @bresch can comment.
Thanks @oursland for the article.
As I understand increasing EKF2_ACC_NOISE with lower the cutoff frequency of the EKF. So the high frequencies will be more smoothed out. However this parameter is clipped (hard coded) to 1.0 m/s/s. And checking my log
the standard deviation of the accelerometer noise is closer to let’s say 5 m/s/s. Although this value of 5 is the optimal, I’m a bit concerned the resulting “cutoff frequency” will be too low for the controller to stabilize the vehicle. Maybe someone knows why this parameter is clipped to 1.0?
I’m aware this (100+ kg) vehicle has a lot of noise. I believe the controller can operate with high frequency noise once it is filtered.
Regarding the position controller, the D term multiplies the derivative of the velocity. So I added a 2nd order low pass filter in mc_pos_control to smooth out the velocity before taking its derivative. That seems to work well and respected the idea of avoiding filtering before the EKF 
Is that something worth doing a PR upstream?
Digging in the code, it seems the discrete differentiation of the local inertial velocity into local inertial acceleration is equipped with a first order low pass filter. On which when I call getLP() returns me 5… 5Hz I imagine. Any idea where this is initialized, and how I can change this cutoff frequency?
- I found the answer to this sub-question, this is MPC_VELD_LP
All inputs to digital filtering should have analog pre-filtering. As someone who is also working on a 100+ kg vehicle, the violent high magnitude vibrations must be filtered out before it is observed by the IMU. This is performed via appropriate mechanical isolation, which could be performed with adequate vibration isolating mounting foam or by more complicated solutions such as wire rope isolators.
Software filtering is the option of last resort. There are several reasons for this, including the inability to filter a saturated sensor, troubles involving signal-to-noise ratio, and aliasing.
The documentation notes this as well: Vibration Isolation | PX4 Guide (main)
1 Like
Thanks for the inputs. We’re now investigating mechanical vibration isolators.
