As I attempted to develop a 1-D Kalman filter, so that I could later implement the harder and more complicated 2-D or 3-D versions, I ran into a debugging problem that took me two days to solve.
I had to compute an iteration by hand of the filter to realize the problem was that what was supposed to be a 2x2 matrix was actually a scalar, and here is why:
When declaring numpy arrays, if you want them to be fixed size matrices, pay attention to the enclosing braces you may add. Here are two examples that may seem identical, but are not.
Ignoring the transposes (which were desperate attempts of me to debug the error), the difference is a subtle extra bracket pair that makes the array size fixed.
This became evident when debugging, and inspecting the data type:
versus:
Happily, now my Kalman code works. I'll leave it here unmodified, to have a good memory. All the temporary variables may be omitted, they were placed there to debug.
""" this works for 1D """for z in measurements: x = F.dot(x) + u P = F.dot(P.dot(F.T)) #print(x, '\n', P, '\n') #########################
y = z - H.dot(x)
S = H.dot(P.dot(H.T)) + R #careful with R
K = P.dot(H.T.dot(np.linalg.inv(S) ) ) x = x + K*(y) #x = x + K.dot(y)
jesus = K.dot(H) Pt = np.identity(2) - jesus P = Pt.dot(P) print(x, '\n', P, '\n')