So I didn’t know where else to put this, but the topic is mainly for learning. I’m not a Kalman Filter guru, by any means, but I have been involved in Testing Kalman Filter implementations, mostly just the state machine, not the accual position calculation testing.
For the NAV systems I worked, it fused the following systems for the KF.
GPS1 - Lat/Long/Alt/Velocity
GPS2 - same
INU1 - Lat/Long/Alt/AccX/Y/Z,VelX/Y/Z/etc
INU2 - Lat/Long/Alt/AccX/Y/Z,VelX/Y/Z/etc
Doppler Velocity Sensor - VelX/Y/Z
And the way the Kalman fused the data was with weighting values, which changed based on certain criteria. For example, the INUs calculated (internally) an approximated drift/time based on Circular Error Probable. Based on each INUs calculation, the KF would weight their influence on the fusion.
For the GPSs, it would take into account HDOP/VDOP/FOM/etc and weight the GPSs influence on the position. This weighting was always in flux, and the only time a sensor wasn’t used at all was when it was below a certain threshold (or turned off of course). This system was/is good enough to perform self-contained navigations and landings, without the use of other sensors, in both day/night and visual/black-out conditions. Just, “follow the bar”
From my initial reasearch into the EKFs, it looks like weighting is either hard coded in code, or set based on parameters. Additionally, it looks like the EKF doesn’t use GPS altitude data at all? Is there a reason for that? When VDOP is high (based on current satellite constellation configuration) then GPS altitude data is extremely reliable (and not subject to sudden Baro changes). I suspect there will be some physicists on here which will shoot down most of what I’ve set. I’m ready! Go!
@lordneeko physicists are no help here, you need an engineer
FWIW I’ve followed ardupilot for years but also havent looked at the code base in years. From what i recall KF initialization params and the process and measurement covariances (the wieghts you refer to) are all hard coded somewhere and do not change. Depending on what measurements are available, those portions of the measurement covariance matrix will be incorporated in the KF innovation step.
This has always baffled me. Typically Kalman filter covariance matrices are tuned to the system. With all the DIYers that blindly use ardupilot without tuning the Kalman filter, its a miracle that things fly as well as they do.
Its either a testament to the robustness of ardupilot or the miracle that is the Kalman Filter.
I haven’t seen any open source code that updates the measurement covariances based on figures of merit from the sensors. I’m also surprised by your attestation to the accuracy of GPS altitude. In practice at least with this hobby grade equipment I’ve found it to be unreliable, hence most drones incorporate a barometer. But honestly the last time i dealt with it closely was before the days of multi-constellation receivers, so its been a while…
The techniques you mention are really interesting, especially considering the advances in MEMs IMUs and GPS receivers. If this kind of stuff isnt in the code base, it should certainly be considered.
I’ve worked on Flight Test programs that lasted 2 years “tuning” a Kalman Filter for an aircraft. So, yeah, it takes a lot of work. You make an extremely good point. How does this thing work?? Or maybe we spent WAY too much money on flight testing lol
I’m an engineer…and I assure you I do NOT do Kalman math. lol My old organization had Math PhDs and physicist to handle that sorta thing. They could solve DCM matrices on a napkin at the restaurant.
I’m curious which of these INS designs is used in AP. I think it is the open-loop design, because I don’t see error feedback into the AP_InertialNav methods.
