Current implementation of pypilot uses RTImuLib2 Kalman filter.
That filter implementation I think has serious flaws.
It assumes zero mean average of noise for sensors.
A good Kalman filter implementation would include sensor biases into
state and process models and it would estimate and take into account
those biases on each step.
That’s I think why there is so much drift on Z axis heading reported by so many users.
Calibration and self calibration of IMU units supported by pypilot
doesn’t fully solve that drift. It can help a little.
I think RTImuLib2 Kalman filter needs to be replaced with a better one.
The one with biases in dynamic model. I have doubts that current
RTImuLib2 Kalman filter is even suitable for navigation in its current implementation.
I see some other projects started using
different IMUs with built in fusion for a compass.
Unfortunately they are not supported by RTImuLib2 either.
I have no personal experience with CMPS14 or other
units that do built in IMU fusion to avoid Z-axis drift,
but the claims of accuracy and reports on other GitHub projects
look like they do much better job than RTImuLib2 used in pypilot.
Basically, yes the gyro biases change over time. I am well aware of this. I improved the implementation to lowpass filter the gyros because we know the boat is over many minutes level and not spinning in circles. So unlike certain applications, on a boat it is relatively safe to use a simple filter, but yes it may take several minutes or longer initially to average the gyros.
You could augment the kalman filter to add gyro bias states, but any time you add more states to the filter it complicates the design and can potentially degrade it in other ways.. but to be fair, using the filter to estimate gyro bias is a good idea and certainly possible, but I dont think there is a huge benefit and never bothered with it.
Can you elaborate more on z-axis drift and what you mean by that? Which users are complaining and where did they complain?
In my experience with Kalman filters in other projects
without biases as states variables in the model the Kalman filter simply converges to
values with error (basically wrong , skewed values).
So having biases and estimating them in Kalman process is essential.
Using Kalman model without estimating biases and relying on sensor self correction leads
to too big systemic errors.
There papers which often publish some Kalman model which assume no bias (zero mean noise)
People code them as is, without extending matrices for biases. These filters do not work with real sensors.
I could never make compass in pypilot work without unreasonable drift.
I’ve done all calibrations. There were fine. I do cicrcles on a boat after leaving marina to give it
chances to auto calibrate. But the drift is too high. And I think it’s due to RTImuLib2 Kalman
not having biases in the dynamic model.
I think you might have it confused. If you do have biases in the state it can convert to skewed values and avoiding these issues significantly complicates things.
I am compensating the bias for all the sensors. The accelerometers can be calibrated and the bias remains stable. The gyro bias is filtered and the long-term average is always zero given the fact the boat is not spinning. The compass bias are compensated with calibration.
accelerometer and compass bias cannot be easily estimated with a simple kalman filter. This is because you need memory and a wide spread of data. If you do try it will converge to the wrong result because it weights more recent measurements higher. We could try to go on and on about this, but essentially the calibration uses the same least squares update as the kalman measurement update. So it is essentially the same algorithm.
As for the gyro bias, as mentioned yes it can be estimated by the kalman filter, but if you wait a few minutes the average always converges to zero. I dont know where you got your statements otherwise. You can plot them with the pypilot scope, or just look at the bias with 'pypilot_client -c imu.gyro' and see this.
What do you mean "drift is too high" ? does the compass keep increasing while the boat faces the same direction, or do you mean the heading changes by more than 90 degrees with 90 degree heading change or?
Which specific sensors are you using? I mostly support the icm20948. Other compass sensors use magnetoresistors which are very precise but not accurate. Hall sensors are sufficient for compassing and much more stable. Is this the reason?
Yes, an augmented kalman filter is possible. RTIMULib2 already has 4 filters implemented, where the kalman is just one. It should be straightforward to make a new filter and augment it with 3 more states for gyro biases. I had always intended to try this but it never has seemed to be important, because if you do plot the actual gyro bias you will see they are near zero when the sensors are not moving given enough time to settle.
I’m using BerryGPS-IMUv3 which has LSM9DS1 chip
Overtime heading drifts from correct value.
After everything calibrated and calibration being accepted.
The CMPS14 is not really an IMU but a compass module with an IMU and a calculator that performs the calibration and determines the heading, roll and pitch. This module is powerful but costs more than $50
I think that what Sean designed with already 3 controllers in the autopilot is already not bad compared to commercial pilots.
2024-09-17, 12:58 AM (This post was last modified: 2024-09-17, 01:20 AM by mgrouch.)
(2024-09-16, 04:21 PM)seandepagnier Wrote: accelerometer and compass bias cannot be easily estimated with a simple kalman filter. This is because you need memory and a wide spread of data. If you do try it will converge to the wrong result because it weights more recent measurements higher. We could try to go on and on about this, but essentially the calibration uses the same least squares update as the kalman measurement update. So it is essentially the same algorithm.
accelerometer bias is easy to estimate in Kalman method for a boat.
On a boat average acceleration is g. Most of AHRS Kalman papers are for drones and not for a boat
so they can’t take that into account. It’s a very strong condition. Might bring much better results for
a boat oriented Kalman filter.
Kalman filter doesn’t really require much memory. It’s few state vectors and transition matrices.
They are sparse and often multiplied by blocks.
It would converge where the model tells it to converge.
2024-09-17, 03:06 PM (This post was last modified: 2024-09-17, 03:06 PM by seandepagnier.)
(2024-09-17, 12:58 AM)mgrouch Wrote: accelerometer bias is easy to estimate in Kalman method for a boat.
To be clear, it uses the same formula (least squares) to estimate accel and compass bias as the kalman filter, except I have memory of points all over the sphere so the result is correct. You cannot find a reliable solution to a non-linear fit like this using an extended kalman filter.. I have tried.
Think about this: If the boat rolls and pitches around a bit in theory it is possible to fit a curve to the points, but in practice, if you think about fitting a circle or sphere to points that are almost all only on one part of it, slight noise can give much larger or smaller circles with different bias. You need a point on the opposite side of the circle which is like having the boat flipped over.
It is possible to use the gyros and/or compass sensors to help find the accelerometer bias and maybe this is what you mean, but the gyros have more errors, such as scale and non-linearity. This is a problem. The most stable and reliable method to find accelerometer bias I have found is to take "still points" by averaging the sensors in different orientations. It does work, and avoids compounding errors from other sensors by using only the accelerometers to compensate themselves. It is possible to get more accuracy by using nonlinear fits, but not really important for the autopilot. The accelerometer bias is most stable of all the sensors as well.
Quote:On a boat average acceleration is g. Most of AHRS Kalman papers are for drones and not for a boat
so they can’t take that into account. It’s a very strong condition. Might bring much better results for
a boat oriented Kalman filter.
Kalman filter doesn’t really require much memory. It’s few state vectors and transition matrices.
They are sparse and often multiplied by blocks.
It would converge where the model tells it to converge.
I have played with kalman filters maybe more than you realize. What you are suggesting works in theory but in practice can diverge. There are ways to prevent this, but basically the reason is the kalman filter has no "memory" besides the covarience matrix and state. This means that it really has no idea what the sensors would read if the boat rotated 180 until it actually does. This means it cannot compensate bias is a reliable way.
What you are right about is the kalman filter could more quickly converge to gyro bias, by estimating them as additional states in the filter. As I mentioned I had always intended to do this, but never got to it, and the dynamics on a boat seem to make it work ok by a simple lowpass, it just takes a few minutes, especially initially as the bias is unknown. The bias drifts over time, but as long as this drift is slower than the lowpass filter it does work, and on a boat the rate of turn is never really fast. I am assuming high speed spinning and maneuvers like a drone could cause the gyro bias to change faster, but I am not sure. What do you think?
I really think the problems you have that no one else reports is because you are using different sensors from everyone else. Maybe your sensors use magneto resistors, or maybe the gyros just are not as stable. Maybe the driver for your sensors has a bug in it. RTIMULib2 had multiple bugs for reading the 9255, and I fixed them. You can compare my fork to others, and without these changes the sensors would do crazy things.
I then wrote the driver for the icm20948 and really tested it carefully. I did not do this work for all of the drivers so the sensors you are using might always cause problems unless you fix rtimulib2, and this would not surprise me.
(2024-09-17, 12:58 AM)mgrouch Wrote: accelerometer bias is easy to estimate in Kalman method for a boat.
To be clear, it uses the same formula (least squares) to estimate accel and compass bias as the kalman filter, except I have memory of points all over the sphere so the result is correct. You cannot find a reliable solution to a non-linear fit like this using an extended kalman filter.. I have tried.
Think about this: If the boat rolls and pitches around a bit in theory it is possible to fit a curve to the points, but in practice, if you think about fitting a circle or sphere to points that are almost all only on one part of it, slight noise can give much larger or smaller circles with different bias. You need a point on the opposite side of the circle which is like having the boat flipped over.
It is possible to use the gyros and/or compass sensors to help find the accelerometer bias and maybe this is what you mean, but the gyros have more errors, such as scale and non-linearity. This is a problem. The most stable and reliable method to find accelerometer bias I have found is to take "still points" by averaging the sensors in different orientations. It does work, and avoids compounding errors from other sensors by using only the accelerometers to compensate themselves. It is possible to get more accuracy by using nonlinear fits, but not really important for the autopilot. The accelerometer bias is most stable of all the sensors as well.
Quote:On a boat average acceleration is g. Most of AHRS Kalman papers are for drones and not for a boat
so they can’t take that into account. It’s a very strong condition. Might bring much better results for
a boat oriented Kalman filter.
Kalman filter doesn’t really require much memory. It’s few state vectors and transition matrices.
They are sparse and often multiplied by blocks.
It would converge where the model tells it to converge.
I have played with kalman filters maybe more than you realize. What you are suggesting works in theory but in practice can diverge. There are ways to prevent this, but basically the reason is the kalman filter has no "memory" besides the covarience matrix and state. This means that it really has no idea what the sensors would read if the boat rotated 180 until it actually does. This means it cannot compensate bias is a reliable way.
What you are right about is the kalman filter could more quickly converge to gyro bias, by estimating them as additional states in the filter. As I mentioned I had always intended to do this, but never got to it, and the dynamics on a boat seem to make it work ok by a simple lowpass, it just takes a few minutes, especially initially as the bias is unknown. The bias drifts over time, but as long as this drift is slower than the lowpass filter it does work, and on a boat the rate of turn is never really fast. I am assuming high speed spinning and maneuvers like a drone could cause the gyro bias to change faster, but I am not sure. What do you think?
I really think the problems you have that no one else reports is because you are using different sensors from everyone else. Maybe your sensors use magneto resistors, or maybe the gyros just are not as stable. Maybe the driver for your sensors has a bug in it. RTIMULib2 had multiple bugs for reading the 9255, and I fixed them. You can compare my fork to others, and without these changes the sensors would do crazy things.
I then wrote the driver for the icm20948 and really tested it carefully. I did not do this work for all of the drivers so the sensors you are using might always cause problems unless you fix rtimulib2, and this would not surprise me.
I have not studied the filter you linked. It is quite recent (2023) It should be relatively straightforward to expand the state-space to include gyro bias, but as I mentioned before there is likely no significant benefit for boat applications. I would find it interesting to explore.
Additionally, it is possible to use redundant sensors, for example 6 or 9 of each axis. This reduces noise (1/sqrt(n)) but more importantly can detect and compensate for failed sensors. Furthermore, additional automatic calibrations can be performed such as non-linearity of gyros and so forth.
The thing to be aware of, is even though the sensors are not perfect, the errors the autopilot at sea must compensate for due to wave actions are more or less an order of magnitude above this. So yes, improvements would help, but it is (very roughly) like optimizing a computer program by 10% speed with significant complication added. This is interesting and useful but most users will not realize there is much difference.
For aircraft/drones especially doing acrobatics the situation is different especially depending on the use case. Also space craft could benefit much much more from such filters because they are not compensating for wave actions constantly, so these filters could instead result in far more accuracy to the filtered output.
You could argue that in flat water this is somewhat the case, but this is already by far the easiest situation for the autopilot and due to the friction of the drive units used to move the rudder, corrections need to be intermittent which essentially dwarfs this improvement as you can only efficiently perform corrections that move the rudder for a minimum amount of time. The friction involved means it is most efficient to move the rudder at a minimum speed so moving the rudder tiny amounts of 1-2 degrees or less in a correction is generally less efficient overall.. but you could certainly come up with a system where it does make sense. So thank you for bringing this up!
Let me know your results with icm20948 testing. I want to be sure you can reach as successful a setup as a typical pypilot system as a basis to compare to before considering additional algorithms.
