Support providing delta time?

[TheButlah]

Instead of specifying a sample rate, could we provide the elapsed time since the last update as part of the update? This would allow us to use hardware timestamps.

[dlaidig]

Unfortunately, a time-varying sampling time is something that I would have liked to support, but it really conflicts with the design of VQF.

The main reason is the linear Butterworth low-pass filter that is used in the accelerometer update. If the sampling time changes at every update step, first of all, the filter coefficients would have to be re-computed all the time. This would be a bit expensive, but probably still okay. However, I have no idea how to adjust the filter state (and if this is even possible). It should certainly be possible to properly apply some low-pass filter to a signal with irregular sampling, but most standard signal processing theory is based on equidistant sampling.

(As a detail, for the gyropscope update it is quite trivial to support irregular sampling. The rotation angle is angle = gyrNorm * gyrTs, so you can just scale the gyroscope measurement with actualGyrTs/gyrTs. In some cases in which the assumption of a fixed sampling time is an issue, just scaling the gyropscope measurements accordingly might be good enough...)

The reason why I didn't pursue this feature further was that I don't really see the use case for it. In my opinion, it should be preferable to configure the IMU to output data at a regular rate. And if this is not possible for some reason, are you really sure that you understand the internal sampling and preprocessing of the IMU well enough to trust your measured sampling intervals more than the nominal sampling rate?

Can you tell me more about your use case? I doubt it will be possible to change VQF but I'm still curious.

[TheButlah]

Disclaimer: Sensor fusion, and making a really good fusion pipeline, is not something I have had much if any experience in. I'm learning as I go.

I've been under the impression that getting hardware timestamps from the sensor itself (many imus provide these hardware timestamps in a FIFO queue) is critical to ensuring highly accurate fusion. I suppose I implicitly assumed that this was because there can be some nontrivial jitter and/or variation in the deltaT between samples. The IMU itself is configured with a fixed ODR, but the delta between two samples might not always be equal.

Perhaps this is an incorrect assumption though, especially when using hardware FIFOs present on most imus. What are your thoughts? Are there any scenarios where the deltaT of a fifo's hardware timestamps would not always be the same?

[Quick-Flash]

@dlaidig

Unfortunately, a time-varying sampling time is something that I would have liked to support, but it really conflicts with the design of VQF.

The main reason is the linear Butterworth low-pass filter that is used in the accelerometer update. If the sampling time changes at every update step, first of all, the filter coefficients would have to be re-computed all the time. This would be a bit expensive, but probably still okay. However, I have no idea how to adjust the filter state (and if this is even possible). It should certainly be possible to properly apply some low-pass filter to a signal with irregular sampling, but most standard signal processing theory is based on equidistant sampling.

From my knowledge of biquad filters as you are using them, you cannot simply change the cutoff or the input frequency without running into some ringing. If you were to change to using a DF1 implementation of the biquad filter you could simply overcome this problem. You could also try double exponential smoothing to get a filter similar to what you are using, but at a lower cpu cost.

Here is a rust code example of a DF1 and DF2 biquad filter. (The DF1 is able to change coefficients without problem, the DF2 is not)

pub struct BiquadDF1 {
    b0: f32, b1: f32, b2: f32, a1: f32, a2: f32,
    x1: f32, x2: f32, y1: f32, y2: f32
}
impl BiquadDF1 {
    pub fn apply(&mut self, input: f32) -> f32 {
        let output = self.b0 * input + self.b1 * self.x1 + self.b2 * self.x2
            - self.a1 * self.y1 - self.a2 * self.y2;

        self.x2 = self.x1;
        self.x1 = input;

        self.y2 = self.y1;
        self.y1 = output;

        output
    }
}

pub struct BiquadDF2 {
    b0: f32, b1: f32, b2: f32, a1: f32, a2: f32,
    x1: f32, x2: f32,
}

impl BiquadDF2 {
    pub fn apply(&mut self, input: f32) -> f32 {
        // compute the result
        let output: f32 = self.b0 * input + self.x1;
        // shift x
        self.x1 = self.b1 * input - self.a1 * result + self.x2;
        self.x2 = self.b2 * input - self.a2 * result;

        output
    }
}

TLDR switch to a DF1 implementation of the biquad filter instead of the DF2 implementation that the VQF is using and you will be able to use a changing input frequency or even a changing cutoff without problems.

[Quick-Flash]

Thinking about it there are some coeffs that would need updating as well if the time signal wasn't consistent. Those would also need updating in order to get it working right.

[dlaidig]

Sorry for the late reply.

The IMU itself is configured with a fixed ODR, but the delta between two samples might not always be equal.

Did you check if there is some jittering in the FIFO timestamps and if there is, how significant it is? I'm not 100 % sure, but I remember that some IMUs (probably BMX160) just put timestamps in the FIFO that increment perfectly with the configured sampling time.

Even if there is some small jittering in the timestamps of IMU data provided in a FIFO, I doubt that taking them into account will have a significant impact. (And it's probably a 50/50 change whether it will increase or decrease accuracy. You would need to design some experiments to find out.)

What are your thoughts? Are there any scenarios where the deltaT of a fifo's hardware timestamps would not always be the same?

I think the more relevant scenario is lost samples. (Either low-level because of a FIFO overrun or in some subsequent steps, e.g., due to wireless transmission.) In that case, the impact can be significant but there is also not much to do. (And the straightforward partial solution, feeding the same sample multiple times, does not require adjustable sampling rates.)

Thinking about it there are some coeffs that would need updating as well if the time signal wasn't consistent. Those would also need updating in order to get it working right.

Thanks for providing the filter example code. I totally agree that, in practice, the DF1 implementation will be a lot more robust to changing coefficients and sampling rates. (At the cost of needing more memory to store the state.)

However, as you noticed, it's not that simple. The derivation of the filter coefficients b0 to a2 is all based on standard signal processing theory and assumes regular sampling (at the very least for the last two time steps since this is the data that the filter coefficients are multiplied with).

[TheButlah]

At least with the icm42688-p, yes the hardware timestamp deltas are always the same number, which corresponds to the sampling period. So there is no jitter and therefore no real point to supporting per-sample deltas if you never drop data.

However, there would be drift between the lower accuracy internal oscillator of the imu and the higher accuracy mcu clock. So synchronizing the two of them would be important. Maybe this could be accomplished sufficiently by occasionally changing the sample rate of the filter.

Would changing the filter's sample rate throw away any important state? Lets say every 1000 samples I updated the sample rate according to an exponential moving average of the ratio of observed sample rate (which may be faster or slower than the target sample rate, due to drift in the high-ppm internal oscillator of the imu)

[dlaidig]

However, there would be drift between the lower accuracy internal oscillator of the imu and the higher accuracy mcu clock. So synchronizing the two of them would be important. Maybe this could be accomplished sufficiently by occasionally changing the sample rate of the filter.

If the less accurate IMU clock is off by a small amount, this will have two effects:

1. The time constants tauAcc and tauMag (and also some other parameters) will have a slightly different meaning, i.e., some converge speeds are a bit different. I'm rather sure that this will not have any noticeable effect.

2. The gyroscope reports the rotation in rad/s or °/s, and the sampling times are used to integrate this signal. And of course, it makes a difference if you, for example, measure 100 °/s for 1.00 seconds or if you measure 100 °/s for 1.02 seconds. But before you try to figure out if and how you want to compensate for this effect, try to answer two questions:

   a. Based on which clock are the gyroscopes calibrated? (And if they are not calibrated, this whole discussion is not relevant.)

   b. If the IMU clock frequency changes over time, how does this affect the gyroscope measurements? Is it more accurate to use the IMU clock to integrate or is it more accurate to use a more accurate clock that does not change over time?

Would changing the filter's sample rate throw away any important state? Lets say every 1000 samples I updated the sample rate according to an exponential moving average of the ratio of observed sample rate (which may be faster or slower than the target sample rate, due to drift in the high-ppm internal oscillator of the imu)

Slightly adjusting the sampling rate from time to time should be quite easy to do. Just take a look at where the sampling time is used. The only tricky part is updating the filter state when the filter coefficients change and you can see in the implementation of setTauAcc how this is handled.

(Or, like I mentioned in the first response, just scaling the gyroscope measurements is a lot easier to implement and has the same effect.)

[dlaidig]

I'm closing this for now. Feel free to re-open and to continue the discussion.