0
$\begingroup$

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.

$\endgroup$
1
$\begingroup$

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.
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.