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

Question

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)) 

def initialize_heading_kalman(dt, h0, w0):
    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])*0.1
    B = np.eye(X.shape[0]) 
    U = np.zeros((X.shape[0],1)) 
    Y = np.array([[h0], [w0]])
    H = np.array([[1, 0], [0, 1]])
    R = np.eye(Y.shape[0])*0.1
    heading_kalman = Kalman(dt, X, P, A, Q, B, U, Y, H, R)
    return heading_kalman

# 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:
            heading_kalman = initialize_heading_kalman(0, angle, angle_speed)
            heading_kalman.predict()
            continue
            
        dt = t - prev_t
        # update dt in kalman filter
        heading_kalman.A = np.array([[1, dt],[0, 1]])
        # update measurements
        heading_kalman.Y = np.array([[angle], [angle_speed]])
        heading_kalman.update()
        heading_kalman.predict()
        
        
        ang = heading_kalman.X[0][0]            
        
        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.

Answer 1

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.