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?
Negative Kalman gains in Bitcraze EKF for the distance measurments
-
- Bitcraze
- Posts: 630
- Joined: Tue Jun 30, 2015 7:47 am
Re: Negative Kalman gains in Bitcraze EKF for the distance measurments
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).
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).
Re: Negative Kalman gains in Bitcraze EKF for the distance measurments
Thanks!
That did help clear it up a fair bit for me.
That did help clear it up a fair bit for me.