Inconsistent HW filtering on accelerometer and gyroscopes

[andyp1per]

Bug report

Issue details

As part of https://github.com/ArduPilot/ardupilot/pull/11516 I have been investigating the HW filtering we do for the different sensors that we support. For fast sampling it is very inconsistent, with some sensors we do quite aggresive LPF and on others we do very little filtering. I think some of this is because for sensors that only support sampling below 32k we can essentially pick the unfiltered option, but for sensors where the unfiltered option is 32k we pick 8k with a lot of filtering rather than 8k with little. The situation is different for accels - we tend to always pick the unfiltered option if we can. Here's a table of the configs I can get my head around

Chip	Gyro Filter	Sample Rate	Accel Filter	Sample Rate
ICM20608	250	8000	1046	4000
ICM20608	176	1000	218	1000
MPU9250/6500	250	8000	1130	4000
MPU9250/6500	184	1000	184	1000
MPU3050	256	8000	X	X
MPU3050	188	1000	X	X
ICM20649/648/948	12106	9000	1209	4500
ICM20649/648/948	197	1125	246	1125
MPU6000	256	8000	260	1000
MPU6000	188	1000	184	1000

For the ICM 20608 I have experimented with DLPF option 7 (8k sample, 3.6k LPF) and it works great.

For fast sampling I think either we should remove the gyro filtering on the sensors that support it or add filtering to the accel filtering. I prefer the former as it reduces latency and the software filters that we use work fine.

Version

master

Platform [X] All [ ] AntennaTracker [ ] Copter [ ] Plane [ ] Rover [ ] Submarine

Airframe type All

Hardware type Affects boards with Invensense fast sampling e.g. ICM 20608 and MPU 9250

Logs

[guglie]

Interesting, I've read AP_InertialSensor_Invensense::_set_filter_register

https://github.com/ArduPilot/ardupilot/blob/ce53ae63ae3bb03f2d148dd7dee41551436ff014/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp#L675-L689

so if INS_ENABLE_FAST_SAMPLING is enabled for current IMU, the raw rate is set based on SCHED_LOOP_RATE (get_sample_rate_hz()):

• 1000 if loop rate < 1000
• 2000 if 1000 < loop rate < 2000
• 4000 if loop rate > 2000

right?

[andyp1per]

I think this is the rate that we sample the samples. So sampling in hardware at 4k, but then sampling those samples at get_sample_rate_hz()? Maybe?

[lthall]

This is really interesting. Thank you very much for looking into this in such detail!!

The data sheets are not always clear how these filter settings impact the data.

Ideally we would ensure that each sensor has an analogue filter to remove any frequency components at above half the sample rate. So if the filter settings define this filter then they should be set to the highest value unless no value defines a default high frequency nyquist filter.

If we are not consuming the data a full data rate then we would ideally use the onboard digital filter settings to remove any frequency components at above half the applied sample rate.

Assuming that most of these sensors are designed properly, with all the necessary analogue filtering in place, then we would ideally be consuming the data at the full sample rate and applying our own filtering to that data. We would leave all internal digital filters turned off.

Tridge has done a lot of testing here and there may be issues with some of these sensors.

[andyp1per]

Ok @lthall if I take your recommendation I end up with this:

Chip	Gyro Filter	Sample Rate	CFG	Accel Filter	Sample Rate	CFG
ICM20608	3281	8000	7	1046	4000	B
ICM20608	176	1000	1	420	1000	7
MPU9250/6500	3600	8000	7	1130	4000	B
MPU9250/6500	184	1000	1	460	1000	7
MPU3050	256	8000	0	X	X
MPU3050	188	1000	1	X	X
ICM20649/648/948	12106	9000	B?	1209	4500	B
ICM20649/648/948	361	1125	7	473	1125	7
MPU6000	256	8000	0	260	1000	0
MPU6000	188	1000	1	184	1000	1

Does this look reasonable? Only the ICM20649 looks suspicious. Maybe @tridge has an opinion here

[lthall]

Tridge will have to look at this as there may be reasons these settings were not chosen in the first place. He is very busy but I am sure he will look asap.

[priseborough]

We should make changes based on testing with high frequency vibration rather than make assumptions about internal data processing I have used a vibration speaker and a phone signal generator app to test boards in the past. Sensitivity to aliasing varied between sensors. My experience with the internal DLPF filter settings on previous model invensense sensor accel data was that they did not protect from aliasing effects.

Edit: I have not tested the current generation of sensors.

Edit: The other issue with having sensor internal DLPF on for accelerometers is that it makes clipping high frequency clipping harder to detect.

[priseborough]

With respect to the gyro data, we would lose less phase in the attitude rate loops by running the sensor internal DLPF at the highest frequency and relying on the driver second order filter. The reasons I can think of to not do that is if either a) the internal DLPF is a second order filter with equivalent group delay to our driver noise filter or b) there is internal analogue anti-aliasing filtering that is modified by the filter frequency set. I have not seen evidence of b).

[priseborough]

I've attached a couple of photos of the test setup I used in the past on another project that used the fmuv4 sensors. The vibration speak is a 25W model no longer available driven by a low power portable audio amplifier with a signal generator app running on a mobile phone providing the input signal.

I was looking at the effect of vibration on the accel data quality for navigation and this test setup would not be useful for evaluating high the sensitivity of gyro data to angular vibration. However from a navigation perspective 99+ % of the high frequency vibration issues are with accelerometer data.

[priseborough]

Here are some sample results obtained with a 4300Hz low amplitude sine wave input applied during the middle third of the log for the pixracer board. My understanding with this board is that IMU1 is a ICM-20608 and IMU2 is a MPU9250.

There is a significant Z accel offset due to what is likely aliasing. Recorded vibe levels were low with no clipping events:

The first graph shows the effect on the Z accel with fast sampling enabled on IMU1 only.

The fast sampling was then disabled on IMU1 and enabled on IMU2:

This illustrates the behaviour of different sensors. In this example with 4300Hz vibration there is a (Edit: increase) in error with IMU1 when fast sampling is enabled. With IMU2 it made no difference. Different input frequencies achieve different results. It also illustrates why we cannot rely on analogue anti-aliasing filtering with this class of sensor and why vibration isolation remains important. 4300Hz may seem like a high value, but can be produced by motor torque ripple, gear tooth mesh, ESC PWM and ESC commutation.

Without testing at a range of frequencies we will be making changes based on anecdotal evidence that A worked better than B on setup C, which may not be best across the range of use cases.

[andyp1per]

Paul, your findings are possibly somewhat consistent with the specs of the sensor. The noise bandwidth for the ICM20608 is spec'd as quite large when picking the "unfiltered" option whereas for the MPU9250 it's constant for all configurations. One thing that mystifies me is that when we configure fast sampling for accel's we are "bypassing" the DLPF, and yet there is still a 3db cutoff documented. I wonder whether there are actually two filters.

[andyp1per]

http://home.earthlink.net/~schultdw/logger/mpu9250/index.html

[andyp1per]

I've also discovered that we apply a one-pole software low-pass filter at 188Hz on both accel and gyro for the Invensense sensors. @tridge, @lthall why do we do this? We seem to have a lot of different filtering pieces which add up to a mish-mash of overall filters which makes it difficult to know the outcome/tune. Shouldn't this filter be closer to the nyquist frequency?

[andyp1per]

Hi @priseborough @lthall and @tridge I now have a good setup for testing this - a KakuteF7 on a vibration speaker:

I have tried various experiments. The first is with fast sampling, the HW LPF set to 4K, the SW LPF set to 495Hz (about the highest I can safely set it without Nyquist issues) and the 188Hz sample filter removed - so I am just averaging over 8 samples. I ran tones at 200Hz, 400Hz and 4125Hz. For gyros the 200Hz and 400Hz signals are very clear, the 4125Hz has no effect. Clearly our software LPF is easily good enough here:

For accels the story is a little different (this is with stock accel filtering with fast sampling):

There is clearly the Z offset due to the 4125Hz signal visible. However if I switch off fast sampling the offset is still obvious

Note that there is not really any associated frequency component, just the error:

So I conclude that the changes I am proposing are safe.

Note that the EKF currently gets an implicit 188Hz LPF in software in the sampling and a 1K LPF in HW (even though it then takes delta angles from these). This makes no sense to me - I think the EKF should take the output of the regular SW LPF (and notch that @guglie is trying to add) and get rid of the sampling filter.

[IamPete1]

Any updates?

[andyp1per]

We removed our software LPF on gyros. I still stand by my results here - we could - and probably should - change the HW filtering options.

[priseborough]

Provided high rate, ie 4kHz data is used, it makes no sense for the any additional digital filtering to be applied to the samples before the trapezoidal integration is applied for the down conversion to delta angles or delta velocities at the loop rate that the EKF uses. What is important to the EKF is an equivalent group delay for gyro and accel data (including the group delay of the sensor) and protection from aliasing.

[priseborough]

Because the HW filters on our IMU's do not protect from aliasing and are low order, I vote for not using them where we can sample the sensor at a high >1kHz rate and adjust the SW filtering in the driver. This will give a better phase loss to noise tradeoff.

[andyp1per]

So @priseborough that would argue in favour of removing the remaing software filter on accels in AP_InertialSensor_Invensense?

Also, pretty sure the different group delay between gyros with notch filtering and accels is what makes flowhold with the notch filter problematic. Maybe we should be dynamically adjusting the accel group delay based on gyro filtering?