Impact of Low-pass Filtering for System-ID

[jgoppert]

I'm currently working on automated system ID/ control tuning for Firmware version 1.6. On the new firmware, our logger sampling rate is around 250 Hz for gyros and accels and the low pass filtering on the sensors has been reduced in order to improve our estimation. The issue is that the real sampling rate is around 1 kHz and we are aliasing the logged output by sampling it at 1/10 the frequency without filtering. So for system ID this is pretty horrible, but it is also nice to see the raw noise levels of the sensor before filtering.

EDIT: After looking more closely at the code I noticed we are always using the integrated imu data at full rate so aliasing was not a problem. We do need to make this more obvious in the code via comments. It is not clear that the integral fields are the only ones being used for estimation and control at first glance.

Does anyone have any good ideas for this?

I have several ideas:

1. One solution would be to just use the values from the attitude estimators and tune them so that the noise levels are appropriate. And then using the estimated roll/pitch/yaw rates, instead of gyro roll/pitch/yaw rates for system ID. The downside is that you have to have a well tuned estimator for this to work and you give up the ability to estimate the estimator lag.

2. The other would be to add a switch to enable a low pass filter in the sensor module, only for logging, that low passes the sensor data before it sends it to be logged for when you want to do system identification. The downside here is you might introduce a bit of lag by low pass filtering at the nyquist frequency of 130/2 = 65 Hz.

3. The third option would be to enable an increased sampling rate for system ID that focuses on the imu data and maybe lowers the rate for other topics. I'm just not sure how fast we can log effectively.

Let me know what you think is the best approach.

@LorenzMeier, @priseborough, @bkueng, @mhkabir, @dagar, @tsc21 @kd0aij

[mhkabir]

I think @CarlOlsson was also thinking along the lines of number 3. Personally, I prefer that way too : https://github.com/PX4/Firmware/issues/7416

[jgoppert]

So no major impact of just setting it to 1200 Hz in flight? How does cpu hold up?

Is there any throttling on the rate that it sends to the attitude estimator. I looked through the code and it seems to just take the output rate of the driver, so this could be bad.

[jgoppert]

The filtering doesn't make much sense to me in the mpu9250. @Stifael @bkueng can you help clarify.

If I'm reading it correctly, there would be no point in sampling higher that 60 Hz, 2 * low pass filter cut off.

We are sampling at 280 Hz, https://github.com/PX4/Firmware/blob/master/src/drivers/mpu9250/mpu9250.h#L185

low pass filtering at 41 Hz on the device, https://github.com/PX4/Firmware/blob/master/src/drivers/mpu9250/mpu9250.h#L191

then low pass filtering again in software at 30 Hz? https://github.com/PX4/Firmware/blob/master/src/drivers/mpu9250/mpu9250.cpp#L152

I would think we would sample at 280 Hz, and low pass filter onboard at the nyquist frequency of 140 Hz, which the closest option in the driver is BITS_DLPF_CFG_92HZ.

Or just leaving it at 41 Hz, would be good for the log, since there should be no frequency above 41 Hz, so definetly no point in logging at 1200 Hz, then 100 Hz would be plenty.

[priseborough]

Ideally we should turn off the sensors internal DLPF, sample at 4kHz (or whatever the maximum is that the device can do), downsample to 250Hz for inertial navigation using trapezoidal integration in the driver to the delta angles and velocities. This will enable us to detect clipping which we are currently unable to do because it is masked by the devices internal filtering

For the control loop feedback quantities, apply a second order DLPF with a damping ratio of 0.7 (flat pass band with minimum phase loss) to calculate a rate and acceleration before downsampling to 250Hz.

[jgoppert]

That sounds like a good proposal to me. We just need to figure out how fast we can manage to sample it. At 4 kHz we may have some issues with the older boards, so maybe we could just make that a board parameter.

[priseborough]

Are we using the FIFO buffer on the MPU9250?

[jgoppert]

It looks like it is to me. So yeah, that could work. We would just need to rip out the filtering and put in the integration.

[jgoppert]

The data sheet says the fifo is accessible via serial, not sure what that means for i2c.

[priseborough]

I think it is only available via SPI, not I2C. I2C is a bad interface to use for these sensors. How many boards would using I2C? hopefully not many.

[nicolaerosia]

@jgoppert could you also take a look if the same issue applies to https://github.com/PX4/Firmware/blob/master/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp and https://github.com/PX4/DriverFramework/tree/master/drivers/mpu9250 ?

[jgoppert]

@nicolaerosia as far as I can tell it is using the same math here: https://github.com/PX4/Firmware/blob/master/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp

This looks different, and closer to what we are suggesting: https://github.com/PX4/DriverFramework/blob/master/drivers/mpu9250/MPU9250.cpp

Why are there so many versions? Which one is currently used on the pixhawk2?

[Stifael]

@bkueng, can you comment? I haven't really looked into the accel/gyro filtering, and to me it makes sense to apply a critical damped DLPF as @priseborough suggested.

[priseborough]

My higher level objectives were to:

• Separate the signal processing chains for autopilot control feedback and inertial navigation so we can use the most appropriate filtering and downsampling technique for each. Inertial navigation is best served by doing high rate integration on the raw data data. For autopilot control feedback, we want to filter motor/prop vibration to acceptable levels with minimal added phase/group delay.

• Expose the raw sensor measurements so that clipping can be detected reliably. This is a significant pain point for many system integrators.

• Expose the raw sensor measurements so that in the future we have the option of adding a special raw data logging mode that can be used to do spectral analysis of vehicle vibration to help with isolation mount design and trouble shooting. This is a significant pain point for many system integrators.

[bkueng]

@jgoppert How did you get the 130Hz? The current logging rate is 280Hz, logging sensor_combined at full rate. CPU-wise the new logger is more efficient than sdlog2, so we can go higher if needed. I'd also go for 3. together with @priseborough 's suggestions, and the new system's architecture (https://github.com/PX4/Firmware/issues/7318), to run the low-level rate controller at 1kHz. I think the system can handle it, but maybe not on all platforms.

[priseborough]

It would be good to be able to make the low level rate controller rate a parameter controlled option. Some applications with only single sensors and minimal IO will be able to run at the faster rate. Also we will be seeing boards with F7 CPU's in the near future. The downsampling of the integrated gyro and accel data into delta angles and delta velocities sent to the sensor module and only used by the estimator does not have to be output at the same rate and could be left at 250 Hz which is fast enough for attitude, velocity and position loops.

[dagar]

WIP PR for icm20608 @ 4k - https://github.com/PX4/Firmware/pull/3360

[jgoppert]

@bkueng Sorry, yes, I'm getting around 248 according to my log. Still, I think we have a lot to improve on here.

[jgoppert]

So if you look at the voting code for sensors, this all makes a lot more sense. The only value we are using for the logged data or estimation is the integrated value, which only uses the data from the raw sensors. So the only filter being applied is the sensor DLPF at 41 Hz. So that is isn't as horrible as it looked, but we should probably remove that and handle it with a filter on our side as @priseborough suggested so we can fix clipping. It also isn't clear form the data sheet, what order the onboard DLPF is.

https://github.com/PX4/Firmware/blob/master/src/modules/sensors/voted_sensors_update.cpp#L670