Kalman filter modification for constant altitude flight #823
Conversation
|
Nice! We'll check it out |
|
It's a really cool project you have done here! We do have a couple of concerns with the contribution though, as there are a lot of changes to the kalman filter, including the state. Have you been able to measure how much this will increase the computational load of the kalman filter? Moreover, we are also in the middle of trying to reduce the complexity of the code and as this is for a quite specific application, this would perhaps not really fit. I'm wondering if it might be rather suitable for perhaps an out of tree estimator build, which is initialized through the app layer? (PS: But it's still quite a cool achievement, so if you are interested in writing a blogpost about this, contact us!) |
|
Hi guys, Unfortunately, no tests concerning the computational load were conducted. Even if tests were conducted, I'd say very likely there is room for improvement as the development did not focus on being time efficient (e.g., if I'm correct About the concrete implementation: As you guys are working more closely with the firmware, it is likely you who can choose the most suitable approach here. Therefore we are happy for your input :) |
|
It's a good question of how to better implement this. Currently the code has compatibility to do an out of three estimator as I told before, but we might be wanting look at unifying the estimator structure in general which is still up for discussion (see #711 ). So it's still a complicated situation actually. You can already try to get it to work as out of tree but maybe it's smarter to wait a little until we have concluded what the final structure of the estimator framework should be |
|
Hi! Just to come back to this issue, another lab has used the out of tree estimator structure to implement their special estimator: https://github.com/HSaugsburgBitcraze/estimator_error_kalman. I think that this is an good example of how to implement your work, so that you just have a small repository with only your changes. I will close this pull request and as we are not able to merge due to the reasons we gave before. It is mostly our recent conquests to declutter our github repo, eventhough we like this feature! (it won't be deleted really, just not in open pull request). But please keep us up to date on your blogpost @dominiknatter! Also let us know if you want help with the out of tree estimator implementation. |
|
Hi @knmcguire, |
|
no problem at all! just give us an update whenever you are ready to write the blogpost or want help with converting your kalman filter |
Overview
This is a proposed solution for flying with constant altitude over obstacles as brought up by this github issue. The solution is based on "step detection with the zranger" as suggested in the issue, but this solution uses a bit more for example by using the upward range measurement form the multiranger for better performance. Below is a video to show how well it works when flying off a table.
I worked on this as part of my summer internship at SINTEF as part of the Ingenious project.
How it works
The state in the Kalman filter was extended with two new states. These are f ("floor height") and r ("roof height"). f is defined as the height of the object under the Crazyflie compared to the height where the z state is defined as 0.
r is defined as the height of the object over the Crazyflie compared to the same reference height. These states are used together with downward and upward range measurements in the Kalman filter for better estimation of z. The states are illustrated below.
Previously the downward range measurement (l_D in illustration) was used as a direct measurement of z, but with the new definitions it now is a measurement of (z - f). Similarly the upward range measurement (l_U in illustration)is a measurement of (r - z). Both of the measurements also have an effect from the angle to the floor (theta) which is used in the same way as previously, that is by dividing by R[2][2] (rotation matrix).
However, using this directly in the Kalman filter is not enough since one is now using two measurments to estimate three states. This is handled by having the new states f and r generally not changing, meaning I assume the floor and roof is flat most of the time. However, there is a detector for both the upward and downward range measurements for detecting fast abrupt changes in the range measurements. When a fast change is detected it is assumed it is the corresponding state (f or r) that is supposed to change. Then either f or r is set to a "best first guess" and given a high variance in the covariance matrix for the Kalman filter to fine tune the estimate of the state that was changed.
In addition to these main changes the optical flow sensor also needed a small change where the distance to the floor is no longer z, but (z - f). Here there is also a difference from the "correct" way to use a Kalman filter where "h_z" and "h_f" are set to zero. This is because setting them to their "correct" values caused the states to drift and converge to completely wrong values.
This new feature can be turned on an off using the parameter "kalman.useFAndR". Setting it to true (or nonzero) causes the EKF to use this feature. When setting it to false the firmware SHOULD work as it did without the change, but I can't be completely sure that it does not cause any difference.
Problems
The optical flow measurement implementation in the Kalman filter assumes the Crazyflie is flying over a flat surface. Now that it can fly over obstacles which are not a flat surface, but can have very large differences in distance to the sensor it sometimes messes up the estimates in x and y. This manifests as the x and y estimates stopping or "going the wrong way" while the Crazyflie is moving over an edge. If one has a set point at the other side of the edge it will cause the Crazyflie to speed up and go too far past the set point. I found that this effect happens less or completely disappears when the distance to the obstacles are "large enough". This is because when having a larger distance to the obstacle the relative difference between the distances to the sensor between the different surfaces gets smaller, which makes the errors smaller. I found that this effect tends to disappear when flying at an altitude that is double of the height of the obstacle, e.g if flying over a box that is 0.5m tall one should fly at an altitude of at least 1.0m to the floor.