Negative Kalman gains in Bitcraze EKF for the distance measurments

All discussions related to the Loco Positioning system
Post Reply
Hubert
Beginner
Posts: 7
Joined: Wed Sep 02, 2020 3:15 pm

Negative Kalman gains in Bitcraze EKF for the distance measurments

Post by Hubert »

Hi all,

The last few weeks, I've been learning the ins and out of (extended) Kalman filters to understand how the fusion of distances is implemented.
To visualize such a filter, I have created a simple Kalman filter algorithm in python that can determine 2D positions from when given range measurements from "virtual" beacon positions. To simplify matters, it is just used to estimate a static position, and the state and state transition functions are thus:

F = [[1, 0]
[0, 1]]

x =[x, y].T

The update part of the filter (should) function exactly like the code from the Crazyflie. But although I intuitively understand the code, I'm not sure why the measurement funcion H the way it is from a mathematical viewpoint. H is now calculated like this:

dx = S[0] - bcn[0]
dy = S[1] - bcn[1]
dz = S[2] - bcn[2]

Where S is the state of the drone, S[0] being the x (position) state estimate, S[1] is y position, and S[2] the z. bcn[_] holds the position of the beacon. So dx is simply the difference in the x-direction between where the drone predicts it is and where the anchor is.

measuredDistance = distance measured to the beacon (the real distance)
predictedDistance = distance between the predicted drone position and the beacon

H is the measurement function:

H[0] = dx / predictedDistance
H[1] = dy / predictedDistance
H[2] = dz / predictedDistance

The error (or innovation) is:
Error = measuredDistance - predictedDistance

The Kalman gain is then calculated through the regular formula:
K = PH' / (HPH' + R)^-1

However, this causes the gain to go negative often. For example, let's say that the beacon from which the drone received a range measurement is at the same height and on the same y-axis as the drone, but located 5 units further to the right compared to the predicted state (i.e. the x position is 5 units larger). The predictedDistance = 5 (the only difference between the predicted position and the position of the drone is in the x-axis). dx is however -5, as the x position of the beacon is larger. That then causes H[0] to become -1. This in turn can cause the K to become negative.

This doesn't seem right. To my understanding, K should always be in the range of [0, 1]. However, the filter does seem to work, but I'm worried that the negative K values might mess up the covariance matrix. Is this on purpose?

Also, what is the mathematical thinking behind the calculation of the H matrix exactly? It seems to be the difference in x (or y or z) with respect to the total difference in position between beacon and predicted drone position. This makes intuitive sense, but is there some literature on this?
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: Negative Kalman gains in Bitcraze EKF for the distance measurments

Post by kristoffer »

I'm not completely fluent int EKFs but I think I can help you with the origin of the H vector.

I'll use x, y, z for the current state S[0], S[1], S[2]

Your prediction p = sqrt((x - bcn[0]) ^ 2 + (y - bcn[1]) ^ 2 + (z - bcn[2]) ^2)
H is the derivative of your prediction H = [dp/dx, dp/dy, dp/dz, ....] = [(x - bcn[0]) / p, (y - bcn[1]) / p, (z - bcn[2]) / p, 0, 0....]

Note that H is a vector and not a matrix in the CF as the implementation of the kalman filter is using a scalar update as opposed to a full matrix update (I think).
Hubert
Beginner
Posts: 7
Joined: Wed Sep 02, 2020 3:15 pm

Re: Negative Kalman gains in Bitcraze EKF for the distance measurments

Post by Hubert »

Thanks!

That did help clear it up a fair bit for me.
Post Reply