Kalman filter starts at (X, Y) = (0.5, 0.5)

[ricardohalfeld]

This initial position is hard-coded. While this can be worked around by setting the parameters kalman.initialX and kalman.initialY to zero before every flight, it looks to me like an early test that hasn't been undone in the code.

I believe it would save us all time if in future versions the hard-coded values were zero, which is consistent with most applications. The burden of setting the parameters would then be transferred to exceptional cases.

The change would happen in estimator_kalman.c, lines 232-233.

[krichardsson]

@mikehamer do you have any input on why the initial position is (0.5, 0.5, 0.5)? Do you see any problem in changing it to (0.0, 0.0, 0.0)?

[mikehamer]

Depending on coordinate system, (0,0,0) is not always the best value. I'm not asserting that (0.5,0.5,0.5) is, however different use cases and different external coordinate systems do call for different initial values.

The reason for not setting the initial value at (0,0,0) was due to my coordinate system having a UWB anchor at location (0,0,0). If this is the case, one can encounter a divide by zero at e.g. https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/estimator_kalman.c#L1015. At the very least, having the estimated position very close to an anchor's position will cause initial position updates to be highly sensitive to measurement noise, until the position has converged to the correct location.

[krichardsson]

Thanks for the input!

As mentioned, @whoenig added functionality to set the initial position using parameters which I think solves the use case where you have a known starting position.

@ricardohalfeld I suppose (0.0, 0.0, 0.0) would be a bit cleaner and makes sense in general. In most setups (assuming absolute positioning) the filter converges very quickly and this has not been an issue in my experience, out of curiosity could you share your use case?

[ricardohalfeld]

Absolutely. I'm running experiments with a controller that needs direct access to the collective thrust. You just happened to have the "send_setpoint" method already implemented, but it wasn't enough for position control. To hold the horizontal position, I've wrapped an external PID controller using the Python library around the CF's internal attitude controller. That's when the initial position became a nuisance.

It took me a couple of days to figure out what was going on. My external PID didn't seem to keep it in place. As soon as it took off it started "drifting". When I noticed the drift was always in the same relative direction (and not downstream of the AC air flow as I initially thought lol) of the starting position, I started pulling this thread and found out my controller was working just fine. It was just regulating X and Y to the wrong origin. Solved it using @whoenig's parameters initially and afterwards flashed my CFs with a custom firmware in which the initial position is zero.

By the way, these tests weren't performed in our LPS setup. The CF is equipped with a Flow Deck over a very feature-rich floor. The CF flies for over a minute with several perturbations and lands within a few centimeters from its starting position every time.

@mikehamer, I took a look at https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/estimator_kalman.c#L1015 and I fail to see where the division by zero might happen. This looks like a norm. Perhaps you linked the wrong line?

Thanks for the attention!

[mikehamer]

Actually you are correct, in my haste I linked the wrong line, which should be 1025. However, it looks like we're dropping the measurement if this were to happen so in that case it should be okay to initialize the position to (0,0,0). The only issue one might encounter is being initially sensitive to noise; that is, if d1 or d0 are very close to 0 we'll have quite high gains.

[krichardsson]

We have tried to fix cases that crashes the kalman filter in #320. It still seems to crash from time to time though so I don't think it is water tight yet.

@ricardohalfeld thanks for sharing!