Open andyp1per opened 1 week ago
@andyp1per note that the rate loop rate and the rate at which IMU samples go into the sensors FIFO (the sensors ODR) are rarely an integer multiple of each other. This means if you want the rate loop period to be constant then you cannot synchronise on the IMU ODR. A simple example is a sensor providing data into the FIFO at 1kHz, but main loop rate is 400Hz. You will sometimes have 2 samples and sometimes 3. The EKF is aware of this via the delta_angle/delta_velocity APIs which provide the time delta of each sample, but the rate loop is not aware of this, so is imposing an artificial delta time with a odd/even style of latency change. This is complicated by the filtering which means that a single filtered sample is composed of data from multiple periods. Each notch filter adds a delay. So are you proposing to have a variable (odd/even) rate loop period? If not, then what does it actually mean to "synchronise" two things which are not running at an integer multiple rate? We could choose an integer multiple and force the rate loop to run at a rate that has a period that is a multiple of the sensor ODR, but that will have odd flow-on effects. For example, a copter configured for 400Hz with a 1000Hz ODR IMU would have to choose 333Hz or 500Hz, and may have to change main loop rate by a large amount on an EKF lane change.
@andyp1per note that the rate loop rate and the rate at which IMU samples go into the sensors FIFO (the sensors ODR) are rarely an integer multiple of each other.
But if we are planning on putting the rate loop in a separate thread we can make it an integer multiple. If we want the best rate loop performance this is exactly what we should do. The rate loop should be run as soon as possible after the IMU gyro read is done.
If we change IMU's that we are using and they are operating on a different update rate then we will need to change the dt but that is something that isn't hard to deal with.
The statements above all assume we can output at significantly higher rates than 400 Hz such that we can achieve minimum latency and minimise dt jitter.
If our update rate is limited by our ability to communicate with the actuators as it is with PWM (nominally 400 Hz), then we are no longer able to operate at a multiple of our gyro sample rate if it is 1kHz. In this case we must synchronise our rate loop with our ability to output the new actuator setting. We need to make assumptions as we do now and that inconsistent timing is treated as timing jitter.
In the case where our IMU reads are done at 800 Hz or 2 kHz we could synchronise our rate loops to run immediately after the sample is read and passed through the filters. This would result in minimal additional latency.
note that the rate loop rate and the rate at which IMU samples go into the sensors FIFO (the sensors ODR) are rarely an integer multiple of each other. This means if you want the rate loop period to be constant then you cannot synchronise on the IMU ODR. A simple example is a sensor providing data into the FIFO at 1kHz, but main loop rate is 400Hz. You will sometimes have 2 samples and sometimes 3. The EKF is aware of this via the delta_angle/delta_velocity APIs which provide the time delta of each sample, but the rate loop is not aware of this, so is imposing an artificial delta time with a odd/even style of latency change. This is complicated by the filtering which means that a single filtered sample is composed of data from multiple periods. Each notch filter adds a delay. So are you proposing to have a variable (odd/even) rate loop period? If not, then what does it actually mean to "synchronise" two things which are not running at an integer multiple rate? We could choose an integer multiple and force the rate loop to run at a rate that has a period that is a multiple of the sensor ODR, but that will have odd flow-on effects. For example, a copter configured for 400Hz with a 1000Hz ODR IMU would have to choose 333Hz or 500Hz, and may have to change main loop rate by a large amount on an EKF lane change.
All of the schemes that involve slowing the FIFO read rate make the assumption that the ODR rate is the sample rate as far as the EKF is concerned. So as you know we are not measuring intervals we are using a mostly fixed dt - this means that whether we read 2 or 3 samples does not matter as far as the EKF is concerned, the dt is still fixed between them and the EKF is happy. Note that I am making the assumption that the device sample rate is high (see my second requirement above) and thus the issue of non-integer multiples is greatly reduced because the ODR is >> 1Khz. Even when you have 2 or 3 samples in a read, what you care about is the latency between the latest sample that you read and the action, it doesn't really matter how many samples preceed that (I mean it does because that's the reason for running a high rate rate thread, but for the slower case it doesn't really matter) but it does matter how long you delay after reading. The synchronization I am talking about is between the main loop and the FIFO read, so I am proposing that (for instance) both are run at 400Hz. What you are trying to ensure is that each time the main loop ticks it has a FIFO read that is as fresh as possible so that the latency is at worst 1/ODR.
All of the schemes that involve slowing the FIFO read rate make the assumption that the ODR rate is the sample rate as far as the EKF is concerned. So as you know we are not measuring intervals we are using a mostly fixed dt - this means that whether we read 2 or 3 samples does not matter as far as the EKF is concerned, the dt is still fixed between them and the EKF is happy
The EKF is already happy. The EKF is designed to handle variable dt, the dt the EKF uses is summed based on the number of actual ODR samples used in an update. The EKF is perfectly happy with this varying between updates. That is why the delta_angle and delta_velocity APIs return a delta_time for each request. That delta_time is the sum of the deltas for each ODR sample. What I'm trying to point out is that while the EKF can handle variable dt between loops, the rate controller doesn't have a mechanism to handle that. So truly synchronising to the ODR data, which is what true synchronising to the actual hardware sample would be, is not possible with the current rate controller design.
Note that I am making the assumption that the device sample rate is high (see my second requirement above) and thus the issue of non-integer multiples is greatly reduced because the ODR is >> 1Khz
we also need to cope with slower IMUs. For example, the popular BMI270 runs at approximately 1600Hz (and actually varies quite a bit from that).
I think we are getting crossed wires on what the term "synchronising" means. Everybody is on the same page that the EKF is designed to handle variable dt properly.
From the perspective of the rate loops we know that you will always have timing jitter because of the non-integer relationship between the ODR sample rate and the and rate loop rate. Understanding that we can only do so much with the current system we want to minimise latency given the limitations of this system.
The timing inconsistency due to the variable dt can be represented as noise from an engineering perspective. So we do have a mathematical way of handling / representing this. We do want to minimise it of course.
Average latency is going to be approximately half the rate loop period + rate loop processing time and signalling delay + the average delay after the ODR sample is taken to the time the rate loop is run.
What Andy is trying to do is minimise the last part of that latency equation. If we can minimise the time delay between the last ODR read and the output of the rate loop we will be maximising the potential of this system.
The rate loop doesn't care when the first few samples are read and processed, just the last sample before it runs. This would suggest that the lowest latency would be achieved by processing and emptying the FIFO and computing the filters just before we are due to run the rate loop so that we can process a single sample then run the rate loop with the lowest possible additional time delay.
This is a brief write-up of what I think we should do for rate control, primarily in copter but affecting other vehicles as well. There are a number of seemingly competing ideas floating about, but I think that there is a happy path for these where everyone gets what they want (and perhaps more).
Requirements:
Rather than delve into the theoretical details, I'll simply put my proposal here:
A couple of things to note:
I think this really takes us into new territory where we are carefully managing the gyro reads, rate update latency and CPU cost in a way that benefits everyone.