# Kalman filter for heading estimation with magnetometerv + gyroscope only considers magnetometer

I implemented a Kalman filter to estimate the heading of a robot that is moving in 2D, given the measurements coming from a magnetometer (X, Y) and a gyroscope (Z). The code is the following:

from numpy import dot
from numpy.linalg import inv

'''
X: state
A: state transition
P: state covariance
Q: process noise covariance
B: input effect
U: control input
Y: measurement
H: measurement matrix
R: measureent covariance matrix
K: kalman gain
'''

class Kalman(object):
def __init__(self, dt, X, P, A, Q, B, U, Y, H, R):
self.dt = dt
self.X = X
self.P = P
self.A = A
self.Q = Q
self.B = B
self.U = U

# Measurement matrices
self.Y = Y
self.H = H
self.R = R

def predict(self):
self.X = dot(self.A, self.X) + dot(self.B, self.U)
self.P = dot(self.A, dot(self.P, self.A.T)) + self.Q

def update(self):
IM = dot(self.H, self.X)
IS = self.R + dot(self.H, dot(self.P, self.H.T))
self.K = dot(self.P, dot(self.H.T, inv(IS)))
self.X = self.X + dot(self.K, (self.Y-IM))
self.P = self.P - dot(self.K, dot(IS, self.K.T))

X = np.array([[h0], [w0]]) # h, w
P = np.diag((0.001, 0.001))
A = np.array([[1, dt],[0, 1]])
Q = np.eye(X.shape)*0.1
B = np.eye(X.shape)
U = np.zeros((X.shape,1))
Y = np.array([[h0], [w0]])
H = np.array([[1, 0], [0, 1]])
R = np.eye(Y.shape)*0.1
heading_kalman = Kalman(dt, X, P, A, Q, B, U, Y, H, R)

# test the filter with some recorded samples
for i in range(n_samples):
# read t, mag_x, mag_y and gyr_z

angle = np.arctan2(mag_y, mag_x)
angle = 180*angle/np.pi

if angle < 0:
angle += 360

angle_speed = gyr_z

if i == 0:
continue

dt = t - prev_t
# update dt in kalman filter
# update measurements

if ang < 0:
ang = ang + 360


The code works but this filter seems to consider only the magnetometer. I tried to insert artificial discontinuities in the measured angle, but the output of the filter closely follows the resulting peaks. I know that I could tune some filter parameters (P, Q, R), but I don't know if the problem lies here or if there are some errors in my implementation.

What I would definitely check is this line

self.P = self.P - dot(self.K, dot(IS, self.K.T))


I tried to reformulate this, and I think it does not match the update equation for the covariance. Usually people use one of the following:

• $$P = \left(I - K C\right)P\left(I - K H\right)^T + KRK^T$$ - this is numerically more stable.
• $$P = \left(I - K C\right)P$$

Steps In the filter, you need to first call predict then update - this is a conceptual thing. Namely, you use the Kalman filter's model to "guess" what the value of the state could be while your system has moved/rotated, but you are still waiting for the measurement. Note: as you run this in a loop, it probably will not change the results, but I encourage you to change the code and to think about this as being a crucial part of the filter.

Tips:

• You can remove dot(self.B, self.U) from predict, as you U is 0.
• As a rule of thumb, P might benefit from having bigger values (play with this)
• Dependent on how much you trust your model/measurements, change the relative values of Q and R
• Use the @ in Python for matrix multiplication, it makes the code easier to manage and read.